42 #include <moveit_msgs/msg/robot_trajectory.hpp>
43 #include <sensor_msgs/msg/joint_state.hpp>
44 #include <std_msgs/msg/string.hpp>
45 #include <rclcpp/rclcpp.hpp>
47 #include <pluginlib/class_loader.hpp>
53 #include <moveit_trajectory_execution_manager_export.h>
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);
194 void enableExecutionDurationMonitoring(
bool flag);
198 void setAllowedExecutionDurationScaling(
double scaling);
202 void setAllowedGoalDurationMargin(
double margin);
206 void setExecutionVelocityScaling(
double scaling);
209 void setAllowedStartTolerance(
double tolerance);
212 void setWaitForTrajectoryCompletion(
bool flag);
216 return controller_mgr_node_;
220 struct ControllerInformation
223 std::set<std::string> joints_;
224 std::set<std::string> overlapping_controllers_;
226 rclcpp::Time last_update_{ 0, 0, RCL_ROS_TIME };
228 bool operator<(ControllerInformation& other)
const
230 if (joints_.size() != other.joints_.size())
231 return joints_.size() < other.joints_.size();
232 return name_ < other.name_;
238 void reloadControllerInformation();
241 bool validate(
const TrajectoryExecutionContext& context)
const;
242 bool configure(TrajectoryExecutionContext& context,
const moveit_msgs::msg::RobotTrajectory& trajectory,
243 const std::vector<std::string>& controllers);
245 void updateControllersState(
const rclcpp::Duration& age);
246 void updateControllerState(
const std::string& controller,
const rclcpp::Duration& age);
247 void updateControllerState(ControllerInformation& ci,
const rclcpp::Duration& age);
249 bool distributeTrajectory(
const moveit_msgs::msg::RobotTrajectory& trajectory,
250 const std::vector<std::string>& controllers,
251 std::vector<moveit_msgs::msg::RobotTrajectory>& parts);
253 bool findControllers(
const std::set<std::string>& actuated_joints, std::size_t controller_count,
254 const std::vector<std::string>& available_controllers,
255 std::vector<std::string>& selected_controllers);
256 bool checkControllerCombination(std::vector<std::string>& controllers,
const std::set<std::string>& actuated_joints);
257 void generateControllerCombination(std::size_t start_index, std::size_t controller_count,
258 const std::vector<std::string>& available_controllers,
259 std::vector<std::string>& selected_controllers,
261 const std::set<std::string>& actuated_joints);
262 bool selectControllers(
const std::set<std::string>& actuated_joints,
263 const std::vector<std::string>& available_controllers,
264 std::vector<std::string>& selected_controllers);
266 void executeThread(
const ExecutionCompleteCallback& callback,
const PathSegmentCompleteCallback& part_callback,
268 bool executePart(std::size_t part_index);
269 bool waitForRobotToStop(
const TrajectoryExecutionContext& context,
double wait_time = 1.0);
271 void stopExecutionInternal();
273 void receiveEvent(
const std_msgs::msg::String::ConstSharedPtr& event);
275 void loadControllerParams();
278 const std::string name_ =
"trajectory_execution_manager";
280 rclcpp::Node::SharedPtr node_;
281 rclcpp::Logger logger_;
282 rclcpp::Node::SharedPtr controller_mgr_node_;
283 std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> private_executor_;
284 std::thread private_executor_thread_;
285 moveit::core::RobotModelConstPtr robot_model_;
286 planning_scene_monitor::CurrentStateMonitorPtr csm_;
287 rclcpp::Subscription<std_msgs::msg::String>::SharedPtr event_topic_subscriber_;
288 std::map<std::string, ControllerInformation> known_controllers_;
289 bool manage_controllers_;
292 std::unique_ptr<std::thread> execution_thread_;
294 std::mutex execution_state_mutex_;
295 std::mutex execution_thread_mutex_;
298 std::condition_variable execution_complete_condition_;
301 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> active_handles_;
302 int current_context_;
303 std::vector<rclcpp::Time> time_index_;
304 mutable std::mutex time_index_mutex_;
305 bool execution_complete_;
307 std::vector<TrajectoryExecutionContext*> trajectories_;
309 std::unique_ptr<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager> > controller_manager_loader_;
310 moveit_controller_manager::MoveItControllerManagerPtr controller_manager_;
314 bool execution_duration_monitoring_;
316 double allowed_execution_duration_scaling_;
317 double allowed_goal_duration_margin_;
320 std::map<std::string, double> controller_allowed_execution_duration_scaling_;
321 std::map<std::string, double> controller_allowed_goal_duration_margin_;
323 double allowed_start_tolerance_;
324 double execution_velocity_scaling_;
325 bool wait_for_trajectory_completion_;
327 rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_;
std::function< void(const moveit_controller_manager::ExecutionStatus &)> ExecutionCompleteCallback
rclcpp::Node::SharedPtr getControllerManagerNode()
static const std::string EXECUTION_EVENT_TOPIC
std::function< void(std::size_t)> PathSegmentCompleteCallback
std::vector< std::vector< std::string > > selected_options
MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager)
The reported execution status.
Each controller known to MoveIt has a state. This structure describes that controller's state.
Data structure that represents information necessary to execute a trajectory.
std::vector< moveit_msgs::msg::RobotTrajectory > trajectory_parts_
std::vector< std::string > controllers_
The controllers to use for executing the different trajectory parts;.