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 wait_for_trajectory_completion_ =
true;
103 control_multi_dof_joint_variables_ = DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES;
105 allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING;
106 allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN;
109 loadControllerParams();
114 controller_manager_loader_ =
115 std::make_unique<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager>>(
116 "moveit_core",
"moveit_controller_manager::MoveItControllerManager");
118 catch (pluginlib::PluginlibException& ex)
120 RCLCPP_FATAL_STREAM(logger_,
"Exception while creating controller manager plugin loader: " << ex.what());
124 if (controller_manager_loader_)
126 std::string controller;
128 if (!node_->get_parameter(
"moveit_controller_manager", controller))
130 const std::vector<std::string>& classes = controller_manager_loader_->getDeclaredClasses();
131 if (classes.size() == 1)
133 controller = classes[0];
135 "Parameter '~moveit_controller_manager' is not specified but only one "
136 "matching plugin was found: '%s'. Using that one.",
141 RCLCPP_FATAL(logger_,
"Parameter '~moveit_controller_manager' not specified. This is needed to "
142 "identify the plugin to use for interacting with controllers. No paths can "
149 if (controller ==
"moveit_ros_control_interface/MoveItControllerManager")
151 RCLCPP_WARN(logger_,
"moveit_ros_control_interface/MoveItControllerManager is deprecated. Replace with "
152 "`moveit_ros_control_interface/Ros2ControlManager.`");
154 if (controller ==
"moveit_ros_control_interface/MoveItMultiControllerManager")
156 RCLCPP_WARN(logger_,
"moveit_ros_control_interface/MoveItMultiControllerManager is deprecated. Replace with "
157 "`moveit_ros_control_interface/Ros2ControlMultiManager.`");
160 if (!controller.empty())
167 rclcpp::NodeOptions opt;
168 opt.allow_undeclared_parameters(
true);
169 opt.automatically_declare_parameters_from_overrides(
true);
170 controller_mgr_node_ = std::make_shared<rclcpp::Node>(
"moveit_simple_controller_manager", opt);
172 auto all_params = node_->get_node_parameters_interface()->get_parameter_overrides();
173 for (
const auto& param : all_params)
174 controller_mgr_node_->set_parameter(
rclcpp::Parameter(param.first, param.second));
176 controller_manager_ = controller_manager_loader_->createUniqueInstance(controller);
177 controller_manager_->initialize(controller_mgr_node_);
178 private_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
179 private_executor_->add_node(controller_mgr_node_);
182 private_executor_thread_ = std::thread([
this]() { private_executor_->spin(); });
184 catch (pluginlib::PluginlibException& ex)
186 RCLCPP_FATAL_STREAM(logger_,
"Exception while loading controller manager '" << controller <<
"': " << ex.what());
192 reloadControllerInformation();
196 auto callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
197 auto options = rclcpp::SubscriptionOptions();
198 options.callback_group = callback_group;
199 event_topic_subscriber_ = node_->create_subscription<std_msgs::msg::String>(
201 [
this](
const std_msgs::msg::String::ConstSharedPtr& event) {
return receiveEvent(event); },
options);
203 controller_mgr_node_->get_parameter(
"trajectory_execution.execution_duration_monitoring",
204 execution_duration_monitoring_);
205 controller_mgr_node_->get_parameter(
"trajectory_execution.allowed_execution_duration_scaling",
206 allowed_execution_duration_scaling_);
207 controller_mgr_node_->get_parameter(
"trajectory_execution.allowed_goal_duration_margin",
208 allowed_goal_duration_margin_);
209 controller_mgr_node_->get_parameter(
"trajectory_execution.allowed_start_tolerance", allowed_start_tolerance_);
210 controller_mgr_node_->get_parameter(
"trajectory_execution.control_multi_dof_joint_variables",
211 control_multi_dof_joint_variables_);
213 if (manage_controllers_)
215 RCLCPP_INFO(logger_,
"Trajectory execution is managing controllers");
219 RCLCPP_INFO(logger_,
"Trajectory execution is not managing controllers");
222 auto controller_mgr_parameter_set_callback = [
this](
const std::vector<rclcpp::Parameter>& parameters) {
223 auto result = rcl_interfaces::msg::SetParametersResult();
224 result.successful =
true;
225 for (
const auto& parameter : parameters)
227 const std::string&
name = parameter.get_name();
228 if (name ==
"trajectory_execution.execution_duration_monitoring")
232 else if (name ==
"trajectory_execution.allowed_execution_duration_scaling")
236 else if (name ==
"trajectory_execution.allowed_goal_duration_margin")
240 else if (name ==
"trajectory_execution.execution_velocity_scaling")
244 else if (name ==
"trajectory_execution.allowed_start_tolerance")
248 else if (name ==
"trajectory_execution.wait_for_trajectory_completion")
254 result.successful =
false;
259 callback_handler_ = controller_mgr_node_->add_on_set_parameters_callback(controller_mgr_parameter_set_callback);
264 execution_duration_monitoring_ = flag;
269 return execution_duration_monitoring_;
274 allowed_execution_duration_scaling_ = scaling;
279 return allowed_execution_duration_scaling_;
284 allowed_goal_duration_margin_ = margin;
289 return allowed_goal_duration_margin_;
294 execution_velocity_scaling_ = scaling;
299 return execution_velocity_scaling_;
304 allowed_start_tolerance_ = tolerance;
309 return allowed_start_tolerance_;
314 wait_for_trajectory_completion_ = flag;
319 return wait_for_trajectory_completion_;
324 return manage_controllers_;
329 return controller_manager_;
340 RCLCPP_WARN_STREAM(logger_,
"Unknown event type: '" << event <<
'\'');
344void TrajectoryExecutionManager::receiveEvent(
const std_msgs::msg::String::ConstSharedPtr& event)
346 RCLCPP_INFO_STREAM(logger_,
"Received event '" << event->data <<
'\'');
352 if (controller.empty())
354 return push(trajectory, std::vector<std::string>());
358 return push(trajectory, std::vector<std::string>(1, controller));
363 const std::string& controller)
365 if (controller.empty())
367 return push(trajectory, std::vector<std::string>());
371 return push(trajectory, std::vector<std::string>(1, controller));
376 const std::vector<std::string>& controllers)
378 moveit_msgs::msg::RobotTrajectory traj;
379 traj.joint_trajectory = trajectory;
380 return push(traj, controllers);
384 const std::vector<std::string>& controllers)
386 if (!execution_complete_)
388 RCLCPP_ERROR(logger_,
"Cannot push a new trajectory while another is being executed");
393 std::optional<moveit_msgs::msg::RobotTrajectory> replaced_trajectory;
394 if (control_multi_dof_joint_variables_ && !trajectory.multi_dof_joint_trajectory.points.empty())
405 std::vector<std::string> all_trajectory_joints = trajectory.joint_trajectory.joint_names;
406 for (
const auto& mdof_joint : trajectory.multi_dof_joint_trajectory.joint_names)
408 all_trajectory_joints.push_back(mdof_joint);
412 const auto joint_trajectory =
417 if (!joint_trajectory.has_value())
419 RCLCPP_ERROR(logger_,
"Failed to convert multi-DOF trajectory to joint trajectory, aborting execution!");
424 RCLCPP_DEBUG(logger_,
"Successfully converted multi-DOF trajectory to joint trajectory for execution.");
425 replaced_trajectory = moveit_msgs::msg::RobotTrajectory();
426 replaced_trajectory->joint_trajectory = joint_trajectory.value();
430 if (configure(*context, replaced_trajectory.value_or(trajectory), controllers))
434 std::stringstream ss;
435 ss <<
"Pushed trajectory for execution using controllers [ ";
436 for (
const std::string& controller : context->
controllers_)
437 ss << controller <<
' ';
442 RCLCPP_INFO_STREAM(logger_, ss.str());
444 trajectories_.push_back(context);
456void TrajectoryExecutionManager::reloadControllerInformation()
458 known_controllers_.clear();
459 if (controller_manager_)
461 std::vector<std::string> names;
462 controller_manager_->getControllersList(names);
463 for (
const std::string& name : names)
465 std::vector<std::string> joints;
466 controller_manager_->getControllerJoints(name, joints);
467 ControllerInformation ci;
469 ci.joints_.insert(joints.begin(), joints.end());
470 known_controllers_[ci.name_] = ci;
474 controller_manager_->getActiveControllers(names);
475 for (
const auto& active_name : names)
477 auto found_it = std::find_if(known_controllers_.begin(), known_controllers_.end(),
478 [&](
const auto& known_controller) { return known_controller.first == active_name; });
479 if (found_it != known_controllers_.end())
481 found_it->second.state_.active_ =
true;
485 for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
486 it != known_controllers_.end(); ++it)
488 for (std::map<std::string, ControllerInformation>::iterator jt = known_controllers_.begin();
489 jt != known_controllers_.end(); ++jt)
493 std::vector<std::string> intersect;
494 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), jt->second.joints_.begin(),
495 jt->second.joints_.end(), std::back_inserter(intersect));
496 if (!intersect.empty())
498 it->second.overlapping_controllers_.insert(jt->first);
499 jt->second.overlapping_controllers_.insert(it->first);
507 RCLCPP_ERROR(logger_,
"Failed to reload controllers: `controller_manager_` does not exist.");
511void TrajectoryExecutionManager::updateControllerState(
const std::string& controller,
const rclcpp::Duration& age)
513 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
514 if (it != known_controllers_.end())
516 updateControllerState(it->second, age);
520 RCLCPP_ERROR(logger_,
"Controller '%s' is not known.", controller.c_str());
524void TrajectoryExecutionManager::updateControllerState(ControllerInformation& ci,
const rclcpp::Duration& age)
526 if (node_->now() - ci.last_update_ >= age)
528 if (controller_manager_)
531 RCLCPP_INFO(logger_,
"Updating information for controller '%s'.", ci.name_.c_str());
532 ci.state_ = controller_manager_->getControllerState(ci.name_);
533 ci.last_update_ = node_->now();
537 RCLCPP_INFO(logger_,
"Information for controller '%s' is assumed to be up to date.", ci.name_.c_str());
540void TrajectoryExecutionManager::updateControllersState(
const rclcpp::Duration& age)
542 for (std::pair<const std::string, ControllerInformation>& known_controller : known_controllers_)
543 updateControllerState(known_controller.second, age);
546bool TrajectoryExecutionManager::checkControllerCombination(std::vector<std::string>& selected,
547 const std::set<std::string>& actuated_joints)
549 std::set<std::string> combined_joints;
550 for (
const std::string& controller : selected)
552 const ControllerInformation& ci = known_controllers_[controller];
553 combined_joints.insert(ci.joints_.begin(), ci.joints_.end());
558 std::stringstream ss, saj, sac;
559 for (
const std::string& controller : selected)
560 ss << controller <<
' ';
561 for (
const std::string& actuated_joint : actuated_joints)
562 saj << actuated_joint <<
' ';
563 for (
const std::string& combined_joint : combined_joints)
564 sac << combined_joint <<
' ';
565 RCLCPP_INFO(logger_,
"Checking if controllers [ %s] operating on joints [ %s] cover joints [ %s]", ss.str().c_str(),
566 sac.str().c_str(), saj.str().c_str());
569 return std::includes(combined_joints.begin(), combined_joints.end(), actuated_joints.begin(), actuated_joints.end());
572void TrajectoryExecutionManager::generateControllerCombination(std::size_t start_index, std::size_t controller_count,
573 const std::vector<std::string>& available_controllers,
574 std::vector<std::string>& selected_controllers,
576 const std::set<std::string>& actuated_joints)
578 if (selected_controllers.size() == controller_count)
580 if (checkControllerCombination(selected_controllers, actuated_joints))
585 for (std::size_t i = start_index; i < available_controllers.size(); ++i)
587 bool overlap =
false;
588 const ControllerInformation& ci = known_controllers_[available_controllers[i]];
589 for (std::size_t j = 0; j < selected_controllers.size() && !overlap; ++j)
591 if (ci.overlapping_controllers_.find(selected_controllers[j]) != ci.overlapping_controllers_.end())
596 selected_controllers.push_back(available_controllers[i]);
597 generateControllerCombination(i + 1, controller_count, available_controllers, selected_controllers,
599 selected_controllers.pop_back();
605struct OrderPotentialControllerCombination
607 bool operator()(
const std::size_t a,
const std::size_t b)
const
610 if (nrdefault[a] > nrdefault[b])
612 if (nrdefault[a] < nrdefault[b])
616 if (nrjoints[a] < nrjoints[b])
618 if (nrjoints[a] > nrjoints[b])
622 if (nractive[a] > nractive[b])
624 if (nractive[a] < nractive[b])
637bool TrajectoryExecutionManager::findControllers(
const std::set<std::string>& actuated_joints,
638 std::size_t controller_count,
639 const std::vector<std::string>& available_controllers,
640 std::vector<std::string>& selected_controllers)
643 std::vector<std::string> work_area;
644 OrderPotentialControllerCombination order;
645 std::vector<std::vector<std::string>>&
selected_options = order.selected_options;
646 generateControllerCombination(0, controller_count, available_controllers, work_area,
selected_options,
651 std::stringstream saj;
652 std::stringstream sac;
653 for (
const std::string& available_controller : available_controllers)
654 sac << available_controller <<
' ';
655 for (
const std::string& actuated_joint : actuated_joints)
656 saj << actuated_joint <<
' ';
657 RCLCPP_INFO(logger_,
"Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.",
658 controller_count, sac.str().c_str(), saj.str().c_str(),
selected_options.size());
684 updateControllerState(
selected_options[i][k], DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
685 const ControllerInformation& ci = known_controllers_[
selected_options[i][k]];
687 if (ci.state_.default_)
688 order.nrdefault[i]++;
689 if (ci.state_.active_)
691 order.nrjoints[i] += ci.joints_.size();
701 std::sort(bijection.begin(), bijection.end(), order);
705 if (!manage_controllers_)
730 for (
const std::string& controller : controllers)
732 updateControllerState(controller, DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
733 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
734 if (it == known_controllers_.end() || !it->second.state_.active_)
740bool TrajectoryExecutionManager::selectControllers(
const std::set<std::string>& actuated_joints,
741 const std::vector<std::string>& available_controllers,
742 std::vector<std::string>& selected_controllers)
744 for (std::size_t i = 1; i <= available_controllers.size(); ++i)
746 if (findControllers(actuated_joints, i, available_controllers, selected_controllers))
751 std::vector<std::string> other_option;
752 for (std::size_t j = i + 1; j <= available_controllers.size(); ++j)
754 if (findControllers(actuated_joints, j, available_controllers, other_option))
758 selected_controllers = other_option;
770bool TrajectoryExecutionManager::distributeTrajectory(
const moveit_msgs::msg::RobotTrajectory& trajectory,
771 const std::vector<std::string>& controllers,
772 std::vector<moveit_msgs::msg::RobotTrajectory>& parts)
775 parts.resize(controllers.size());
777 std::set<std::string> actuated_joints_mdof;
778 actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
779 trajectory.multi_dof_joint_trajectory.joint_names.end());
780 std::set<std::string> actuated_joints_single;
781 for (
const std::string& joint_name : trajectory.joint_trajectory.joint_names)
788 actuated_joints_single.insert(joint_name);
792 for (std::size_t i = 0; i < controllers.size(); ++i)
794 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
795 if (it == known_controllers_.end())
797 RCLCPP_ERROR_STREAM(logger_,
"Controller " << controllers[i] <<
" not found.");
800 std::vector<std::string> intersect_mdof;
801 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_mdof.begin(),
802 actuated_joints_mdof.end(), std::back_inserter(intersect_mdof));
803 std::vector<std::string> intersect_single;
804 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_single.begin(),
805 actuated_joints_single.end(), std::back_inserter(intersect_single));
806 if (intersect_mdof.empty() && intersect_single.empty())
807 RCLCPP_WARN_STREAM(logger_,
"No joints to be distributed for controller " << controllers[i]);
809 if (!intersect_mdof.empty())
811 std::vector<std::string>& jnames = parts[i].multi_dof_joint_trajectory.joint_names;
812 jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
813 std::map<std::string, std::size_t> index;
814 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
815 index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
816 std::vector<std::size_t> bijection(jnames.size());
817 for (std::size_t j = 0; j < jnames.size(); ++j)
818 bijection[j] = index[jnames[j]];
820 parts[i].multi_dof_joint_trajectory.header.frame_id = trajectory.multi_dof_joint_trajectory.header.frame_id;
821 parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
822 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.points.size(); ++j)
824 parts[i].multi_dof_joint_trajectory.points[j].time_from_start =
825 trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
826 parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
827 for (std::size_t k = 0; k < bijection.size(); ++k)
829 parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
830 trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
832 if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
834 parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());
836 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
837 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.x * execution_velocity_scaling_;
839 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
840 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.y * execution_velocity_scaling_;
842 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
843 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.z * execution_velocity_scaling_;
845 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
846 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.x * execution_velocity_scaling_;
848 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
849 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.y * execution_velocity_scaling_;
851 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
852 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.z * execution_velocity_scaling_;
857 if (!intersect_single.empty())
859 std::vector<std::string>& jnames = parts[i].joint_trajectory.joint_names;
860 jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
861 parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
862 std::map<std::string, std::size_t> index;
863 for (std::size_t j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j)
864 index[trajectory.joint_trajectory.joint_names[j]] = j;
865 std::vector<std::size_t> bijection(jnames.size());
866 for (std::size_t j = 0; j < jnames.size(); ++j)
867 bijection[j] = index[jnames[j]];
868 parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
869 for (std::size_t j = 0; j < trajectory.joint_trajectory.points.size(); ++j)
871 parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
872 if (!trajectory.joint_trajectory.points[j].positions.empty())
874 parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
875 for (std::size_t k = 0; k < bijection.size(); ++k)
877 parts[i].joint_trajectory.points[j].positions[k] =
878 trajectory.joint_trajectory.points[j].positions[bijection[k]];
881 if (!trajectory.joint_trajectory.points[j].velocities.empty())
883 parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
884 for (std::size_t k = 0; k < bijection.size(); ++k)
886 parts[i].joint_trajectory.points[j].velocities[k] =
887 trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
890 if (!trajectory.joint_trajectory.points[j].accelerations.empty())
892 parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
893 for (std::size_t k = 0; k < bijection.size(); ++k)
895 parts[i].joint_trajectory.points[j].accelerations[k] =
896 trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
899 if (!trajectory.joint_trajectory.points[j].effort.empty())
901 parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
902 for (std::size_t k = 0; k < bijection.size(); ++k)
904 parts[i].joint_trajectory.points[j].effort[k] =
905 trajectory.joint_trajectory.points[j].effort[bijection[k]];
915bool TrajectoryExecutionManager::validate(
const TrajectoryExecutionContext& context)
const
917 if (allowed_start_tolerance_ == 0)
920 RCLCPP_INFO(logger_,
"Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
922 moveit::core::RobotStatePtr current_state;
923 if (!csm_->waitForCurrentState(node_->now()) || !(current_state = csm_->getCurrentState()))
925 RCLCPP_WARN(logger_,
"Failed to validate trajectory: couldn't receive full current joint state within 1s");
929 for (
const auto& trajectory : context.trajectory_parts_)
931 if (!trajectory.joint_trajectory.points.empty())
934 const std::vector<double>& positions = trajectory.joint_trajectory.points.front().positions;
935 const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
936 if (positions.size() != joint_names.size())
938 RCLCPP_ERROR(logger_,
"Wrong trajectory: #joints: %zu != #positions: %zu", joint_names.size(), positions.size());
942 std::set<const moveit::core::JointModel*> joints;
943 for (
const auto& joint_name : joint_names)
948 RCLCPP_ERROR_STREAM(logger_,
"Unknown joint in trajectory: " << joint_name);
959 reference_state.setVariablePositions(joint_names, positions);
961 for (
const auto joint : joints)
963 reference_state.enforcePositionBounds(joint);
964 current_state->enforcePositionBounds(joint);
965 if (reference_state.distance(*current_state, joint) > allowed_start_tolerance_)
967 RCLCPP_ERROR(logger_,
968 "Invalid Trajectory: start point deviates from current robot state more than %g at joint '%s'."
969 "\nEnable DEBUG for detailed state info.",
970 allowed_start_tolerance_, joint->getName().c_str());
971 RCLCPP_DEBUG(logger_,
"| Joint | Expected | Current |");
972 for (
const auto& joint_name : joint_names)
974 RCLCPP_DEBUG(logger_,
"| %s | %g | %g |", joint_name.c_str(),
975 reference_state.getVariablePosition(joint_name),
976 current_state->getVariablePosition(joint_name));
983 if (!trajectory.multi_dof_joint_trajectory.points.empty())
986 const std::vector<geometry_msgs::msg::Transform>& transforms =
987 trajectory.multi_dof_joint_trajectory.points.front().transforms;
988 const std::vector<std::string>& joint_names = trajectory.multi_dof_joint_trajectory.joint_names;
989 if (transforms.size() != joint_names.size())
991 RCLCPP_ERROR(logger_,
"Wrong trajectory: #joints: %zu != #transforms: %zu", joint_names.size(),
996 for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
1001 RCLCPP_ERROR_STREAM(logger_,
"Unknown joint in trajectory: " << joint_names[i]);
1007 Eigen::Isometry3d cur_transform, start_transform;
1010 start_transform = tf2::transformToEigen(transforms[i]);
1011 ASSERT_ISOMETRY(start_transform)
1012 Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation();
1013 Eigen::AngleAxisd rotation;
1014 rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear());
1015 if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_)
1017 RCLCPP_ERROR_STREAM(logger_,
"\nInvalid Trajectory: start point deviates from current robot state more than "
1018 << allowed_start_tolerance_ <<
"\nmulti-dof joint '" << joint_names[i]
1019 <<
"': pos delta: " << offset.transpose()
1020 <<
" rot delta: " << rotation.angle());
1029bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context,
1030 const moveit_msgs::msg::RobotTrajectory& trajectory,
1031 const std::vector<std::string>& controllers)
1033 if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
1039 reloadControllerInformation();
1040 std::set<std::string> actuated_joints;
1042 auto is_actuated = [
this](
const std::string& joint_name) ->
bool {
1046 for (
const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names)
1048 if (is_actuated(joint_name))
1049 actuated_joints.insert(joint_name);
1051 for (
const std::string& joint_name : trajectory.joint_trajectory.joint_names)
1053 if (is_actuated(joint_name))
1054 actuated_joints.insert(joint_name);
1057 if (actuated_joints.empty())
1059 RCLCPP_WARN(logger_,
"The trajectory to execute specifies no joints");
1063 if (controllers.empty())
1066 bool reloaded =
false;
1070 std::vector<std::string> all_controller_names;
1071 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1072 it != known_controllers_.end(); ++it)
1073 all_controller_names.push_back(it->first);
1074 if (selectControllers(actuated_joints, all_controller_names, context.controllers_))
1076 if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1084 reloadControllerInformation();
1095 bool reloaded =
false;
1096 for (
const std::string& controller : controllers)
1098 if (known_controllers_.find(controller) == known_controllers_.end())
1100 reloadControllerInformation();
1107 for (
const std::string& controller : controllers)
1109 if (known_controllers_.find(controller) == known_controllers_.end())
1111 std::stringstream stream;
1112 for (
const auto& controller : known_controllers_)
1114 stream <<
" `" << controller.first <<
'`';
1116 RCLCPP_ERROR_STREAM(logger_,
1117 "Controller " << controller <<
" is not known. Known controllers: " << stream.str());
1122 if (selectControllers(actuated_joints, controllers, context.controllers_))
1124 if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1128 std::stringstream ss;
1129 for (
const std::string& actuated_joint : actuated_joints)
1130 ss << actuated_joint <<
' ';
1131 RCLCPP_ERROR(logger_,
"Unable to identify any set of controllers that can actuate the specified joints: [ %s]",
1134 std::stringstream ss2;
1135 std::map<std::string, ControllerInformation>::const_iterator mi;
1136 for (mi = known_controllers_.begin(); mi != known_controllers_.end(); ++mi)
1138 ss2 <<
"controller '" << mi->second.name_ <<
"' controls joints:\n";
1140 std::set<std::string>::const_iterator ji;
1141 for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ++ji)
1143 ss2 <<
" " << *ji <<
'\n';
1146 RCLCPP_ERROR(logger_,
"Known controllers and their joints:\n%s", ss2.str().c_str());
1148 if (!trajectory.multi_dof_joint_trajectory.joint_names.empty())
1150 RCLCPP_WARN(logger_,
"Hint: You can control multi-dof waypoints as joint trajectory by setting "
1151 "`trajectory_execution.control_multi_dof_joint_variables=True`");
1163void TrajectoryExecutionManager::stopExecutionInternal()
1166 for (moveit_controller_manager::MoveItControllerHandlePtr& active_handle : active_handles_)
1170 active_handle->cancelExecution();
1172 catch (std::exception& ex)
1174 RCLCPP_ERROR(logger_,
"Caught %s when canceling execution.", ex.what());
1181 if (!execution_complete_)
1183 execution_state_mutex_.lock();
1184 if (!execution_complete_)
1190 execution_complete_ =
true;
1191 stopExecutionInternal();
1195 execution_state_mutex_.unlock();
1196 RCLCPP_INFO(logger_,
"Stopped trajectory execution.");
1199 std::scoped_lock lock(execution_thread_mutex_);
1200 if (execution_thread_)
1202 execution_thread_->join();
1203 execution_thread_.reset();
1210 execution_state_mutex_.unlock();
1212 else if (execution_thread_)
1215 std::scoped_lock lock(execution_thread_mutex_);
1216 if (execution_thread_)
1218 execution_thread_->join();
1219 execution_thread_.reset();
1234 if (trajectories_.empty())
1240 if (!trajectories_.empty() && !validate(*trajectories_.front()))
1246 callback(last_execution_status_);
1251 execution_complete_ =
false;
1252 execution_thread_ = std::make_unique<std::thread>(&TrajectoryExecutionManager::executeThread,
this, callback,
1253 part_callback, auto_clear);
1259 std::unique_lock<std::mutex> ulock(execution_state_mutex_);
1260 while (!execution_complete_)
1261 execution_complete_condition_.wait(ulock);
1267 return last_execution_status_;
1272 if (execution_complete_)
1276 trajectories_.clear();
1279 RCLCPP_ERROR(logger_,
"Cannot push a new trajectory while another is being executed");
1282void TrajectoryExecutionManager::executeThread(
const ExecutionCompleteCallback& callback,
1283 const PathSegmentCompleteCallback& part_callback,
bool auto_clear)
1286 if (execution_complete_)
1290 callback(last_execution_status_);
1294 RCLCPP_INFO(logger_,
"Starting trajectory execution ...");
1301 for (; i < trajectories_.size(); ++i)
1303 bool epart = executePart(i);
1304 if (epart && part_callback)
1306 if (!epart || execution_complete_)
1315 waitForRobotToStop(*trajectories_[i - 1]);
1317 RCLCPP_INFO(logger_,
"Completed trajectory execution with status %s ...", last_execution_status_.
asString().c_str());
1320 execution_state_mutex_.lock();
1321 execution_complete_ =
true;
1322 execution_state_mutex_.unlock();
1323 execution_complete_condition_.notify_all();
1331 callback(last_execution_status_);
1334bool TrajectoryExecutionManager::executePart(std::size_t part_index)
1336 TrajectoryExecutionContext& context = *trajectories_[part_index];
1342 if (execution_complete_)
1345 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
1347 std::scoped_lock slock(execution_state_mutex_);
1348 if (!execution_complete_)
1351 time_index_mutex_.lock();
1352 current_context_ = part_index;
1353 time_index_mutex_.unlock();
1354 active_handles_.resize(context.controllers_.size());
1355 for (std::size_t i = 0; i < context.controllers_.size(); ++i)
1357 moveit_controller_manager::MoveItControllerHandlePtr h;
1360 h = controller_manager_->getControllerHandle(context.controllers_[i]);
1362 catch (std::exception& ex)
1364 RCLCPP_ERROR(logger_,
"Caught %s when retrieving controller handle", ex.what());
1368 active_handles_.clear();
1369 current_context_ = -1;
1371 RCLCPP_ERROR(logger_,
"No controller handle for controller '%s'. Aborting.",
1372 context.controllers_[i].c_str());
1375 active_handles_[i] = h;
1377 handles = active_handles_;
1378 for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1383 ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
1385 catch (std::exception& ex)
1387 RCLCPP_ERROR(logger_,
"Caught %s when sending trajectory to controller", ex.what());
1391 for (std::size_t j = 0; j < i; ++j)
1395 active_handles_[j]->cancelExecution();
1397 catch (std::exception& ex)
1399 RCLCPP_ERROR(logger_,
"Caught %s when canceling execution", ex.what());
1402 RCLCPP_ERROR(logger_,
"Failed to send trajectory part %zu of %zu to controller %s", i + 1,
1403 context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
1405 RCLCPP_ERROR(logger_,
"Cancelling previously sent trajectory parts");
1406 active_handles_.clear();
1407 current_context_ = -1;
1416 rclcpp::Time current_time = node_->now();
1417 auto expected_trajectory_duration = rclcpp::Duration::from_seconds(0);
1418 int longest_part = -1;
1419 for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1421 auto d = rclcpp::Duration::from_seconds(0);
1422 if (!(context.trajectory_parts_[i].joint_trajectory.points.empty() &&
1423 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty()))
1425 if (rclcpp::Time(context.trajectory_parts_[i].joint_trajectory.header.stamp) > current_time)
1426 d = rclcpp::Time(context.trajectory_parts_[i].joint_trajectory.header.stamp) - current_time;
1427 if (rclcpp::Time(context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp) > current_time)
1429 d = std::max(d, rclcpp::Time(context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp) -
1433 std::max(context.trajectory_parts_[i].joint_trajectory.points.empty() ?
1434 rclcpp::Duration::from_seconds(0) :
1435 rclcpp::Duration(context.trajectory_parts_[i].joint_trajectory.points.back().time_from_start),
1436 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty() ?
1437 rclcpp::Duration::from_seconds(0) :
1439 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start));
1441 if (longest_part < 0 ||
1442 std::max(context.trajectory_parts_[i].joint_trajectory.points.size(),
1443 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.size()) >
1444 std::max(context.trajectory_parts_[longest_part].joint_trajectory.points.size(),
1445 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
1452 std::map<std::string, double>::const_iterator scaling_it =
1453 controller_allowed_execution_duration_scaling_.find(context.controllers_[i]);
1454 const double current_scaling = scaling_it != controller_allowed_execution_duration_scaling_.end() ?
1455 scaling_it->second :
1456 allowed_execution_duration_scaling_;
1458 std::map<std::string, double>::const_iterator margin_it =
1459 controller_allowed_goal_duration_margin_.find(context.controllers_[i]);
1460 const double current_margin = margin_it != controller_allowed_goal_duration_margin_.end() ?
1462 allowed_goal_duration_margin_;
1465 expected_trajectory_duration =
1466 std::max(d * current_scaling + rclcpp::Duration::from_seconds(current_margin), expected_trajectory_duration);
1470 if (longest_part >= 0)
1472 std::scoped_lock slock(time_index_mutex_);
1474 if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >=
1475 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
1477 auto d = rclcpp::Duration::from_seconds(0);
1478 if (rclcpp::Time(context.trajectory_parts_[longest_part].joint_trajectory.header.stamp) > current_time)
1479 d = rclcpp::Time(context.trajectory_parts_[longest_part].joint_trajectory.header.stamp) - current_time;
1480 for (trajectory_msgs::msg::JointTrajectoryPoint& point :
1481 context.trajectory_parts_[longest_part].joint_trajectory.points)
1482 time_index_.push_back(current_time + d +
rclcpp::Duration(point.time_from_start));
1486 auto d = rclcpp::Duration::from_seconds(0);
1487 if (rclcpp::Time(context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp) > current_time)
1489 d = rclcpp::Time(context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp) -
1492 for (trajectory_msgs::msg::MultiDOFJointTrajectoryPoint& point :
1493 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points)
1494 time_index_.push_back(current_time + d +
rclcpp::Duration(point.time_from_start));
1499 for (moveit_controller_manager::MoveItControllerHandlePtr& handle : handles)
1501 if (execution_duration_monitoring_)
1503 if (!handle->waitForExecution(expected_trajectory_duration))
1505 if (!execution_complete_ && node_->now() - current_time > expected_trajectory_duration)
1507 RCLCPP_ERROR(logger_,
1508 "Controller is taking too long to execute trajectory (the expected upper "
1509 "bound for the trajectory execution was %lf seconds). Stopping trajectory.",
1510 expected_trajectory_duration.seconds());
1512 std::scoped_lock slock(execution_state_mutex_);
1513 stopExecutionInternal();
1523 handle->waitForExecution();
1526 if (execution_complete_)
1533 RCLCPP_WARN_STREAM(logger_,
"Controller handle " << handle->getName() <<
" reports status "
1534 << handle->getLastExecutionStatus().asString());
1535 last_execution_status_ = handle->getLastExecutionStatus();
1541 execution_state_mutex_.lock();
1542 active_handles_.clear();
1545 time_index_mutex_.lock();
1546 time_index_.clear();
1547 current_context_ = -1;
1548 time_index_mutex_.unlock();
1550 execution_state_mutex_.unlock();
1555 RCLCPP_ERROR(logger_,
"Active status of required controllers can not be assured.");
1561bool TrajectoryExecutionManager::waitForRobotToStop(
const TrajectoryExecutionContext& context,
double wait_time)
1564 if (allowed_start_tolerance_ == 0 || !wait_for_trajectory_completion_)
1566 RCLCPP_INFO(logger_,
"Not waiting for trajectory completion");
1570 auto start = std::chrono::system_clock::now();
1571 double time_remaining = wait_time;
1573 moveit::core::RobotStatePtr prev_state, cur_state;
1574 prev_state = csm_->getCurrentState();
1575 prev_state->enforceBounds();
1578 unsigned int no_motion_count = 0;
1579 while (time_remaining > 0. && no_motion_count < 3)
1581 if (!csm_->waitForCurrentState(node_->now(), time_remaining) || !(cur_state = csm_->getCurrentState()))
1583 RCLCPP_WARN(logger_,
"Failed to receive current joint state");
1586 cur_state->enforceBounds();
1587 std::chrono::duration<double> elapsed_seconds = std::chrono::system_clock::now() - start;
1588 time_remaining = wait_time - elapsed_seconds.count();
1592 for (
const auto& trajectory : context.trajectory_parts_)
1594 const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
1595 const std::size_t n = joint_names.size();
1597 for (std::size_t i = 0; i < n && !moved; ++i)
1603 if (fabs(jm->
distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) >
1604 allowed_start_tolerance_)
1607 no_motion_count = 0;
1616 std::swap(prev_state, cur_state);
1619 return time_remaining > 0;
1624 std::scoped_lock slock(time_index_mutex_);
1625 if (current_context_ < 0)
1626 return std::make_pair(-1, -1);
1627 if (time_index_.empty())
1628 return std::make_pair(
static_cast<int>(current_context_), -1);
1629 std::vector<rclcpp::Time>::const_iterator time_index_it =
1630 std::lower_bound(time_index_.begin(), time_index_.end(), node_->now());
1631 int pos = time_index_it - time_index_.begin();
1632 return std::make_pair(
static_cast<int>(current_context_), pos);
1635const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>&
1638 return trajectories_;
1643 return last_execution_status_;
1649 if (joint_model_group)
1661 std::vector<std::string> all_controller_names;
1662 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1663 it != known_controllers_.end(); ++it)
1664 all_controller_names.push_back(it->first);
1665 std::vector<std::string> selected_controllers;
1666 std::set<std::string> jset;
1667 for (
const std::string& joint : joints)
1678 if (selectControllers(jset, all_controller_names, selected_controllers))
1695 reloadControllerInformation();
1697 updateControllersState(DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
1699 if (manage_controllers_)
1701 std::vector<std::string> controllers_to_activate;
1702 std::vector<std::string> controllers_to_deactivate;
1703 std::set<std::string> joints_to_be_activated;
1704 std::set<std::string> joints_to_be_deactivated;
1705 for (
const std::string& controller : controllers)
1707 std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.find(controller);
1708 if (it == known_controllers_.end())
1710 std::stringstream stream;
1711 for (
const auto& controller : known_controllers_)
1713 stream <<
" `" << controller.first <<
'`';
1715 RCLCPP_WARN_STREAM(logger_,
"Controller " << controller <<
" is not known. Known controllers: " << stream.str());
1718 if (!it->second.state_.active_)
1720 RCLCPP_DEBUG_STREAM(logger_,
"Need to activate " << controller);
1721 controllers_to_activate.push_back(controller);
1722 joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
1723 for (
const std::string& overlapping_controller : it->second.overlapping_controllers_)
1725 const ControllerInformation& ci = known_controllers_[overlapping_controller];
1726 if (ci.state_.active_)
1728 controllers_to_deactivate.push_back(overlapping_controller);
1729 joints_to_be_deactivated.insert(ci.joints_.begin(), ci.joints_.end());
1734 RCLCPP_DEBUG_STREAM(logger_,
"Controller " << controller <<
" is already active");
1736 std::set<std::string> diff;
1737 std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
1738 joints_to_be_activated.begin(), joints_to_be_activated.end(), std::inserter(diff, diff.end()));
1742 std::vector<std::string> possible_additional_controllers;
1743 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1744 it != known_controllers_.end(); ++it)
1747 for (
const std::string& controller_to_activate : controllers_to_activate)
1749 if (it->second.overlapping_controllers_.find(controller_to_activate) !=
1750 it->second.overlapping_controllers_.end())
1757 possible_additional_controllers.push_back(it->first);
1761 std::vector<std::string> additional_controllers;
1762 if (selectControllers(diff, possible_additional_controllers, additional_controllers))
1764 controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(),
1765 additional_controllers.end());
1772 if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
1774 if (controller_manager_)
1777 for (
const std::string& controller_to_activate : controllers_to_activate)
1779 ControllerInformation& ci = known_controllers_[controller_to_activate];
1780 ci.last_update_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
1783 for (
const std::string& controller_to_deactivate : controllers_to_deactivate)
1784 known_controllers_[controller_to_deactivate].last_update_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
1785 return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
1795 std::set<std::string> originally_active;
1796 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1797 it != known_controllers_.end(); ++it)
1799 if (it->second.state_.active_)
1801 originally_active.insert(it->first);
1804 return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
1808void TrajectoryExecutionManager::loadControllerParams()
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.
void clear()
Clear the trajectories to execute.
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;.