38 #ifndef MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE 39 #define MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE 42 #include <control_msgs/FollowJointTrajectoryAction.h> 66 if (!trajectory.multi_dof_joint_trajectory.points.empty())
68 ROS_WARN_NAMED(
"FollowJointTrajectoryController",
"%s cannot execute multi-dof trajectories.",
name_.c_str());
75 "sending continuation for the currently executed trajectory to " <<
name_);
77 control_msgs::FollowJointTrajectoryGoal goal;
78 goal.trajectory = trajectory.joint_trajectory;
90 const control_msgs::FollowJointTrajectoryResultConstPtr& result)
95 switch (result->error_code)
97 case control_msgs::FollowJointTrajectoryResult::INVALID_GOAL:
100 case control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS:
103 case control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP:
106 case control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED:
109 case control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED:
134 #endif // MOVEIT_PLUGINS_FOLLOW_TRAJECTORY_CONTROLLER_HANDLE
virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_WARN_NAMED(name,...)
void controllerFeedbackCallback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &feedback)
moveit_controller_manager::ExecutionStatus last_exec_
void controllerDoneCallback(const actionlib::SimpleClientGoalState &state, const control_msgs::FollowJointTrajectoryResultConstPtr &result)
void controllerActiveCallback()
#define ROS_WARN_STREAM(args)
FollowJointTrajectoryControllerHandle(const std::string &name, const std::string &action_ns)
std::shared_ptr< actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > > controller_action_client_
void finishControllerExecution(const actionlib::SimpleClientGoalState &state)