40#include <geometric_shapes/check_isometry.h>
42#include <tf2_eigen/tf2_eigen.hpp>
50static const auto DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE = rclcpp::Duration::from_seconds(1);
51static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 0.5;
54static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING =
56static const bool DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES =
false;
59 const moveit::core::RobotModelConstPtr& robot_model,
60 const planning_scene_monitor::CurrentStateMonitorPtr& csm)
62 , logger_(
moveit::getLogger(
"moveit.ros.trajectory_execution_manager"))
63 , robot_model_(robot_model)
66 if (!node_->get_parameter(
"moveit_manage_controllers", manage_controllers_))
67 manage_controllers_ =
false;
72 const moveit::core::RobotModelConstPtr& robot_model,
73 const planning_scene_monitor::CurrentStateMonitorPtr& csm,
74 bool manage_controllers)
76 , logger_(
moveit::getLogger(
"moveit.ros.trajectory_execution_manager"))
77 , robot_model_(robot_model)
79 , manage_controllers_(manage_controllers)
87 if (private_executor_)
88 private_executor_->cancel();
89 if (private_executor_thread_.joinable())
90 private_executor_thread_.join();
93void TrajectoryExecutionManager::initialize()
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;
106 allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING;
107 allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN;
110 loadControllerParams();
115 controller_manager_loader_ =
116 std::make_unique<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager>>(
117 "moveit_core",
"moveit_controller_manager::MoveItControllerManager");
119 catch (pluginlib::PluginlibException& ex)
121 RCLCPP_FATAL_STREAM(logger_,
"Exception while creating controller manager plugin loader: " << ex.what());
125 if (controller_manager_loader_)
127 std::string controller;
129 if (!node_->get_parameter(
"moveit_controller_manager", controller))
131 const std::vector<std::string>& classes = controller_manager_loader_->getDeclaredClasses();
132 if (classes.size() == 1)
134 controller = classes[0];
136 "Parameter '~moveit_controller_manager' is not specified but only one "
137 "matching plugin was found: '%s'. Using that one.",
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 "
150 if (controller ==
"moveit_ros_control_interface/MoveItControllerManager")
152 RCLCPP_WARN(logger_,
"moveit_ros_control_interface/MoveItControllerManager is deprecated. Replace with "
153 "`moveit_ros_control_interface/Ros2ControlManager.`");
155 if (controller ==
"moveit_ros_control_interface/MoveItMultiControllerManager")
157 RCLCPP_WARN(logger_,
"moveit_ros_control_interface/MoveItMultiControllerManager is deprecated. Replace with "
158 "`moveit_ros_control_interface/Ros2ControlMultiManager.`");
161 if (!controller.empty())
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);
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));
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_);
183 private_executor_thread_ = std::thread([
this]() { private_executor_->spin(); });
185 catch (pluginlib::PluginlibException& ex)
187 RCLCPP_FATAL_STREAM(logger_,
"Exception while loading controller manager '" << controller <<
"': " << ex.what());
193 reloadControllerInformation();
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>(
202 [
this](
const std_msgs::msg::String::ConstSharedPtr& event) {
return receiveEvent(event); },
options);
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_);
214 initializeAllowedStartToleranceJoints();
216 if (manage_controllers_)
218 RCLCPP_INFO(logger_,
"Trajectory execution is managing controllers");
222 RCLCPP_INFO(logger_,
"Trajectory execution is not managing controllers");
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)
230 const std::string&
name = parameter.get_name();
231 if (name ==
"trajectory_execution.execution_duration_monitoring")
235 else if (name ==
"trajectory_execution.allowed_execution_duration_scaling")
239 else if (name ==
"trajectory_execution.allowed_goal_duration_margin")
243 else if (name ==
"trajectory_execution.execution_velocity_scaling")
247 else if (name ==
"trajectory_execution.allowed_start_tolerance")
251 else if (
name.find(
"trajectory_execution.allowed_start_tolerance_joints.") == 0)
253 setAllowedStartToleranceJoint(name, parameter.as_double());
255 else if (name ==
"trajectory_execution.wait_for_trajectory_completion")
261 result.successful =
false;
266 callback_handler_ = controller_mgr_node_->add_on_set_parameters_callback(controller_mgr_parameter_set_callback);
271 execution_duration_monitoring_ = flag;
276 return execution_duration_monitoring_;
281 allowed_execution_duration_scaling_ = scaling;
286 return allowed_execution_duration_scaling_;
291 allowed_goal_duration_margin_ = margin;
296 return allowed_goal_duration_margin_;
301 execution_velocity_scaling_ = scaling;
306 return execution_velocity_scaling_;
311 allowed_start_tolerance_ = tolerance;
316 return allowed_start_tolerance_;
321 wait_for_trajectory_completion_ = flag;
326 return wait_for_trajectory_completion_;
331 return manage_controllers_;
336 return controller_manager_;
347 RCLCPP_WARN_STREAM(logger_,
"Unknown event type: '" << event <<
'\'');
351void TrajectoryExecutionManager::receiveEvent(
const std_msgs::msg::String::ConstSharedPtr& event)
353 RCLCPP_INFO_STREAM(logger_,
"Received event '" << event->data <<
'\'');
359 if (controller.empty())
361 return push(trajectory, std::vector<std::string>());
365 return push(trajectory, std::vector<std::string>(1, controller));
370 const std::string& controller)
372 if (controller.empty())
374 return push(trajectory, std::vector<std::string>());
378 return push(trajectory, std::vector<std::string>(1, controller));
383 const std::vector<std::string>& controllers)
385 moveit_msgs::msg::RobotTrajectory traj;
386 traj.joint_trajectory = trajectory;
387 return push(traj, controllers);
391 const std::vector<std::string>& controllers)
393 if (!execution_complete_)
395 RCLCPP_ERROR(logger_,
"Cannot push a new trajectory while another is being executed");
400 std::optional<moveit_msgs::msg::RobotTrajectory> replaced_trajectory;
401 if (control_multi_dof_joint_variables_ && !trajectory.multi_dof_joint_trajectory.points.empty())
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)
415 all_trajectory_joints.push_back(mdof_joint);
419 const auto joint_trajectory =
424 if (!joint_trajectory.has_value())
426 RCLCPP_ERROR(logger_,
"Failed to convert multi-DOF trajectory to joint trajectory, aborting execution!");
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();
437 if (configure(*context, replaced_trajectory.value_or(trajectory), controllers))
441 std::stringstream ss;
442 ss <<
"Pushed trajectory for execution using controllers [ ";
443 for (
const std::string& controller : context->
controllers_)
444 ss << controller <<
' ';
449 RCLCPP_INFO_STREAM(logger_, ss.str());
451 trajectories_.push_back(context);
463void TrajectoryExecutionManager::reloadControllerInformation()
465 known_controllers_.clear();
466 if (controller_manager_)
468 std::vector<std::string> names;
469 controller_manager_->getControllersList(names);
470 for (
const std::string& name : names)
472 std::vector<std::string> joints;
473 controller_manager_->getControllerJoints(name, joints);
474 ControllerInformation ci;
476 ci.joints_.insert(joints.begin(), joints.end());
477 known_controllers_[ci.name_] = ci;
481 controller_manager_->getActiveControllers(names);
482 for (
const auto& active_name : names)
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())
488 found_it->second.state_.active_ =
true;
492 for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
493 it != known_controllers_.end(); ++it)
495 for (std::map<std::string, ControllerInformation>::iterator jt = known_controllers_.begin();
496 jt != known_controllers_.end(); ++jt)
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())
505 it->second.overlapping_controllers_.insert(jt->first);
506 jt->second.overlapping_controllers_.insert(it->first);
514 RCLCPP_ERROR(logger_,
"Failed to reload controllers: `controller_manager_` does not exist.");
518void TrajectoryExecutionManager::updateControllerState(
const std::string& controller,
const rclcpp::Duration& age)
520 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
521 if (it != known_controllers_.end())
523 updateControllerState(it->second, age);
527 RCLCPP_ERROR(logger_,
"Controller '%s' is not known.", controller.c_str());
531void TrajectoryExecutionManager::updateControllerState(ControllerInformation& ci,
const rclcpp::Duration& age)
533 if (node_->now() - ci.last_update_ >= age)
535 if (controller_manager_)
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();
544 RCLCPP_INFO(logger_,
"Information for controller '%s' is assumed to be up to date.", ci.name_.c_str());
547void TrajectoryExecutionManager::updateControllersState(
const rclcpp::Duration& age)
549 for (std::pair<const std::string, ControllerInformation>& known_controller : known_controllers_)
550 updateControllerState(known_controller.second, age);
553bool TrajectoryExecutionManager::checkControllerCombination(std::vector<std::string>& selected,
554 const std::set<std::string>& actuated_joints)
556 std::set<std::string> combined_joints;
557 for (
const std::string& controller : selected)
559 const ControllerInformation& ci = known_controllers_[controller];
560 combined_joints.insert(ci.joints_.begin(), ci.joints_.end());
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());
576 return std::includes(combined_joints.begin(), combined_joints.end(), actuated_joints.begin(), actuated_joints.end());
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,
583 const std::set<std::string>& actuated_joints)
585 if (selected_controllers.size() == controller_count)
587 if (checkControllerCombination(selected_controllers, actuated_joints))
592 for (std::size_t i = start_index; i < available_controllers.size(); ++i)
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)
598 if (ci.overlapping_controllers_.find(selected_controllers[j]) != ci.overlapping_controllers_.end())
603 selected_controllers.push_back(available_controllers[i]);
604 generateControllerCombination(i + 1, controller_count, available_controllers, selected_controllers,
606 selected_controllers.pop_back();
612struct OrderPotentialControllerCombination
614 bool operator()(
const std::size_t a,
const std::size_t b)
const
617 if (nrdefault[a] > nrdefault[b])
619 if (nrdefault[a] < nrdefault[b])
623 if (nrjoints[a] < nrjoints[b])
625 if (nrjoints[a] > nrjoints[b])
629 if (nractive[a] > nractive[b])
631 if (nractive[a] < nractive[b])
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)
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,
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());
691 updateControllerState(
selected_options[i][k], DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
692 const ControllerInformation& ci = known_controllers_[
selected_options[i][k]];
694 if (ci.state_.default_)
695 order.nrdefault[i]++;
696 if (ci.state_.active_)
698 order.nrjoints[i] += ci.joints_.size();
708 std::sort(bijection.begin(), bijection.end(), order);
712 if (!manage_controllers_)
737 for (
const std::string& controller : controllers)
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_)
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)
751 for (std::size_t i = 1; i <= available_controllers.size(); ++i)
753 if (findControllers(actuated_joints, i, available_controllers, selected_controllers))
758 std::vector<std::string> other_option;
759 for (std::size_t j = i + 1; j <= available_controllers.size(); ++j)
761 if (findControllers(actuated_joints, j, available_controllers, other_option))
765 selected_controllers = other_option;
777bool TrajectoryExecutionManager::distributeTrajectory(
const moveit_msgs::msg::RobotTrajectory& trajectory,
778 const std::vector<std::string>& controllers,
779 std::vector<moveit_msgs::msg::RobotTrajectory>& parts)
782 parts.resize(controllers.size());
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)
795 actuated_joints_single.insert(joint_name);
799 for (std::size_t i = 0; i < controllers.size(); ++i)
801 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
802 if (it == known_controllers_.end())
804 RCLCPP_ERROR_STREAM(logger_,
"Controller " << controllers[i] <<
" not found.");
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]);
816 if (!intersect_mdof.empty())
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]];
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)
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)
836 parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
837 trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
839 if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
841 parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());
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_;
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_;
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_;
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_;
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_;
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_;
864 if (!intersect_single.empty())
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)
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())
881 parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
882 for (std::size_t k = 0; k < bijection.size(); ++k)
884 parts[i].joint_trajectory.points[j].positions[k] =
885 trajectory.joint_trajectory.points[j].positions[bijection[k]];
888 if (!trajectory.joint_trajectory.points[j].velocities.empty())
890 parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
891 for (std::size_t k = 0; k < bijection.size(); ++k)
893 parts[i].joint_trajectory.points[j].velocities[k] =
894 trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
897 if (!trajectory.joint_trajectory.points[j].accelerations.empty())
899 parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
900 for (std::size_t k = 0; k < bijection.size(); ++k)
902 parts[i].joint_trajectory.points[j].accelerations[k] =
903 trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
906 if (!trajectory.joint_trajectory.points[j].effort.empty())
908 parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
909 for (std::size_t k = 0; k < bijection.size(); ++k)
911 parts[i].joint_trajectory.points[j].effort[k] =
912 trajectory.joint_trajectory.points[j].effort[bijection[k]];
922bool TrajectoryExecutionManager::validate(
const TrajectoryExecutionContext& context)
const
924 if (allowed_start_tolerance_ == 0 && allowed_start_tolerance_joints_.empty())
927 if (allowed_start_tolerance_joints_.empty())
929 RCLCPP_INFO_STREAM(logger_,
"Validating trajectory with allowed_start_tolerance " << allowed_start_tolerance_);
933 std::ostringstream ss;
934 for (
const auto& [joint_name, joint_start_tolerance] : allowed_start_tolerance_joints_)
938 ss << joint_name <<
": " << joint_start_tolerance;
940 RCLCPP_INFO_STREAM(logger_,
"Validating trajectory with allowed_start_tolerance "
941 << allowed_start_tolerance_ <<
" and allowed_start_tolerance_joints {" << ss.str()
945 moveit::core::RobotStatePtr current_state;
946 if (!csm_->waitForCurrentState(node_->now()) || !(current_state = csm_->getCurrentState()))
948 RCLCPP_WARN(logger_,
"Failed to validate trajectory: couldn't receive full current joint state within 1s");
952 for (
const auto& trajectory : context.trajectory_parts_)
954 if (!trajectory.joint_trajectory.points.empty())
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())
961 RCLCPP_ERROR(logger_,
"Wrong trajectory: #joints: %zu != #positions: %zu", joint_names.size(), positions.size());
965 std::set<const moveit::core::JointModel*> joints;
966 for (
const auto& joint_name : joint_names)
971 RCLCPP_ERROR_STREAM(logger_,
"Unknown joint in trajectory: " << joint_name);
982 reference_state.setVariablePositions(joint_names, positions);
984 for (
const auto joint : joints)
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)
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)
998 RCLCPP_DEBUG(logger_,
"| %s | %g | %g |", joint_name.c_str(),
999 reference_state.getVariablePosition(joint_name),
1000 current_state->getVariablePosition(joint_name));
1007 if (!trajectory.multi_dof_joint_trajectory.points.empty())
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())
1015 RCLCPP_ERROR(logger_,
"Wrong trajectory: #joints: %zu != #transforms: %zu", joint_names.size(),
1020 for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
1025 RCLCPP_ERROR_STREAM(logger_,
"Unknown joint in trajectory: " << joint_names[i]);
1031 Eigen::Isometry3d cur_transform, start_transform;
1034 start_transform = tf2::transformToEigen(transforms[i]);
1035 ASSERT_ISOMETRY(start_transform)
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))
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());
1055bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context,
1056 const moveit_msgs::msg::RobotTrajectory& trajectory,
1057 const std::vector<std::string>& controllers)
1059 if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
1065 reloadControllerInformation();
1066 std::set<std::string> actuated_joints;
1068 auto is_actuated = [
this](
const std::string& joint_name) ->
bool {
1072 for (
const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names)
1074 if (is_actuated(joint_name))
1075 actuated_joints.insert(joint_name);
1077 for (
const std::string& joint_name : trajectory.joint_trajectory.joint_names)
1079 if (is_actuated(joint_name))
1080 actuated_joints.insert(joint_name);
1083 if (actuated_joints.empty())
1085 RCLCPP_WARN(logger_,
"The trajectory to execute specifies no joints");
1089 if (controllers.empty())
1092 bool reloaded =
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_))
1102 if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1110 reloadControllerInformation();
1121 bool reloaded =
false;
1122 for (
const std::string& controller : controllers)
1124 if (known_controllers_.find(controller) == known_controllers_.end())
1126 reloadControllerInformation();
1133 for (
const std::string& controller : controllers)
1135 if (known_controllers_.find(controller) == known_controllers_.end())
1137 std::stringstream stream;
1138 for (
const auto& controller : known_controllers_)
1140 stream <<
" `" << controller.first <<
'`';
1142 RCLCPP_ERROR_STREAM(logger_,
1143 "Controller " << controller <<
" is not known. Known controllers: " << stream.str());
1148 if (selectControllers(actuated_joints, controllers, context.controllers_))
1150 if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
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]",
1160 std::stringstream ss2;
1161 std::map<std::string, ControllerInformation>::const_iterator mi;
1162 for (mi = known_controllers_.begin(); mi != known_controllers_.end(); ++mi)
1164 ss2 <<
"controller '" << mi->second.name_ <<
"' controls joints:\n";
1166 std::set<std::string>::const_iterator ji;
1167 for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ++ji)
1169 ss2 <<
" " << *ji <<
'\n';
1172 RCLCPP_ERROR(logger_,
"Known controllers and their joints:\n%s", ss2.str().c_str());
1174 if (!trajectory.multi_dof_joint_trajectory.joint_names.empty())
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`");
1189void TrajectoryExecutionManager::stopExecutionInternal()
1192 for (moveit_controller_manager::MoveItControllerHandlePtr& active_handle : active_handles_)
1196 active_handle->cancelExecution();
1198 catch (std::exception& ex)
1200 RCLCPP_ERROR(logger_,
"Caught %s when canceling execution.", ex.what());
1207 if (!execution_complete_)
1209 execution_state_mutex_.lock();
1210 if (!execution_complete_)
1216 execution_complete_ =
true;
1217 stopExecutionInternal();
1221 execution_state_mutex_.unlock();
1222 RCLCPP_INFO(logger_,
"Stopped trajectory execution.");
1225 std::scoped_lock lock(execution_thread_mutex_);
1226 if (execution_thread_)
1228 execution_thread_->join();
1229 execution_thread_.reset();
1236 execution_state_mutex_.unlock();
1238 else if (execution_thread_)
1241 std::scoped_lock lock(execution_thread_mutex_);
1242 if (execution_thread_)
1244 execution_thread_->join();
1245 execution_thread_.reset();
1260 if (trajectories_.empty())
1266 if (!trajectories_.empty() && !validate(*trajectories_.front()))
1272 callback(last_execution_status_);
1277 execution_complete_ =
false;
1278 execution_thread_ = std::make_unique<std::thread>(&TrajectoryExecutionManager::executeThread,
this, callback,
1279 part_callback, auto_clear);
1285 std::unique_lock<std::mutex> ulock(execution_state_mutex_);
1286 while (!execution_complete_)
1287 execution_complete_condition_.wait(ulock);
1293 return last_execution_status_;
1296void TrajectoryExecutionManager::clear()
1298 if (execution_complete_)
1300 std::scoped_lock slock(execution_state_mutex_);
1301 for (TrajectoryExecutionContext* trajectory : trajectories_)
1303 trajectories_.clear();
1306 RCLCPP_FATAL(logger_,
"Expecting execution_complete_ to be true!");
1309void TrajectoryExecutionManager::executeThread(
const ExecutionCompleteCallback& callback,
1310 const PathSegmentCompleteCallback& part_callback,
bool auto_clear)
1313 if (execution_complete_)
1317 callback(last_execution_status_);
1321 RCLCPP_INFO(logger_,
"Starting trajectory execution ...");
1328 for (; i < trajectories_.size(); ++i)
1330 bool epart = executePart(i);
1331 if (epart && part_callback)
1333 if (!epart || execution_complete_)
1343 std::scoped_lock slock(execution_state_mutex_);
1344 if (!execution_complete_)
1346 waitForRobotToStop(*trajectories_[i - 1]);
1350 RCLCPP_INFO(logger_,
"Completed trajectory execution with status %s ...", last_execution_status_.
asString().c_str());
1353 execution_state_mutex_.lock();
1354 execution_complete_ =
true;
1355 execution_state_mutex_.unlock();
1356 execution_complete_condition_.notify_all();
1364 callback(last_execution_status_);
1367bool TrajectoryExecutionManager::executePart(std::size_t part_index)
1369 TrajectoryExecutionContext& context = *trajectories_[part_index];
1375 if (execution_complete_)
1378 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
1380 std::scoped_lock slock(execution_state_mutex_);
1381 if (!execution_complete_)
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)
1390 moveit_controller_manager::MoveItControllerHandlePtr h;
1393 h = controller_manager_->getControllerHandle(context.controllers_[i]);
1395 catch (std::exception& ex)
1397 RCLCPP_ERROR(logger_,
"Caught %s when retrieving controller handle", ex.what());
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());
1408 active_handles_[i] = h;
1410 handles = active_handles_;
1411 for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1416 ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
1418 catch (std::exception& ex)
1420 RCLCPP_ERROR(logger_,
"Caught %s when sending trajectory to controller", ex.what());
1424 for (std::size_t j = 0; j < i; ++j)
1428 active_handles_[j]->cancelExecution();
1430 catch (std::exception& ex)
1432 RCLCPP_ERROR(logger_,
"Caught %s when canceling execution", ex.what());
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());
1438 RCLCPP_ERROR(logger_,
"Cancelling previously sent trajectory parts");
1439 active_handles_.clear();
1440 current_context_ = -1;
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)
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()))
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)
1462 d = std::max(d, rclcpp::Time(context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp) -
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) :
1472 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start));
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()))
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_;
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() ?
1495 allowed_goal_duration_margin_;
1498 expected_trajectory_duration =
1499 std::max(d * current_scaling + rclcpp::Duration::from_seconds(current_margin), expected_trajectory_duration);
1503 if (longest_part >= 0)
1505 std::scoped_lock slock(time_index_mutex_);
1507 if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >=
1508 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
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));
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)
1522 d = rclcpp::Time(context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp) -
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));
1532 for (moveit_controller_manager::MoveItControllerHandlePtr& handle : handles)
1534 if (execution_duration_monitoring_)
1536 if (!handle->waitForExecution(expected_trajectory_duration))
1538 if (!execution_complete_ && node_->now() - current_time > expected_trajectory_duration)
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());
1545 std::scoped_lock slock(execution_state_mutex_);
1546 stopExecutionInternal();
1556 handle->waitForExecution();
1559 if (execution_complete_)
1566 RCLCPP_WARN_STREAM(logger_,
"Controller handle " << handle->getName() <<
" reports status "
1567 << handle->getLastExecutionStatus().asString());
1568 last_execution_status_ = handle->getLastExecutionStatus();
1574 execution_state_mutex_.lock();
1575 active_handles_.clear();
1578 time_index_mutex_.lock();
1579 time_index_.clear();
1580 current_context_ = -1;
1581 time_index_mutex_.unlock();
1583 execution_state_mutex_.unlock();
1588 RCLCPP_ERROR(logger_,
"Active status of required controllers can not be assured.");
1594bool TrajectoryExecutionManager::waitForRobotToStop(
const TrajectoryExecutionContext& context,
double wait_time)
1597 if ((allowed_start_tolerance_ == 0 && allowed_start_tolerance_joints_.empty()) || !wait_for_trajectory_completion_)
1599 RCLCPP_INFO(logger_,
"Not waiting for trajectory completion");
1603 auto start = std::chrono::system_clock::now();
1604 double time_remaining = wait_time;
1606 moveit::core::RobotStatePtr prev_state, cur_state;
1607 prev_state = csm_->getCurrentState();
1608 prev_state->enforceBounds();
1611 unsigned int no_motion_count = 0;
1612 while (time_remaining > 0. && no_motion_count < 3)
1614 if (!csm_->waitForCurrentState(node_->now(), time_remaining) || !(cur_state = csm_->getCurrentState()))
1616 RCLCPP_WARN(logger_,
"Failed to receive current joint state");
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();
1625 for (
const auto& trajectory : context.trajectory_parts_)
1627 const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
1628 const std::size_t n = joint_names.size();
1630 for (std::size_t i = 0; i < n && !moved; ++i)
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)
1641 no_motion_count = 0;
1650 std::swap(prev_state, cur_state);
1653 return time_remaining > 0;
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);
1669const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>&
1672 return trajectories_;
1677 return last_execution_status_;
1683 if (joint_model_group)
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)
1712 if (selectControllers(jset, all_controller_names, selected_controllers))
1729 reloadControllerInformation();
1731 updateControllersState(DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
1733 if (manage_controllers_)
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)
1741 std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.find(controller);
1742 if (it == known_controllers_.end())
1744 std::stringstream stream;
1745 for (
const auto& controller : known_controllers_)
1747 stream <<
" `" << controller.first <<
'`';
1749 RCLCPP_WARN_STREAM(logger_,
"Controller " << controller <<
" is not known. Known controllers: " << stream.str());
1752 if (!it->second.state_.active_)
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_)
1759 const ControllerInformation& ci = known_controllers_[overlapping_controller];
1760 if (ci.state_.active_)
1762 controllers_to_deactivate.push_back(overlapping_controller);
1763 joints_to_be_deactivated.insert(ci.joints_.begin(), ci.joints_.end());
1768 RCLCPP_DEBUG_STREAM(logger_,
"Controller " << controller <<
" is already active");
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()));
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)
1781 for (
const std::string& controller_to_activate : controllers_to_activate)
1783 if (it->second.overlapping_controllers_.find(controller_to_activate) !=
1784 it->second.overlapping_controllers_.end())
1791 possible_additional_controllers.push_back(it->first);
1795 std::vector<std::string> additional_controllers;
1796 if (selectControllers(diff, possible_additional_controllers, additional_controllers))
1798 controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(),
1799 additional_controllers.end());
1806 if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
1808 if (controller_manager_)
1811 for (
const std::string& controller_to_activate : controllers_to_activate)
1813 ControllerInformation& ci = known_controllers_[controller_to_activate];
1814 ci.last_update_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
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);
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)
1833 if (it->second.state_.active_)
1835 originally_active.insert(it->first);
1838 return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
1842void TrajectoryExecutionManager::loadControllerParams()
1865double TrajectoryExecutionManager::getAllowedStartToleranceJoint(
const std::string& joint_name)
const
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_;
1872void TrajectoryExecutionManager::setAllowedStartToleranceJoint(
const std::string& parameter_name,
1873 double joint_start_tolerance)
1875 if (joint_start_tolerance < 0)
1877 RCLCPP_WARN(logger_,
"%s has a negative value. The start tolerance value for that joint was not updated.",
1878 parameter_name.c_str());
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());
1888 if (!robot_model_->hasJointModel(joint_name))
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());
1897 allowed_start_tolerance_joints_.insert_or_assign(joint_name, joint_start_tolerance);
1900void TrajectoryExecutionManager::initializeAllowedStartToleranceJoints()
1902 allowed_start_tolerance_joints_.clear();
1906 for (
const auto& joint_name : robot_model_->getJointModelNames())
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))
1912 if (joint_start_tolerance < 0)
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());
1920 allowed_start_tolerance_joints_.insert({ joint_name, joint_start_tolerance });
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...
void setAllowedGoalDurationMargin(double margin)
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.
void setAllowedExecutionDurationScaling(double scaling)
moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const
Return the controller status for the last attempted execution.
std::pair< int, int > getCurrentExpectedTrajectoryIndex() const
double allowedExecutionDurationScaling() const
Get the current scaling of the duration of a trajectory to get the allowed duration of execution.
static const std::string EXECUTION_EVENT_TOPIC
void enableExecutionDurationMonitoring(bool flag)
bool push(const moveit_msgs::msg::RobotTrajectory &trajectory, const std::string &controller="")
void setExecutionVelocityScaling(double scaling)
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)
moveit_controller_manager::ExecutionStatus waitForExecution()
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.
std::function< void(std::size_t)> PathSegmentCompleteCallback
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.
The reported execution status.
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;.