65 context_->planning_scene_monitor_->updateFrameTransforms();
67 moveit_msgs::MoveGroupResult action_res;
68 if (goal->planning_options.plan_only || !
context_->allow_trajectory_execution_)
70 if (!goal->planning_options.plan_only)
71 ROS_WARN(
"This instance of MoveGroup is not allowed to execute trajectories but the goal request has plan_only " 72 "set to false. Only a motion plan will be computed anyway.");
79 std::string response =
80 getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only);
81 if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
85 if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
97 moveit_msgs::MoveGroupResult& action_res)
99 ROS_INFO(
"Combined planning and execution request received for MoveGroup action. Forwarding to planning and " 100 "execution pipeline.");
105 const robot_state::RobotState& current_state = lscene->getCurrentState();
108 for (std::size_t i = 0; i < goal->request.goal_constraints.size(); ++i)
109 if (lscene->isStateConstrained(current_state,
111 goal->request.path_constraints)))
113 ROS_INFO(
"Goal constraints are already satisfied. No need to plan or execute any motions");
114 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
121 const moveit_msgs::MotionPlanRequest& motion_plan_request =
124 const moveit_msgs::PlanningScene& planning_scene_diff =
126 goal->planning_options.planning_scene_diff :
129 opt.
replan_ = goal->planning_options.replan;
136 if (goal->planning_options.look_around &&
context_->plan_with_sensing_)
139 _1, opt.
plan_callback_, goal->planning_options.look_around_attempts,
140 goal->planning_options.max_safe_execution_cost);
147 ROS_INFO(
"Preempt requested before the goal is planned and executed.");
148 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
152 context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);
161 moveit_msgs::MoveGroupResult& action_res)
163 ROS_INFO(
"Planning request received for MoveGroup action. Forwarding to planning pipeline.");
169 const planning_scene::PlanningSceneConstPtr& the_scene =
171 static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) :
172 lscene->diff(goal->planning_options.planning_scene_diff);
177 ROS_INFO(
"Preempt requested before the goal is planned.");
178 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
184 context_->planning_pipeline_->generatePlan(the_scene, goal->request, res);
186 catch (std::exception& ex)
188 ROS_ERROR(
"Planning pipeline threw an exception: %s", ex.what());
189 res.
error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
209 catch (std::exception& ex)
211 ROS_ERROR(
"Planning pipeline threw an exception: %s", ex.what());
212 res.
error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
robot_trajectory::RobotTrajectoryPtr trajectory_
void preemptMoveCallback()
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
static const std::string MOVE_ACTION
std::string stateToStr(MoveGroupState state) const
unsigned int replan_attempts_
moveit_msgs::MoveItErrorCodes error_code_
void executeMoveCallback_PlanAndExecute(const moveit_msgs::MoveGroupGoalConstPtr &goal, moveit_msgs::MoveGroupResult &action_res)
moveit_msgs::MoveItErrorCodes error_code_
MoveGroupContextPtr context_
planning_interface::MotionPlanRequest clearRequestStartState(const planning_interface::MotionPlanRequest &request) const
planning_scene::PlanningSceneConstPtr planning_scene_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
moveit_msgs::MoveGroupFeedback move_feedback_
bool computePlan(ExecutableMotionPlan &plan, const ExecutableMotionPlanComputationFn &motion_planner, unsigned int max_look_attempts, double max_safe_path_cost)
MoveGroupState move_state_
void executeMoveCallback_PlanOnly(const moveit_msgs::MoveGroupGoalConstPtr &goal, moveit_msgs::MoveGroupResult &action_res)
ExecutableMotionPlanComputationFn plan_callback_
robot_trajectory::RobotTrajectoryPtr executed_trajectory_
std::string getActionResultString(const moveit_msgs::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only)
bool isTrajectoryEmpty(const moveit_msgs::RobotTrajectory &trajectory)
void executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr &goal)
moveit_msgs::MotionPlanRequest MotionPlanRequest
void convertToMsg(const std::vector< plan_execution::ExecutableTrajectory > &trajectory, moveit_msgs::RobotState &first_state_msg, std::vector< moveit_msgs::RobotTrajectory > &trajectory_msg) const
virtual void initialize()
std::unique_ptr< actionlib::SimpleActionServer< moveit_msgs::MoveGroupAction > > move_action_server_
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
void setMoveState(MoveGroupState state)
boost::function< void()> before_execution_callback_
ros::NodeHandle root_node_handle_
moveit_msgs::PlanningScene clearSceneRobotState(const moveit_msgs::PlanningScene &scene) const
bool planUsingPlanningPipeline(const planning_interface::MotionPlanRequest &req, plan_execution::ExecutableMotionPlan &plan)
std::vector< ExecutableTrajectory > plan_components_
void startMoveExecutionCallback()
CLASS_LOADER_REGISTER_CLASS(Dog, Base)
void startMoveLookCallback()