joint_trajectory_action_controller.cpp
Go to the documentation of this file.
00001 
00013 #include "sr_mechanism_controllers/joint_trajectory_action_controller.hpp"
00014 #include <std_msgs/Float64.h>
00015 #include <boost/algorithm/string.hpp>
00016 #include <string>
00017 
00018 namespace shadowrobot
00019 {
00020   JointTrajectoryActionController::JointTrajectoryActionController()
00021   {
00022     action_server = boost::shared_ptr<JTAS> (
00023         new JTAS("/r_arm_controller/joint_trajectory_action",
00024         boost::bind(&JointTrajectoryActionController::execute_trajectory, this, _1),
00025         false)
00026     );
00027 
00028     //Create a map of joint names to their command publishers
00029     //Hand joints
00030     //TODO: this could be read from the controller manager
00031     // rosservice call /pr2_controller_manager/list_controllers
00032     std::string hand_names[] = {
00033       "ffj0", "ffj3", "ffj4",
00034       "lfj0", "lfj3", "lfj4", "lfj5",
00035       "mfj0", "mfj3", "mfj4",
00036       "rfj0", "rfj3", "rfj4",
00037       "thj1", "thj2", "thj3", "thj4", "thj5",
00038       "wrj1", "wrj2"
00039     };
00040     for (size_t i = 0; i < 20; i++)
00041     {
00042       joint_pub[hand_names[i]] = nh.advertise<std_msgs::Float64>(
00043           "/sh_"+hand_names[i]+"_mixed_position_velocity_controller/command", 2);
00044 
00045       joint_pub[ boost::to_upper_copy(hand_names[i]) ] = nh.advertise<std_msgs::Float64>(
00046           "/sh_"+hand_names[i]+"_mixed_position_velocity_controller/command", 2);
00047     }
00048 
00049     //Arm joints
00050     std::string arm_names[] = {"sr", "ss", "es", "er"};
00051     for (size_t i = 0; i < 4; i++)
00052     {
00053       joint_pub[arm_names[i]] = nh.advertise<std_msgs::Float64>(
00054           "/sa_"+arm_names[i]+"_position_controller/command", 2);
00055     }
00056 
00057     //Arm joints: 2 naming conventions
00058     std::string arm_names_2[] = {"ShoulderJRotate", "ShoulderJSwing", "ElbowJSwing", "ElbowJRotate"};
00059     for (size_t i = 0; i < 4; i++)
00060     {
00061       joint_pub[arm_names_2[i]] = nh.advertise<std_msgs::Float64>(
00062           "/sa_"+arm_names[i]+"_position_controller/command", 2);
00063     }
00064 
00065     ROS_DEBUG("Starting JointTrajectoryActionController server");
00066     action_server->start();
00067   }
00068 
00069   JointTrajectoryActionController::~JointTrajectoryActionController()
00070   {
00071 
00072   }
00073 
00074   void JointTrajectoryActionController::execute_trajectory(
00075       const control_msgs::FollowJointTrajectoryGoalConstPtr& goal
00076   ){
00077     bool success = true;
00078     std::vector<std::string> joint_names = goal->trajectory.joint_names;
00079     JointTrajectoryPointVec trajectory_points = goal->trajectory.points;
00080     trajectory_msgs::JointTrajectoryPoint trajectory_step;
00081 
00082     // TODO - We should probably be looking at goal->trajectory.header.stamp to
00083     // work out what time to start the action.
00084     //std::cout << goal->trajectory.header.stamp << " - " << ros::Time::now() << std::endl;
00085 
00086     //loop through the steps
00087     ros::Duration sleeping_time(0.0), last_time(0.0);
00088     for(size_t index_step = 0; index_step < trajectory_points.size(); ++index_step)
00089     {
00090       trajectory_step = trajectory_points[index_step];
00091 
00092       //check if preempted (cancelled), bail out if we are
00093       if (action_server->isPreemptRequested() || !ros::ok())
00094       {
00095         ROS_INFO("Joint Trajectory Action Preempted");
00096         action_server->setPreempted();
00097         success = false;
00098         break;
00099       }
00100 
00101       //send out the positions for this step to the joints
00102       for( size_t i = 0; i < joint_names.size(); i++ )
00103       {
00104         ROS_DEBUG_STREAM("trajectory: " << joint_names[i] << " " << trajectory_step.positions[i] << " / sleep: " << sleeping_time.toSec() );
00105         ros::Publisher pub = joint_pub[joint_names[i]];
00106         std_msgs::Float64 msg;
00107         msg.data = trajectory_step.positions[i];
00108         pub.publish(msg);
00109       }
00110       // Wait until this step is supposed to be completed.
00111       // TODO: This assumes that the movement will be instant! We should really
00112       // be working out how long the movement will take?
00113       sleeping_time =  trajectory_step.time_from_start - last_time;
00114       sleeping_time.sleep();
00115       last_time = trajectory_step.time_from_start;
00116     }
00117 
00118     //send the result back
00119     control_msgs::FollowJointTrajectoryResult joint_trajectory_result;
00120     if(success)
00121       action_server->setSucceeded(joint_trajectory_result);
00122     else
00123       action_server->setAborted(joint_trajectory_result);
00124   }
00125 }
00126 
00127 
00128 int main(int argc, char** argv)
00129 {
00130   ros::init(argc, argv, "sr_joint_trajectory_action_controller");
00131 
00132   ros::AsyncSpinner spinner(1); //Use 1 thread
00133   spinner.start();
00134   shadowrobot::JointTrajectoryActionController jac;
00135   ros::spin();
00136   //ros::waitForShutdown();
00137 
00138   return 0;
00139 }
00140 
00141 
00142 /* For the emacs weenies in the crowd.
00143 Local Variables:
00144    c-basic-offset: 2
00145 End:
00146 */
00147 // 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