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


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:18:14