52 moveit_msgs::GetMotionPlan::Response& res)
54 ROS_INFO(
"Received new planning service request...");
56 if (req.motion_plan_request.start_state.is_diff ==
true)
58 context_->planning_scene_monitor_->updateFrameTransforms();
64 context_->planning_pipeline_->generatePlan(ps, req.motion_plan_request, mp_res);
67 catch (std::exception& ex)
69 ROS_ERROR(
"Planning pipeline threw an exception: %s", ex.what());
70 res.motion_plan_response.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
virtual void initialize()
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void getMessage(moveit_msgs::MotionPlanResponse &msg) const
MoveGroupContextPtr context_
bool computePlanService(moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::GetMotionPlan::Response &res)
ros::ServiceServer plan_service_
static const std::string PLANNER_SERVICE_NAME
ros::NodeHandle root_node_handle_
CLASS_LOADER_REGISTER_CLASS(Dog, Base)