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 
40 
41 void move_group::MoveGroupCapability::setContext(const MoveGroupContextPtr& context)
42 {
43  context_ = context;
44 }
45 
46 void move_group::MoveGroupCapability::convertToMsg(const std::vector<plan_execution::ExecutableTrajectory>& trajectory,
47  moveit_msgs::RobotState& first_state_msg,
48  std::vector<moveit_msgs::RobotTrajectory>& trajectory_msg) const
49 {
50  if (!trajectory.empty())
51  {
52  bool first = true;
53  trajectory_msg.resize(trajectory.size());
54  for (std::size_t i = 0; i < trajectory.size(); ++i)
55  {
56  if (trajectory[i].trajectory_)
57  {
58  if (first && !trajectory[i].trajectory_->empty())
59  {
60  robot_state::robotStateToRobotStateMsg(trajectory[i].trajectory_->getFirstWayPoint(), first_state_msg);
61  first = false;
62  }
63  trajectory[i].trajectory_->getRobotTrajectoryMsg(trajectory_msg[i]);
64  }
65  }
66  }
67 }
68 
69 void move_group::MoveGroupCapability::convertToMsg(const robot_trajectory::RobotTrajectoryPtr& trajectory,
70  moveit_msgs::RobotState& first_state_msg,
71  moveit_msgs::RobotTrajectory& trajectory_msg) const
72 {
73  if (trajectory && !trajectory->empty())
74  {
75  robot_state::robotStateToRobotStateMsg(trajectory->getFirstWayPoint(), first_state_msg);
76  trajectory->getRobotTrajectoryMsg(trajectory_msg);
77  }
78 }
79 
80 void move_group::MoveGroupCapability::convertToMsg(const std::vector<plan_execution::ExecutableTrajectory>& trajectory,
81  moveit_msgs::RobotState& first_state_msg,
82  moveit_msgs::RobotTrajectory& trajectory_msg) const
83 {
84  if (trajectory.size() > 1)
85  ROS_ERROR_STREAM("Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!");
86  if (!trajectory.empty())
87  convertToMsg(trajectory[0].trajectory_, first_state_msg, trajectory_msg);
88 }
89 
92 {
94  r.start_state = moveit_msgs::RobotState();
95  r.start_state.is_diff = true;
96  ROS_WARN("Execution of motions should always start at the robot's current state. Ignoring the state supplied as "
97  "start state in the motion planning request");
98  return r;
99 }
100 
101 moveit_msgs::PlanningScene
102 move_group::MoveGroupCapability::clearSceneRobotState(const moveit_msgs::PlanningScene& scene) const
103 {
104  moveit_msgs::PlanningScene r = scene;
105  r.robot_state = moveit_msgs::RobotState();
106  r.robot_state.is_diff = true;
107  ROS_WARN("Execution of motions should always start at the robot's current state. Ignoring the state supplied as "
108  "difference in the planning scene diff");
109  return r;
110 }
111 
112 std::string move_group::MoveGroupCapability::getActionResultString(const moveit_msgs::MoveItErrorCodes& error_code,
113  bool planned_trajectory_empty, bool plan_only)
114 {
115  if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
116  {
117  if (planned_trajectory_empty)
118  return "Requested path and goal constraints are already met.";
119  else
120  {
121  if (plan_only)
122  return "Motion plan was computed succesfully.";
123  else
124  return "Solution was found and executed.";
125  }
126  }
127  else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME)
128  return "Must specify group in motion plan request";
129  else if (error_code.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED ||
130  error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN)
131  {
132  if (planned_trajectory_empty)
133  return "No motion plan found. No execution attempted.";
134  else
135  return "Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.";
136  }
137  else if (error_code.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
138  return "Motion plan was found but it seems to be too costly and looking around did not help.";
139  else if (error_code.val == moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
140  return "Solution found but the environment changed during execution and the path was aborted";
141  else if (error_code.val == moveit_msgs::MoveItErrorCodes::CONTROL_FAILED)
142  return "Solution found but controller failed during execution";
143  else if (error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT)
144  return "Timeout reached";
145  else if (error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
146  return "Preempted";
147  else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS)
148  return "Invalid goal constraints";
149  else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME)
150  return "Invalid object name";
151  else if (error_code.val == moveit_msgs::MoveItErrorCodes::FAILURE)
152  return "Catastrophic failure";
153  return "Unknown event";
154 }
155 
157 {
158  switch (state)
159  {
160  case IDLE:
161  return "IDLE";
162  case PLANNING:
163  return "PLANNING";
164  case MONITOR:
165  return "MONITOR";
166  case LOOK:
167  return "LOOK";
168  default:
169  return "UNKNOWN";
170  }
171 }
172 
173 bool move_group::MoveGroupCapability::performTransform(geometry_msgs::PoseStamped& pose_msg,
174  const std::string& target_frame) const
175 {
176  if (!context_ || !context_->planning_scene_monitor_->getTFClient())
177  return false;
178  if (pose_msg.header.frame_id == target_frame)
179  return true;
180  if (pose_msg.header.frame_id.empty())
181  {
182  pose_msg.header.frame_id = target_frame;
183  return true;
184  }
185 
186  try
187  {
188  geometry_msgs::TransformStamped common_tf = context_->planning_scene_monitor_->getTFClient()->lookupTransform(
189  pose_msg.header.frame_id, target_frame, ros::Time(0.0));
190  geometry_msgs::PoseStamped pose_msg_in(pose_msg);
191  pose_msg_in.header.stamp = common_tf.header.stamp;
192  context_->planning_scene_monitor_->getTFClient()->transform(pose_msg_in, pose_msg, target_frame);
193  }
194  catch (tf2::TransformException& ex)
195  {
196  ROS_ERROR("TF Problem: %s", ex.what());
197  return false;
198  }
199  return true;
200 }
#define ROS_ERROR(...)
#define ROS_WARN(...)
planning_interface::MotionPlanRequest clearRequestStartState(const planning_interface::MotionPlanRequest &request) const
bool performTransform(geometry_msgs::PoseStamped &pose_msg, const std::string &target_frame) const
std::string stateToStr(MoveGroupState state) const
std::string getActionResultString(const moveit_msgs::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only)
moveit_msgs::PlanningScene clearSceneRobotState(const moveit_msgs::PlanningScene &scene) const
moveit_msgs::MotionPlanRequest MotionPlanRequest
void setContext(const MoveGroupContextPtr &context)
void convertToMsg(const std::vector< plan_execution::ExecutableTrajectory > &trajectory, moveit_msgs::RobotState &first_state_msg, std::vector< moveit_msgs::RobotTrajectory > &trajectory_msg) const
#define ROS_ERROR_STREAM(args)
r


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sat Jul 20 2019 03:56:26