63 moveit_msgs::ExecuteTrajectoryResult action_res;
64 if (!
context_->trajectory_execution_manager_)
66 const std::string
response =
"Cannot execute trajectory since ~allow_trajectory_execution was set to false";
67 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
75 if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
79 else if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
92 moveit_msgs::ExecuteTrajectoryResult& action_res)
96 context_->trajectory_execution_manager_->clear();
97 if (
context_->trajectory_execution_manager_->push(goal->trajectory))
100 context_->trajectory_execution_manager_->execute();
104 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
108 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
112 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
116 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
122 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
128 context_->trajectory_execution_manager_->stopExecution(
true);
133 moveit_msgs::ExecuteTrajectoryFeedback execute_feedback;
#define ROS_INFO_NAMED(name,...)
std::string stateToStr(MoveGroupState state) const
void executePathCallback(const moveit_msgs::ExecuteTrajectoryGoalConstPtr &goal)
#define ROS_INFO_STREAM_NAMED(name, args)
MoveGroupContextPtr context_
std::string capability_name_
MoveGroupExecuteTrajectoryAction()
std::string getActionResultString(const moveit_msgs::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only)
std::unique_ptr< actionlib::SimpleActionServer< moveit_msgs::ExecuteTrajectoryAction > > execute_action_server_
static const std::string EXECUTE_ACTION_NAME
void preemptExecuteTrajectoryCallback()
ros::NodeHandle root_node_handle_
void setExecuteTrajectoryState(MoveGroupState state)
virtual void initialize()
void executePath(const moveit_msgs::ExecuteTrajectoryGoalConstPtr &goal, moveit_msgs::ExecuteTrajectoryResult &action_res)
const std::string response
std::string asString() const
CLASS_LOADER_REGISTER_CLASS(Dog, Base)