simple_trajectory.cpp
Go to the documentation of this file.
00001 // http://www.ros.org/wiki/pr2_controllers/Tutorials/Moving%20the%20arm%20using%20the%20Joint%20Trajectory%20Action
00002 
00003 #include <ros/ros.h>
00004 #include <control_msgs/FollowJointTrajectoryAction.h>
00005 #include <actionlib/client/simple_action_client.h>
00006 
00007 typedef actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > TrajClient;
00008 
00009 class ShadowTrajectory
00010 {
00011 private:
00012   // Action client for the joint trajectory action
00013   // used to trigger the arm movement action
00014   TrajClient* traj_client_;
00015 
00016 public:
00018   ShadowTrajectory()
00019   {
00020     // tell the action client that we want to spin a thread by default
00021     traj_client_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);
00022 
00023     // wait for action server to come up
00024     while(!traj_client_->waitForServer(ros::Duration(5.0))){
00025       ROS_INFO("Waiting for the joint_trajectory_action server");
00026     }
00027   }
00028 
00030   ~ShadowTrajectory()
00031   {
00032     delete traj_client_;
00033   }
00034 
00036   void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal)
00037   {
00038     // When to start the trajectory: 1s from now
00039     goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00040     traj_client_->sendGoal(goal);
00041   }
00042 
00044   void waitTrajectory() {
00045     while(!getState().isDone() && ros::ok()) { usleep(50000); }
00046   }
00047 
00049 
00054   control_msgs::FollowJointTrajectoryGoal fingerWiggleTrajectory()
00055   {
00056     //our goal variable
00057     control_msgs::FollowJointTrajectoryGoal goal;
00058 
00059     // First, the joint names, which apply to all waypoints
00060     goal.trajectory.joint_names.push_back("ffj0");
00061     goal.trajectory.joint_names.push_back("lfj0");
00062 
00063     // Set number of waypoints in this goal trajectory
00064     goal.trajectory.points.resize(3);
00065 
00066     // First trajectory point
00067     // Positions
00068     int ind = 0;
00069     goal.trajectory.points[ind].positions.resize(2);
00070     goal.trajectory.points[ind].positions[0] = 0.0;
00071     goal.trajectory.points[ind].positions[1] = 0.0;
00072     // Points also have velocities, but the shadow action server doesn't use them
00073     //goal.trajectory.points[ind].velocities.resize(2);
00074     //goal.trajectory.points[ind].velocities[0] = 0.0;
00075     //goal.trajectory.points[ind].velocities[0] = 0.0;
00076     // To be reached 1.0 second after starting along the trajectory
00077     goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);
00078 
00079     // 2nd trajectory point
00080     ind += 1;
00081     goal.trajectory.points[ind].positions.resize(2);
00082     goal.trajectory.points[ind].positions[0] = 1.0;
00083     goal.trajectory.points[ind].positions[1] = 1.0;
00084     goal.trajectory.points[ind].time_from_start = ros::Duration(3.0);
00085 
00086     // 3rd trajectory point
00087     ind += 1;
00088     goal.trajectory.points[ind].positions.resize(2);
00089     goal.trajectory.points[ind].positions[0] = 0.0;
00090     goal.trajectory.points[ind].positions[1] = 0.0;
00091     goal.trajectory.points[ind].time_from_start = ros::Duration(10.0);
00092 
00093     return goal;
00094   }
00095 
00097   control_msgs::FollowJointTrajectoryGoal armWaveTrajectory()
00098   {
00099     control_msgs::FollowJointTrajectoryGoal goal;
00100 
00101     goal.trajectory.joint_names.push_back("er");
00102     goal.trajectory.joint_names.push_back("es");
00103 
00104     goal.trajectory.points.resize(3);
00105 
00106     int ind = 0;
00107     goal.trajectory.points[ind].positions.resize(2);
00108     goal.trajectory.points[ind].positions[0] = 0.0;
00109     goal.trajectory.points[ind].positions[1] = 0.0;
00110     goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);
00111 
00112     ind += 1;
00113     goal.trajectory.points[ind].positions.resize(2);
00114     goal.trajectory.points[ind].positions[0] = 1.0;
00115     goal.trajectory.points[ind].positions[1] = 1.0;
00116     goal.trajectory.points[ind].time_from_start = ros::Duration(3.0);
00117 
00118     ind += 1;
00119     goal.trajectory.points[ind].positions.resize(2);
00120     goal.trajectory.points[ind].positions[0] = 0.0;
00121     goal.trajectory.points[ind].positions[1] = 0.0;
00122     goal.trajectory.points[ind].time_from_start = ros::Duration(10.0);
00123 
00124     return goal;
00125   }
00126 
00128   actionlib::SimpleClientGoalState getState()
00129   {
00130     return traj_client_->getState();
00131   }
00132  
00133 };
00134 
00135 int main(int argc, char** argv)
00136 {
00137   // Init the ROS node
00138   ros::init(argc, argv, "shadow_trajectory_driver");
00139 
00140   ShadowTrajectory sj;
00141   sj.startTrajectory(sj.fingerWiggleTrajectory());
00142   sj.waitTrajectory();
00143 
00144   sj.startTrajectory(sj.armWaveTrajectory());
00145   sj.waitTrajectory();
00146 }
00147 
00148 // vim: sw=2:ts=2


sr_mechanism_controllers
Author(s): Ugo Cupcic / ugo@shadowrobot.com , contact@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:54:39