41 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
53 moveit_msgs::msg::RobotState& first_state_msg,
54 std::vector<moveit_msgs::msg::RobotTrajectory>& trajectory_msg)
const
56 if (!trajectory.empty())
59 trajectory_msg.resize(trajectory.size());
60 for (std::size_t i = 0; i < trajectory.size(); ++i)
62 if (trajectory[i].trajectory)
64 if (first && !trajectory[i].trajectory->empty())
69 trajectory[i].trajectory->getRobotTrajectoryMsg(trajectory_msg[i]);
76 moveit_msgs::msg::RobotState& first_state_msg,
77 moveit_msgs::msg::RobotTrajectory& trajectory_msg)
const
79 if (trajectory && !trajectory->empty())
82 trajectory->getRobotTrajectoryMsg(trajectory_msg);
87 moveit_msgs::msg::RobotState& first_state_msg,
88 moveit_msgs::msg::RobotTrajectory& trajectory_msg)
const
90 if (trajectory.size() > 1)
93 "Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!");
95 if (!trajectory.empty())
97 convertToMsg(trajectory[0].trajectory, first_state_msg, trajectory_msg);
105 r.start_state = moveit_msgs::msg::RobotState();
106 r.start_state.is_diff =
true;
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");
113 moveit_msgs::msg::PlanningScene
116 moveit_msgs::msg::PlanningScene r = scene;
117 r.robot_state = moveit_msgs::msg::RobotState();
118 r.robot_state.is_diff =
true;
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");
126 bool planned_trajectory_empty,
bool plan_only)
128 switch (error_code.val)
130 case moveit_msgs::msg::MoveItErrorCodes::SUCCESS:
131 if (planned_trajectory_empty)
133 return "Requested path and goal constraints are already met.";
139 return "Motion plan was computed successfully.";
143 return "Solution was found and executed.";
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)
152 return "No motion plan found. No execution attempted.";
156 return "Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.";
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";
185 const std::string& target_frame)
const
187 if (!context_ || !context_->planning_scene_monitor_->getTFClient())
189 if (pose_msg.header.frame_id == target_frame)
191 if (pose_msg.header.frame_id.empty())
193 pose_msg.header.frame_id = target_frame;
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);
205 catch (tf2::TransformException& ex)
213 planning_pipeline::PlanningPipelinePtr
216 if (pipeline_id.empty())
219 return context_->planning_pipeline_;
226 auto pipeline = context_->moveit_cpp_->getPlanningPipelines().at(pipeline_id);
227 RCLCPP_INFO(
moveit::getLogger(),
"Using planning pipeline '%s'", pipeline_id.c_str());
230 catch (
const std::out_of_range&)
232 RCLCPP_WARN(
moveit::getLogger(),
"Couldn't find requested planning pipeline '%s'", pipeline_id.c_str());
236 return planning_pipeline::PlanningPipelinePtr();
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
MoveGroupContextPtr context_
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()
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest