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, [this](const auto& goal) { executeMoveCallback(goal); }, false);
59  move_action_server_->registerPreemptCallback([this] { preemptMoveCallback(); });
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 
107 
108  const moveit_msgs::MotionPlanRequest& motion_plan_request =
109  moveit::core::isEmpty(goal->request.start_state) ? goal->request : clearRequestStartState(goal->request);
110  const moveit_msgs::PlanningScene& planning_scene_diff =
111  moveit::core::isEmpty(goal->planning_options.planning_scene_diff.robot_state) ?
112  goal->planning_options.planning_scene_diff :
113  clearSceneRobotState(goal->planning_options.planning_scene_diff);
114 
115  opt.replan_ = goal->planning_options.replan;
116  opt.replan_attempts_ = goal->planning_options.replan_attempts;
117  opt.replan_delay_ = goal->planning_options.replan_delay;
118  opt.before_execution_callback_ = [this] { startMoveExecutionCallback(); };
119 
120  opt.plan_callback_ = [this, &motion_plan_request](plan_execution::ExecutableMotionPlan& plan) {
121  return planUsingPlanningPipeline(motion_plan_request, plan);
122  };
123  if (goal->planning_options.look_around && context_->plan_with_sensing_)
124  {
125  opt.plan_callback_ = [plan_with_sensing = context_->plan_with_sensing_.get(), planner = opt.plan_callback_,
126  attempts = goal->planning_options.look_around_attempts,
127  safe_execution_cost = goal->planning_options.max_safe_execution_cost](
129  return plan_with_sensing->computePlan(plan, planner, attempts, safe_execution_cost);
130  };
131  context_->plan_with_sensing_->setBeforeLookCallback([this] { return startMoveLookCallback(); });
132  }
133 
135  if (preempt_requested_)
136  {
137  ROS_INFO_NAMED(getName(), "Preempt requested before the goal is planned and executed.");
138  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
139  return;
140  }
141 
142  context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);
143 
144  convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.planned_trajectory);
145  if (plan.executed_trajectory_)
146  plan.executed_trajectory_->getRobotTrajectoryMsg(action_res.executed_trajectory);
147  action_res.error_code = plan.error_code_;
148 }
149 
150 void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal,
151  moveit_msgs::MoveGroupResult& action_res)
152 {
153  ROS_INFO_NAMED(getName(), "Planning request received for MoveGroup action. Forwarding to planning pipeline.");
154 
155  // lock the scene so that it does not modify the world representation while diff() is called
156  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
157  const planning_scene::PlanningSceneConstPtr& the_scene =
158  (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) ?
159  static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) :
160  lscene->diff(goal->planning_options.planning_scene_diff);
162 
163  if (preempt_requested_)
164  {
165  ROS_INFO_NAMED(getName(), "Preempt requested before the goal is planned.");
166  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
167  return;
168  }
169 
170  // Select planning_pipeline to handle request
171  const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(goal->request.pipeline_id);
172  if (!planning_pipeline)
173  {
174  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
175  return;
176  }
177 
178  try
179  {
180  planning_pipeline->generatePlan(the_scene, goal->request, res);
181  }
182  catch (std::exception& ex)
183  {
184  ROS_ERROR_NAMED(getName(), "Planning pipeline threw an exception: %s", ex.what());
185  res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
186  }
187 
188  convertToMsg(res.trajectory_, action_res.trajectory_start, action_res.planned_trajectory);
189  action_res.error_code = res.error_code_;
190  action_res.planning_time = res.planning_time_;
191 }
192 
193 bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::MotionPlanRequest& req,
195 {
196  setMoveState(PLANNING);
197 
198  bool solved = false;
200 
201  // Select planning_pipeline to handle request
202  const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req.pipeline_id);
203  if (!planning_pipeline)
204  {
205  res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
206  return solved;
207  }
208 
210  try
211  {
212  solved = planning_pipeline->generatePlan(plan.planning_scene_, req, res);
213  }
214  catch (std::exception& ex)
215  {
216  ROS_ERROR_NAMED(getName(), "Planning pipeline threw an exception: %s", ex.what());
217  res.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
218  }
219  if (res.trajectory_)
220  {
221  plan.plan_components_.resize(1);
222  plan.plan_components_[0].trajectory_ = res.trajectory_;
223  plan.plan_components_[0].description_ = "plan";
224  }
226  return solved;
227 }
228 
229 void MoveGroupMoveAction::startMoveExecutionCallback()
230 {
231  setMoveState(MONITOR);
232 }
233 
234 void MoveGroupMoveAction::startMoveLookCallback()
235 {
236  setMoveState(LOOK);
237 }
238 
239 void MoveGroupMoveAction::preemptMoveCallback()
240 {
241  preempt_requested_ = true;
242  context_->plan_execution_->stop();
243 }
244 
245 void MoveGroupMoveAction::setMoveState(MoveGroupState state)
246 {
247  move_state_ = state;
248  move_feedback_.state = stateToStr(state);
249  move_action_server_->publishFeedback(move_feedback_);
250 }
251 } // namespace move_group
252 
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)
plan_execution::PlanExecution::Options
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
planning_interface::MotionPlanResponse::error_code_
moveit::core::MoveItErrorCode error_code_
plan_execution::PlanExecution::Options::replan_
bool replan_
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
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_
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 Sun Mar 3 2024 03:24:41