moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
trajectory_execution_manager.cpp
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
40#include <geometric_shapes/check_isometry.h>
41#include <memory>
42#include <tf2_eigen/tf2_eigen.hpp>
44
46{
47
48const std::string TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC = "trajectory_execution_event";
49
50static const auto DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE = rclcpp::Duration::from_seconds(1);
51static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 0.5; // allow 0.5s more than the expected execution time
52 // before triggering a trajectory cancel (applied
53 // after scaling)
54static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING =
55 1.1; // allow the execution of a trajectory to take more time than expected (scaled by a value > 1)
56static const bool DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES = false;
57
59 const moveit::core::RobotModelConstPtr& robot_model,
60 const planning_scene_monitor::CurrentStateMonitorPtr& csm)
61 : node_(node)
62 , logger_(moveit::getLogger("moveit.ros.trajectory_execution_manager"))
63 , robot_model_(robot_model)
64 , csm_(csm)
65{
66 if (!node_->get_parameter("moveit_manage_controllers", manage_controllers_))
67 manage_controllers_ = false;
68 initialize();
69}
70
72 const moveit::core::RobotModelConstPtr& robot_model,
73 const planning_scene_monitor::CurrentStateMonitorPtr& csm,
74 bool manage_controllers)
75 : node_(node)
76 , logger_(moveit::getLogger("moveit.ros.trajectory_execution_manager"))
77 , robot_model_(robot_model)
78 , csm_(csm)
79 , manage_controllers_(manage_controllers)
80{
81 initialize();
82}
83
85{
86 stopExecution(true);
87 if (private_executor_)
88 private_executor_->cancel();
89 if (private_executor_thread_.joinable())
90 private_executor_thread_.join();
91}
92
93void TrajectoryExecutionManager::initialize()
94{
95 verbose_ = false;
96 execution_complete_ = true;
97 current_context_ = -1;
99 execution_duration_monitoring_ = true;
100 execution_velocity_scaling_ = 1.0;
101 allowed_start_tolerance_ = 0.01;
102 allowed_start_tolerance_joints_.clear();
103 wait_for_trajectory_completion_ = true;
104 control_multi_dof_joint_variables_ = DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES;
105
106 allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING;
107 allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN;
108
109 // load controller-specific values for allowed_execution_duration_scaling and allowed_goal_duration_margin
110 loadControllerParams();
111
112 // load the controller manager plugin
113 try
114 {
115 controller_manager_loader_ =
116 std::make_unique<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager>>(
117 "moveit_core", "moveit_controller_manager::MoveItControllerManager");
118 }
119 catch (pluginlib::PluginlibException& ex)
120 {
121 RCLCPP_FATAL_STREAM(logger_, "Exception while creating controller manager plugin loader: " << ex.what());
122 return;
123 }
124
125 if (controller_manager_loader_)
126 {
127 std::string controller;
128
129 if (!node_->get_parameter("moveit_controller_manager", controller))
130 {
131 const std::vector<std::string>& classes = controller_manager_loader_->getDeclaredClasses();
132 if (classes.size() == 1)
133 {
134 controller = classes[0];
135 RCLCPP_WARN(logger_,
136 "Parameter '~moveit_controller_manager' is not specified but only one "
137 "matching plugin was found: '%s'. Using that one.",
138 controller.c_str());
139 }
140 else
141 {
142 RCLCPP_FATAL(logger_, "Parameter '~moveit_controller_manager' not specified. This is needed to "
143 "identify the plugin to use for interacting with controllers. No paths can "
144 "be executed.");
145 return;
146 }
147 }
148
149 // Deprecation warnings, October 2022
150 if (controller == "moveit_ros_control_interface/MoveItControllerManager")
151 {
152 RCLCPP_WARN(logger_, "moveit_ros_control_interface/MoveItControllerManager is deprecated. Replace with "
153 "`moveit_ros_control_interface/Ros2ControlManager.`");
154 }
155 if (controller == "moveit_ros_control_interface/MoveItMultiControllerManager")
156 {
157 RCLCPP_WARN(logger_, "moveit_ros_control_interface/MoveItMultiControllerManager is deprecated. Replace with "
158 "`moveit_ros_control_interface/Ros2ControlMultiManager.`");
159 }
160
161 if (!controller.empty())
162 {
163 try
164 {
165 // We make a node called moveit_simple_controller_manager so it's able to
166 // receive callbacks on another thread. We then copy parameters from the move_group node
167 // and then add it to the multithreadedexecutor
168 rclcpp::NodeOptions opt;
169 opt.allow_undeclared_parameters(true);
170 opt.automatically_declare_parameters_from_overrides(true);
171 controller_mgr_node_ = std::make_shared<rclcpp::Node>("moveit_simple_controller_manager", opt);
172
173 auto all_params = node_->get_node_parameters_interface()->get_parameter_overrides();
174 for (const auto& param : all_params)
175 controller_mgr_node_->set_parameter(rclcpp::Parameter(param.first, param.second));
176
177 controller_manager_ = controller_manager_loader_->createUniqueInstance(controller);
178 controller_manager_->initialize(controller_mgr_node_);
179 private_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
180 private_executor_->add_node(controller_mgr_node_);
181
182 // start executor on a different thread now
183 private_executor_thread_ = std::thread([this]() { private_executor_->spin(); });
184 }
185 catch (pluginlib::PluginlibException& ex)
186 {
187 RCLCPP_FATAL_STREAM(logger_, "Exception while loading controller manager '" << controller << "': " << ex.what());
188 }
189 }
190 }
191
192 // other configuration steps
193 reloadControllerInformation();
194 // The default callback group for rclcpp::Node is MutuallyExclusive which means we cannot call
195 // receiveEvent while processing a different callback. To fix this we create a new callback group (the type is not
196 // important since we only use it to process one callback) and associate event_topic_subscriber_ with this callback group
197 auto callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
198 auto options = rclcpp::SubscriptionOptions();
199 options.callback_group = callback_group;
200 event_topic_subscriber_ = node_->create_subscription<std_msgs::msg::String>(
201 EXECUTION_EVENT_TOPIC, rclcpp::SystemDefaultsQoS(),
202 [this](const std_msgs::msg::String::ConstSharedPtr& event) { return receiveEvent(event); }, options);
203
204 controller_mgr_node_->get_parameter("trajectory_execution.execution_duration_monitoring",
205 execution_duration_monitoring_);
206 controller_mgr_node_->get_parameter("trajectory_execution.allowed_execution_duration_scaling",
207 allowed_execution_duration_scaling_);
208 controller_mgr_node_->get_parameter("trajectory_execution.allowed_goal_duration_margin",
209 allowed_goal_duration_margin_);
210 controller_mgr_node_->get_parameter("trajectory_execution.allowed_start_tolerance", allowed_start_tolerance_);
211 controller_mgr_node_->get_parameter("trajectory_execution.control_multi_dof_joint_variables",
212 control_multi_dof_joint_variables_);
213
214 initializeAllowedStartToleranceJoints();
215
216 if (manage_controllers_)
217 {
218 RCLCPP_INFO(logger_, "Trajectory execution is managing controllers");
219 }
220 else
221 {
222 RCLCPP_INFO(logger_, "Trajectory execution is not managing controllers");
223 }
224
225 auto controller_mgr_parameter_set_callback = [this](const std::vector<rclcpp::Parameter>& parameters) {
226 auto result = rcl_interfaces::msg::SetParametersResult();
227 result.successful = true;
228 for (const auto& parameter : parameters)
229 {
230 const std::string& name = parameter.get_name();
231 if (name == "trajectory_execution.execution_duration_monitoring")
232 {
233 enableExecutionDurationMonitoring(parameter.as_bool());
234 }
235 else if (name == "trajectory_execution.allowed_execution_duration_scaling")
236 {
237 setAllowedExecutionDurationScaling(parameter.as_double());
238 }
239 else if (name == "trajectory_execution.allowed_goal_duration_margin")
240 {
241 setAllowedGoalDurationMargin(parameter.as_double());
242 }
243 else if (name == "trajectory_execution.execution_velocity_scaling")
244 {
245 setExecutionVelocityScaling(parameter.as_double());
246 }
247 else if (name == "trajectory_execution.allowed_start_tolerance")
248 {
249 setAllowedStartTolerance(parameter.as_double());
250 }
251 else if (name.find("trajectory_execution.allowed_start_tolerance_joints.") == 0)
252 {
253 setAllowedStartToleranceJoint(name, parameter.as_double());
254 }
255 else if (name == "trajectory_execution.wait_for_trajectory_completion")
256 {
257 setWaitForTrajectoryCompletion(parameter.as_bool());
258 }
259 else
260 {
261 result.successful = false;
262 }
263 }
264 return result;
265 };
266 callback_handler_ = controller_mgr_node_->add_on_set_parameters_callback(controller_mgr_parameter_set_callback);
267}
268
270{
271 execution_duration_monitoring_ = flag;
272}
273
275{
276 return execution_duration_monitoring_;
277}
278
280{
281 allowed_execution_duration_scaling_ = scaling;
282}
283
285{
286 return allowed_execution_duration_scaling_;
287}
288
290{
291 allowed_goal_duration_margin_ = margin;
292}
293
295{
296 return allowed_goal_duration_margin_;
297}
298
300{
301 execution_velocity_scaling_ = scaling;
302}
303
305{
306 return execution_velocity_scaling_;
307}
308
310{
311 allowed_start_tolerance_ = tolerance;
312}
313
315{
316 return allowed_start_tolerance_;
317}
318
320{
321 wait_for_trajectory_completion_ = flag;
322}
323
325{
326 return wait_for_trajectory_completion_;
327}
328
330{
331 return manage_controllers_;
332}
333
334const moveit_controller_manager::MoveItControllerManagerPtr& TrajectoryExecutionManager::getControllerManager() const
335{
336 return controller_manager_;
337}
338
339void TrajectoryExecutionManager::processEvent(const std::string& event)
340{
341 if (event == "stop")
342 {
343 stopExecution(true);
344 }
345 else
346 {
347 RCLCPP_WARN_STREAM(logger_, "Unknown event type: '" << event << '\'');
348 }
349}
350
351void TrajectoryExecutionManager::receiveEvent(const std_msgs::msg::String::ConstSharedPtr& event)
352{
353 RCLCPP_INFO_STREAM(logger_, "Received event '" << event->data << '\'');
354 processEvent(event->data);
355}
356
357bool TrajectoryExecutionManager::push(const moveit_msgs::msg::RobotTrajectory& trajectory, const std::string& controller)
358{
359 if (controller.empty())
360 {
361 return push(trajectory, std::vector<std::string>());
362 }
363 else
364 {
365 return push(trajectory, std::vector<std::string>(1, controller));
366 }
367}
368
369bool TrajectoryExecutionManager::push(const trajectory_msgs::msg::JointTrajectory& trajectory,
370 const std::string& controller)
371{
372 if (controller.empty())
373 {
374 return push(trajectory, std::vector<std::string>());
375 }
376 else
377 {
378 return push(trajectory, std::vector<std::string>(1, controller));
379 }
380}
381
382bool TrajectoryExecutionManager::push(const trajectory_msgs::msg::JointTrajectory& trajectory,
383 const std::vector<std::string>& controllers)
384{
385 moveit_msgs::msg::RobotTrajectory traj;
386 traj.joint_trajectory = trajectory;
387 return push(traj, controllers);
388}
389
390bool TrajectoryExecutionManager::push(const moveit_msgs::msg::RobotTrajectory& trajectory,
391 const std::vector<std::string>& controllers)
392{
393 if (!execution_complete_)
394 {
395 RCLCPP_ERROR(logger_, "Cannot push a new trajectory while another is being executed");
396 return false;
397 }
398
399 // Optionally, convert multi dof waypoints to joint states and replace trajectory for execution
400 std::optional<moveit_msgs::msg::RobotTrajectory> replaced_trajectory;
401 if (control_multi_dof_joint_variables_ && !trajectory.multi_dof_joint_trajectory.points.empty())
402 {
403 // We convert the trajectory message into a RobotTrajectory first,
404 // since the conversion to a combined JointTrajectory depends on the local variables
405 // of the Multi-DOF joint types.
406 moveit::core::RobotState reference_state(robot_model_);
407 reference_state.setToDefaultValues();
408 robot_trajectory::RobotTrajectory tmp_trajectory(robot_model_);
409 tmp_trajectory.setRobotTrajectoryMsg(reference_state, trajectory);
410
411 // Combine all joints for filtering the joint trajectory waypoints
412 std::vector<std::string> all_trajectory_joints = trajectory.joint_trajectory.joint_names;
413 for (const auto& mdof_joint : trajectory.multi_dof_joint_trajectory.joint_names)
414 {
415 all_trajectory_joints.push_back(mdof_joint);
416 }
417
418 // Convert back to single joint trajectory including the MDOF joint variables, e.g. position/x, position/y, ...
419 const auto joint_trajectory =
420 robot_trajectory::toJointTrajectory(tmp_trajectory, true /* include_mdof_joints */, all_trajectory_joints);
421
422 // Check success of conversion
423 // This should never happen when using MoveIt's interfaces, but users can pass anything into TEM::push() directly
424 if (!joint_trajectory.has_value())
425 {
426 RCLCPP_ERROR(logger_, "Failed to convert multi-DOF trajectory to joint trajectory, aborting execution!");
427 return false;
428 }
429
430 // Create a new robot trajectory message that only contains the combined joint trajectory
431 RCLCPP_DEBUG(logger_, "Successfully converted multi-DOF trajectory to joint trajectory for execution.");
432 replaced_trajectory = moveit_msgs::msg::RobotTrajectory();
433 replaced_trajectory->joint_trajectory = joint_trajectory.value();
434 }
435
437 if (configure(*context, replaced_trajectory.value_or(trajectory), controllers))
438 {
439 if (verbose_)
440 {
441 std::stringstream ss;
442 ss << "Pushed trajectory for execution using controllers [ ";
443 for (const std::string& controller : context->controllers_)
444 ss << controller << ' ';
445 ss << "]:" << '\n';
446 // TODO: Provide message serialization
447 // for (const moveit_msgs::msg::RobotTrajectory& trajectory_part : context->trajectory_parts_)
448 // ss << trajectory_part << '\n';
449 RCLCPP_INFO_STREAM(logger_, ss.str());
450 }
451 trajectories_.push_back(context);
452 return true;
453 }
454 else
455 {
456 delete context;
458 }
459
460 return false;
461}
462
463void TrajectoryExecutionManager::reloadControllerInformation()
464{
465 known_controllers_.clear();
466 if (controller_manager_)
467 {
468 std::vector<std::string> names;
469 controller_manager_->getControllersList(names);
470 for (const std::string& name : names)
471 {
472 std::vector<std::string> joints;
473 controller_manager_->getControllerJoints(name, joints);
474 ControllerInformation ci;
475 ci.name_ = name;
476 ci.joints_.insert(joints.begin(), joints.end());
477 known_controllers_[ci.name_] = ci;
478 }
479
480 names.clear();
481 controller_manager_->getActiveControllers(names);
482 for (const auto& active_name : names)
483 {
484 auto found_it = std::find_if(known_controllers_.begin(), known_controllers_.end(),
485 [&](const auto& known_controller) { return known_controller.first == active_name; });
486 if (found_it != known_controllers_.end())
487 {
488 found_it->second.state_.active_ = true;
489 }
490 }
491
492 for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
493 it != known_controllers_.end(); ++it)
494 {
495 for (std::map<std::string, ControllerInformation>::iterator jt = known_controllers_.begin();
496 jt != known_controllers_.end(); ++jt)
497 {
498 if (it != jt)
499 {
500 std::vector<std::string> intersect;
501 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), jt->second.joints_.begin(),
502 jt->second.joints_.end(), std::back_inserter(intersect));
503 if (!intersect.empty())
504 {
505 it->second.overlapping_controllers_.insert(jt->first);
506 jt->second.overlapping_controllers_.insert(it->first);
507 }
508 }
509 }
510 }
511 }
512 else
513 {
514 RCLCPP_ERROR(logger_, "Failed to reload controllers: `controller_manager_` does not exist.");
515 }
516}
517
518void TrajectoryExecutionManager::updateControllerState(const std::string& controller, const rclcpp::Duration& age)
519{
520 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
521 if (it != known_controllers_.end())
522 {
523 updateControllerState(it->second, age);
524 }
525 else
526 {
527 RCLCPP_ERROR(logger_, "Controller '%s' is not known.", controller.c_str());
528 }
529}
530
531void TrajectoryExecutionManager::updateControllerState(ControllerInformation& ci, const rclcpp::Duration& age)
532{
533 if (node_->now() - ci.last_update_ >= age)
534 {
535 if (controller_manager_)
536 {
537 if (verbose_)
538 RCLCPP_INFO(logger_, "Updating information for controller '%s'.", ci.name_.c_str());
539 ci.state_ = controller_manager_->getControllerState(ci.name_);
540 ci.last_update_ = node_->now();
541 }
542 }
543 else if (verbose_)
544 RCLCPP_INFO(logger_, "Information for controller '%s' is assumed to be up to date.", ci.name_.c_str());
545}
546
547void TrajectoryExecutionManager::updateControllersState(const rclcpp::Duration& age)
548{
549 for (std::pair<const std::string, ControllerInformation>& known_controller : known_controllers_)
550 updateControllerState(known_controller.second, age);
551}
552
553bool TrajectoryExecutionManager::checkControllerCombination(std::vector<std::string>& selected,
554 const std::set<std::string>& actuated_joints)
555{
556 std::set<std::string> combined_joints;
557 for (const std::string& controller : selected)
558 {
559 const ControllerInformation& ci = known_controllers_[controller];
560 combined_joints.insert(ci.joints_.begin(), ci.joints_.end());
561 }
562
563 if (verbose_)
564 {
565 std::stringstream ss, saj, sac;
566 for (const std::string& controller : selected)
567 ss << controller << ' ';
568 for (const std::string& actuated_joint : actuated_joints)
569 saj << actuated_joint << ' ';
570 for (const std::string& combined_joint : combined_joints)
571 sac << combined_joint << ' ';
572 RCLCPP_INFO(logger_, "Checking if controllers [ %s] operating on joints [ %s] cover joints [ %s]", ss.str().c_str(),
573 sac.str().c_str(), saj.str().c_str());
574 }
575
576 return std::includes(combined_joints.begin(), combined_joints.end(), actuated_joints.begin(), actuated_joints.end());
577}
578
579void TrajectoryExecutionManager::generateControllerCombination(std::size_t start_index, std::size_t controller_count,
580 const std::vector<std::string>& available_controllers,
581 std::vector<std::string>& selected_controllers,
582 std::vector<std::vector<std::string>>& selected_options,
583 const std::set<std::string>& actuated_joints)
584{
585 if (selected_controllers.size() == controller_count)
586 {
587 if (checkControllerCombination(selected_controllers, actuated_joints))
588 selected_options.push_back(selected_controllers);
589 return;
590 }
591
592 for (std::size_t i = start_index; i < available_controllers.size(); ++i)
593 {
594 bool overlap = false;
595 const ControllerInformation& ci = known_controllers_[available_controllers[i]];
596 for (std::size_t j = 0; j < selected_controllers.size() && !overlap; ++j)
597 {
598 if (ci.overlapping_controllers_.find(selected_controllers[j]) != ci.overlapping_controllers_.end())
599 overlap = true;
600 }
601 if (overlap)
602 continue;
603 selected_controllers.push_back(available_controllers[i]);
604 generateControllerCombination(i + 1, controller_count, available_controllers, selected_controllers,
605 selected_options, actuated_joints);
606 selected_controllers.pop_back();
607 }
608}
609
610namespace
611{
612struct OrderPotentialControllerCombination
613{
614 bool operator()(const std::size_t a, const std::size_t b) const
615 {
616 // preference is given to controllers marked as default
617 if (nrdefault[a] > nrdefault[b])
618 return true;
619 if (nrdefault[a] < nrdefault[b])
620 return false;
621
622 // and then to ones that operate on fewer joints
623 if (nrjoints[a] < nrjoints[b])
624 return true;
625 if (nrjoints[a] > nrjoints[b])
626 return false;
627
628 // and then to active ones
629 if (nractive[a] > nractive[b])
630 return true;
631 if (nractive[a] < nractive[b])
632 return false;
633
634 return false;
635 }
636
637 std::vector<std::vector<std::string>> selected_options;
638 std::vector<std::size_t> nrdefault;
639 std::vector<std::size_t> nrjoints;
640 std::vector<std::size_t> nractive;
641};
642} // namespace
643
644bool TrajectoryExecutionManager::findControllers(const std::set<std::string>& actuated_joints,
645 std::size_t controller_count,
646 const std::vector<std::string>& available_controllers,
647 std::vector<std::string>& selected_controllers)
648{
649 // generate all combinations of controller_count controllers that operate on disjoint sets of joints
650 std::vector<std::string> work_area;
651 OrderPotentialControllerCombination order;
652 std::vector<std::vector<std::string>>& selected_options = order.selected_options;
653 generateControllerCombination(0, controller_count, available_controllers, work_area, selected_options,
654 actuated_joints);
655
656 if (verbose_)
657 {
658 std::stringstream saj;
659 std::stringstream sac;
660 for (const std::string& available_controller : available_controllers)
661 sac << available_controller << ' ';
662 for (const std::string& actuated_joint : actuated_joints)
663 saj << actuated_joint << ' ';
664 RCLCPP_INFO(logger_, "Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.",
665 controller_count, sac.str().c_str(), saj.str().c_str(), selected_options.size());
666 }
667
668 // if none was found, this is a problem
669 if (selected_options.empty())
670 return false;
671
672 // if only one was found, return it
673 if (selected_options.size() == 1)
674 {
675 selected_controllers.swap(selected_options[0]);
676 return true;
677 }
678
679 // if more options were found, evaluate them all and return the best one
680
681 // count how many default controllers are used in each reported option, and how many joints are actuated in total by
682 // the selected controllers,
683 // to use that in the ranking of the options
684 order.nrdefault.resize(selected_options.size(), 0);
685 order.nrjoints.resize(selected_options.size(), 0);
686 order.nractive.resize(selected_options.size(), 0);
687 for (std::size_t i = 0; i < selected_options.size(); ++i)
688 {
689 for (std::size_t k = 0; k < selected_options[i].size(); ++k)
690 {
691 updateControllerState(selected_options[i][k], DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
692 const ControllerInformation& ci = known_controllers_[selected_options[i][k]];
693
694 if (ci.state_.default_)
695 order.nrdefault[i]++;
696 if (ci.state_.active_)
697 order.nractive[i]++;
698 order.nrjoints[i] += ci.joints_.size();
699 }
700 }
701
702 // define a bijection to compute the raking of the found options
703 std::vector<std::size_t> bijection(selected_options.size(), 0);
704 for (std::size_t i = 0; i < selected_options.size(); ++i)
705 bijection[i] = i;
706
707 // sort the options
708 std::sort(bijection.begin(), bijection.end(), order);
709
710 // depending on whether we are allowed to load & unload controllers,
711 // we have different preference on deciding between options
712 if (!manage_controllers_)
713 {
714 // if we can't load different options at will, just choose one that is already loaded
715 for (std::size_t i = 0; i < selected_options.size(); ++i)
716 {
717 if (areControllersActive(selected_options[bijection[i]]))
718 {
719 selected_controllers.swap(selected_options[bijection[i]]);
720 return true;
721 }
722 }
723 }
724
725 // otherwise, just use the first valid option
726 selected_controllers.swap(selected_options[bijection[0]]);
727 return true;
728}
729
730bool TrajectoryExecutionManager::isControllerActive(const std::string& controller)
731{
732 return areControllersActive(std::vector<std::string>(1, controller));
733}
734
735bool TrajectoryExecutionManager::areControllersActive(const std::vector<std::string>& controllers)
736{
737 for (const std::string& controller : controllers)
738 {
739 updateControllerState(controller, DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
740 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
741 if (it == known_controllers_.end() || !it->second.state_.active_)
742 return false;
743 }
744 return true;
745}
746
747bool TrajectoryExecutionManager::selectControllers(const std::set<std::string>& actuated_joints,
748 const std::vector<std::string>& available_controllers,
749 std::vector<std::string>& selected_controllers)
750{
751 for (std::size_t i = 1; i <= available_controllers.size(); ++i)
752 {
753 if (findControllers(actuated_joints, i, available_controllers, selected_controllers))
754 {
755 // if we are not managing controllers, prefer to use active controllers even if there are more of them
756 if (!manage_controllers_ && !areControllersActive(selected_controllers))
757 {
758 std::vector<std::string> other_option;
759 for (std::size_t j = i + 1; j <= available_controllers.size(); ++j)
760 {
761 if (findControllers(actuated_joints, j, available_controllers, other_option))
762 {
763 if (areControllersActive(other_option))
764 {
765 selected_controllers = other_option;
766 break;
767 }
768 }
769 }
770 }
771 return true;
772 }
773 }
774 return false;
775}
776
777bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory,
778 const std::vector<std::string>& controllers,
779 std::vector<moveit_msgs::msg::RobotTrajectory>& parts)
780{
781 parts.clear();
782 parts.resize(controllers.size());
783
784 std::set<std::string> actuated_joints_mdof;
785 actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
786 trajectory.multi_dof_joint_trajectory.joint_names.end());
787 std::set<std::string> actuated_joints_single;
788 for (const std::string& joint_name : trajectory.joint_trajectory.joint_names)
789 {
790 const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_name);
791 if (jm)
792 {
793 if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == moveit::core::JointModel::FIXED)
794 continue;
795 actuated_joints_single.insert(joint_name);
796 }
797 }
798
799 for (std::size_t i = 0; i < controllers.size(); ++i)
800 {
801 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
802 if (it == known_controllers_.end())
803 {
804 RCLCPP_ERROR_STREAM(logger_, "Controller " << controllers[i] << " not found.");
805 return false;
806 }
807 std::vector<std::string> intersect_mdof;
808 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_mdof.begin(),
809 actuated_joints_mdof.end(), std::back_inserter(intersect_mdof));
810 std::vector<std::string> intersect_single;
811 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_single.begin(),
812 actuated_joints_single.end(), std::back_inserter(intersect_single));
813 if (intersect_mdof.empty() && intersect_single.empty())
814 RCLCPP_WARN_STREAM(logger_, "No joints to be distributed for controller " << controllers[i]);
815 {
816 if (!intersect_mdof.empty())
817 {
818 std::vector<std::string>& jnames = parts[i].multi_dof_joint_trajectory.joint_names;
819 jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
820 std::map<std::string, std::size_t> index;
821 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
822 index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
823 std::vector<std::size_t> bijection(jnames.size());
824 for (std::size_t j = 0; j < jnames.size(); ++j)
825 bijection[j] = index[jnames[j]];
826
827 parts[i].multi_dof_joint_trajectory.header.frame_id = trajectory.multi_dof_joint_trajectory.header.frame_id;
828 parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
829 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.points.size(); ++j)
830 {
831 parts[i].multi_dof_joint_trajectory.points[j].time_from_start =
832 trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
833 parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
834 for (std::size_t k = 0; k < bijection.size(); ++k)
835 {
836 parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
837 trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
838
839 if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
840 {
841 parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());
842
843 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
844 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.x * execution_velocity_scaling_;
845
846 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
847 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.y * execution_velocity_scaling_;
848
849 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
850 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.z * execution_velocity_scaling_;
851
852 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
853 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.x * execution_velocity_scaling_;
854
855 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
856 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.y * execution_velocity_scaling_;
857
858 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
859 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.z * execution_velocity_scaling_;
860 }
861 }
862 }
863 }
864 if (!intersect_single.empty())
865 {
866 std::vector<std::string>& jnames = parts[i].joint_trajectory.joint_names;
867 jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
868 parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
869 std::map<std::string, std::size_t> index;
870 for (std::size_t j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j)
871 index[trajectory.joint_trajectory.joint_names[j]] = j;
872 std::vector<std::size_t> bijection(jnames.size());
873 for (std::size_t j = 0; j < jnames.size(); ++j)
874 bijection[j] = index[jnames[j]];
875 parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
876 for (std::size_t j = 0; j < trajectory.joint_trajectory.points.size(); ++j)
877 {
878 parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
879 if (!trajectory.joint_trajectory.points[j].positions.empty())
880 {
881 parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
882 for (std::size_t k = 0; k < bijection.size(); ++k)
883 {
884 parts[i].joint_trajectory.points[j].positions[k] =
885 trajectory.joint_trajectory.points[j].positions[bijection[k]];
886 }
887 }
888 if (!trajectory.joint_trajectory.points[j].velocities.empty())
889 {
890 parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
891 for (std::size_t k = 0; k < bijection.size(); ++k)
892 {
893 parts[i].joint_trajectory.points[j].velocities[k] =
894 trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
895 }
896 }
897 if (!trajectory.joint_trajectory.points[j].accelerations.empty())
898 {
899 parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
900 for (std::size_t k = 0; k < bijection.size(); ++k)
901 {
902 parts[i].joint_trajectory.points[j].accelerations[k] =
903 trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
904 }
905 }
906 if (!trajectory.joint_trajectory.points[j].effort.empty())
907 {
908 parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
909 for (std::size_t k = 0; k < bijection.size(); ++k)
910 {
911 parts[i].joint_trajectory.points[j].effort[k] =
912 trajectory.joint_trajectory.points[j].effort[bijection[k]];
913 }
914 }
915 }
916 }
917 }
918 }
919 return true;
920}
921
922bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& context) const
923{
924 if (allowed_start_tolerance_ == 0 && allowed_start_tolerance_joints_.empty()) // skip validation on this magic number
925 return true;
926
927 if (allowed_start_tolerance_joints_.empty())
928 {
929 RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance " << allowed_start_tolerance_);
930 }
931 else
932 {
933 std::ostringstream ss;
934 for (const auto& [joint_name, joint_start_tolerance] : allowed_start_tolerance_joints_)
935 {
936 if (ss.tellp() > 1)
937 ss << ", "; // skip the comma at the end
938 ss << joint_name << ": " << joint_start_tolerance;
939 }
940 RCLCPP_INFO_STREAM(logger_, "Validating trajectory with allowed_start_tolerance "
941 << allowed_start_tolerance_ << " and allowed_start_tolerance_joints {" << ss.str()
942 << "}");
943 }
944
945 moveit::core::RobotStatePtr current_state;
946 if (!csm_->waitForCurrentState(node_->now()) || !(current_state = csm_->getCurrentState()))
947 {
948 RCLCPP_WARN(logger_, "Failed to validate trajectory: couldn't receive full current joint state within 1s");
949 return false;
950 }
951 moveit::core::RobotState reference_state(*current_state);
952 for (const auto& trajectory : context.trajectory_parts_)
953 {
954 if (!trajectory.joint_trajectory.points.empty())
955 {
956 // Check single-dof trajectory
957 const std::vector<double>& positions = trajectory.joint_trajectory.points.front().positions;
958 const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
959 if (positions.size() != joint_names.size())
960 {
961 RCLCPP_ERROR(logger_, "Wrong trajectory: #joints: %zu != #positions: %zu", joint_names.size(), positions.size());
962 return false;
963 }
964
965 std::set<const moveit::core::JointModel*> joints;
966 for (const auto& joint_name : joint_names)
967 {
968 const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_name);
969 if (!jm)
970 {
971 RCLCPP_ERROR_STREAM(logger_, "Unknown joint in trajectory: " << joint_name);
972 return false;
973 }
974
975 joints.insert(jm);
976 }
977
978 // Copy all variable positions to reference state, and then compare start state joint distance within bounds
979 // Note on multi-DOF joints: Instead of comparing the translation and rotation distances like it's done for
980 // the multi-dof trajectory, this check will use the joint's internal distance implementation instead.
981 // This is more accurate, but may require special treatment for cases like the diff drive's turn path geometry.
982 reference_state.setVariablePositions(joint_names, positions);
983
984 for (const auto joint : joints)
985 {
986 const double joint_start_tolerance = getAllowedStartToleranceJoint(joint->getName());
987 reference_state.enforcePositionBounds(joint);
988 current_state->enforcePositionBounds(joint);
989 if (joint_start_tolerance != 0 && reference_state.distance(*current_state, joint) > joint_start_tolerance)
990 {
991 RCLCPP_ERROR(logger_,
992 "Invalid Trajectory: start point deviates from current robot state more than %g at joint '%s'."
993 "\nEnable DEBUG for detailed state info.",
994 joint_start_tolerance, joint->getName().c_str());
995 RCLCPP_DEBUG(logger_, "| Joint | Expected | Current |");
996 for (const auto& joint_name : joint_names)
997 {
998 RCLCPP_DEBUG(logger_, "| %s | %g | %g |", joint_name.c_str(),
999 reference_state.getVariablePosition(joint_name),
1000 current_state->getVariablePosition(joint_name));
1001 }
1002 return false;
1003 }
1004 }
1005 }
1006
1007 if (!trajectory.multi_dof_joint_trajectory.points.empty())
1008 {
1009 // Check multi-dof trajectory
1010 const std::vector<geometry_msgs::msg::Transform>& transforms =
1011 trajectory.multi_dof_joint_trajectory.points.front().transforms;
1012 const std::vector<std::string>& joint_names = trajectory.multi_dof_joint_trajectory.joint_names;
1013 if (transforms.size() != joint_names.size())
1014 {
1015 RCLCPP_ERROR(logger_, "Wrong trajectory: #joints: %zu != #transforms: %zu", joint_names.size(),
1016 transforms.size());
1017 return false;
1018 }
1019
1020 for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
1021 {
1022 const moveit::core::JointModel* jm = current_state->getJointModel(joint_names[i]);
1023 if (!jm)
1024 {
1025 RCLCPP_ERROR_STREAM(logger_, "Unknown joint in trajectory: " << joint_names[i]);
1026 return false;
1027 }
1028
1029 // compute difference (offset vector and rotation angle) between current transform
1030 // and start transform in trajectory
1031 Eigen::Isometry3d cur_transform, start_transform;
1032 // computeTransform() computes a valid isometry by contract
1033 jm->computeTransform(current_state->getJointPositions(jm), cur_transform);
1034 start_transform = tf2::transformToEigen(transforms[i]);
1035 ASSERT_ISOMETRY(start_transform) // unsanitized input, could contain a non-isometry
1036 Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation();
1037 Eigen::AngleAxisd rotation;
1038 rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear());
1039 const double joint_start_tolerance = getAllowedStartToleranceJoint(joint_names[i]);
1040 if (joint_start_tolerance != 0 &&
1041 ((offset.array() > joint_start_tolerance).any() || rotation.angle() > joint_start_tolerance))
1042 {
1043 RCLCPP_ERROR_STREAM(logger_, "\nInvalid Trajectory: start point deviates from current robot state more than "
1044 << joint_start_tolerance << "\nmulti-dof joint '" << joint_names[i]
1045 << "': pos delta: " << offset.transpose()
1046 << " rot delta: " << rotation.angle());
1047 return false;
1048 }
1049 }
1050 }
1051 }
1052 return true;
1053}
1054
1055bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context,
1056 const moveit_msgs::msg::RobotTrajectory& trajectory,
1057 const std::vector<std::string>& controllers)
1058{
1059 if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
1060 {
1061 // empty trajectories don't need to configure anything
1062 return true;
1063 }
1064
1065 reloadControllerInformation();
1066 std::set<std::string> actuated_joints;
1067
1068 auto is_actuated = [this](const std::string& joint_name) -> bool {
1069 const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_name);
1070 return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != moveit::core::JointModel::FIXED);
1071 };
1072 for (const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names)
1073 {
1074 if (is_actuated(joint_name))
1075 actuated_joints.insert(joint_name);
1076 }
1077 for (const std::string& joint_name : trajectory.joint_trajectory.joint_names)
1078 {
1079 if (is_actuated(joint_name))
1080 actuated_joints.insert(joint_name);
1081 }
1082
1083 if (actuated_joints.empty())
1084 {
1085 RCLCPP_WARN(logger_, "The trajectory to execute specifies no joints");
1086 return false;
1087 }
1088
1089 if (controllers.empty())
1090 {
1091 bool retry = true;
1092 bool reloaded = false;
1093 while (retry)
1094 {
1095 retry = false;
1096 std::vector<std::string> all_controller_names;
1097 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1098 it != known_controllers_.end(); ++it)
1099 all_controller_names.push_back(it->first);
1100 if (selectControllers(actuated_joints, all_controller_names, context.controllers_))
1101 {
1102 if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1103 return true;
1104 }
1105 else
1106 {
1107 // maybe we failed because we did not have a complete list of controllers
1108 if (!reloaded)
1109 {
1110 reloadControllerInformation();
1111 reloaded = true;
1112 retry = true;
1113 }
1114 }
1115 }
1116 }
1117 else
1118 {
1119 // check if the specified controllers are valid names;
1120 // if they appear not to be, try to reload the controller information, just in case they are new in the system
1121 bool reloaded = false;
1122 for (const std::string& controller : controllers)
1123 {
1124 if (known_controllers_.find(controller) == known_controllers_.end())
1125 {
1126 reloadControllerInformation();
1127 reloaded = true;
1128 break;
1129 }
1130 }
1131 if (reloaded)
1132 {
1133 for (const std::string& controller : controllers)
1134 {
1135 if (known_controllers_.find(controller) == known_controllers_.end())
1136 {
1137 std::stringstream stream;
1138 for (const auto& controller : known_controllers_)
1139 {
1140 stream << " `" << controller.first << '`';
1141 }
1142 RCLCPP_ERROR_STREAM(logger_,
1143 "Controller " << controller << " is not known. Known controllers: " << stream.str());
1144 return false;
1145 }
1146 }
1147 }
1148 if (selectControllers(actuated_joints, controllers, context.controllers_))
1149 {
1150 if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1151 return true;
1152 }
1153 }
1154 std::stringstream ss;
1155 for (const std::string& actuated_joint : actuated_joints)
1156 ss << actuated_joint << ' ';
1157 RCLCPP_ERROR(logger_, "Unable to identify any set of controllers that can actuate the specified joints: [ %s]",
1158 ss.str().c_str());
1159
1160 std::stringstream ss2;
1161 std::map<std::string, ControllerInformation>::const_iterator mi;
1162 for (mi = known_controllers_.begin(); mi != known_controllers_.end(); ++mi)
1163 {
1164 ss2 << "controller '" << mi->second.name_ << "' controls joints:\n";
1165
1166 std::set<std::string>::const_iterator ji;
1167 for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ++ji)
1168 {
1169 ss2 << " " << *ji << '\n';
1170 }
1171 }
1172 RCLCPP_ERROR(logger_, "Known controllers and their joints:\n%s", ss2.str().c_str());
1173
1174 if (!trajectory.multi_dof_joint_trajectory.joint_names.empty())
1175 {
1176 RCLCPP_WARN(logger_, "Hint: You can control multi-dof waypoints as joint trajectory by setting "
1177 "`trajectory_execution.control_multi_dof_joint_variables=True`");
1178 }
1179
1180 return false;
1181}
1182
1188
1189void TrajectoryExecutionManager::stopExecutionInternal()
1190{
1191 // execution_state_mutex_ needs to have been locked by the caller
1192 for (moveit_controller_manager::MoveItControllerHandlePtr& active_handle : active_handles_)
1193 {
1194 try
1195 {
1196 active_handle->cancelExecution();
1197 }
1198 catch (std::exception& ex)
1199 {
1200 RCLCPP_ERROR(logger_, "Caught %s when canceling execution.", ex.what());
1201 }
1202 }
1203}
1204
1206{
1207 if (!execution_complete_)
1208 {
1209 execution_state_mutex_.lock();
1210 if (!execution_complete_)
1211 {
1212 // we call cancel for all active handles; we know these are not being modified as we loop through them because of
1213 // the lock
1214 // we mark execution_complete_ as true ahead of time. Using this flag, executePart() will know that an external
1215 // trigger to stop has been received
1216 execution_complete_ = true;
1217 stopExecutionInternal();
1218
1219 // we set the status here; executePart() will not set status when execution_complete_ is true ahead of time
1221 execution_state_mutex_.unlock();
1222 RCLCPP_INFO(logger_, "Stopped trajectory execution.");
1223
1224 // wait for the execution thread to finish
1225 std::scoped_lock lock(execution_thread_mutex_);
1226 if (execution_thread_)
1227 {
1228 execution_thread_->join();
1229 execution_thread_.reset();
1230 }
1231
1232 if (auto_clear)
1233 clear();
1234 }
1235 else
1236 execution_state_mutex_.unlock();
1237 }
1238 else if (execution_thread_) // just in case we have some thread waiting to be joined from some point in the past, we
1239 // join it now
1240 {
1241 std::scoped_lock lock(execution_thread_mutex_);
1242 if (execution_thread_)
1243 {
1244 execution_thread_->join();
1245 execution_thread_.reset();
1246 }
1247 }
1248}
1249
1251{
1252 execute(callback, PathSegmentCompleteCallback(), auto_clear);
1253}
1254
1256 const PathSegmentCompleteCallback& part_callback, bool auto_clear)
1257{
1258 // skip execution if no trajectory have been pushed
1259 // it crashes otherwise
1260 if (trajectories_.empty())
1261 return;
1262
1263 stopExecution(false);
1264
1265 // check whether first trajectory starts at current robot state
1266 if (!trajectories_.empty() && !validate(*trajectories_.front()))
1267 {
1269 if (auto_clear)
1270 clear();
1271 if (callback)
1272 callback(last_execution_status_);
1273 return;
1274 }
1275
1276 // start the execution thread
1277 execution_complete_ = false;
1278 execution_thread_ = std::make_unique<std::thread>(&TrajectoryExecutionManager::executeThread, this, callback,
1279 part_callback, auto_clear);
1280}
1281
1283{
1284 {
1285 std::unique_lock<std::mutex> ulock(execution_state_mutex_);
1286 while (!execution_complete_)
1287 execution_complete_condition_.wait(ulock);
1288 }
1289
1290 // this will join the thread for executing sequences of trajectories
1291 stopExecution(false);
1292
1293 return last_execution_status_;
1294}
1295
1296void TrajectoryExecutionManager::clear()
1297{
1298 if (execution_complete_)
1299 {
1300 std::scoped_lock slock(execution_state_mutex_);
1301 for (TrajectoryExecutionContext* trajectory : trajectories_)
1302 delete trajectory;
1303 trajectories_.clear();
1304 }
1305 else
1306 RCLCPP_FATAL(logger_, "Expecting execution_complete_ to be true!");
1307}
1308
1309void TrajectoryExecutionManager::executeThread(const ExecutionCompleteCallback& callback,
1310 const PathSegmentCompleteCallback& part_callback, bool auto_clear)
1311{
1312 // if we already got a stop request before we even started anything, we abort
1313 if (execution_complete_)
1314 {
1316 if (callback)
1317 callback(last_execution_status_);
1318 return;
1319 }
1320
1321 RCLCPP_INFO(logger_, "Starting trajectory execution ...");
1322 // assume everything will be OK
1324
1325 // execute each trajectory, one after the other (executePart() is blocking) or until one fails.
1326 // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success)
1327 std::size_t i = 0;
1328 for (; i < trajectories_.size(); ++i)
1329 {
1330 bool epart = executePart(i);
1331 if (epart && part_callback)
1332 part_callback(i);
1333 if (!epart || execution_complete_)
1334 {
1335 ++i;
1336 break;
1337 }
1338 }
1339
1340 // only report that execution finished successfully when the robot actually stopped moving
1341 if (last_execution_status_ == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
1342 {
1343 std::scoped_lock slock(execution_state_mutex_);
1344 if (!execution_complete_)
1345 {
1346 waitForRobotToStop(*trajectories_[i - 1]);
1347 }
1348 }
1349
1350 RCLCPP_INFO(logger_, "Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str());
1351
1352 // notify whoever is waiting for the event of trajectory completion
1353 execution_state_mutex_.lock();
1354 execution_complete_ = true;
1355 execution_state_mutex_.unlock();
1356 execution_complete_condition_.notify_all();
1357
1358 // clear the paths just executed, if needed
1359 if (auto_clear)
1360 clear();
1361
1362 // call user-specified callback
1363 if (callback)
1364 callback(last_execution_status_);
1365}
1366
1367bool TrajectoryExecutionManager::executePart(std::size_t part_index)
1368{
1369 TrajectoryExecutionContext& context = *trajectories_[part_index];
1370
1371 // first make sure desired controllers are active
1372 if (ensureActiveControllers(context.controllers_))
1373 {
1374 // stop if we are already asked to do so
1375 if (execution_complete_)
1376 return false;
1377
1378 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
1379 {
1380 std::scoped_lock slock(execution_state_mutex_);
1381 if (!execution_complete_)
1382 {
1383 // time indexing uses this member too, so we lock this mutex as well
1384 time_index_mutex_.lock();
1385 current_context_ = part_index;
1386 time_index_mutex_.unlock();
1387 active_handles_.resize(context.controllers_.size());
1388 for (std::size_t i = 0; i < context.controllers_.size(); ++i)
1389 {
1390 moveit_controller_manager::MoveItControllerHandlePtr h;
1391 try
1392 {
1393 h = controller_manager_->getControllerHandle(context.controllers_[i]);
1394 }
1395 catch (std::exception& ex)
1396 {
1397 RCLCPP_ERROR(logger_, "Caught %s when retrieving controller handle", ex.what());
1398 }
1399 if (!h)
1400 {
1401 active_handles_.clear();
1402 current_context_ = -1;
1404 RCLCPP_ERROR(logger_, "No controller handle for controller '%s'. Aborting.",
1405 context.controllers_[i].c_str());
1406 return false;
1407 }
1408 active_handles_[i] = h;
1409 }
1410 handles = active_handles_; // keep a copy for later, to avoid thread safety issues
1411 for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1412 {
1413 bool ok = false;
1414 try
1415 {
1416 ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
1417 }
1418 catch (std::exception& ex)
1419 {
1420 RCLCPP_ERROR(logger_, "Caught %s when sending trajectory to controller", ex.what());
1421 }
1422 if (!ok)
1423 {
1424 for (std::size_t j = 0; j < i; ++j)
1425 {
1426 try
1427 {
1428 active_handles_[j]->cancelExecution();
1429 }
1430 catch (std::exception& ex)
1431 {
1432 RCLCPP_ERROR(logger_, "Caught %s when canceling execution", ex.what());
1433 }
1434 }
1435 RCLCPP_ERROR(logger_, "Failed to send trajectory part %zu of %zu to controller %s", i + 1,
1436 context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
1437 if (i > 0)
1438 RCLCPP_ERROR(logger_, "Cancelling previously sent trajectory parts");
1439 active_handles_.clear();
1440 current_context_ = -1;
1442 return false;
1443 }
1444 }
1445 }
1446 }
1447
1448 // compute the expected duration of the trajectory and find the part of the trajectory that takes longest to execute
1449 rclcpp::Time current_time = node_->now();
1450 auto expected_trajectory_duration = rclcpp::Duration::from_seconds(0);
1451 int longest_part = -1;
1452 for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1453 {
1454 auto d = rclcpp::Duration::from_seconds(0);
1455 if (!(context.trajectory_parts_[i].joint_trajectory.points.empty() &&
1456 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty()))
1457 {
1458 if (rclcpp::Time(context.trajectory_parts_[i].joint_trajectory.header.stamp) > current_time)
1459 d = rclcpp::Time(context.trajectory_parts_[i].joint_trajectory.header.stamp) - current_time;
1460 if (rclcpp::Time(context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp) > current_time)
1461 {
1462 d = std::max(d, rclcpp::Time(context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp) -
1463 current_time);
1464 }
1465 d = d +
1466 std::max(context.trajectory_parts_[i].joint_trajectory.points.empty() ?
1467 rclcpp::Duration::from_seconds(0) :
1468 rclcpp::Duration(context.trajectory_parts_[i].joint_trajectory.points.back().time_from_start),
1469 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty() ?
1470 rclcpp::Duration::from_seconds(0) :
1471 rclcpp::Duration(
1472 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start));
1473
1474 if (longest_part < 0 ||
1475 std::max(context.trajectory_parts_[i].joint_trajectory.points.size(),
1476 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.size()) >
1477 std::max(context.trajectory_parts_[longest_part].joint_trajectory.points.size(),
1478 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
1479 longest_part = i;
1480 }
1481
1482 // prefer controller-specific values over global ones if defined
1483 // TODO: the controller-specific parameters are static, but override
1484 // the global ones are configurable via dynamic reconfigure
1485 std::map<std::string, double>::const_iterator scaling_it =
1486 controller_allowed_execution_duration_scaling_.find(context.controllers_[i]);
1487 const double current_scaling = scaling_it != controller_allowed_execution_duration_scaling_.end() ?
1488 scaling_it->second :
1489 allowed_execution_duration_scaling_;
1490
1491 std::map<std::string, double>::const_iterator margin_it =
1492 controller_allowed_goal_duration_margin_.find(context.controllers_[i]);
1493 const double current_margin = margin_it != controller_allowed_goal_duration_margin_.end() ?
1494 margin_it->second :
1495 allowed_goal_duration_margin_;
1496
1497 // expected duration is the duration of the longest part
1498 expected_trajectory_duration =
1499 std::max(d * current_scaling + rclcpp::Duration::from_seconds(current_margin), expected_trajectory_duration);
1500 }
1501
1502 // construct a map from expected time to state index, for easy access to expected state location
1503 if (longest_part >= 0)
1504 {
1505 std::scoped_lock slock(time_index_mutex_);
1506
1507 if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >=
1508 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
1509 {
1510 auto d = rclcpp::Duration::from_seconds(0);
1511 if (rclcpp::Time(context.trajectory_parts_[longest_part].joint_trajectory.header.stamp) > current_time)
1512 d = rclcpp::Time(context.trajectory_parts_[longest_part].joint_trajectory.header.stamp) - current_time;
1513 for (trajectory_msgs::msg::JointTrajectoryPoint& point :
1514 context.trajectory_parts_[longest_part].joint_trajectory.points)
1515 time_index_.push_back(current_time + d + rclcpp::Duration(point.time_from_start));
1516 }
1517 else
1518 {
1519 auto d = rclcpp::Duration::from_seconds(0);
1520 if (rclcpp::Time(context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp) > current_time)
1521 {
1522 d = rclcpp::Time(context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp) -
1523 current_time;
1524 }
1525 for (trajectory_msgs::msg::MultiDOFJointTrajectoryPoint& point :
1526 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points)
1527 time_index_.push_back(current_time + d + rclcpp::Duration(point.time_from_start));
1528 }
1529 }
1530
1531 bool result = true;
1532 for (moveit_controller_manager::MoveItControllerHandlePtr& handle : handles)
1533 {
1534 if (execution_duration_monitoring_)
1535 {
1536 if (!handle->waitForExecution(expected_trajectory_duration))
1537 {
1538 if (!execution_complete_ && node_->now() - current_time > expected_trajectory_duration)
1539 {
1540 RCLCPP_ERROR(logger_,
1541 "Controller is taking too long to execute trajectory (the expected upper "
1542 "bound for the trajectory execution was %lf seconds). Stopping trajectory.",
1543 expected_trajectory_duration.seconds());
1544 {
1545 std::scoped_lock slock(execution_state_mutex_);
1546 stopExecutionInternal(); // this is really tricky. we can't call stopExecution() here, so we call the
1547 // internal function only
1548 }
1550 result = false;
1551 break;
1552 }
1553 }
1554 }
1555 else
1556 handle->waitForExecution();
1557
1558 // if something made the trajectory stop, we stop this thread too
1559 if (execution_complete_)
1560 {
1561 result = false;
1562 break;
1563 }
1564 else if (handle->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::SUCCEEDED)
1565 {
1566 RCLCPP_WARN_STREAM(logger_, "Controller handle " << handle->getName() << " reports status "
1567 << handle->getLastExecutionStatus().asString());
1568 last_execution_status_ = handle->getLastExecutionStatus();
1569 result = false;
1570 }
1571 }
1572
1573 // clear the active handles
1574 execution_state_mutex_.lock();
1575 active_handles_.clear();
1576
1577 // clear the time index
1578 time_index_mutex_.lock();
1579 time_index_.clear();
1580 current_context_ = -1;
1581 time_index_mutex_.unlock();
1582
1583 execution_state_mutex_.unlock();
1584 return result;
1585 }
1586 else
1587 {
1588 RCLCPP_ERROR(logger_, "Active status of required controllers can not be assured.");
1590 return false;
1591 }
1592}
1593
1594bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time)
1595{
1596 // skip waiting for convergence?
1597 if ((allowed_start_tolerance_ == 0 && allowed_start_tolerance_joints_.empty()) || !wait_for_trajectory_completion_)
1598 {
1599 RCLCPP_INFO(logger_, "Not waiting for trajectory completion");
1600 return true;
1601 }
1602
1603 auto start = std::chrono::system_clock::now();
1604 double time_remaining = wait_time;
1605
1606 moveit::core::RobotStatePtr prev_state, cur_state;
1607 prev_state = csm_->getCurrentState();
1608 prev_state->enforceBounds();
1609
1610 // assume robot stopped when 3 consecutive checks yield the same robot state
1611 unsigned int no_motion_count = 0; // count iterations with no motion
1612 while (time_remaining > 0. && no_motion_count < 3)
1613 {
1614 if (!csm_->waitForCurrentState(node_->now(), time_remaining) || !(cur_state = csm_->getCurrentState()))
1615 {
1616 RCLCPP_WARN(logger_, "Failed to receive current joint state");
1617 return false;
1618 }
1619 cur_state->enforceBounds();
1620 std::chrono::duration<double> elapsed_seconds = std::chrono::system_clock::now() - start;
1621 time_remaining = wait_time - elapsed_seconds.count(); // remaining wait_time
1622
1623 // check for motion in effected joints of execution context
1624 bool moved = false;
1625 for (const auto& trajectory : context.trajectory_parts_)
1626 {
1627 const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
1628 const std::size_t n = joint_names.size();
1629
1630 for (std::size_t i = 0; i < n && !moved; ++i)
1631 {
1632 const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_names[i]);
1633 if (!jm)
1634 continue; // joint vanished from robot state (shouldn't happen), but we don't care
1635
1636 const double joint_start_tolerance = getAllowedStartToleranceJoint(joint_names[i]);
1637 if (fabs(jm->distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) >
1638 joint_start_tolerance)
1639 {
1640 moved = true;
1641 no_motion_count = 0;
1642 break;
1643 }
1644 }
1645 }
1646
1647 if (!moved)
1648 ++no_motion_count;
1649
1650 std::swap(prev_state, cur_state);
1651 }
1652
1653 return time_remaining > 0;
1654}
1655
1657{
1658 std::scoped_lock slock(time_index_mutex_);
1659 if (current_context_ < 0)
1660 return std::make_pair(-1, -1);
1661 if (time_index_.empty())
1662 return std::make_pair(static_cast<int>(current_context_), -1);
1663 std::vector<rclcpp::Time>::const_iterator time_index_it =
1664 std::lower_bound(time_index_.begin(), time_index_.end(), node_->now());
1665 int pos = time_index_it - time_index_.begin();
1666 return std::make_pair(static_cast<int>(current_context_), pos);
1667}
1668
1669const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>&
1671{
1672 return trajectories_;
1673}
1674
1679
1681{
1682 const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group);
1683 if (joint_model_group)
1684 {
1685 return ensureActiveControllersForJoints(joint_model_group->getJointModelNames());
1686 }
1687 else
1688 {
1689 return false;
1690 }
1691}
1692
1693bool TrajectoryExecutionManager::ensureActiveControllersForJoints(const std::vector<std::string>& joints)
1694{
1695 std::vector<std::string> all_controller_names;
1696 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1697 it != known_controllers_.end(); ++it)
1698 all_controller_names.push_back(it->first);
1699 std::vector<std::string> selected_controllers;
1700 std::set<std::string> jset;
1701 for (const std::string& joint : joints)
1702 {
1703 const moveit::core::JointModel* jm = robot_model_->getJointModel(joint);
1704 if (jm)
1705 {
1706 if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == moveit::core::JointModel::FIXED)
1707 continue;
1708 jset.insert(joint);
1709 }
1710 }
1711
1712 if (selectControllers(jset, all_controller_names, selected_controllers))
1713 {
1714 return ensureActiveControllers(selected_controllers);
1715 }
1716 else
1717 {
1718 return false;
1719 }
1720}
1721
1723{
1724 return ensureActiveControllers(std::vector<std::string>(1, controller));
1725}
1726
1727bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector<std::string>& controllers)
1728{
1729 reloadControllerInformation();
1730
1731 updateControllersState(DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
1732
1733 if (manage_controllers_)
1734 {
1735 std::vector<std::string> controllers_to_activate;
1736 std::vector<std::string> controllers_to_deactivate;
1737 std::set<std::string> joints_to_be_activated;
1738 std::set<std::string> joints_to_be_deactivated;
1739 for (const std::string& controller : controllers)
1740 {
1741 std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.find(controller);
1742 if (it == known_controllers_.end())
1743 {
1744 std::stringstream stream;
1745 for (const auto& controller : known_controllers_)
1746 {
1747 stream << " `" << controller.first << '`';
1748 }
1749 RCLCPP_WARN_STREAM(logger_, "Controller " << controller << " is not known. Known controllers: " << stream.str());
1750 return false;
1751 }
1752 if (!it->second.state_.active_)
1753 {
1754 RCLCPP_DEBUG_STREAM(logger_, "Need to activate " << controller);
1755 controllers_to_activate.push_back(controller);
1756 joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
1757 for (const std::string& overlapping_controller : it->second.overlapping_controllers_)
1758 {
1759 const ControllerInformation& ci = known_controllers_[overlapping_controller];
1760 if (ci.state_.active_)
1761 {
1762 controllers_to_deactivate.push_back(overlapping_controller);
1763 joints_to_be_deactivated.insert(ci.joints_.begin(), ci.joints_.end());
1764 }
1765 }
1766 }
1767 else
1768 RCLCPP_DEBUG_STREAM(logger_, "Controller " << controller << " is already active");
1769 }
1770 std::set<std::string> diff;
1771 std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
1772 joints_to_be_activated.begin(), joints_to_be_activated.end(), std::inserter(diff, diff.end()));
1773 if (!diff.empty())
1774 {
1775 // find the set of controllers that do not overlap with the ones we want to activate so far
1776 std::vector<std::string> possible_additional_controllers;
1777 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1778 it != known_controllers_.end(); ++it)
1779 {
1780 bool ok = true;
1781 for (const std::string& controller_to_activate : controllers_to_activate)
1782 {
1783 if (it->second.overlapping_controllers_.find(controller_to_activate) !=
1784 it->second.overlapping_controllers_.end())
1785 {
1786 ok = false;
1787 break;
1788 }
1789 }
1790 if (ok)
1791 possible_additional_controllers.push_back(it->first);
1792 }
1793
1794 // out of the allowable controllers, try to find a subset of controllers that covers the joints to be actuated
1795 std::vector<std::string> additional_controllers;
1796 if (selectControllers(diff, possible_additional_controllers, additional_controllers))
1797 {
1798 controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(),
1799 additional_controllers.end());
1800 }
1801 else
1802 {
1803 return false;
1804 }
1805 }
1806 if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
1807 {
1808 if (controller_manager_)
1809 {
1810 // load controllers to be activated, if needed, and reset the state update cache
1811 for (const std::string& controller_to_activate : controllers_to_activate)
1812 {
1813 ControllerInformation& ci = known_controllers_[controller_to_activate];
1814 ci.last_update_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
1815 }
1816 // reset the state update cache
1817 for (const std::string& controller_to_deactivate : controllers_to_deactivate)
1818 known_controllers_[controller_to_deactivate].last_update_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
1819 return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
1820 }
1821 else
1822 return false;
1823 }
1824 else
1825 return true;
1826 }
1827 else
1828 {
1829 std::set<std::string> originally_active;
1830 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1831 it != known_controllers_.end(); ++it)
1832 {
1833 if (it->second.state_.active_)
1834 {
1835 originally_active.insert(it->first);
1836 }
1837 }
1838 return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
1839 }
1840}
1841
1842void TrajectoryExecutionManager::loadControllerParams()
1843{
1844 // TODO: Revise XmlRpc parameter lookup
1845 // XmlRpc::XmlRpcValue controller_list;
1846 // if (node_->get_parameter("controller_list", controller_list) &&
1847 // controller_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
1848 // {
1849 // for (int i = 0; i < controller_list.size(); ++i) // NOLINT(modernize-loop-convert)
1850 // {
1851 // XmlRpc::XmlRpcValue& controller = controller_list[i];
1852 // if (controller.hasMember("name"))
1853 // {
1854 // if (controller.hasMember("allowed_execution_duration_scaling"))
1855 // controller_allowed_execution_duration_scaling_[std::string(controller["name"])] =
1856 // controller["allowed_execution_duration_scaling"];
1857 // if (controller.hasMember("allowed_goal_duration_margin"))
1858 // controller_allowed_goal_duration_margin_[std::string(controller["name"])] =
1859 // controller["allowed_goal_duration_margin"];
1860 // }
1861 // }
1862 // }
1863}
1864
1865double TrajectoryExecutionManager::getAllowedStartToleranceJoint(const std::string& joint_name) const
1866{
1867 auto start_tolerance_it = allowed_start_tolerance_joints_.find(joint_name);
1868 return start_tolerance_it != allowed_start_tolerance_joints_.end() ? start_tolerance_it->second :
1869 allowed_start_tolerance_;
1870}
1871
1872void TrajectoryExecutionManager::setAllowedStartToleranceJoint(const std::string& parameter_name,
1873 double joint_start_tolerance)
1874{
1875 if (joint_start_tolerance < 0)
1876 {
1877 RCLCPP_WARN(logger_, "%s has a negative value. The start tolerance value for that joint was not updated.",
1878 parameter_name.c_str());
1879 return;
1880 }
1881
1882 // get the joint name by removing the parameter prefix if necessary
1883 std::string joint_name = parameter_name;
1884 const std::string parameter_prefix = "trajectory_execution.allowed_start_tolerance_joints.";
1885 if (parameter_name.find(parameter_prefix) == 0)
1886 joint_name = joint_name.substr(parameter_prefix.length()); // remove prefix
1887
1888 if (!robot_model_->hasJointModel(joint_name))
1889 {
1890 RCLCPP_WARN(logger_,
1891 "Joint '%s' was not found in the robot model. "
1892 "The start tolerance value for that joint was not updated.",
1893 joint_name.c_str());
1894 return;
1895 }
1896
1897 allowed_start_tolerance_joints_.insert_or_assign(joint_name, joint_start_tolerance);
1898}
1899
1900void TrajectoryExecutionManager::initializeAllowedStartToleranceJoints()
1901{
1902 allowed_start_tolerance_joints_.clear();
1903
1904 // retrieve all parameters under "trajectory_execution.allowed_start_tolerance_joints"
1905 // that correspond to existing joints in the robot model
1906 for (const auto& joint_name : robot_model_->getJointModelNames())
1907 {
1908 double joint_start_tolerance;
1909 const std::string parameter_name = "trajectory_execution.allowed_start_tolerance_joints." + joint_name;
1910 if (node_->get_parameter(parameter_name, joint_start_tolerance))
1911 {
1912 if (joint_start_tolerance < 0)
1913 {
1914 RCLCPP_WARN(logger_,
1915 "%s has a negative value. The start tolerance value for that joint "
1916 "will default to allowed_start_tolerance.",
1917 parameter_name.c_str());
1918 continue;
1919 }
1920 allowed_start_tolerance_joints_.insert({ joint_name, joint_start_tolerance });
1921 }
1922 }
1923}
1924
1925} // namespace trajectory_execution_manager
const std::vector< std::string > & getJointModelNames() const
Get the names of the joints in this group. These are the names of the joints returned by getJointMode...
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
const JointModel * getMimic() const
Get the joint this one is mimicking.
virtual double distance(const double *value1, const double *value2) const =0
Compute the distance between two joint states of the same model (represented by the variable values)
bool isPassive() const
Check if this joint is passive.
virtual void computeTransform(const double *joint_values, Eigen::Isometry3d &transf) const =0
Given the joint values for a joint, compute the corresponding transform. The computed transform is gu...
JointType getType() const
Get the type of joint.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Maintain a sequence of waypoints and the time durations between these waypoints.
RobotTrajectory & setRobotTrajectoryMsg(const moveit::core::RobotState &reference_state, const trajectory_msgs::msg::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
TrajectoryExecutionManager(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModelConstPtr &robot_model, const planning_scene_monitor::CurrentStateMonitorPtr &csm)
Load the controller manager plugin, start listening for events on a topic.
~TrajectoryExecutionManager()
Destructor. Cancels all running trajectories (if any)
void execute(const ExecutionCompleteCallback &callback=ExecutionCompleteCallback(), bool auto_clear=true)
Start the execution of pushed trajectories; this does not wait for completion, but calls a callback w...
void stopExecution(bool auto_clear=true)
Stop whatever executions are active, if any.
void setWaitForTrajectoryCompletion(bool flag)
Enable or disable waiting for trajectory completion.
double allowedStartTolerance() const
Get the current joint-value for validating trajectory's start point against current robot state.
bool ensureActiveControllersForGroup(const std::string &group)
Make sure the active controllers are such that trajectories that actuate joints in the specified grou...
const std::vector< TrajectoryExecutionContext * > & getTrajectories() const
Get the trajectories to be executed.
std::function< void(const moveit_controller_manager::ExecutionStatus &)> ExecutionCompleteCallback
bool ensureActiveController(const std::string &controller)
Make sure a particular controller is active.
moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const
Return the controller status for the last attempted execution.
double allowedExecutionDurationScaling() const
Get the current scaling of the duration of a trajectory to get the allowed duration of execution.
bool push(const moveit_msgs::msg::RobotTrajectory &trajectory, const std::string &controller="")
bool isManagingControllers() const
If this function returns true, then this instance of the manager is allowed to load/unload/switch con...
bool isControllerActive(const std::string &controller)
Check if a controller is active.
bool executionDurationMonitoring() const
Get the current status of the monitoring of trajectory execution duration.
double allowedGoalDurationMargin() const
Get the current margin of the duration of a trajectory to get the allowed duration of execution.
bool waitForTrajectoryCompletion() const
Get the current state of waiting for the trajectory being completed.
moveit_controller_manager::ExecutionStatus executeAndWait(bool auto_clear=true)
void setAllowedStartTolerance(double tolerance)
Set joint-value tolerance for validating trajectory's start point against current robot state.
void processEvent(const std::string &event)
Execute a named event (e.g., 'stop')
double executionVelocityScaling() const
Get the current scaling of the execution velocities.
bool areControllersActive(const std::vector< std::string > &controllers)
Check if a set of controllers is active.
bool ensureActiveControllersForJoints(const std::vector< std::string > &joints)
Make sure the active controllers are such that trajectories that actuate joints in the specified set ...
bool ensureActiveControllers(const std::vector< std::string > &controllers)
Make sure a particular set of controllers are active.
const moveit_controller_manager::MoveItControllerManagerPtr & getControllerManager() const
Get the instance of the controller manager used (this is the plugin instance loaded)
std::vector< std::size_t > nrjoints
std::vector< std::size_t > nrdefault
std::vector< std::size_t > nractive
std::vector< std::vector< std::string > > selected_options
Main namespace for MoveIt.
std::optional< trajectory_msgs::msg::JointTrajectory > toJointTrajectory(const RobotTrajectory &trajectory, bool include_mdof_joints=false, const std::vector< std::string > &joint_filter={})
Converts a RobotTrajectory to a JointTrajectory message.
name
Definition setup.py:7
std::string asString() const
Convert the execution status to a string.
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;.