simple_trajectory.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <control_msgs/FollowJointTrajectoryAction.h>
00003 #include <actionlib/client/simple_action_client.h>
00004 
00005 typedef actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > TrajClient;
00006 
00007 class RobotArm
00008 {
00009 private:
00010   // Action client for the joint trajectory action 
00011   // used to trigger the arm movement action
00012   TrajClient* traj_client_;
00013 
00014 public:
00016   RobotArm() 
00017   {
00018     // tell the action client that we want to spin a thread by default
00019     //traj_client_ = new TrajClient("/wam_wrapper/follow_joint_trajectory", true);
00020     traj_client_ = new TrajClient("/iri_wam_controller/follow_joint_trajectory", true);
00021 
00022     // wait for action server to come up
00023     while(!traj_client_->waitForServer(ros::Duration(5.0))){
00024       ROS_INFO("Waiting for the joint_trajectory_action server");
00025     }
00026   }
00027 
00029   ~RobotArm()
00030   {
00031     delete traj_client_;
00032   }
00033 
00035   void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal)
00036   {
00037     // When to start the trajectory: 1s from now
00038     goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00039     traj_client_->sendGoal(goal);
00040   }
00041 
00043 
00048   control_msgs::FollowJointTrajectoryGoal armExtensionTrajectory()
00049   {
00050     //our goal variable
00051     control_msgs::FollowJointTrajectoryGoal goal;
00052     
00053     // frame_id
00054     goal.trajectory.header.frame_id = "/wam_link_footprint";
00055     // joint names
00056     goal.trajectory.joint_names.resize(7);
00057     goal.trajectory.joint_names[0] = "wam_joint_1";
00058     goal.trajectory.joint_names[1] = "wam_joint_2";
00059     goal.trajectory.joint_names[2] = "wam_joint_3";
00060     goal.trajectory.joint_names[3] = "wam_joint_4";
00061     goal.trajectory.joint_names[4] = "wam_joint_5";
00062     goal.trajectory.joint_names[5] = "wam_joint_6";
00063     goal.trajectory.joint_names[6] = "wam_joint_7";
00064 
00065     // Waypoints in this goal trajectory
00066     goal.trajectory.points.resize(5);
00067 
00068     // First trajectory point
00069     // Positions
00070     int ind = 0;
00071     goal.trajectory.points[ind].positions.resize(7);
00072     goal.trajectory.points[ind].positions[0] = 0.0;
00073     goal.trajectory.points[ind].positions[1] = 0.0;
00074     goal.trajectory.points[ind].positions[2] = 0.0;
00075     goal.trajectory.points[ind].positions[3] = 0.0;
00076     goal.trajectory.points[ind].positions[4] = 0.0;
00077     goal.trajectory.points[ind].positions[5] = 0.0;
00078     goal.trajectory.points[ind].positions[6] = 0.0;
00079 
00080     ind += 1;
00081     goal.trajectory.points[ind].positions.resize(7);
00082     goal.trajectory.points[ind].positions[0] =  0.49259;
00083     goal.trajectory.points[ind].positions[1] =  0.212558;
00084     goal.trajectory.points[ind].positions[2] = -0.169996;
00085     goal.trajectory.points[ind].positions[3] =  2.09482;
00086     goal.trajectory.points[ind].positions[4] = -0.14391;
00087     goal.trajectory.points[ind].positions[5] =  0.919756;
00088     goal.trajectory.points[ind].positions[6] = -0.0255835;
00089 
00090     ind += 1;
00091     goal.trajectory.points[ind].positions.resize(7);
00092     goal.trajectory.points[ind].positions[0] =  1.67741;
00093     goal.trajectory.points[ind].positions[1] =  0.357458;
00094     goal.trajectory.points[ind].positions[2] = -0.273308;
00095     goal.trajectory.points[ind].positions[3] =  1.74678;
00096     goal.trajectory.points[ind].positions[4] = -0.113625;
00097     goal.trajectory.points[ind].positions[5] =  1.08573;
00098     goal.trajectory.points[ind].positions[6] = -2.00137;
00099 
00100     ind += 1;
00101     goal.trajectory.points[ind].positions.resize(7);
00102     goal.trajectory.points[ind].positions[0] =  1.67752;
00103     goal.trajectory.points[ind].positions[1] =  0.581419;
00104     goal.trajectory.points[ind].positions[2] = -0.327541;
00105     goal.trajectory.points[ind].positions[3] =  1.31991;
00106     goal.trajectory.points[ind].positions[4] = -0.099946;
00107     goal.trajectory.points[ind].positions[5] =  1.26008;
00108     goal.trajectory.points[ind].positions[6] = -2.00137;
00109 
00110     ind += 1;
00111     goal.trajectory.points[ind].positions.resize(7);
00112     goal.trajectory.points[ind].positions[0] = 1.67748;
00113     goal.trajectory.points[ind].positions[1] = 0.280732;
00114     goal.trajectory.points[ind].positions[2] = -0.297391;
00115     goal.trajectory.points[ind].positions[3] = 2.00713;
00116     goal.trajectory.points[ind].positions[4] = -0.0907737;
00117     goal.trajectory.points[ind].positions[5] = 0.802098;
00118     goal.trajectory.points[ind].positions[6] = -2.00106;
00119 
00120     return goal;
00121   }
00122 
00124   actionlib::SimpleClientGoalState getState()
00125   {
00126     return traj_client_->getState();
00127   }
00128  
00129 };
00130 
00131 int main(int argc, char** argv)
00132 {
00133   // Init the ROS node
00134   ros::init(argc, argv, "robot_driver");
00135 
00136   RobotArm arm;
00137   // Start the trajectory
00138   arm.startTrajectory(arm.armExtensionTrajectory());
00139   // Wait for trajectory completion
00140   while(!arm.getState().isDone() && ros::ok())
00141   {
00142     usleep(50000);
00143   }
00144 }
00145 


iri_wam_controller
Author(s): galenya
autogenerated on Fri Dec 6 2013 20:08:29