88 const planning_scene_monitor::CurrentStateMonitorPtr& csm);
92 const planning_scene_monitor::CurrentStateMonitorPtr& csm,
bool manage_controllers);
98 bool isManagingControllers()
const;
101 const moveit_controller_manager::MoveItControllerManagerPtr& getControllerManager()
const;
104 void processEvent(
const std::string& event);
110 bool ensureActiveControllersForGroup(
const std::string& group);
116 bool ensureActiveControllersForJoints(
const std::vector<std::string>& joints);
121 bool ensureActiveController(
const std::string& controller);
126 bool ensureActiveControllers(
const std::vector<std::string>& controllers);
129 bool isControllerActive(
const std::string& controller);
132 bool areControllersActive(
const std::vector<std::string>& controllers);
136 bool push(
const moveit_msgs::msg::RobotTrajectory& trajectory,
const std::string& controller =
"");
140 bool push(
const trajectory_msgs::msg::JointTrajectory& trajectory,
const std::string& controller =
"");
147 bool push(
const trajectory_msgs::msg::JointTrajectory& trajectory,
const std::vector<std::string>& controllers);
154 bool push(
const moveit_msgs::msg::RobotTrajectory& trajectory,
const std::vector<std::string>& controllers);
157 const std::vector<TrajectoryExecutionContext*>& getTrajectories()
const;
165 bool auto_clear =
true);
181 std::pair<int, int> getCurrentExpectedTrajectoryIndex()
const;
187 void stopExecution(
bool auto_clear =
true);
191 void enableExecutionDurationMonitoring(
bool flag);
194 bool executionDurationMonitoring()
const;
198 void setAllowedExecutionDurationScaling(
double scaling);
201 double allowedExecutionDurationScaling()
const;
205 void setAllowedGoalDurationMargin(
double margin);
208 double allowedGoalDurationMargin()
const;
212 void setExecutionVelocityScaling(
double scaling);
215 double executionVelocityScaling()
const;
218 void setAllowedStartTolerance(
double tolerance);
221 double allowedStartTolerance()
const;
224 void setWaitForTrajectoryCompletion(
bool flag);
227 bool waitForTrajectoryCompletion()
const;
231 return controller_mgr_node_;
235 struct ControllerInformation
238 std::set<std::string> joints_;
239 std::set<std::string> overlapping_controllers_;
241 rclcpp::Time last_update_{ 0, 0, RCL_ROS_TIME };
243 bool operator<(ControllerInformation& other)
const
245 if (joints_.size() != other.joints_.size())
246 return joints_.size() < other.joints_.size();
247 return name_ < other.name_;
253 void reloadControllerInformation();
256 bool validate(
const TrajectoryExecutionContext& context)
const;
257 bool configure(TrajectoryExecutionContext& context,
const moveit_msgs::msg::RobotTrajectory& trajectory,
258 const std::vector<std::string>& controllers);
260 void updateControllersState(
const rclcpp::Duration& age);
261 void updateControllerState(
const std::string& controller,
const rclcpp::Duration& age);
262 void updateControllerState(ControllerInformation& ci,
const rclcpp::Duration& age);
264 bool distributeTrajectory(
const moveit_msgs::msg::RobotTrajectory& trajectory,
265 const std::vector<std::string>& controllers,
266 std::vector<moveit_msgs::msg::RobotTrajectory>& parts);
268 bool findControllers(
const std::set<std::string>& actuated_joints, std::size_t controller_count,
269 const std::vector<std::string>& available_controllers,
270 std::vector<std::string>& selected_controllers);
271 bool checkControllerCombination(std::vector<std::string>& controllers,
const std::set<std::string>& actuated_joints);
272 void generateControllerCombination(std::size_t start_index, std::size_t controller_count,
273 const std::vector<std::string>& available_controllers,
274 std::vector<std::string>& selected_controllers,
276 const std::set<std::string>& actuated_joints);
277 bool selectControllers(
const std::set<std::string>& actuated_joints,
278 const std::vector<std::string>& available_controllers,
279 std::vector<std::string>& selected_controllers);
281 void executeThread(
const ExecutionCompleteCallback& callback,
const PathSegmentCompleteCallback& part_callback,
283 bool executePart(std::size_t part_index);
284 bool waitForRobotToStop(
const TrajectoryExecutionContext& context,
double wait_time = 1.0);
289 void stopExecutionInternal();
291 void receiveEvent(
const std_msgs::msg::String::ConstSharedPtr& event);
293 void loadControllerParams();
295 double getAllowedStartToleranceJoint(
const std::string& joint_name)
const;
296 void setAllowedStartToleranceJoint(
const std::string& joint_name,
double joint_start_tolerance);
297 void initializeAllowedStartToleranceJoints();
300 const std::string name_ =
"trajectory_execution_manager";
302 rclcpp::Node::SharedPtr node_;
303 rclcpp::Logger logger_;
304 rclcpp::Node::SharedPtr controller_mgr_node_;
305 std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> private_executor_;
306 std::thread private_executor_thread_;
307 moveit::core::RobotModelConstPtr robot_model_;
308 planning_scene_monitor::CurrentStateMonitorPtr csm_;
309 rclcpp::Subscription<std_msgs::msg::String>::SharedPtr event_topic_subscriber_;
310 std::map<std::string, ControllerInformation> known_controllers_;
311 bool manage_controllers_;
314 std::unique_ptr<std::thread> execution_thread_;
316 std::mutex execution_state_mutex_;
317 std::mutex execution_thread_mutex_;
320 std::condition_variable execution_complete_condition_;
323 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> active_handles_;
324 int current_context_;
325 std::vector<rclcpp::Time> time_index_;
326 mutable std::mutex time_index_mutex_;
327 bool execution_complete_;
329 std::vector<TrajectoryExecutionContext*> trajectories_;
331 std::unique_ptr<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager> > controller_manager_loader_;
332 moveit_controller_manager::MoveItControllerManagerPtr controller_manager_;
336 bool execution_duration_monitoring_;
338 double allowed_execution_duration_scaling_;
339 double allowed_goal_duration_margin_;
340 bool control_multi_dof_joint_variables_;
343 std::map<std::string, double> controller_allowed_execution_duration_scaling_;
344 std::map<std::string, double> controller_allowed_goal_duration_margin_;
346 double allowed_start_tolerance_;
348 std::map<std::string, double> allowed_start_tolerance_joints_;
349 double execution_velocity_scaling_;
350 bool wait_for_trajectory_completion_;
352 rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_;