moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 {
49 static const rclcpp::Logger LOGGER =
50  rclcpp::get_logger("moveit_move_group_default_capabilities.move_action_capability");
51 
53  : MoveGroupCapability("MoveAction"), move_state_(IDLE), preempt_requested_{ false }
54 {
55 }
56 
58 {
59  // start the move action server
60  auto node = context_->moveit_cpp_->getNode();
61  execute_action_server_ = rclcpp_action::create_server<MGAction>(
62  node, MOVE_ACTION,
63  [](const rclcpp_action::GoalUUID& /*unused*/, const std::shared_ptr<const MGAction::Goal>& /*unused*/) {
64  RCLCPP_INFO(LOGGER, "Received request");
65  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
66  },
67  [](const std::shared_ptr<MGActionGoal>& /*unused*/) {
68  RCLCPP_INFO(LOGGER, "Received request to cancel goal");
69  return rclcpp_action::CancelResponse::ACCEPT;
70  },
71  [this](const std::shared_ptr<MGActionGoal>& goal) { return executeMoveCallback(goal); });
72 }
73 
74 void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptr<MGActionGoal>& goal)
75 {
76  RCLCPP_INFO(LOGGER, "executing..");
77  setMoveState(PLANNING, goal);
78  // before we start planning, ensure that we have the latest robot state received...
79  auto node = context_->moveit_cpp_->getNode();
80  context_->planning_scene_monitor_->waitForCurrentRobotState(node->get_clock()->now());
81  context_->planning_scene_monitor_->updateFrameTransforms();
82 
83  auto action_res = std::make_shared<MGAction::Result>();
84  if (goal->get_goal()->planning_options.plan_only || !context_->allow_trajectory_execution_)
85  {
86  if (!goal->get_goal()->planning_options.plan_only)
87  RCLCPP_WARN(LOGGER, "This instance of MoveGroup is not allowed to execute trajectories "
88  "but the goal request has plan_only set to false. "
89  "Only a motion plan will be computed anyway.");
90  executeMoveCallbackPlanOnly(goal, action_res);
91  }
92  else
93  executeMoveCallbackPlanAndExecute(goal, action_res);
94 
95  bool planned_trajectory_empty = trajectory_processing::isTrajectoryEmpty(action_res->planned_trajectory);
96  // @todo: Response messages
97  RCLCPP_INFO_STREAM(LOGGER, getActionResultString(action_res->error_code, planned_trajectory_empty,
98  goal->get_goal()->planning_options.plan_only));
99  if (action_res->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
100  goal->succeed(action_res);
101  else if (action_res->error_code.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED)
102  goal->canceled(action_res);
103  else
104  goal->abort(action_res);
105 
106  setMoveState(IDLE, goal);
107  preempt_requested_ = false;
108 }
109 
110 void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_ptr<MGActionGoal>& goal,
111  std::shared_ptr<MGAction::Result>& action_res)
112 {
113  RCLCPP_INFO(LOGGER, "Combined planning and execution request received for MoveGroup action. "
114  "Forwarding to planning and execution pipeline.");
115 
116  if (moveit::core::isEmpty(goal->get_goal()->planning_options.planning_scene_diff))
117  {
118  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
119  const moveit::core::RobotState& current_state = lscene->getCurrentState();
120 
121  // check to see if the desired constraints are already met
122  for (std::size_t i = 0; i < goal->get_goal()->request.goal_constraints.size(); ++i)
123  if (lscene->isStateConstrained(
124  current_state, kinematic_constraints::mergeConstraints(goal->get_goal()->request.goal_constraints[i],
125  goal->get_goal()->request.path_constraints)))
126  {
127  RCLCPP_INFO(LOGGER, "Goal constraints are already satisfied. No need to plan or execute any motions");
128  action_res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
129  return;
130  }
131  }
132 
134 
135  const moveit_msgs::msg::MotionPlanRequest& motion_plan_request =
136  moveit::core::isEmpty(goal->get_goal()->request.start_state) ? goal->get_goal()->request :
137  clearRequestStartState(goal->get_goal()->request);
138  const moveit_msgs::msg::PlanningScene& planning_scene_diff =
139  moveit::core::isEmpty(goal->get_goal()->planning_options.planning_scene_diff.robot_state) ?
140  goal->get_goal()->planning_options.planning_scene_diff :
141  clearSceneRobotState(goal->get_goal()->planning_options.planning_scene_diff);
142 
143  opt.replan_ = goal->get_goal()->planning_options.replan;
144  opt.replan_attempts_ = goal->get_goal()->planning_options.replan_attempts;
145  opt.replan_delay_ = goal->get_goal()->planning_options.replan_delay;
146  opt.before_execution_callback_ = [this] { startMoveExecutionCallback(); };
147 
148  opt.plan_callback_ = [this, &motion_plan_request](plan_execution::ExecutableMotionPlan& plan) {
149  return planUsingPlanningPipeline(motion_plan_request, plan);
150  };
151 
153  if (preempt_requested_)
154  {
155  RCLCPP_INFO(LOGGER, "Preempt requested before the goal is planned and executed.");
156  action_res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED;
157  return;
158  }
159 
160  context_->plan_execution_->planAndExecute(plan, planning_scene_diff, opt);
161 
162  convertToMsg(plan.plan_components_, action_res->trajectory_start, action_res->planned_trajectory);
163  if (plan.executed_trajectory_)
164  plan.executed_trajectory_->getRobotTrajectoryMsg(action_res->executed_trajectory);
165  action_res->error_code = plan.error_code_;
166 }
167 
168 void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptr<MGActionGoal>& goal,
169  std::shared_ptr<MGAction::Result>& action_res)
170 {
171  RCLCPP_INFO(LOGGER, "Planning request received for MoveGroup action. Forwarding to planning pipeline.");
172 
174 
175  if (preempt_requested_)
176  {
177  RCLCPP_INFO(LOGGER, "Preempt requested before the goal is planned.");
178  action_res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED;
179  return;
180  }
181 
182  // Select planning_pipeline to handle request
183  const planning_pipeline::PlanningPipelinePtr planning_pipeline =
184  resolvePlanningPipeline(goal->get_goal()->request.pipeline_id);
185  if (!planning_pipeline)
186  {
187  action_res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
188  return;
189  }
190 
191  try
192  {
193  auto scene =
194  context_->planning_scene_monitor_->copyPlanningScene(goal->get_goal()->planning_options.planning_scene_diff);
195  planning_pipeline->generatePlan(scene, goal->get_goal()->request, res);
196  }
197  catch (std::exception& ex)
198  {
199  RCLCPP_ERROR(LOGGER, "Planning pipeline threw an exception: %s", ex.what());
200  res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
201  }
202 
203  convertToMsg(res.trajectory_, action_res->trajectory_start, action_res->planned_trajectory);
204  action_res->error_code = res.error_code_;
205  action_res->planning_time = res.planning_time_;
206 }
207 
208 bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::MotionPlanRequest& req,
210 {
211  setMoveState(PLANNING, nullptr);
212 
213  bool solved = false;
215 
216  // Select planning_pipeline to handle request
217  const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req.pipeline_id);
218  if (!planning_pipeline)
219  {
220  res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
221  return solved;
222  }
223 
224  try
225  {
226  solved = planning_pipeline->generatePlan(plan.copyPlanningScene(), req, res);
227  }
228  catch (std::exception& ex)
229  {
230  RCLCPP_ERROR(LOGGER, "Planning pipeline threw an exception: %s", ex.what());
231  res.error_code_.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
232  }
233  if (res.trajectory_)
234  {
235  plan.plan_components_.resize(1);
236  plan.plan_components_[0].trajectory_ = res.trajectory_;
237  plan.plan_components_[0].description_ = "plan";
238  }
239  plan.error_code_ = res.error_code_;
240 
241  return solved;
242 }
243 
244 void MoveGroupMoveAction::startMoveExecutionCallback()
245 {
246  setMoveState(MONITOR, nullptr);
247 }
248 
249 void MoveGroupMoveAction::startMoveLookCallback()
250 {
251  setMoveState(LOOK, nullptr);
252 }
253 
254 void MoveGroupMoveAction::preemptMoveCallback()
255 {
256  preempt_requested_ = true;
257  context_->plan_execution_->stop();
258 }
259 
260 void MoveGroupMoveAction::setMoveState(MoveGroupState state, const std::shared_ptr<MGActionGoal>& goal)
261 {
262  move_state_ = state;
263 
264  if (goal)
265  {
266  auto move_feedback = std::make_shared<MGAction::Feedback>();
267  move_feedback->state = stateToStr(state);
268  goal->publish_feedback(move_feedback);
269  }
270 }
271 } // namespace move_group
272 
273 #include <pluginlib/class_list_macros.hpp>
274 
PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin< kdl_kinematics_plugin::KDLKinematicsPlugin >, kinematics::KinematicsBase)
moveit_msgs::msg::PlanningScene clearSceneRobotState(const moveit_msgs::msg::PlanningScene &scene) const
planning_interface::MotionPlanRequest clearRequestStartState(const planning_interface::MotionPlanRequest &request) const
void convertToMsg(const std::vector< plan_execution::ExecutableTrajectory > &trajectory, moveit_msgs::msg::RobotState &first_state_msg, std::vector< moveit_msgs::msg::RobotTrajectory > &trajectory_msg) const
std::string getActionResultString(const moveit_msgs::msg::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only)
std::string stateToStr(MoveGroupState state) const
planning_pipeline::PlanningPipelinePtr resolvePlanningPipeline(const std::string &pipeline_id) const
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints &first, const moveit_msgs::msg::Constraints &second)
Merge two sets of constraints into one.
Definition: utils.cpp:56
bool isEmpty(const moveit_msgs::msg::PlanningScene &msg)
Check if a message includes any information about a planning scene, or whether it is empty.
scene
Definition: pick.py:52
Definition: plan.py:1
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
Planning pipeline.
const rclcpp::Logger LOGGER
Definition: async_test.h:31
bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory &trajectory)
A generic representation on what a computed motion plan looks like.
robot_trajectory::RobotTrajectoryPtr trajectory_
moveit_msgs::msg::MoveItErrorCodes error_code_