44 , spinner_(1 , &callback_queue_)
60 ops.template init<moveit_msgs::ExecuteKnownTrajectory::Request, moveit_msgs::ExecuteKnownTrajectory::Response>(
68 moveit_msgs::ExecuteKnownTrajectory::Response& res)
70 ROS_INFO(
"Received new trajectory execution service request...");
71 if (!
context_->trajectory_execution_manager_)
73 ROS_ERROR(
"Cannot execute trajectory since ~allow_trajectory_execution was set to false");
74 res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
81 context_->trajectory_execution_manager_->clear();
82 if (
context_->trajectory_execution_manager_->push(req.trajectory))
84 context_->trajectory_execution_manager_->execute();
85 if (req.wait_for_execution)
89 res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
91 res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
93 res.error_code.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
95 res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
100 ROS_INFO(
"Trajectory was successfully forwarded to the controller");
101 res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
106 res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
CallbackQueueInterface * callback_queue
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
MoveGroupContextPtr context_
bool executeTrajectoryService(moveit_msgs::ExecuteKnownTrajectory::Request &req, moveit_msgs::ExecuteKnownTrajectory::Response &res)
~MoveGroupExecuteService()
virtual void initialize()
ros::ServiceServer execute_service_
ros::AsyncSpinner spinner_
#define ROS_INFO_STREAM(args)
ros::NodeHandle root_node_handle_
MoveGroupExecuteService()
std::string asString() const
CLASS_LOADER_REGISTER_CLASS(Dog, Base)
ros::CallbackQueue callback_queue_
static const std::string EXECUTE_SERVICE_NAME