move_action_capability.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "move_action_capability.h"
38 
46 
47 namespace move_group
48 {
50  : MoveGroupCapability("MoveAction"), move_state_(IDLE), preempt_requested_{ false }
51 {
52 }
53 
54 void MoveGroupMoveAction::initialize()
55 {
56  // start the move action server
57  move_action_server_ = std::make_unique<actionlib::SimpleActionServer<moveit_msgs::MoveGroupAction>>(
58  root_node_handle_, MOVE_ACTION, boost::bind(&MoveGroupMoveAction::executeMoveCallback, this, _1), false);
59  move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupMoveAction::preemptMoveCallback, this));
60  move_action_server_->start();
61 }
62 
63 void MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal)
64 {
65  setMoveState(PLANNING);
66  // before we start planning, ensure that we have the latest robot state received...
67  context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
68  context_->planning_scene_monitor_->updateFrameTransforms();
69 
70  moveit_msgs::MoveGroupResult action_res;
71  if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
72  {
73  if (!goal->planning_options.plan_only)
74  ROS_WARN_NAMED(getName(), "This instance of MoveGroup is not allowed to execute trajectories "
75  "but the goal request has plan_only set to false. "
76  "Only a motion plan will be computed anyway.");
77  executeMoveCallbackPlanOnly(goal, action_res);
78  }
79  else
80  executeMoveCallbackPlanAndExecute(goal, action_res);
81 
82  bool planned_trajectory_empty = trajectory_processing::isTrajectoryEmpty(action_res.planned_trajectory);
83  std::string response =
84  getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only);
85  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
86  move_action_server_->setSucceeded(action_res, response);
87  else
88  {
89  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
90  move_action_server_->setPreempted(action_res, response);
91  else
92  move_action_server_->setAborted(action_res, response);
93  }
94 
95  setMoveState(IDLE);
96 
97  preempt_requested_ = false;
98 }
99 
100 void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const moveit_msgs::MoveGroupGoalConstPtr& goal,
101  moveit_msgs::MoveGroupResult& action_res)
102 {
103  ROS_INFO_NAMED(getName(), "Combined planning and execution request received for MoveGroup action. "
104  "Forwarding to planning and execution pipeline.");
105 
106  if (moveit::core::isEmpty(goal->planning_options.planning_scene_diff))
107  {
108  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
109  const moveit::core::RobotState& current_state = lscene->getCurrentState();
110 
111  // check to see if the desired constraints are already met
112  for (std::size_t i = 0; i < goal->request.goal_constraints.size(); ++i)
113  if (lscene->isStateConstrained(current_state,
114  kinematic_constraints::mergeConstraints(goal->request.goal_constraints[i],
115  goal->request.path_constraints)))
116  {
117  ROS_INFO_NAMED(getName(), "Goal constraints are already satisfied. No need to plan or execute any motions");
118  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
119  return;
120  }
121  }
122 
124 
125  const moveit_msgs::MotionPlanRequest& motion_plan_request =
126  moveit::core::isEmpty(goal->request.start_state) ? goal->request : clearRequestStartState(goal->request);
127  const moveit_msgs::PlanningScene& planning_scene_diff =
128  moveit::core::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ?
129  goal->planning_options.planning_scene_diff :
130  clearSceneRobotState(goal->planning_options.planning_scene_diff);
131 
132  opt.replan_ = goal->planning_options.replan;
133  opt.replan_attempts_ = goal->planning_options.replan_attempts;
134  opt.replan_delay_ = goal->planning_options.replan_delay;
135  opt.before_execution_callback_ = boost::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this);
136 
137  opt.plan_callback_ =
138  boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, boost::cref(motion_plan_request), _1);
139  if (goal->planning_options.look_around && context_->plan_with_sensing_)
140  {
141  opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
142  _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
143  goal->planning_options.max_safe_execution_cost);
144  context_->plan_with_sensing_->setBeforeLookCallback(boost::bind(&MoveGroupMoveAction::startMoveLookCallback, this));
145  }
146 
148  if (preempt_requested_)
149  {
150  ROS_INFO_NAMED(getName(), "Preempt requested before the goal is planned and executed.");
151  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
152  return;
153  }
154 
155  context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);
156 
157  convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.planned_trajectory);
158  if (plan.executed_trajectory_)
159  plan.executed_trajectory_->getRobotTrajectoryMsg(action_res.executed_trajectory);
160  action_res.error_code = plan.error_code_;
161 }
162 
163 void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal,
164  moveit_msgs::MoveGroupResult& action_res)
165 {
166  ROS_INFO_NAMED(getName(), "Planning request received for MoveGroup action. Forwarding to planning pipeline.");
167 
168  // lock the scene so that it does not modify the world representation while diff() is called
169  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
170  const planning_scene::PlanningSceneConstPtr& the_scene =
171  (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) ?
172  static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) :
173  lscene->diff(goal->planning_options.planning_scene_diff);
175 
176  if (preempt_requested_)
177  {
178  ROS_INFO_NAMED(getName(), "Preempt requested before the goal is planned.");
179  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
180  return;
181  }
182 
183  // Select planning_pipeline to handle request
184  const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(goal->request.pipeline_id);
185  if (!planning_pipeline)
186  {
187  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
188  return;
189  }
190 
191  try
192  {
193  planning_pipeline->generatePlan(the_scene, goal->request, res);
194  }
195  catch (std::exception& ex)
196  {
197  ROS_ERROR_NAMED(getName(), "Planning pipeline threw an exception: %s", ex.what());
198  res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
199  }
200 
201  convertToMsg(res.trajectory_, action_res.trajectory_start, action_res.planned_trajectory);
202  action_res.error_code = res.error_code_;
203  action_res.planning_time = res.planning_time_;
204 }
205 
206 bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::MotionPlanRequest& req,
208 {
209  setMoveState(PLANNING);
210 
211  bool solved = false;
213 
214  // Select planning_pipeline to handle request
215  const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req.pipeline_id);
216  if (!planning_pipeline)
217  {
218  res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
219  return solved;
220  }
221 
223  try
224  {
225  solved = planning_pipeline->generatePlan(plan.planning_scene_, req, res);
226  }
227  catch (std::exception& ex)
228  {
229  ROS_ERROR_NAMED(getName(), "Planning pipeline threw an exception: %s", ex.what());
230  res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
231  }
232  if (res.trajectory_)
233  {
234  plan.plan_components_.resize(1);
235  plan.plan_components_[0].trajectory_ = res.trajectory_;
236  plan.plan_components_[0].description_ = "plan";
237  }
239  return solved;
240 }
241 
242 void MoveGroupMoveAction::startMoveExecutionCallback()
243 {
244  setMoveState(MONITOR);
245 }
246 
247 void MoveGroupMoveAction::startMoveLookCallback()
248 {
249  setMoveState(LOOK);
250 }
251 
252 void MoveGroupMoveAction::preemptMoveCallback()
253 {
254  preempt_requested_ = true;
255  context_->plan_execution_->stop();
256 }
257 
258 void MoveGroupMoveAction::setMoveState(MoveGroupState state)
259 {
260  move_state_ = state;
261  move_feedback_.state = stateToStr(state);
262  move_action_server_->publishFeedback(move_feedback_);
263 }
264 } // namespace move_group
265 
plan_execution::ExecutableMotionPlan::planning_scene_
planning_scene::PlanningSceneConstPtr planning_scene_
response
const std::string response
plan_execution::ExecutableMotionPlan::executed_trajectory_
robot_trajectory::RobotTrajectoryPtr executed_trajectory_
move_group::MoveGroupState
MoveGroupState
Definition: move_group_capability.h:79
plan_execution::ExecutableMotionPlan::planning_scene_monitor_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
move_group::LOOK
@ LOOK
Definition: move_group_capability.h:116
move_group::MoveGroupCapability
Definition: move_group_capability.h:89
move_group::IDLE
@ IDLE
Definition: move_group_capability.h:113
planning_interface::MotionPlanResponse
utils.h
trajectory_processing::isTrajectoryEmpty
bool isTrajectoryEmpty(const moveit_msgs::RobotTrajectory &trajectory)
planning_interface::MotionPlanResponse::error_code_
moveit_msgs::MoveItErrorCodes error_code_
plan_execution::PlanExecution::Options
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
plan_execution::PlanExecution::Options::replan_
bool replan_
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
move_group::MoveGroupMoveAction::MoveGroupMoveAction
MoveGroupMoveAction()
Definition: move_action_capability.cpp:81
plan_with_sensing.h
move_group::MOVE_ACTION
static const std::string MOVE_ACTION
Definition: capability_names.h:84
message_checks.h
moveit::core::isEmpty
bool isEmpty(const geometry_msgs::Pose &msg)
move_group::MoveGroupMoveAction
Definition: move_action_capability.h:78
planning_interface::MotionPlanResponse::trajectory_
robot_trajectory::RobotTrajectoryPtr trajectory_
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
plan_execution::ExecutableMotionPlan::plan_components_
std::vector< ExecutableTrajectory > plan_components_
plan_execution::PlanWithSensing::computePlan
bool computePlan(ExecutableMotionPlan &plan, const ExecutableMotionPlanComputationFn &motion_planner, unsigned int max_look_attempts, double max_safe_path_cost)
kinematic_constraints::mergeConstraints
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
move_group
Definition: capability_names.h:41
plan_execution::PlanExecution::Options::plan_callback_
ExecutableMotionPlanComputationFn plan_callback_
class_loader.hpp
plan_execution::PlanExecution::Options::replan_delay_
double replan_delay_
planning_pipeline.h
planning_interface::MotionPlanRequest
moveit_msgs::MotionPlanRequest MotionPlanRequest
move_group::MONITOR
@ MONITOR
Definition: move_group_capability.h:115
plan_execution::PlanExecution::Options::replan_attempts_
unsigned int replan_attempts_
plan_execution::ExecutableMotionPlan::error_code_
moveit_msgs::MoveItErrorCodes error_code_
planning_pipeline
plan_execution::PlanExecution::Options::before_execution_callback_
boost::function< void()> before_execution_callback_
plan_execution::ExecutableMotionPlan
planning_scene_monitor::LockedPlanningSceneRO
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
CLASS_LOADER_REGISTER_CLASS
CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddIterativeSplineParameterization, planning_request_adapter::PlanningRequestAdapter)
move_action_capability.h
trajectory_tools.h
planning_interface::MotionPlanResponse::planning_time_
double planning_time_
move_group::PLANNING
@ PLANNING
Definition: move_group_capability.h:114
capability_names.h
plan_execution.h
ros::Time::now
static Time now()


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Tue Oct 12 2021 02:25:39