moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
trajectory_execution_manager.hpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ioan Sucan */
36
37#pragma once
38
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>
48
49#include <memory>
50#include <deque>
51#include <thread>
52
53#include <moveit_trajectory_execution_manager_export.h>
54
56{
57MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); // Defines TrajectoryExecutionManagerPtr, ConstPtr, WeakPtr... etc
58
59// Two modes:
60// Managed controllers
61// Unmanaged controllers: given the trajectory,
62class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager
63{
64public:
65 static const std::string EXECUTION_EVENT_TOPIC;
66
70
73 using PathSegmentCompleteCallback = std::function<void(std::size_t)>;
74
77 {
79 std::vector<std::string> controllers_;
80
81 // The trajectory to execute, split in different parts (by joints), each set of joints corresponding to one
82 // controller
83 std::vector<moveit_msgs::msg::RobotTrajectory> trajectory_parts_;
84 };
85
87 TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model,
88 const planning_scene_monitor::CurrentStateMonitorPtr& csm);
89
91 TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model,
92 const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers);
93
96
98 bool isManagingControllers() const;
99
101 const moveit_controller_manager::MoveItControllerManagerPtr& getControllerManager() const;
102
104 void processEvent(const std::string& event);
105
110 bool ensureActiveControllersForGroup(const std::string& group);
111
116 bool ensureActiveControllersForJoints(const std::vector<std::string>& joints);
117
121 bool ensureActiveController(const std::string& controller);
122
126 bool ensureActiveControllers(const std::vector<std::string>& controllers);
127
129 bool isControllerActive(const std::string& controller);
130
132 bool areControllersActive(const std::vector<std::string>& controllers);
133
136 bool push(const moveit_msgs::msg::RobotTrajectory& trajectory, const std::string& controller = "");
137
140 bool push(const trajectory_msgs::msg::JointTrajectory& trajectory, const std::string& controller = "");
141
147 bool push(const trajectory_msgs::msg::JointTrajectory& trajectory, const std::vector<std::string>& controllers);
148
154 bool push(const moveit_msgs::msg::RobotTrajectory& trajectory, const std::vector<std::string>& controllers);
155
157 const std::vector<TrajectoryExecutionContext*>& getTrajectories() const;
158
160 void execute(const ExecutionCompleteCallback& callback = ExecutionCompleteCallback(), bool auto_clear = true);
161
164 void execute(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback,
165 bool auto_clear = true);
166
169 moveit_controller_manager::ExecutionStatus executeAndWait(bool auto_clear = true);
170
174
181 std::pair<int, int> getCurrentExpectedTrajectoryIndex() const;
182
184 moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const;
185
187 void stopExecution(bool auto_clear = true);
188
191 void enableExecutionDurationMonitoring(bool flag);
192
194 bool executionDurationMonitoring() const;
195
198 void setAllowedExecutionDurationScaling(double scaling);
199
201 double allowedExecutionDurationScaling() const;
202
205 void setAllowedGoalDurationMargin(double margin);
206
208 double allowedGoalDurationMargin() const;
209
212 void setExecutionVelocityScaling(double scaling);
213
215 double executionVelocityScaling() const;
216
218 void setAllowedStartTolerance(double tolerance);
219
221 double allowedStartTolerance() const;
222
224 void setWaitForTrajectoryCompletion(bool flag);
225
227 bool waitForTrajectoryCompletion() const;
228
229 rclcpp::Node::SharedPtr getControllerManagerNode()
230 {
231 return controller_mgr_node_;
232 }
233
234private:
235 struct ControllerInformation
236 {
237 std::string name_;
238 std::set<std::string> joints_;
239 std::set<std::string> overlapping_controllers_;
241 rclcpp::Time last_update_{ 0, 0, RCL_ROS_TIME };
242
243 bool operator<(ControllerInformation& other) const
244 {
245 if (joints_.size() != other.joints_.size())
246 return joints_.size() < other.joints_.size();
247 return name_ < other.name_;
248 }
249 };
250
251 void initialize();
252
253 void reloadControllerInformation();
254
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);
259
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);
263
264 bool distributeTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory,
265 const std::vector<std::string>& controllers,
266 std::vector<moveit_msgs::msg::RobotTrajectory>& parts);
267
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,
275 std::vector<std::vector<std::string> >& selected_options,
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);
280
281 void executeThread(const ExecutionCompleteCallback& callback, const PathSegmentCompleteCallback& part_callback,
282 bool auto_clear);
283 bool executePart(std::size_t part_index);
284 bool waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time = 1.0);
285
287 void clear();
288
289 void stopExecutionInternal();
290
291 void receiveEvent(const std_msgs::msg::String::ConstSharedPtr& event);
292
293 void loadControllerParams();
294
295 double getAllowedStartToleranceJoint(const std::string& joint_name) const;
296 void setAllowedStartToleranceJoint(const std::string& joint_name, double joint_start_tolerance);
297 void initializeAllowedStartToleranceJoints();
298
299 // Name of this class for logging
300 const std::string name_ = "trajectory_execution_manager";
301
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_;
312
313 // thread used to execute trajectories using the execute() command
314 std::unique_ptr<std::thread> execution_thread_;
315
316 std::mutex execution_state_mutex_;
317 std::mutex execution_thread_mutex_;
318
319 // this condition is used to notify the completion of execution for given trajectories
320 std::condition_variable execution_complete_condition_;
321
322 moveit_controller_manager::ExecutionStatus last_execution_status_;
323 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> active_handles_;
324 int current_context_;
325 std::vector<rclcpp::Time> time_index_; // used to find current expected trajectory location
326 mutable std::mutex time_index_mutex_;
327 bool execution_complete_;
328
329 std::vector<TrajectoryExecutionContext*> trajectories_;
330
331 std::unique_ptr<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager> > controller_manager_loader_;
332 moveit_controller_manager::MoveItControllerManagerPtr controller_manager_;
333
334 bool verbose_;
335
336 bool execution_duration_monitoring_;
337 // 'global' values
338 double allowed_execution_duration_scaling_;
339 double allowed_goal_duration_margin_;
340 bool control_multi_dof_joint_variables_;
341 // controller-specific values
342 // override the 'global' values
343 std::map<std::string, double> controller_allowed_execution_duration_scaling_;
344 std::map<std::string, double> controller_allowed_goal_duration_margin_;
345
346 double allowed_start_tolerance_; // joint tolerance for validate(): radians for revolute joints
347 // tolerance per joint, overrides global allowed_start_tolerance_.
348 std::map<std::string, double> allowed_start_tolerance_joints_;
349 double execution_velocity_scaling_;
350 bool wait_for_trajectory_completion_;
351
352 rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_;
353};
354} // namespace trajectory_execution_manager
#define MOVEIT_CLASS_FORWARD(C)
std::function< void(const moveit_controller_manager::ExecutionStatus &)> ExecutionCompleteCallback
std::vector< std::vector< std::string > > selected_options
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< std::string > controllers_
The controllers to use for executing the different trajectory parts;.