44 #include <moveit_msgs/msg/motion_plan_response.hpp>
45 #include <moveit_msgs/msg/motion_sequence_request.hpp>
51 #include <cartesian_limits_parameters.hpp>
55 using RobotTrajCont = std::vector<robot_trajectory::RobotTrajectoryPtr>;
59 moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
61 moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
64 moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
74 CommandListManager(
const rclcpp::Node::SharedPtr& node,
const moveit::core::RobotModelConstPtr& model);
109 const moveit_msgs::msg::MotionSequenceRequest& req_list);
112 using MotionResponseCont = std::vector<planning_interface::MotionPlanResponse>;
113 using RobotState_OptRef =
const std::optional<std::reference_wrapper<const moveit::core::RobotState>>;
114 using RadiiCont = std::vector<double>;
115 using GroupNamesCont = std::vector<std::string>;
125 void checkForOverlappingRadii(
const MotionResponseCont& resp_cont,
const RadiiCont& radii)
const;
136 MotionResponseCont solveSequenceItems(
const planning_scene::PlanningSceneConstPtr&
planning_scene,
138 const moveit_msgs::msg::MotionSequenceRequest& req_list)
const;
153 static RobotState_OptRef getPreviousEndState(
const MotionResponseCont& motion_plan_responses,
154 const std::string& group_name);
160 static void setStartState(
const MotionResponseCont& motion_plan_responses,
const std::string& group_name,
161 moveit_msgs::msg::RobotState& start_state);
172 const moveit_msgs::msg::MotionSequenceRequest& req_list);
181 const moveit_msgs::msg::MotionSequenceItem& item_A,
182 const moveit_msgs::msg::MotionSequenceItem& item_B);
187 static void checkForNegativeRadii(
const moveit_msgs::msg::MotionSequenceRequest& req_list);
192 static void checkLastBlendRadiusZero(
const moveit_msgs::msg::MotionSequenceRequest& req_list);
198 static void checkStartStatesOfGroup(
const moveit_msgs::msg::MotionSequenceRequest& req_list,
199 const std::string& group_name);
205 static void checkStartStates(
const moveit_msgs::msg::MotionSequenceRequest& req_list);
211 static GroupNamesCont getGroupNames(
const moveit_msgs::msg::MotionSequenceRequest& req_list);
215 const rclcpp::Node::SharedPtr node_;
218 moveit::core::RobotModelConstPtr model_;
224 std::shared_ptr<cartesian_limits::ParamListener> param_listener_;
225 cartesian_limits::Params params_;
228 inline void CommandListManager::checkLastBlendRadiusZero(
const moveit_msgs::msg::MotionSequenceRequest& req_list)
230 if (req_list.items.back().blend_radius != 0.0)
232 throw LastBlendRadiusNotZeroException(
"The last blending radius must be zero");
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
This class orchestrates the planning of single commands and command lists.
CommandListManager(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModelConstPtr &model)
RobotTrajCont solve(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_pipeline::PlanningPipelinePtr &planning_pipeline, const moveit_msgs::msg::MotionSequenceRequest &req_list)
Generates trajectories for the specified list of motion commands.
Helper class to encapsulate the merge and blend process of trajectories.
Maintain a sequence of waypoints and the time durations between these waypoints.
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN)
std::vector< robot_trajectory::RobotTrajectoryPtr > RobotTrajCont
This namespace includes the central class for representing planning contexts.