GraspSponge.cpp
Go to the documentation of this file.
00001 
00002 /*
00003  * GraspSponge.cpp
00004  *
00005  *  Created on: Sep 27, 2012
00006  *      Author: tidyup
00007  */
00008 
00009 #include <ros/ros.h>
00010 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00011 #include <actionlib/client/simple_action_client.h>
00012 
00013 typedef actionlib::SimpleActionClient<
00014                 pr2_controllers_msgs::JointTrajectoryAction> TrajClient;
00015 
00016 class GraspSponge {
00017 private:
00018         // Action client for the joint trajectory action
00019         // used to trigger the arm movement action
00020         TrajClient* traj_client_;
00021 
00022 public:
00024         GraspSponge();
00025         ~GraspSponge();
00026 
00028         void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal);
00029         pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory();
00030 
00032         GraspSponge::GraspSponge() {
00033                 // tell the action client that we want to spin a thread by default
00034                 traj_client_ = new TrajClient(
00035                                 "r_arm_controller/joint_trajectory_action", true);
00036 
00037                 // wait for action server to come up
00038                 while (!traj_client_->waitForServer(ros::Duration(5.0))) {
00039                         ROS_INFO("Waiting for the joint_trajectory_action server");
00040                 }
00041         }
00042 };
00044 GraspSponge::~GraspSponge() {
00045         delete traj_client_;
00046 }
00047 
00049 void GraspSponge::startTrajectory(
00050                 pr2_controllers_msgs::JointTrajectoryGoal goal) {
00051         // When to start the trajectory: 1s from now
00052         goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00053         traj_client_->sendGoal(goal);
00054 }
00055 
00057 
00062 pr2_controllers_msgs::JointTrajectoryGoal GraspSponge::armExtensionTrajectory() {
00063         //our goal variable
00064         pr2_controllers_msgs::JointTrajectoryGoal goal;
00065 
00066         // First, the joint names, which apply to all waypoints
00067         goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
00068         goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
00069         goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
00070         goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
00071         goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
00072         goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
00073         goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
00074 
00075         // We will have two waypoints in this goal trajectory
00076         goal.trajectory.points.resize(2);
00077 
00078         // First trajectory point
00079         // Positions
00080         int ind = 0;
00081         goal.trajectory.points[ind].positions.resize(7);
00082         goal.trajectory.points[ind].positions[0] = 0.0;
00083         goal.trajectory.points[ind].positions[1] = 0.0;
00084         goal.trajectory.points[ind].positions[2] = 0.0;
00085         goal.trajectory.points[ind].positions[3] = 0.0;
00086         goal.trajectory.points[ind].positions[4] = 0.0;
00087         goal.trajectory.points[ind].positions[5] = 0.0;
00088         goal.trajectory.points[ind].positions[6] = 0.0;
00089         // Velocities
00090         goal.trajectory.points[ind].velocities.resize(7);
00091         for (size_t j = 0; j < 7; ++j) {
00092                 goal.trajectory.points[ind].velocities[j] = 0.0;
00093         }
00094         // To be reached 1 second after starting along the trajectory
00095         goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);
00096 
00097         // Second trajectory point
00098         // Positions
00099         ind += 1;
00100         goal.trajectory.points[ind].positions.resize(7);
00101         goal.trajectory.points[ind].positions[0] = -0.3;
00102         goal.trajectory.points[ind].positions[1] = 0.2;
00103         goal.trajectory.points[ind].positions[2] = -0.1;
00104         goal.trajectory.points[ind].positions[3] = -1.2;
00105         goal.trajectory.points[ind].positions[4] = 1.5;
00106         goal.trajectory.points[ind].positions[5] = -0.3;
00107         goal.trajectory.points[ind].positions[6] = 0.5;
00108         // Velocities
00109         goal.trajectory.points[ind].velocities.resize(7);
00110         for (size_t j = 0; j < 7; ++j) {
00111                 goal.trajectory.points[ind].velocities[j] = 0.0;
00112         }
00113         // To be reached 2 seconds after starting along the trajectory
00114         goal.trajectory.points[ind].time_from_start = ros::Duration(2.0);
00115 
00116         //we are done; return the goal
00117         return goal;
00118 }
00119 
00121 actionlib::SimpleClientGoalState GraspSponge::getState() {
00122         return traj_client_->getState();
00123 }
00124 
00125 int main(int argc, char** argv) {
00126         // Init the ROS node
00127         ros::init(argc, argv, "robot_driver");
00128 
00129         GraspSponge arm;
00130         // Start the trajectory
00131         arm.startTrajectory(arm.armExtensionTrajectory());
00132         // Wait for trajectory completion
00133         while (!arm.getState().isDone() && ros::ok()) {
00134                 usleep(50000);
00135         }
00136 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


coverage_3d_tools
Author(s): Juergen Hess
autogenerated on Wed Dec 26 2012 15:25:37