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
00052 class FollowJointTrajectoryControllerHandle : public ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>
00053 {
00054 public:
00055
00056 FollowJointTrajectoryControllerHandle(const std::string &name, const std::string &action_ns) :
00057 ActionBasedControllerHandle<control_msgs::FollowJointTrajectoryAction>(name, action_ns)
00058 {
00059 }
00060
00061 virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
00062 {
00063 ROS_DEBUG_STREAM("FollowJointTrajectoryController: new trajectory to " << name_);
00064
00065 if (!controller_action_client_)
00066 return false;
00067
00068 if (!trajectory.multi_dof_joint_trajectory.points.empty())
00069 {
00070 ROS_ERROR("FollowJointTrajectoryController: cannot execute multi-dof trajectories.");
00071 return false;
00072 }
00073
00074 if (done_)
00075 ROS_DEBUG_STREAM("FollowJointTrajectoryController: sending trajectory to " << name_);
00076 else
00077 ROS_DEBUG_STREAM("FollowJointTrajectoryController: sending continuation for the currently executed trajectory to " << name_);
00078
00079 control_msgs::FollowJointTrajectoryGoal goal;
00080 goal.trajectory = trajectory.joint_trajectory;
00081 controller_action_client_->sendGoal(goal,
00082 boost::bind(&FollowJointTrajectoryControllerHandle::controllerDoneCallback, this, _1, _2),
00083 boost::bind(&FollowJointTrajectoryControllerHandle::controllerActiveCallback, this),
00084 boost::bind(&FollowJointTrajectoryControllerHandle::controllerFeedbackCallback, this, _1));
00085 done_ = false;
00086 last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
00087 return true;
00088 }
00089
00090 protected:
00091
00092 void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
00093 const control_msgs::FollowJointTrajectoryResultConstPtr& result)
00094 {
00095 finishControllerExecution(state);
00096 }
00097
00098 void controllerActiveCallback()
00099 {
00100 ROS_DEBUG_STREAM("FollowJointTrajectoryController: " << name_ << " started execution");
00101 }
00102
00103 void controllerFeedbackCallback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback)
00104 {
00105
00106 }
00107 };
00108
00109
00110 }
00111
00112 #endif // MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE