39 #include <geometric_shapes/check_isometry.h>
40 #include <tf2_eigen/tf2_eigen.hpp>
48 static const auto DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE = rclcpp::Duration::from_seconds(1);
49 static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 0.5;
52 static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING =
56 const moveit::core::RobotModelConstPtr& robot_model,
57 const planning_scene_monitor::CurrentStateMonitorPtr& csm)
58 : node_(node), logger_(
moveit::
getLogger(
"trajectory_execution_manager")), robot_model_(robot_model), csm_(csm)
60 if (!node_->get_parameter(
"moveit_manage_controllers", manage_controllers_))
61 manage_controllers_ =
false;
66 const moveit::core::RobotModelConstPtr& robot_model,
67 const planning_scene_monitor::CurrentStateMonitorPtr& csm,
68 bool manage_controllers)
71 , robot_model_(robot_model)
73 , manage_controllers_(manage_controllers)
81 if (private_executor_)
82 private_executor_->cancel();
83 if (private_executor_thread_.joinable())
84 private_executor_thread_.join();
87 void TrajectoryExecutionManager::initialize()
90 execution_complete_ =
true;
91 current_context_ = -1;
93 execution_duration_monitoring_ =
true;
94 execution_velocity_scaling_ = 1.0;
95 allowed_start_tolerance_ = 0.01;
96 wait_for_trajectory_completion_ =
true;
98 allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING;
99 allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN;
102 loadControllerParams();
107 controller_manager_loader_ =
108 std::make_unique<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager>>(
109 "moveit_core",
"moveit_controller_manager::MoveItControllerManager");
111 catch (pluginlib::PluginlibException& ex)
113 RCLCPP_FATAL_STREAM(logger_,
"Exception while creating controller manager plugin loader: " << ex.what());
117 if (controller_manager_loader_)
119 std::string controller;
121 if (!node_->get_parameter(
"moveit_controller_manager", controller))
123 const std::vector<std::string>& classes = controller_manager_loader_->getDeclaredClasses();
124 if (classes.size() == 1)
126 controller = classes[0];
128 "Parameter '~moveit_controller_manager' is not specified but only one "
129 "matching plugin was found: '%s'. Using that one.",
134 RCLCPP_FATAL(logger_,
"Parameter '~moveit_controller_manager' not specified. This is needed to "
135 "identify the plugin to use for interacting with controllers. No paths can "
142 if (controller ==
"moveit_ros_control_interface/MoveItControllerManager")
144 RCLCPP_WARN(logger_,
"moveit_ros_control_interface/MoveItControllerManager is deprecated. Replace with "
145 "`moveit_ros_control_interface/Ros2ControlManager.`");
147 if (controller ==
"moveit_ros_control_interface/MoveItMultiControllerManager")
149 RCLCPP_WARN(logger_,
"moveit_ros_control_interface/MoveItMultiControllerManager is deprecated. Replace with "
150 "`moveit_ros_control_interface/Ros2ControlMultiManager.`");
153 if (!controller.empty())
160 rclcpp::NodeOptions opt;
161 opt.allow_undeclared_parameters(
true);
162 opt.automatically_declare_parameters_from_overrides(
true);
163 controller_mgr_node_.reset(
new rclcpp::Node(
"moveit_simple_controller_manager", opt));
165 auto all_params = node_->get_node_parameters_interface()->get_parameter_overrides();
166 for (
const auto& param : all_params)
167 controller_mgr_node_->set_parameter(rclcpp::Parameter(param.first, param.second));
169 controller_manager_ = controller_manager_loader_->createUniqueInstance(controller);
170 controller_manager_->initialize(controller_mgr_node_);
171 private_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
172 private_executor_->add_node(controller_mgr_node_);
175 private_executor_thread_ = std::thread([
this]() { private_executor_->spin(); });
177 catch (pluginlib::PluginlibException& ex)
179 RCLCPP_FATAL_STREAM(logger_,
"Exception while loading controller manager '" << controller <<
"': " << ex.what());
185 reloadControllerInformation();
189 auto callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
190 auto options = rclcpp::SubscriptionOptions();
191 options.callback_group = callback_group;
192 event_topic_subscriber_ = node_->create_subscription<std_msgs::msg::String>(
194 [
this](
const std_msgs::msg::String::ConstSharedPtr& event) {
return receiveEvent(event); },
options);
196 controller_mgr_node_->get_parameter(
"trajectory_execution.execution_duration_monitoring",
197 execution_duration_monitoring_);
198 controller_mgr_node_->get_parameter(
"trajectory_execution.allowed_execution_duration_scaling",
199 allowed_execution_duration_scaling_);
200 controller_mgr_node_->get_parameter(
"trajectory_execution.allowed_goal_duration_margin",
201 allowed_goal_duration_margin_);
202 controller_mgr_node_->get_parameter(
"trajectory_execution.allowed_start_tolerance", allowed_start_tolerance_);
204 if (manage_controllers_)
206 RCLCPP_INFO(logger_,
"Trajectory execution is managing controllers");
210 RCLCPP_INFO(logger_,
"Trajectory execution is not managing controllers");
213 auto controller_mgr_parameter_set_callback = [
this](
const std::vector<rclcpp::Parameter>& parameters) {
214 auto result = rcl_interfaces::msg::SetParametersResult();
215 result.successful =
true;
216 for (
const auto& parameter : parameters)
218 const std::string&
name = parameter.get_name();
219 if (
name ==
"trajectory_execution.execution_duration_monitoring")
223 else if (
name ==
"trajectory_execution.allowed_execution_duration_scaling")
227 else if (
name ==
"trajectory_execution.allowed_goal_duration_margin")
231 else if (
name ==
"trajectory_execution.execution_velocity_scaling")
235 else if (
name ==
"trajectory_execution.allowed_start_tolerance")
239 else if (
name ==
"trajectory_execution.wait_for_trajectory_completion")
245 result.successful =
false;
250 callback_handler_ = controller_mgr_node_->add_on_set_parameters_callback(controller_mgr_parameter_set_callback);
255 execution_duration_monitoring_ = flag;
260 allowed_execution_duration_scaling_ = scaling;
265 allowed_goal_duration_margin_ = margin;
270 execution_velocity_scaling_ = scaling;
275 allowed_start_tolerance_ = tolerance;
280 wait_for_trajectory_completion_ = flag;
285 return manage_controllers_;
290 return controller_manager_;
301 RCLCPP_WARN_STREAM(logger_,
"Unknown event type: '" << event <<
'\'');
305 void TrajectoryExecutionManager::receiveEvent(
const std_msgs::msg::String::ConstSharedPtr& event)
307 RCLCPP_INFO_STREAM(logger_,
"Received event '" << event->data <<
'\'');
313 if (controller.empty())
315 return push(trajectory, std::vector<std::string>());
319 return push(trajectory, std::vector<std::string>(1, controller));
324 const std::string& controller)
326 if (controller.empty())
328 return push(trajectory, std::vector<std::string>());
332 return push(trajectory, std::vector<std::string>(1, controller));
337 const std::vector<std::string>& controllers)
339 moveit_msgs::msg::RobotTrajectory traj;
340 traj.joint_trajectory = trajectory;
341 return push(traj, controllers);
345 const std::vector<std::string>& controllers)
347 if (!execution_complete_)
349 RCLCPP_ERROR(logger_,
"Cannot push a new trajectory while another is being executed");
354 if (configure(*context, trajectory, controllers))
358 std::stringstream ss;
359 ss <<
"Pushed trajectory for execution using controllers [ ";
360 for (
const std::string& controller : context->
controllers_)
361 ss << controller <<
' ';
366 RCLCPP_INFO_STREAM(logger_, ss.str());
368 trajectories_.push_back(context);
380 void TrajectoryExecutionManager::reloadControllerInformation()
382 known_controllers_.clear();
383 if (controller_manager_)
385 std::vector<std::string> names;
386 controller_manager_->getControllersList(names);
387 for (
const std::string&
name : names)
389 std::vector<std::string> joints;
390 controller_manager_->getControllerJoints(
name, joints);
391 ControllerInformation ci;
393 ci.joints_.insert(joints.begin(), joints.end());
394 known_controllers_[ci.name_] = ci;
398 controller_manager_->getActiveControllers(names);
399 for (
const auto& active_name : names)
401 auto found_it = std::find_if(known_controllers_.begin(), known_controllers_.end(),
402 [&](
const auto& known_controller) { return known_controller.first == active_name; });
403 if (found_it != known_controllers_.end())
405 found_it->second.state_.active_ =
true;
409 for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
410 it != known_controllers_.end(); ++it)
412 for (std::map<std::string, ControllerInformation>::iterator jt = known_controllers_.begin();
413 jt != known_controllers_.end(); ++jt)
417 std::vector<std::string> intersect;
418 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), jt->second.joints_.begin(),
419 jt->second.joints_.end(), std::back_inserter(intersect));
420 if (!intersect.empty())
422 it->second.overlapping_controllers_.insert(jt->first);
423 jt->second.overlapping_controllers_.insert(it->first);
431 RCLCPP_ERROR(logger_,
"Failed to reload controllers: `controller_manager_` does not exist.");
435 void TrajectoryExecutionManager::updateControllerState(
const std::string& controller,
const rclcpp::Duration& age)
437 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
438 if (it != known_controllers_.end())
440 updateControllerState(it->second, age);
444 RCLCPP_ERROR(logger_,
"Controller '%s' is not known.", controller.c_str());
448 void TrajectoryExecutionManager::updateControllerState(ControllerInformation& ci,
const rclcpp::Duration& age)
450 if (node_->now() - ci.last_update_ >= age)
452 if (controller_manager_)
455 RCLCPP_INFO(logger_,
"Updating information for controller '%s'.", ci.name_.c_str());
456 ci.state_ = controller_manager_->getControllerState(ci.name_);
457 ci.last_update_ = node_->now();
461 RCLCPP_INFO(logger_,
"Information for controller '%s' is assumed to be up to date.", ci.name_.c_str());
464 void TrajectoryExecutionManager::updateControllersState(
const rclcpp::Duration& age)
466 for (std::pair<const std::string, ControllerInformation>& known_controller : known_controllers_)
467 updateControllerState(known_controller.second, age);
470 bool TrajectoryExecutionManager::checkControllerCombination(std::vector<std::string>& selected,
471 const std::set<std::string>& actuated_joints)
473 std::set<std::string> combined_joints;
474 for (
const std::string& controller : selected)
476 const ControllerInformation& ci = known_controllers_[controller];
477 combined_joints.insert(ci.joints_.begin(), ci.joints_.end());
482 std::stringstream ss, saj, sac;
483 for (
const std::string& controller : selected)
484 ss << controller <<
' ';
485 for (
const std::string& actuated_joint : actuated_joints)
486 saj << actuated_joint <<
' ';
487 for (
const std::string& combined_joint : combined_joints)
488 sac << combined_joint <<
' ';
489 RCLCPP_INFO(logger_,
"Checking if controllers [ %s] operating on joints [ %s] cover joints [ %s]", ss.str().c_str(),
490 sac.str().c_str(), saj.str().c_str());
493 return std::includes(combined_joints.begin(), combined_joints.end(), actuated_joints.begin(), actuated_joints.end());
496 void TrajectoryExecutionManager::generateControllerCombination(std::size_t start_index, std::size_t controller_count,
497 const std::vector<std::string>& available_controllers,
498 std::vector<std::string>& selected_controllers,
500 const std::set<std::string>& actuated_joints)
502 if (selected_controllers.size() == controller_count)
504 if (checkControllerCombination(selected_controllers, actuated_joints))
509 for (std::size_t i = start_index; i < available_controllers.size(); ++i)
511 bool overlap =
false;
512 const ControllerInformation& ci = known_controllers_[available_controllers[i]];
513 for (std::size_t j = 0; j < selected_controllers.size() && !overlap; ++j)
515 if (ci.overlapping_controllers_.find(selected_controllers[j]) != ci.overlapping_controllers_.end())
520 selected_controllers.push_back(available_controllers[i]);
521 generateControllerCombination(i + 1, controller_count, available_controllers, selected_controllers,
523 selected_controllers.pop_back();
529 struct OrderPotentialControllerCombination
531 bool operator()(
const std::size_t a,
const std::size_t b)
const
561 bool TrajectoryExecutionManager::findControllers(
const std::set<std::string>& actuated_joints,
562 std::size_t controller_count,
563 const std::vector<std::string>& available_controllers,
564 std::vector<std::string>& selected_controllers)
567 std::vector<std::string> work_area;
568 OrderPotentialControllerCombination order;
569 std::vector<std::vector<std::string>>&
selected_options = order.selected_options;
570 generateControllerCombination(0, controller_count, available_controllers, work_area,
selected_options,
575 std::stringstream saj;
576 std::stringstream sac;
577 for (
const std::string& available_controller : available_controllers)
578 sac << available_controller <<
' ';
579 for (
const std::string& actuated_joint : actuated_joints)
580 saj << actuated_joint <<
' ';
581 RCLCPP_INFO(logger_,
"Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.",
582 controller_count, sac.str().c_str(), saj.str().c_str(),
selected_options.size());
608 updateControllerState(
selected_options[i][k], DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
609 const ControllerInformation& ci = known_controllers_[
selected_options[i][k]];
611 if (ci.state_.default_)
612 order.nrdefault[i]++;
613 if (ci.state_.active_)
615 order.nrjoints[i] += ci.joints_.size();
625 std::sort(bijection.begin(), bijection.end(), order);
629 if (!manage_controllers_)
654 for (
const std::string& controller : controllers)
656 updateControllerState(controller, DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
657 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
658 if (it == known_controllers_.end() || !it->second.state_.active_)
664 bool TrajectoryExecutionManager::selectControllers(
const std::set<std::string>& actuated_joints,
665 const std::vector<std::string>& available_controllers,
666 std::vector<std::string>& selected_controllers)
668 for (std::size_t i = 1; i <= available_controllers.size(); ++i)
670 if (findControllers(actuated_joints, i, available_controllers, selected_controllers))
675 std::vector<std::string> other_option;
676 for (std::size_t j = i + 1; j <= available_controllers.size(); ++j)
678 if (findControllers(actuated_joints, j, available_controllers, other_option))
682 selected_controllers = other_option;
694 bool TrajectoryExecutionManager::distributeTrajectory(
const moveit_msgs::msg::RobotTrajectory& trajectory,
695 const std::vector<std::string>& controllers,
696 std::vector<moveit_msgs::msg::RobotTrajectory>& parts)
699 parts.resize(controllers.size());
701 std::set<std::string> actuated_joints_mdof;
702 actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
703 trajectory.multi_dof_joint_trajectory.joint_names.end());
704 std::set<std::string> actuated_joints_single;
705 for (
const std::string& joint_name : trajectory.joint_trajectory.joint_names)
712 actuated_joints_single.insert(jm->
getName());
716 for (std::size_t i = 0; i < controllers.size(); ++i)
718 std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
719 if (it == known_controllers_.end())
721 RCLCPP_ERROR_STREAM(logger_,
"Controller " << controllers[i] <<
" not found.");
724 std::vector<std::string> intersect_mdof;
725 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_mdof.begin(),
726 actuated_joints_mdof.end(), std::back_inserter(intersect_mdof));
727 std::vector<std::string> intersect_single;
728 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_single.begin(),
729 actuated_joints_single.end(), std::back_inserter(intersect_single));
730 if (intersect_mdof.empty() && intersect_single.empty())
731 RCLCPP_WARN_STREAM(logger_,
"No joints to be distributed for controller " << controllers[i]);
733 if (!intersect_mdof.empty())
735 std::vector<std::string>& jnames = parts[i].multi_dof_joint_trajectory.joint_names;
736 jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
737 std::map<std::string, std::size_t> index;
738 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
739 index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
740 std::vector<std::size_t> bijection(jnames.size());
741 for (std::size_t j = 0; j < jnames.size(); ++j)
742 bijection[j] = index[jnames[j]];
744 parts[i].multi_dof_joint_trajectory.header.frame_id = trajectory.multi_dof_joint_trajectory.header.frame_id;
745 parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
746 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.points.size(); ++j)
748 parts[i].multi_dof_joint_trajectory.points[j].time_from_start =
749 trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
750 parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
751 for (std::size_t k = 0; k < bijection.size(); ++k)
753 parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
754 trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
756 if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
758 parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());
760 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
761 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.x * execution_velocity_scaling_;
763 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
764 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.y * execution_velocity_scaling_;
766 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
767 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.z * execution_velocity_scaling_;
769 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
770 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.x * execution_velocity_scaling_;
772 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
773 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.y * execution_velocity_scaling_;
775 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
776 trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.z * execution_velocity_scaling_;
781 if (!intersect_single.empty())
783 std::vector<std::string>& jnames = parts[i].joint_trajectory.joint_names;
784 jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
785 parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
786 std::map<std::string, std::size_t> index;
787 for (std::size_t j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j)
788 index[trajectory.joint_trajectory.joint_names[j]] = j;
789 std::vector<std::size_t> bijection(jnames.size());
790 for (std::size_t j = 0; j < jnames.size(); ++j)
791 bijection[j] = index[jnames[j]];
792 parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
793 for (std::size_t j = 0; j < trajectory.joint_trajectory.points.size(); ++j)
795 parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
796 if (!trajectory.joint_trajectory.points[j].positions.empty())
798 parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
799 for (std::size_t k = 0; k < bijection.size(); ++k)
801 parts[i].joint_trajectory.points[j].positions[k] =
802 trajectory.joint_trajectory.points[j].positions[bijection[k]];
805 if (!trajectory.joint_trajectory.points[j].velocities.empty())
807 parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
808 for (std::size_t k = 0; k < bijection.size(); ++k)
810 parts[i].joint_trajectory.points[j].velocities[k] =
811 trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
814 if (!trajectory.joint_trajectory.points[j].accelerations.empty())
816 parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
817 for (std::size_t k = 0; k < bijection.size(); ++k)
819 parts[i].joint_trajectory.points[j].accelerations[k] =
820 trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
823 if (!trajectory.joint_trajectory.points[j].effort.empty())
825 parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
826 for (std::size_t k = 0; k < bijection.size(); ++k)
828 parts[i].joint_trajectory.points[j].effort[k] =
829 trajectory.joint_trajectory.points[j].effort[bijection[k]];
839 bool TrajectoryExecutionManager::validate(
const TrajectoryExecutionContext& context)
const
841 if (allowed_start_tolerance_ == 0)
844 RCLCPP_INFO(logger_,
"Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
846 moveit::core::RobotStatePtr current_state;
847 if (!csm_->waitForCurrentState(node_->now()) || !(current_state = csm_->getCurrentState()))
849 RCLCPP_WARN(logger_,
"Failed to validate trajectory: couldn't receive full current joint state within 1s");
853 for (
const auto& trajectory : context.trajectory_parts_)
855 if (!trajectory.joint_trajectory.points.empty())
858 const std::vector<double>& positions = trajectory.joint_trajectory.points.front().positions;
859 const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
860 if (positions.size() != joint_names.size())
862 RCLCPP_ERROR(logger_,
"Wrong trajectory: #joints: %zu != #positions: %zu", joint_names.size(), positions.size());
866 for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
871 RCLCPP_ERROR_STREAM(logger_,
"Unknown joint in trajectory: " << joint_names[i]);
875 double cur_position = current_state->getJointPositions(jm)[0];
876 double traj_position = positions[i];
880 if (jm->
distance(&cur_position, &traj_position) > allowed_start_tolerance_)
882 RCLCPP_ERROR(logger_,
883 "\nInvalid Trajectory: start point deviates from current robot state more than %g"
884 "\njoint '%s': expected: %g, current: %g",
885 allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position);
890 if (!trajectory.multi_dof_joint_trajectory.points.empty())
893 const std::vector<geometry_msgs::msg::Transform>& transforms =
894 trajectory.multi_dof_joint_trajectory.points.front().transforms;
895 const std::vector<std::string>& joint_names = trajectory.multi_dof_joint_trajectory.joint_names;
896 if (transforms.size() != joint_names.size())
898 RCLCPP_ERROR(logger_,
"Wrong trajectory: #joints: %zu != #transforms: %zu", joint_names.size(),
903 for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
908 RCLCPP_ERROR_STREAM(logger_,
"Unknown joint in trajectory: " << joint_names[i]);
914 Eigen::Isometry3d cur_transform, start_transform;
917 start_transform = tf2::transformToEigen(transforms[i]);
918 ASSERT_ISOMETRY(start_transform)
919 Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation();
920 Eigen::AngleAxisd rotation;
921 rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear());
922 if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_)
924 RCLCPP_ERROR_STREAM(logger_,
"\nInvalid Trajectory: start point deviates from current robot state more than "
925 << allowed_start_tolerance_ <<
"\nmulti-dof joint '" << joint_names[i]
926 <<
"': pos delta: " << offset.transpose()
927 <<
" rot delta: " << rotation.angle());
936 bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context,
937 const moveit_msgs::msg::RobotTrajectory& trajectory,
938 const std::vector<std::string>& controllers)
940 if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
946 reloadControllerInformation();
947 std::set<std::string> actuated_joints;
949 auto is_actuated = [
this](
const std::string& joint_name) ->
bool {
953 for (
const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names)
955 if (is_actuated(joint_name))
956 actuated_joints.insert(joint_name);
958 for (
const std::string& joint_name : trajectory.joint_trajectory.joint_names)
960 if (is_actuated(joint_name))
961 actuated_joints.insert(joint_name);
964 if (actuated_joints.empty())
966 RCLCPP_WARN(logger_,
"The trajectory to execute specifies no joints");
970 if (controllers.empty())
973 bool reloaded =
false;
977 std::vector<std::string> all_controller_names;
978 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
979 it != known_controllers_.end(); ++it)
980 all_controller_names.push_back(it->first);
981 if (selectControllers(actuated_joints, all_controller_names, context.controllers_))
983 if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
991 reloadControllerInformation();
1002 bool reloaded =
false;
1003 for (
const std::string& controller : controllers)
1005 if (known_controllers_.find(controller) == known_controllers_.end())
1007 reloadControllerInformation();
1014 for (
const std::string& controller : controllers)
1016 if (known_controllers_.find(controller) == known_controllers_.end())
1018 RCLCPP_ERROR(logger_,
"Controller '%s' is not known", controller.c_str());
1023 if (selectControllers(actuated_joints, controllers, context.controllers_))
1025 if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1029 std::stringstream ss;
1030 for (
const std::string& actuated_joint : actuated_joints)
1031 ss << actuated_joint <<
' ';
1032 RCLCPP_ERROR(logger_,
"Unable to identify any set of controllers that can actuate the specified joints: [ %s]",
1035 std::stringstream ss2;
1036 std::map<std::string, ControllerInformation>::const_iterator mi;
1037 for (mi = known_controllers_.begin(); mi != known_controllers_.end(); ++mi)
1039 ss2 <<
"controller '" << mi->second.name_ <<
"' controls joints:\n";
1041 std::set<std::string>::const_iterator ji;
1042 for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ++ji)
1044 ss2 <<
" " << *ji <<
'\n';
1047 RCLCPP_ERROR(logger_,
"Known controllers and their joints:\n%s", ss2.str().c_str());
1057 void TrajectoryExecutionManager::stopExecutionInternal()
1060 for (moveit_controller_manager::MoveItControllerHandlePtr& active_handle : active_handles_)
1064 active_handle->cancelExecution();
1066 catch (std::exception& ex)
1068 RCLCPP_ERROR(logger_,
"Caught %s when canceling execution.", ex.what());
1075 if (!execution_complete_)
1077 execution_state_mutex_.lock();
1078 if (!execution_complete_)
1084 execution_complete_ =
true;
1085 stopExecutionInternal();
1089 execution_state_mutex_.unlock();
1090 RCLCPP_INFO(logger_,
"Stopped trajectory execution.");
1093 std::scoped_lock lock(execution_thread_mutex_);
1094 if (execution_thread_)
1096 execution_thread_->join();
1097 execution_thread_.reset();
1104 execution_state_mutex_.unlock();
1106 else if (execution_thread_)
1109 std::scoped_lock lock(execution_thread_mutex_);
1110 if (execution_thread_)
1112 execution_thread_->join();
1113 execution_thread_.reset();
1128 if (trajectories_.empty())
1134 if (!trajectories_.empty() && !validate(*trajectories_.front()))
1140 callback(last_execution_status_);
1145 execution_complete_ =
false;
1146 execution_thread_ = std::make_unique<std::thread>(&TrajectoryExecutionManager::executeThread,
this, callback,
1147 part_callback, auto_clear);
1153 std::unique_lock<std::mutex> ulock(execution_state_mutex_);
1154 while (!execution_complete_)
1155 execution_complete_condition_.wait(ulock);
1161 return last_execution_status_;
1166 if (execution_complete_)
1170 trajectories_.clear();
1173 RCLCPP_ERROR(logger_,
"Cannot push a new trajectory while another is being executed");
1176 void TrajectoryExecutionManager::executeThread(
const ExecutionCompleteCallback& callback,
1177 const PathSegmentCompleteCallback& part_callback,
bool auto_clear)
1180 if (execution_complete_)
1184 callback(last_execution_status_);
1188 RCLCPP_INFO(logger_,
"Starting trajectory execution ...");
1195 for (; i < trajectories_.size(); ++i)
1197 bool epart = executePart(i);
1198 if (epart && part_callback)
1200 if (!epart || execution_complete_)
1209 waitForRobotToStop(*trajectories_[i - 1]);
1211 RCLCPP_INFO(logger_,
"Completed trajectory execution with status %s ...", last_execution_status_.
asString().c_str());
1214 execution_state_mutex_.lock();
1215 execution_complete_ =
true;
1216 execution_state_mutex_.unlock();
1217 execution_complete_condition_.notify_all();
1225 callback(last_execution_status_);
1228 bool TrajectoryExecutionManager::executePart(std::size_t part_index)
1230 TrajectoryExecutionContext& context = *trajectories_[part_index];
1236 if (execution_complete_)
1239 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
1241 std::scoped_lock slock(execution_state_mutex_);
1242 if (!execution_complete_)
1245 time_index_mutex_.lock();
1246 current_context_ = part_index;
1247 time_index_mutex_.unlock();
1248 active_handles_.resize(context.controllers_.size());
1249 for (std::size_t i = 0; i < context.controllers_.size(); ++i)
1251 moveit_controller_manager::MoveItControllerHandlePtr h;
1254 h = controller_manager_->getControllerHandle(context.controllers_[i]);
1256 catch (std::exception& ex)
1258 RCLCPP_ERROR(logger_,
"Caught %s when retrieving controller handle", ex.what());
1262 active_handles_.clear();
1263 current_context_ = -1;
1265 RCLCPP_ERROR(logger_,
"No controller handle for controller '%s'. Aborting.",
1266 context.controllers_[i].c_str());
1269 active_handles_[i] = h;
1271 handles = active_handles_;
1272 for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1277 ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
1279 catch (std::exception& ex)
1281 RCLCPP_ERROR(logger_,
"Caught %s when sending trajectory to controller", ex.what());
1285 for (std::size_t j = 0; j < i; ++j)
1289 active_handles_[j]->cancelExecution();
1291 catch (std::exception& ex)
1293 RCLCPP_ERROR(logger_,
"Caught %s when canceling execution", ex.what());
1296 RCLCPP_ERROR(logger_,
"Failed to send trajectory part %zu of %zu to controller %s", i + 1,
1297 context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
1299 RCLCPP_ERROR(logger_,
"Cancelling previously sent trajectory parts");
1300 active_handles_.clear();
1301 current_context_ = -1;
1310 rclcpp::Time current_time = node_->now();
1311 auto expected_trajectory_duration = rclcpp::Duration::from_seconds(0);
1312 int longest_part = -1;
1313 for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1315 auto d = rclcpp::Duration::from_seconds(0);
1316 if (!(context.trajectory_parts_[i].joint_trajectory.points.empty() &&
1317 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty()))
1319 if (rclcpp::Time(context.trajectory_parts_[i].joint_trajectory.header.stamp) > current_time)
1320 d = rclcpp::Time(context.trajectory_parts_[i].joint_trajectory.header.stamp) - current_time;
1321 if (rclcpp::Time(context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp) > current_time)
1323 d = std::max(d, rclcpp::Time(context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp) -
1327 std::max(context.trajectory_parts_[i].joint_trajectory.points.empty() ?
1328 rclcpp::Duration::from_seconds(0) :
1329 rclcpp::Duration(context.trajectory_parts_[i].joint_trajectory.points.back().time_from_start),
1330 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty() ?
1331 rclcpp::Duration::from_seconds(0) :
1333 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start));
1335 if (longest_part < 0 ||
1336 std::max(context.trajectory_parts_[i].joint_trajectory.points.size(),
1337 context.trajectory_parts_[i].multi_dof_joint_trajectory.points.size()) >
1338 std::max(context.trajectory_parts_[longest_part].joint_trajectory.points.size(),
1339 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
1346 std::map<std::string, double>::const_iterator scaling_it =
1347 controller_allowed_execution_duration_scaling_.find(context.controllers_[i]);
1348 const double current_scaling = scaling_it != controller_allowed_execution_duration_scaling_.end() ?
1349 scaling_it->second :
1350 allowed_execution_duration_scaling_;
1352 std::map<std::string, double>::const_iterator margin_it =
1353 controller_allowed_goal_duration_margin_.find(context.controllers_[i]);
1354 const double current_margin = margin_it != controller_allowed_goal_duration_margin_.end() ?
1356 allowed_goal_duration_margin_;
1359 expected_trajectory_duration =
1360 std::max(d * current_scaling + rclcpp::Duration::from_seconds(current_margin), expected_trajectory_duration);
1364 if (longest_part >= 0)
1366 std::scoped_lock slock(time_index_mutex_);
1368 if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >=
1369 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
1371 auto d = rclcpp::Duration::from_seconds(0);
1372 if (rclcpp::Time(context.trajectory_parts_[longest_part].joint_trajectory.header.stamp) > current_time)
1373 d = rclcpp::Time(context.trajectory_parts_[longest_part].joint_trajectory.header.stamp) - current_time;
1374 for (trajectory_msgs::msg::JointTrajectoryPoint& point :
1375 context.trajectory_parts_[longest_part].joint_trajectory.points)
1376 time_index_.push_back(current_time + d + rclcpp::Duration(point.time_from_start));
1380 auto d = rclcpp::Duration::from_seconds(0);
1381 if (rclcpp::Time(context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp) > current_time)
1383 d = rclcpp::Time(context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp) -
1386 for (trajectory_msgs::msg::MultiDOFJointTrajectoryPoint& point :
1387 context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points)
1388 time_index_.push_back(current_time + d + rclcpp::Duration(point.time_from_start));
1393 for (moveit_controller_manager::MoveItControllerHandlePtr& handle : handles)
1395 if (execution_duration_monitoring_)
1397 if (!handle->waitForExecution(expected_trajectory_duration))
1399 if (!execution_complete_ && node_->now() - current_time > expected_trajectory_duration)
1401 RCLCPP_ERROR(logger_,
1402 "Controller is taking too long to execute trajectory (the expected upper "
1403 "bound for the trajectory execution was %lf seconds). Stopping trajectory.",
1404 expected_trajectory_duration.seconds());
1406 std::scoped_lock slock(execution_state_mutex_);
1407 stopExecutionInternal();
1417 handle->waitForExecution();
1420 if (execution_complete_)
1427 RCLCPP_WARN_STREAM(logger_,
"Controller handle " << handle->getName() <<
" reports status "
1428 << handle->getLastExecutionStatus().asString());
1429 last_execution_status_ = handle->getLastExecutionStatus();
1435 execution_state_mutex_.lock();
1436 active_handles_.clear();
1439 time_index_mutex_.lock();
1440 time_index_.clear();
1441 current_context_ = -1;
1442 time_index_mutex_.unlock();
1444 execution_state_mutex_.unlock();
1449 RCLCPP_ERROR(logger_,
"Active status of required controllers can not be assured.");
1455 bool TrajectoryExecutionManager::waitForRobotToStop(
const TrajectoryExecutionContext& context,
double wait_time)
1458 if (allowed_start_tolerance_ == 0 || !wait_for_trajectory_completion_)
1460 RCLCPP_INFO(logger_,
"Not waiting for trajectory completion");
1464 auto start = std::chrono::system_clock::now();
1465 double time_remaining = wait_time;
1467 moveit::core::RobotStatePtr prev_state, cur_state;
1468 prev_state = csm_->getCurrentState();
1469 prev_state->enforceBounds();
1472 unsigned int no_motion_count = 0;
1473 while (time_remaining > 0. && no_motion_count < 3)
1475 if (!csm_->waitForCurrentState(node_->now(), time_remaining) || !(cur_state = csm_->getCurrentState()))
1477 RCLCPP_WARN(logger_,
"Failed to receive current joint state");
1480 cur_state->enforceBounds();
1481 std::chrono::duration<double> elapsed_seconds = std::chrono::system_clock::now() - start;
1482 time_remaining = wait_time - elapsed_seconds.count();
1486 for (
const auto& trajectory : context.trajectory_parts_)
1488 const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
1489 const std::size_t n = joint_names.size();
1491 for (std::size_t i = 0; i < n && !moved; ++i)
1497 if (fabs(cur_state->getJointPositions(jm)[0] - prev_state->getJointPositions(jm)[0]) > allowed_start_tolerance_)
1500 no_motion_count = 0;
1509 std::swap(prev_state, cur_state);
1512 return time_remaining > 0;
1517 std::scoped_lock slock(time_index_mutex_);
1518 if (current_context_ < 0)
1519 return std::make_pair(-1, -1);
1520 if (time_index_.empty())
1521 return std::make_pair(
static_cast<int>(current_context_), -1);
1522 std::vector<rclcpp::Time>::const_iterator time_index_it =
1523 std::lower_bound(time_index_.begin(), time_index_.end(), node_->now());
1524 int pos = time_index_it - time_index_.begin();
1525 return std::make_pair(
static_cast<int>(current_context_), pos);
1528 const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>&
1531 return trajectories_;
1536 return last_execution_status_;
1542 if (joint_model_group)
1554 std::vector<std::string> all_controller_names;
1555 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1556 it != known_controllers_.end(); ++it)
1557 all_controller_names.push_back(it->first);
1558 std::vector<std::string> selected_controllers;
1559 std::set<std::string> jset;
1560 for (
const std::string& joint : joints)
1571 if (selectControllers(jset, all_controller_names, selected_controllers))
1588 reloadControllerInformation();
1590 updateControllersState(DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE);
1592 if (manage_controllers_)
1594 std::vector<std::string> controllers_to_activate;
1595 std::vector<std::string> controllers_to_deactivate;
1596 std::set<std::string> joints_to_be_activated;
1597 std::set<std::string> joints_to_be_deactivated;
1598 for (
const std::string& controller : controllers)
1600 std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.find(controller);
1601 if (it == known_controllers_.end())
1603 std::stringstream stream;
1604 for (
const auto& controller : known_controllers_)
1606 stream <<
" `" << controller.first <<
'`';
1608 RCLCPP_WARN_STREAM(logger_,
"Controller " << controller <<
" is not known. Known controllers: " << stream.str());
1611 if (!it->second.state_.active_)
1613 RCLCPP_DEBUG_STREAM(logger_,
"Need to activate " << controller);
1614 controllers_to_activate.push_back(controller);
1615 joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
1616 for (
const std::string& overlapping_controller : it->second.overlapping_controllers_)
1618 const ControllerInformation& ci = known_controllers_[overlapping_controller];
1619 if (ci.state_.active_)
1621 controllers_to_deactivate.push_back(overlapping_controller);
1622 joints_to_be_deactivated.insert(ci.joints_.begin(), ci.joints_.end());
1627 RCLCPP_DEBUG_STREAM(logger_,
"Controller " << controller <<
" is already active");
1629 std::set<std::string> diff;
1630 std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
1631 joints_to_be_activated.begin(), joints_to_be_activated.end(), std::inserter(diff, diff.end()));
1635 std::vector<std::string> possible_additional_controllers;
1636 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1637 it != known_controllers_.end(); ++it)
1640 for (
const std::string& controller_to_activate : controllers_to_activate)
1642 if (it->second.overlapping_controllers_.find(controller_to_activate) !=
1643 it->second.overlapping_controllers_.end())
1650 possible_additional_controllers.push_back(it->first);
1654 std::vector<std::string> additional_controllers;
1655 if (selectControllers(diff, possible_additional_controllers, additional_controllers))
1657 controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(),
1658 additional_controllers.end());
1665 if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
1667 if (controller_manager_)
1670 for (
const std::string& controller_to_activate : controllers_to_activate)
1672 ControllerInformation& ci = known_controllers_[controller_to_activate];
1673 ci.last_update_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
1676 for (
const std::string& controller_to_deactivate : controllers_to_deactivate)
1677 known_controllers_[controller_to_deactivate].last_update_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
1678 return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
1688 std::set<std::string> originally_active;
1689 for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1690 it != known_controllers_.end(); ++it)
1692 if (it->second.state_.active_)
1694 originally_active.insert(it->first);
1697 return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
1701 void 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 std::string & getName() const
Get the name of the joint.
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...
bool enforcePositionBounds(double *values) const
Force the specified values to be inside bounds and normalized. Quaternions are normalized,...
JointType getType() const
Get the type of joint.
const JointModel * getMimic() const
Get the joint this one is mimicking.
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.
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
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.
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')
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
rclcpp::Logger getLogger()
Vec3fX< details::Vec3Data< double > > Vector3d
Main namespace for MoveIt.
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;.