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
00051 class FollowJointTrajectoryControllerHandle : public ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>
00052 {
00053 public:
00054
00055 FollowJointTrajectoryControllerHandle(const std::string &name, const std::string &action_ns) :
00056 ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>(name, action_ns)
00057 {
00058 }
00059
00060 virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
00061 {
00062 ROS_DEBUG_STREAM("FollowJointTrajectoryController: new trajectory to " << name_);
00063
00064 if (!controller_action_client_)
00065 return false;
00066
00067 if (!trajectory.multi_dof_joint_trajectory.points.empty())
00068 {
00069 ROS_WARN("FollowJointTrajectoryController: %s cannot execute multi-dof trajectories.", name_.c_str());
00070 }
00071
00072 if (done_)
00073 ROS_DEBUG_STREAM("FollowJointTrajectoryController: sending trajectory to " << name_);
00074 else
00075 ROS_DEBUG_STREAM("FollowJointTrajectoryController: sending continuation for the currently executed trajectory to " << name_);
00076
00077 control_msgs::FollowJointTrajectoryGoal goal;
00078 goal.trajectory = trajectory.joint_trajectory;
00079 controller_action_client_->sendGoal(goal,
00080 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
00090 void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
00091 const control_msgs::FollowJointTrajectoryResultConstPtr& result)
00092 {
00093
00094 switch( result->error_code )
00095 {
00096 case control_msgs::FollowJointTrajectoryResult::INVALID_GOAL:
00097 ROS_WARN_STREAM("Controller " << name_ << " failed with error code INVALID_GOAL");
00098 break;
00099 case control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS:
00100 ROS_WARN_STREAM("Controller " << name_ << " failed with error code INVALID_JOINTS");
00101 break;
00102 case control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP:
00103 ROS_WARN_STREAM("Controller " << name_ << " failed with error code OLD_HEADER_TIMESTAMP");
00104 break;
00105 case control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED:
00106 ROS_WARN_STREAM("Controller " << name_ << " failed with error code PATH_TOLERANCE_VIOLATED");
00107 break;
00108 case control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED:
00109 ROS_WARN_STREAM("Controller " << name_ << " failed with error code GOAL_TOLERANCE_VIOLATED");
00110 break;
00111 }
00112
00113 finishControllerExecution(state);
00114 }
00115
00116 void controllerActiveCallback()
00117 {
00118 ROS_DEBUG_STREAM("FollowJointTrajectoryController: " << name_ << " started execution");
00119 }
00120
00121 void controllerFeedbackCallback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback)
00122 {
00123 }
00124 };
00125
00126
00127 }
00128
00129 #endif // MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE