moveit2
The MoveIt Motion Planning Framework for ROS 2.
move_group_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 
41 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
42 #include <moveit/utils/logger.hpp>
43 
44 #include <sstream>
45 #include <string>
46 
47 void move_group::MoveGroupCapability::setContext(const MoveGroupContextPtr& context)
48 {
49  context_ = context;
50 }
51 
52 void move_group::MoveGroupCapability::convertToMsg(const std::vector<plan_execution::ExecutableTrajectory>& trajectory,
53  moveit_msgs::msg::RobotState& first_state_msg,
54  std::vector<moveit_msgs::msg::RobotTrajectory>& trajectory_msg) const
55 {
56  if (!trajectory.empty())
57  {
58  bool first = true;
59  trajectory_msg.resize(trajectory.size());
60  for (std::size_t i = 0; i < trajectory.size(); ++i)
61  {
62  if (trajectory[i].trajectory)
63  {
64  if (first && !trajectory[i].trajectory->empty())
65  {
66  moveit::core::robotStateToRobotStateMsg(trajectory[i].trajectory->getFirstWayPoint(), first_state_msg);
67  first = false;
68  }
69  trajectory[i].trajectory->getRobotTrajectoryMsg(trajectory_msg[i]);
70  }
71  }
72  }
73 }
74 
75 void move_group::MoveGroupCapability::convertToMsg(const robot_trajectory::RobotTrajectoryPtr& trajectory,
76  moveit_msgs::msg::RobotState& first_state_msg,
77  moveit_msgs::msg::RobotTrajectory& trajectory_msg) const
78 {
79  if (trajectory && !trajectory->empty())
80  {
81  moveit::core::robotStateToRobotStateMsg(trajectory->getFirstWayPoint(), first_state_msg);
82  trajectory->getRobotTrajectoryMsg(trajectory_msg);
83  }
84 }
85 
86 void move_group::MoveGroupCapability::convertToMsg(const std::vector<plan_execution::ExecutableTrajectory>& trajectory,
87  moveit_msgs::msg::RobotState& first_state_msg,
88  moveit_msgs::msg::RobotTrajectory& trajectory_msg) const
89 {
90  if (trajectory.size() > 1)
91  {
92  RCLCPP_ERROR_STREAM(moveit::getLogger(),
93  "Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!");
94  }
95  if (!trajectory.empty())
96  {
97  convertToMsg(trajectory[0].trajectory, first_state_msg, trajectory_msg);
98  }
99 }
100 
103 {
105  r.start_state = moveit_msgs::msg::RobotState();
106  r.start_state.is_diff = true;
107  RCLCPP_WARN(moveit::getLogger(),
108  "Execution of motions should always start at the robot's current state. Ignoring the state supplied as "
109  "start state in the motion planning request");
110  return r;
111 }
112 
113 moveit_msgs::msg::PlanningScene
114 move_group::MoveGroupCapability::clearSceneRobotState(const moveit_msgs::msg::PlanningScene& scene) const
115 {
116  moveit_msgs::msg::PlanningScene r = scene;
117  r.robot_state = moveit_msgs::msg::RobotState();
118  r.robot_state.is_diff = true;
119  RCLCPP_WARN(moveit::getLogger(),
120  "Execution of motions should always start at the robot's current state. Ignoring the state supplied as "
121  "difference in the planning scene diff");
122  return r;
123 }
124 
125 std::string move_group::MoveGroupCapability::getActionResultString(const moveit_msgs::msg::MoveItErrorCodes& error_code,
126  bool planned_trajectory_empty, bool plan_only)
127 {
128  switch (error_code.val)
129  {
130  case moveit_msgs::msg::MoveItErrorCodes::SUCCESS:
131  if (planned_trajectory_empty)
132  {
133  return "Requested path and goal constraints are already met.";
134  }
135  else
136  {
137  if (plan_only)
138  {
139  return "Motion plan was computed successfully.";
140  }
141  else
142  {
143  return "Solution was found and executed.";
144  }
145  }
146  case moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME:
147  return "Invalid group in motion plan request";
148  case moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED:
149  case moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN:
150  if (planned_trajectory_empty)
151  {
152  return "No motion plan found. No execution attempted.";
153  }
154  else
155  {
156  return "Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.";
157  }
158  case moveit_msgs::msg::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA:
159  return "Motion plan was found but it seems to be too costly and looking around did not help.";
160  case moveit_msgs::msg::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE:
161  return "Solution found but the environment changed during execution and the path was aborted";
162  default:
163  return moveit::core::errorCodeToString(error_code);
164  }
165 }
166 
168 {
169  switch (state)
170  {
171  case IDLE:
172  return "IDLE";
173  case PLANNING:
174  return "PLANNING";
175  case MONITOR:
176  return "MONITOR";
177  case LOOK:
178  return "LOOK";
179  default:
180  return "UNKNOWN";
181  }
182 }
183 
184 bool move_group::MoveGroupCapability::performTransform(geometry_msgs::msg::PoseStamped& pose_msg,
185  const std::string& target_frame) const
186 {
187  if (!context_ || !context_->planning_scene_monitor_->getTFClient())
188  return false;
189  if (pose_msg.header.frame_id == target_frame)
190  return true;
191  if (pose_msg.header.frame_id.empty())
192  {
193  pose_msg.header.frame_id = target_frame;
194  return true;
195  }
196 
197  try
198  {
199  geometry_msgs::msg::TransformStamped common_tf = context_->planning_scene_monitor_->getTFClient()->lookupTransform(
200  pose_msg.header.frame_id, target_frame, rclcpp::Time(0.0));
201  geometry_msgs::msg::PoseStamped pose_msg_in(pose_msg);
202  pose_msg_in.header.stamp = common_tf.header.stamp;
203  context_->planning_scene_monitor_->getTFClient()->transform(pose_msg_in, pose_msg, target_frame);
204  }
205  catch (tf2::TransformException& ex)
206  {
207  RCLCPP_ERROR(moveit::getLogger(), "TF Problem: %s", ex.what());
208  return false;
209  }
210  return true;
211 }
212 
213 planning_pipeline::PlanningPipelinePtr
214 move_group::MoveGroupCapability::resolvePlanningPipeline(const std::string& pipeline_id) const
215 {
216  if (pipeline_id.empty())
217  {
218  // Without specified planning pipeline we use the default
219  return context_->planning_pipeline_;
220  }
221  else
222  {
223  // Attempt to get the planning pipeline for the specified identifier
224  try
225  {
226  auto pipeline = context_->moveit_cpp_->getPlanningPipelines().at(pipeline_id);
227  RCLCPP_INFO(moveit::getLogger(), "Using planning pipeline '%s'", pipeline_id.c_str());
228  return pipeline;
229  }
230  catch (const std::out_of_range&)
231  {
232  RCLCPP_WARN(moveit::getLogger(), "Couldn't find requested planning pipeline '%s'", pipeline_id.c_str());
233  }
234  }
235 
236  return planning_pipeline::PlanningPipelinePtr();
237 }
void setContext(const MoveGroupContextPtr &context)
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
bool performTransform(geometry_msgs::msg::PoseStamped &pose_msg, const std::string &target_frame) 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
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
std::string errorCodeToString(MoveItErrorCode error_code)
Convenience function to translated error message into string.
const rclcpp::Logger & getLogger()
Definition: logger.cpp:88
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest