39 #include <moveit_ros_planning/TrajectoryExecutionDynamicReconfigureConfig.h>
41 #include <dynamic_reconfigure/server.h>
45 static const std::string
LOGNAME =
"trajectory_execution_manager";
58 using namespace moveit_ros_planning;
64 : owner_(owner), dynamic_reconfigure_server_(
ros::NodeHandle(
"~/trajectory_execution"))
66 dynamic_reconfigure_server_.setCallback(
67 [
this](
const auto& config, uint32_t level) { dynamicReconfigureCallback(config, level); });
73 owner_->enableExecutionDurationMonitoring(config.execution_duration_monitoring);
74 owner_->setAllowedExecutionDurationScaling(config.allowed_execution_duration_scaling);
75 owner_->setAllowedGoalDurationMargin(config.allowed_goal_duration_margin);
76 owner_->setExecutionVelocityScaling(config.execution_velocity_scaling);
77 owner_->setAllowedStartTolerance(config.allowed_start_tolerance);
78 owner_->setWaitForTrajectoryCompletion(config.wait_for_trajectory_completion);
86 const planning_scene_monitor::CurrentStateMonitorPtr& csm)
87 : robot_model_(robot_model), csm_(csm), node_handle_(
"~")
96 const planning_scene_monitor::CurrentStateMonitorPtr& csm,
97 bool manage_controllers)
98 : robot_model_(robot_model), csm_(csm), node_handle_(
"~"), manage_controllers_(manage_controllers)
130 std::make_unique<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager>>(
131 "moveit_core",
"moveit_controller_manager::MoveItControllerManager");
141 std::string controller;
145 if (classes.size() == 1)
147 controller = classes[0];
149 "Parameter '~moveit_controller_manager' is not specified but only one "
150 "matching plugin was found: '%s'. Using that one.",
155 "identify the plugin to use for interacting with controllers. No paths can "
159 if (!controller.empty())
167 "Exception while loading controller manager '" << controller <<
"': " << ex.what());
241 if (controller.empty())
242 return push(trajectory, std::vector<std::string>());
244 return push(trajectory, std::vector<std::string>(1, controller));
249 if (controller.empty())
250 return push(trajectory, std::vector<std::string>());
252 return push(trajectory, std::vector<std::string>(1, controller));
256 const std::vector<std::string>& controllers)
258 moveit_msgs::RobotTrajectory traj;
259 traj.joint_trajectory = trajectory;
260 return push(traj, controllers);
264 const std::vector<std::string>& controllers)
273 if (
configure(*context, trajectory, controllers))
277 std::stringstream ss;
278 ss <<
"Pushed trajectory for execution using controllers [ ";
279 for (
const std::string& controller : context->
controllers_)
280 ss << controller <<
" ";
281 ss <<
"]:" << std::endl;
282 for (
const moveit_msgs::RobotTrajectory& trajectory_part : context->
trajectory_parts_)
283 ss << trajectory_part << std::endl;
303 std::vector<std::string> names;
305 for (
const std::string&
name : names)
307 std::vector<std::string> joints;
311 ci.
joints_.insert(joints.begin(), joints.end());
315 for (std::map<std::string, ControllerInformation>::iterator it =
known_controllers_.begin();
317 for (std::map<std::string, ControllerInformation>::iterator jt =
known_controllers_.begin();
321 std::vector<std::string> intersect;
322 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), jt->second.joints_.begin(),
323 jt->second.joints_.end(), std::back_inserter(intersect));
324 if (!intersect.empty())
326 it->second.overlapping_controllers_.insert(jt->first);
327 jt->second.overlapping_controllers_.insert(it->first);
335 std::map<std::string, ControllerInformation>::iterator it =
known_controllers_.find(controller);
360 for (std::pair<const std::string, ControllerInformation>& known_controller :
known_controllers_)
365 const std::set<std::string>& actuated_joints)
367 std::set<std::string> combined_joints;
368 for (
const std::string& controller : selected)
376 std::stringstream ss, saj, sac;
377 for (
const std::string& controller : selected)
378 ss << controller <<
" ";
379 for (
const std::string& actuated_joint : actuated_joints)
380 saj << actuated_joint <<
" ";
381 for (
const std::string& combined_joint : combined_joints)
382 sac << combined_joint <<
" ";
383 ROS_INFO_NAMED(
LOGNAME,
"Checking if controllers [ %s] operating on joints [ %s] cover joints [ %s]",
384 ss.str().c_str(), sac.str().c_str(), saj.str().c_str());
387 return std::includes(combined_joints.begin(), combined_joints.end(), actuated_joints.begin(), actuated_joints.end());
391 const std::vector<std::string>& available_controllers,
392 std::vector<std::string>& selected_controllers,
394 const std::set<std::string>& actuated_joints)
396 if (selected_controllers.size() == controller_count)
403 for (std::size_t i = start_index; i < available_controllers.size(); ++i)
407 for (std::size_t j = 0; j < selected_controllers.size() && !
overlap; ++j)
414 selected_controllers.push_back(available_controllers[i]);
417 selected_controllers.pop_back();
423 struct OrderPotentialControllerCombination
425 bool operator()(
const std::size_t a,
const std::size_t b)
const
456 std::size_t controller_count,
457 const std::vector<std::string>& available_controllers,
458 std::vector<std::string>& selected_controllers)
461 std::vector<std::string> work_area;
462 OrderPotentialControllerCombination order;
463 std::vector<std::vector<std::string>>&
selected_options = order.selected_options;
469 std::stringstream saj;
470 std::stringstream sac;
471 for (
const std::string& available_controller : available_controllers)
472 sac << available_controller <<
" ";
473 for (
const std::string& actuated_joint : actuated_joints)
474 saj << actuated_joint <<
" ";
475 ROS_INFO_NAMED(
LOGNAME,
"Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.",
476 controller_count, sac.str().c_str(), saj.str().c_str(),
selected_options.size());
506 order.nrdefault[i]++;
509 order.nrjoints[i] += ci.
joints_.size();
519 std::sort(bijection.begin(), bijection.end(), order);
546 for (
const std::string& controller : controllers)
549 std::map<std::string, ControllerInformation>::iterator it =
known_controllers_.find(controller);
557 const std::vector<std::string>& available_controllers,
558 std::vector<std::string>& selected_controllers)
560 for (std::size_t i = 1; i <= available_controllers.size(); ++i)
561 if (
findControllers(actuated_joints, i, available_controllers, selected_controllers))
566 std::vector<std::string> other_option;
567 for (std::size_t j = i + 1; j <= available_controllers.size(); ++j)
568 if (
findControllers(actuated_joints, j, available_controllers, other_option))
572 selected_controllers = other_option;
583 const std::vector<std::string>& controllers,
584 std::vector<moveit_msgs::RobotTrajectory>& parts)
587 parts.resize(controllers.size());
589 std::set<std::string> actuated_joints_mdof;
590 actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
591 trajectory.multi_dof_joint_trajectory.joint_names.end());
592 std::set<std::string> actuated_joints_single;
593 for (
const std::string& joint_name : trajectory.joint_trajectory.joint_names)
600 actuated_joints_single.insert(jm->
getName());
604 for (std::size_t i = 0; i < controllers.size(); ++i)
606 std::map<std::string, ControllerInformation>::iterator it =
known_controllers_.find(controllers[i]);
612 std::vector<std::string> intersect_mdof;
613 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_mdof.begin(),
614 actuated_joints_mdof.end(), std::back_inserter(intersect_mdof));
615 std::vector<std::string> intersect_single;
616 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_single.begin(),
617 actuated_joints_single.end(), std::back_inserter(intersect_single));
618 if (intersect_mdof.empty() && intersect_single.empty())
621 if (!intersect_mdof.empty())
623 std::vector<std::string>& jnames = parts[i].multi_dof_joint_trajectory.joint_names;
624 jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
625 std::map<std::string, std::size_t>
index;
626 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
627 index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
628 std::vector<std::size_t> bijection(jnames.size());
629 for (std::size_t j = 0; j < jnames.size(); ++j)
630 bijection[j] =
index[jnames[j]];
632 parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
633 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.points.size(); ++j)
635 parts[i].multi_dof_joint_trajectory.points[j].time_from_start =
636 trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
637 parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
638 for (std::size_t k = 0; k < bijection.size(); ++k)
640 parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
641 trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
643 if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
645 parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());
647 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
650 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
653 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
656 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
659 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
662 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
668 if (!intersect_single.empty())
670 std::vector<std::string>& jnames = parts[i].joint_trajectory.joint_names;
671 jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
672 parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
673 std::map<std::string, std::size_t>
index;
674 for (std::size_t j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j)
675 index[trajectory.joint_trajectory.joint_names[j]] = j;
676 std::vector<std::size_t> bijection(jnames.size());
677 for (std::size_t j = 0; j < jnames.size(); ++j)
678 bijection[j] =
index[jnames[j]];
679 parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
680 for (std::size_t j = 0; j < trajectory.joint_trajectory.points.size(); ++j)
682 parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
683 if (!trajectory.joint_trajectory.points[j].positions.empty())
685 parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
686 for (std::size_t k = 0; k < bijection.size(); ++k)
687 parts[i].joint_trajectory.points[j].positions[k] =
688 trajectory.joint_trajectory.points[j].positions[bijection[k]];
690 if (!trajectory.joint_trajectory.points[j].velocities.empty())
692 parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
693 for (std::size_t k = 0; k < bijection.size(); ++k)
694 parts[i].joint_trajectory.points[j].velocities[k] =
697 if (!trajectory.joint_trajectory.points[j].accelerations.empty())
699 parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
700 for (std::size_t k = 0; k < bijection.size(); ++k)
701 parts[i].joint_trajectory.points[j].accelerations[k] =
702 trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
704 if (!trajectory.joint_trajectory.points[j].effort.empty())
706 parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
707 for (std::size_t k = 0; k < bijection.size(); ++k)
708 parts[i].joint_trajectory.points[j].effort[k] =
709 trajectory.joint_trajectory.points[j].effort[bijection[k]];
725 moveit::core::RobotStatePtr current_state;
728 ROS_WARN_NAMED(
LOGNAME,
"Failed to validate trajectory: couldn't receive full current joint state within 1s");
734 if (!trajectory.joint_trajectory.points.empty())
737 const std::vector<double>& positions = trajectory.joint_trajectory.points.front().positions;
738 const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
739 if (positions.size() != joint_names.size())
746 for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
755 double cur_position = current_state->getJointPositions(jm)[0];
756 double traj_position = positions[i];
763 "\nInvalid Trajectory: start point deviates from current robot state more than %g"
764 "\njoint '%s': expected: %g, current: %g",
770 if (!trajectory.multi_dof_joint_trajectory.points.empty())
773 const std::vector<geometry_msgs::Transform>& transforms =
774 trajectory.multi_dof_joint_trajectory.points.front().transforms;
775 const std::vector<std::string>& joint_names = trajectory.multi_dof_joint_trajectory.joint_names;
776 if (transforms.size() != joint_names.size())
783 for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
794 Eigen::Isometry3d cur_transform, start_transform;
799 Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation();
800 Eigen::AngleAxisd rotation;
801 rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear());
805 "\nInvalid Trajectory: start point deviates from current robot state more than "
807 <<
"': pos delta: " << offset.transpose() <<
" rot delta: " << rotation.angle());
817 const moveit_msgs::RobotTrajectory& trajectory,
818 const std::vector<std::string>& controllers)
821 if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
825 if ((trajectory.joint_trajectory.header.stamp + trajectory.joint_trajectory.points.back().time_from_start).isZero())
832 std::set<std::string> actuated_joints;
834 auto is_actuated = [
this](
const std::string& joint_name) ->
bool {
838 for (
const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names)
839 if (is_actuated(joint_name))
840 actuated_joints.insert(joint_name);
841 for (
const std::string& joint_name : trajectory.joint_trajectory.joint_names)
842 if (is_actuated(joint_name))
843 actuated_joints.insert(joint_name);
845 if (actuated_joints.empty())
851 if (controllers.empty())
854 bool reloaded =
false;
858 std::vector<std::string> all_controller_names;
859 for (std::map<std::string, ControllerInformation>::const_iterator it =
known_controllers_.begin();
861 all_controller_names.push_back(it->first);
883 bool reloaded =
false;
884 for (
const std::string& controller : controllers)
892 for (
const std::string& controller : controllers)
904 std::stringstream ss;
905 for (
const std::string& actuated_joint : actuated_joints)
906 ss << actuated_joint <<
" ";
907 ROS_ERROR_NAMED(
LOGNAME,
"Unable to identify any set of controllers that can actuate the specified joints: [ %s]",
910 std::stringstream ss2;
911 std::map<std::string, ControllerInformation>::const_iterator mi;
914 ss2 <<
"controller '" << mi->second.name_ <<
"' controls joints:\n";
916 std::set<std::string>::const_iterator ji;
917 for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ji++)
919 ss2 <<
" " << *ji << std::endl;
935 for (moveit_controller_manager::MoveItControllerHandlePtr& active_handle :
active_handles_)
938 active_handle->cancelExecution();
940 catch (std::exception& ex)
1020 part_callback, auto_clear);
1071 if (epart && part_callback)
1113 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
1123 for (std::size_t i = 0; i < context.
controllers_.size(); ++i)
1125 moveit_controller_manager::MoveItControllerHandlePtr h;
1130 catch (std::exception& ex)
1153 catch (std::exception& ex)
1159 for (std::size_t j = 0; j < i; ++j)
1164 catch (std::exception& ex)
1184 int longest_part = -1;
1193 if (context.
trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp > current_time)
1194 d = std::max(
d, context.
trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp - current_time);
1200 context.
trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start);
1202 if (longest_part < 0 ||
1205 std::max(context.
trajectory_parts_[longest_part].joint_trajectory.points.size(),
1206 context.
trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
1213 std::map<std::string, double>::const_iterator scaling_it =
1216 scaling_it->second :
1219 std::map<std::string, double>::const_iterator margin_it =
1226 expected_trajectory_duration =
1227 std::max(
d * current_scaling +
ros::Duration(current_margin), expected_trajectory_duration);
1231 if (longest_part >= 0)
1236 context.
trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
1239 if (context.
trajectory_parts_[longest_part].joint_trajectory.header.stamp > current_time)
1240 d = context.
trajectory_parts_[longest_part].joint_trajectory.header.stamp - current_time;
1241 for (trajectory_msgs::JointTrajectoryPoint&
point :
1248 if (context.
trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp > current_time)
1249 d = context.
trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp - current_time;
1250 for (trajectory_msgs::MultiDOFJointTrajectoryPoint&
point :
1257 for (moveit_controller_manager::MoveItControllerHandlePtr& handle : handles)
1261 if (!handle->waitForExecution(expected_trajectory_duration))
1265 "Controller is taking too long to execute trajectory (the expected upper "
1266 "bound for the trajectory execution was %lf seconds). Stopping trajectory.",
1267 expected_trajectory_duration.
toSec());
1279 handle->waitForExecution();
1290 << handle->getLastExecutionStatus().asString());
1326 double time_remaining = wait_time;
1328 moveit::core::RobotStatePtr prev_state, cur_state;
1329 prev_state =
csm_->getCurrentState();
1330 prev_state->enforceBounds();
1333 unsigned int no_motion_count = 0;
1334 while (time_remaining > 0. && no_motion_count < 3)
1336 if (!
csm_->waitForCurrentState(
ros::Time::now(), time_remaining) || !(cur_state =
csm_->getCurrentState()))
1341 cur_state->enforceBounds();
1348 const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
1349 const std::size_t n = joint_names.size();
1351 for (std::size_t i = 0; i < n && !moved; ++i)
1360 no_motion_count = 0;
1369 std::swap(prev_state, cur_state);
1372 return time_remaining > 0;
1379 return std::make_pair(-1, -1);
1382 std::vector<ros::Time>::const_iterator time_index_it =
1388 const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>&
1402 if (joint_model_group)
1410 std::vector<std::string> all_controller_names;
1411 for (std::map<std::string, ControllerInformation>::const_iterator it =
known_controllers_.begin();
1413 all_controller_names.push_back(it->first);
1414 std::vector<std::string> selected_controllers;
1415 std::set<std::string> jset;
1416 for (
const std::string& joint : joints)
1444 std::vector<std::string> controllers_to_activate;
1445 std::vector<std::string> controllers_to_deactivate;
1446 std::set<std::string> joints_to_be_activated;
1447 std::set<std::string> joints_to_be_deactivated;
1448 for (
const std::string& controller : controllers)
1450 std::map<std::string, ControllerInformation>::const_iterator it =
known_controllers_.find(controller);
1456 if (!it->second.state_.active_)
1459 controllers_to_activate.push_back(controller);
1460 joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
1461 for (
const std::string& overlapping_controller : it->second.overlapping_controllers_)
1466 controllers_to_deactivate.push_back(overlapping_controller);
1467 joints_to_be_deactivated.insert(ci.
joints_.begin(), ci.
joints_.end());
1474 std::set<std::string> diff;
1475 std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
1476 joints_to_be_activated.begin(), joints_to_be_activated.end(), std::inserter(diff, diff.end()));
1480 std::vector<std::string> possible_additional_controllers;
1481 for (std::map<std::string, ControllerInformation>::const_iterator it =
known_controllers_.begin();
1485 for (
const std::string& controller_to_activate : controllers_to_activate)
1486 if (it->second.overlapping_controllers_.find(controller_to_activate) !=
1487 it->second.overlapping_controllers_.end())
1493 possible_additional_controllers.push_back(it->first);
1497 std::vector<std::string> additional_controllers;
1498 if (
selectControllers(diff, possible_additional_controllers, additional_controllers))
1499 controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(),
1500 additional_controllers.end());
1504 if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
1509 for (
const std::string& controller_to_activate : controllers_to_activate)
1515 for (
const std::string& controller_to_activate : controllers_to_deactivate)
1517 return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
1527 std::set<std::string> originally_active;
1528 for (std::map<std::string, ControllerInformation>::const_iterator it =
known_controllers_.begin();
1530 if (it->second.state_.active_)
1531 originally_active.insert(it->first);
1532 return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
1542 for (
int i = 0; i < controller_list.
size(); ++i)
1547 if (controller.
hasMember(
"allowed_execution_duration_scaling"))
1549 controller[
"allowed_execution_duration_scaling"];
1550 if (controller.
hasMember(
"allowed_goal_duration_margin"))
1552 controller[
"allowed_goal_duration_margin"];