Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE
00039 #define MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE
00040
00041 #include <moveit_simple_controller_manager/action_based_controller_handle.h>
00042 #include <control_msgs/FollowJointTrajectoryAction.h>
00043
00044 namespace moveit_simple_controller_manager
00045 {
00046
00047
00048
00049
00050 class FollowJointTrajectoryControllerHandle
00051 : public ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>
00052 {
00053 public:
00054 FollowJointTrajectoryControllerHandle(const std::string& name, const std::string& action_ns)
00055 : ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>(name, action_ns)
00056 {
00057 }
00058
00059 virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory)
00060 {
00061 ROS_DEBUG_STREAM("FollowJointTrajectoryController: new trajectory to " << name_);
00062
00063 if (!controller_action_client_)
00064 return false;
00065
00066 if (!trajectory.multi_dof_joint_trajectory.points.empty())
00067 {
00068 ROS_WARN("FollowJointTrajectoryController: %s cannot execute multi-dof trajectories.", name_.c_str());
00069 }
00070
00071 if (done_)
00072 ROS_DEBUG_STREAM("FollowJointTrajectoryController: sending trajectory to " << name_);
00073 else
00074 ROS_DEBUG_STREAM("FollowJointTrajectoryController: sending continuation for the currently executed trajectory to "
00075 << name_);
00076
00077 control_msgs::FollowJointTrajectoryGoal goal;
00078 goal.trajectory = trajectory.joint_trajectory;
00079 controller_action_client_->sendGoal(
00080 goal, boost::bind(&FollowJointTrajectoryControllerHandle::controllerDoneCallback, this, _1, _2),
00081 boost::bind(&FollowJointTrajectoryControllerHandle::controllerActiveCallback, this),
00082 boost::bind(&FollowJointTrajectoryControllerHandle::controllerFeedbackCallback, this, _1));
00083 done_ = false;
00084 last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
00085 return true;
00086 }
00087
00088 protected:
00089 void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
00090 const control_msgs::FollowJointTrajectoryResultConstPtr& result)
00091 {
00092
00093 if (result)
00094 {
00095 switch (result->error_code)
00096 {
00097 case control_msgs::FollowJointTrajectoryResult::INVALID_GOAL:
00098 ROS_WARN_STREAM("Controller " << name_ << " failed with error code INVALID_GOAL");
00099 break;
00100 case control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS:
00101 ROS_WARN_STREAM("Controller " << name_ << " failed with error code INVALID_JOINTS");
00102 break;
00103 case control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP:
00104 ROS_WARN_STREAM("Controller " << name_ << " failed with error code OLD_HEADER_TIMESTAMP");
00105 break;
00106 case control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED:
00107 ROS_WARN_STREAM("Controller " << name_ << " failed with error code PATH_TOLERANCE_VIOLATED");
00108 break;
00109 case control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED:
00110 ROS_WARN_STREAM("Controller " << name_ << " failed with error code GOAL_TOLERANCE_VIOLATED");
00111 break;
00112 }
00113 }
00114 else
00115 {
00116 ROS_WARN_STREAM("Controller " << name_ << ": no result returned");
00117 }
00118
00119 finishControllerExecution(state);
00120 }
00121
00122 void controllerActiveCallback()
00123 {
00124 ROS_DEBUG_STREAM("FollowJointTrajectoryController: " << name_ << " started execution");
00125 }
00126
00127 void controllerFeedbackCallback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback)
00128 {
00129 }
00130 };
00131
00132 }
00133
00134 #endif // MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE