46 moveit_msgs::RobotState& first_state_msg,
47 std::vector<moveit_msgs::RobotTrajectory>& trajectory_msg)
const 49 if (!trajectory.empty())
52 trajectory_msg.resize(trajectory.size());
53 for (std::size_t i = 0; i < trajectory.size(); ++i)
55 if (trajectory[i].trajectory_)
57 if (first && !trajectory[i].trajectory_->empty())
59 robot_state::robotStateToRobotStateMsg(trajectory[i].trajectory_->getFirstWayPoint(), first_state_msg);
62 trajectory[i].trajectory_->getRobotTrajectoryMsg(trajectory_msg[i]);
69 moveit_msgs::RobotState& first_state_msg,
70 moveit_msgs::RobotTrajectory& trajectory_msg)
const 72 if (trajectory && !trajectory->empty())
74 robot_state::robotStateToRobotStateMsg(trajectory->getFirstWayPoint(), first_state_msg);
75 trajectory->getRobotTrajectoryMsg(trajectory_msg);
80 moveit_msgs::RobotState& first_state_msg,
81 moveit_msgs::RobotTrajectory& trajectory_msg)
const 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);
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");
100 moveit_msgs::PlanningScene
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");
112 bool planned_trajectory_empty,
bool plan_only)
114 if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
116 if (planned_trajectory_empty)
117 return "Requested path and goal constraints are already met.";
121 return "Motion plan was computed succesfully.";
123 return "Solution was found and executed.";
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)
131 if (planned_trajectory_empty)
132 return "No motion plan found. No execution attempted.";
134 return "Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.";
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)
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";
173 const std::string& target_frame)
const 177 if (pose_msg.header.frame_id == target_frame)
179 if (pose_msg.header.frame_id.empty())
181 pose_msg.header.frame_id = target_frame;
189 context_->planning_scene_monitor_->getTFClient()->getLatestCommonTime(pose_msg.header.frame_id, target_frame,
190 common_time, &error);
192 ROS_ERROR(
"TF Problem: %s", error.c_str());
196 pose_tf.
stamp_ = common_time;
197 context_->planning_scene_monitor_->getTFClient()->transformPose(target_frame, pose_tf, pose_tf_out);
std::string stateToStr(MoveGroupState state) const
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
MoveGroupContextPtr context_
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)
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)
moveit_msgs::PlanningScene clearSceneRobotState(const moveit_msgs::PlanningScene &scene) const