39 #include <moveit_ros_planning/TrajectoryExecutionDynamicReconfigureConfig.h> 40 #include <dynamic_reconfigure/server.h> 60 : owner_(owner), dynamic_reconfigure_server_(
ros::NodeHandle(
"~/trajectory_execution"))
62 dynamic_reconfigure_server_.setCallback(
63 boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback,
this, _1, _2));
69 owner_->enableExecutionDurationMonitoring(config.execution_duration_monitoring);
70 owner_->setAllowedExecutionDurationScaling(config.allowed_execution_duration_scaling);
71 owner_->setAllowedGoalDurationMargin(config.allowed_goal_duration_margin);
72 owner_->setExecutionVelocityScaling(config.execution_velocity_scaling);
73 owner_->setAllowedStartTolerance(config.allowed_start_tolerance);
74 owner_->setWaitForTrajectoryCompletion(config.wait_for_trajectory_completion);
82 const planning_scene_monitor::CurrentStateMonitorPtr& csm)
83 : robot_model_(kmodel), csm_(csm), node_handle_(
"~")
92 const planning_scene_monitor::CurrentStateMonitorPtr& csm,
93 bool manage_controllers)
107 "\nDeprecation warning: parameter '%s' moved into namespace 'trajectory_execution'." 108 "\nPlease, adjust file trajectory_execution.launch.xml!";
140 "moveit_core",
"moveit_controller_manager::MoveItControllerManager"));
150 std::string controller;
154 if (classes.size() == 1)
156 controller = classes[0];
157 ROS_WARN_NAMED(
name_,
"Parameter '~moveit_controller_manager' is not specified but only one " 158 "matching plugin was found: '%s'. Using that one.",
162 ROS_FATAL_NAMED(
name_,
"Parameter '~moveit_controller_manager' not specified. This is needed to " 163 "identify the plugin to use for interacting with controllers. No paths can " 167 if (!controller.empty())
175 <<
"': " << ex.what());
249 if (controller.empty())
250 return push(trajectory, std::vector<std::string>());
252 return push(trajectory, std::vector<std::string>(1, controller));
257 if (controller.empty())
258 return push(trajectory, std::vector<std::string>());
260 return push(trajectory, std::vector<std::string>(1, controller));
264 const std::vector<std::string>& controllers)
266 moveit_msgs::RobotTrajectory traj;
267 traj.joint_trajectory = trajectory;
268 return push(traj, controllers);
272 const std::vector<std::string>& controllers)
281 if (
configure(*context, trajectory, controllers))
285 std::stringstream ss;
286 ss <<
"Pushed trajectory for execution using controllers [ ";
287 for (std::size_t i = 0; i < context->
controllers_.size(); ++i)
289 ss <<
"]:" << std::endl;
307 const std::string& controller)
309 if (controller.empty())
312 return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
316 const std::string& controller)
318 if (controller.empty())
321 return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
326 if (controller.empty())
329 return pushAndExecute(state, std::vector<std::string>(1, controller));
333 const std::vector<std::string>& controllers)
335 moveit_msgs::RobotTrajectory traj;
336 traj.joint_trajectory = trajectory;
341 const std::vector<std::string>& controllers)
343 moveit_msgs::RobotTrajectory traj;
344 traj.joint_trajectory.header = state.header;
345 traj.joint_trajectory.joint_names = state.name;
346 traj.joint_trajectory.points.resize(1);
347 traj.joint_trajectory.points[0].positions = state.position;
348 traj.joint_trajectory.points[0].velocities = state.velocity;
349 traj.joint_trajectory.points[0].effort = state.effort;
350 traj.joint_trajectory.points[0].time_from_start =
ros::Duration(0, 0);
355 const std::vector<std::string>& controllers)
359 ROS_ERROR_NAMED(
name_,
"Cannot push & execute a new trajectory while another is being executed");
364 if (
configure(*context, trajectory, controllers))
387 std::set<moveit_controller_manager::MoveItControllerHandlePtr> used_handles;
399 for (std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin();
400 uit != used_handles.end(); ++uit)
402 (*uit)->cancelExecution();
403 used_handles.clear();
428 std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin();
429 while (uit != used_handles.end())
432 std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator toErase = uit;
434 used_handles.erase(toErase);
445 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles(context->
controllers_.size());
446 for (std::size_t i = 0; i < context->
controllers_.size(); ++i)
448 moveit_controller_manager::MoveItControllerHandlePtr h;
453 catch (std::exception& ex)
475 if (!handles.empty())
483 catch (std::exception& ex)
489 for (std::size_t j = 0; j < i; ++j)
492 handles[j]->cancelExecution();
494 catch (std::exception& ex)
510 for (std::size_t i = 0; i < handles.size(); ++i)
511 used_handles.insert(handles[i]);
515 ROS_ERROR_NAMED(
name_,
"Not all needed controllers are active. Cannot push and execute. You can try " 516 "calling ensureActiveControllers() before pushAndExecute()");
529 std::vector<std::string> names;
531 for (std::size_t i = 0; i < names.size(); ++i)
533 std::vector<std::string> joints;
537 ci.
joints_.insert(joints.begin(), joints.end());
541 for (std::map<std::string, ControllerInformation>::iterator it =
known_controllers_.begin();
543 for (std::map<std::string, ControllerInformation>::iterator jt =
known_controllers_.begin();
547 std::vector<std::string> intersect;
548 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), jt->second.joints_.begin(),
549 jt->second.joints_.end(), std::back_inserter(intersect));
550 if (!intersect.empty())
553 jt->second.overlapping_controllers_.insert(it->first);
561 std::map<std::string, ControllerInformation>::iterator it =
known_controllers_.find(controller);
586 for (std::map<std::string, ControllerInformation>::iterator it =
known_controllers_.begin();
592 const std::set<std::string>& actuated_joints)
594 std::set<std::string> combined_joints;
595 for (std::size_t i = 0; i < selected.size(); ++i)
603 std::stringstream ss, saj, sac;
604 for (std::size_t i = 0; i < selected.size(); ++i)
605 ss << selected[i] <<
" ";
606 for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
608 for (std::set<std::string>::const_iterator it = combined_joints.begin(); it != combined_joints.end(); ++it)
610 ROS_INFO_NAMED(
name_,
"Checking if controllers [ %s] operating on joints [ %s] cover joints [ %s]",
611 ss.str().c_str(), sac.str().c_str(), saj.str().c_str());
614 return std::includes(combined_joints.begin(), combined_joints.end(), actuated_joints.begin(), actuated_joints.end());
618 const std::vector<std::string>& available_controllers,
619 std::vector<std::string>& selected_controllers,
621 const std::set<std::string>& actuated_joints)
623 if (selected_controllers.size() == controller_count)
630 for (std::size_t i = start_index; i < available_controllers.size(); ++i)
632 bool overlap =
false;
634 for (std::size_t j = 0; j < selected_controllers.size() && !overlap; ++j)
641 selected_controllers.push_back(available_controllers[i]);
644 selected_controllers.pop_back();
650 struct OrderPotentialControllerCombination
652 bool operator()(
const std::size_t a,
const std::size_t b)
const 683 std::size_t controller_count,
684 const std::vector<std::string>& available_controllers,
685 std::vector<std::string>& selected_controllers)
688 std::vector<std::string> work_area;
689 OrderPotentialControllerCombination order;
690 std::vector<std::vector<std::string> >&
selected_options = order.selected_options;
696 std::stringstream saj;
697 std::stringstream sac;
698 for (std::size_t i = 0; i < available_controllers.size(); ++i)
699 sac << available_controllers[i] <<
" ";
700 for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
702 ROS_INFO_NAMED(
name_,
"Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.",
703 controller_count, sac.str().c_str(), saj.str().c_str(), selected_options.size());
707 if (selected_options.empty())
711 if (selected_options.size() == 1)
713 selected_controllers.swap(selected_options[0]);
722 order.nrdefault.resize(selected_options.size(), 0);
723 order.nrjoints.resize(selected_options.size(), 0);
724 order.nractive.resize(selected_options.size(), 0);
725 for (std::size_t i = 0; i < selected_options.size(); ++i)
727 for (std::size_t k = 0; k < selected_options[i].size(); ++k)
733 order.nrdefault[i]++;
736 order.nrjoints[i] += ci.
joints_.size();
741 std::vector<std::size_t> bijection(selected_options.size(), 0);
742 for (std::size_t i = 0; i < selected_options.size(); ++i)
746 std::sort(bijection.begin(), bijection.end(), order);
753 for (std::size_t i = 0; i < selected_options.size(); ++i)
756 selected_controllers.swap(selected_options[bijection[i]]);
762 selected_controllers.swap(selected_options[bijection[0]]);
773 for (std::size_t i = 0; i < controllers.size(); ++i)
776 std::map<std::string, ControllerInformation>::iterator it =
known_controllers_.find(controllers[i]);
784 const std::vector<std::string>& available_controllers,
785 std::vector<std::string>& selected_controllers)
787 for (std::size_t i = 1; i <= available_controllers.size(); ++i)
788 if (
findControllers(actuated_joints, i, available_controllers, selected_controllers))
793 std::vector<std::string> other_option;
794 for (std::size_t j = i + 1; j <= available_controllers.size(); ++j)
795 if (
findControllers(actuated_joints, j, available_controllers, other_option))
799 selected_controllers = other_option;
810 const std::vector<std::string>& controllers,
811 std::vector<moveit_msgs::RobotTrajectory>& parts)
814 parts.resize(controllers.size());
816 std::set<std::string> actuated_joints_mdof;
817 actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
818 trajectory.multi_dof_joint_trajectory.joint_names.end());
819 std::set<std::string> actuated_joints_single;
820 for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i)
822 const robot_model::JointModel* jm =
robot_model_->getJointModel(trajectory.joint_trajectory.joint_names[i]);
825 if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
827 actuated_joints_single.insert(jm->getName());
831 for (std::size_t i = 0; i < controllers.size(); ++i)
833 std::map<std::string, ControllerInformation>::iterator it =
known_controllers_.find(controllers[i]);
839 std::vector<std::string> intersect_mdof;
840 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_mdof.begin(),
841 actuated_joints_mdof.end(), std::back_inserter(intersect_mdof));
842 std::vector<std::string> intersect_single;
843 std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_single.begin(),
844 actuated_joints_single.end(), std::back_inserter(intersect_single));
845 if (intersect_mdof.empty() && intersect_single.empty())
848 if (!intersect_mdof.empty())
850 std::vector<std::string>& jnames = parts[i].multi_dof_joint_trajectory.joint_names;
851 jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
852 std::map<std::string, std::size_t>
index;
853 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
854 index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
855 std::vector<std::size_t> bijection(jnames.size());
856 for (std::size_t j = 0; j < jnames.size(); ++j)
857 bijection[j] = index[jnames[j]];
859 parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
860 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.points.size(); ++j)
862 parts[i].multi_dof_joint_trajectory.points[j].time_from_start =
863 trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
864 parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
865 for (std::size_t k = 0; k < bijection.size(); ++k)
867 parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
868 trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
870 if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
872 parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());
874 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
877 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
880 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
883 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
886 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
889 parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
895 if (!intersect_single.empty())
897 std::vector<std::string>& jnames = parts[i].joint_trajectory.joint_names;
898 jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
899 parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
900 std::map<std::string, std::size_t>
index;
901 for (std::size_t j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j)
902 index[trajectory.joint_trajectory.joint_names[j]] = j;
903 std::vector<std::size_t> bijection(jnames.size());
904 for (std::size_t j = 0; j < jnames.size(); ++j)
905 bijection[j] = index[jnames[j]];
906 parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
907 for (std::size_t j = 0; j < trajectory.joint_trajectory.points.size(); ++j)
909 parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
910 if (!trajectory.joint_trajectory.points[j].positions.empty())
912 parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
913 for (std::size_t k = 0; k < bijection.size(); ++k)
914 parts[i].joint_trajectory.points[j].positions[k] =
915 trajectory.joint_trajectory.points[j].positions[bijection[k]];
917 if (!trajectory.joint_trajectory.points[j].velocities.empty())
919 parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
920 for (std::size_t k = 0; k < bijection.size(); ++k)
921 parts[i].joint_trajectory.points[j].velocities[k] =
924 if (!trajectory.joint_trajectory.points[j].accelerations.empty())
926 parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
927 for (std::size_t k = 0; k < bijection.size(); ++k)
928 parts[i].joint_trajectory.points[j].accelerations[k] =
929 trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
931 if (!trajectory.joint_trajectory.points[j].effort.empty())
933 parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
934 for (std::size_t k = 0; k < bijection.size(); ++k)
935 parts[i].joint_trajectory.points[j].effort[k] =
936 trajectory.joint_trajectory.points[j].effort[bijection[k]];
952 robot_state::RobotStatePtr current_state;
955 ROS_WARN_NAMED(
name_,
"Failed to validate trajectory: couldn't receive full current joint state within 1s");
964 const std::vector<double>& positions =
trajectory.joint_trajectory.points.front().positions;
965 const std::vector<std::string>& joint_names =
trajectory.joint_trajectory.joint_names;
966 if (positions.size() != joint_names.size())
973 for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
975 const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]);
982 double cur_position = current_state->getJointPositions(jm)[0];
983 double traj_position = positions[i];
985 jm->enforcePositionBounds(&cur_position);
986 jm->enforcePositionBounds(&traj_position);
989 ROS_ERROR_NAMED(
name_,
"\nInvalid Trajectory: start point deviates from current robot state more than %g" 990 "\njoint '%s': expected: %g, current: %g",
999 const std::vector<geometry_msgs::Transform>& transforms =
1000 trajectory.multi_dof_joint_trajectory.points.front().transforms;
1001 const std::vector<std::string>& joint_names =
trajectory.multi_dof_joint_trajectory.joint_names;
1002 if (transforms.size() != joint_names.size())
1004 ROS_ERROR_NAMED(
name_,
"Wrong trajectory: #joints: %zu != #transforms: %zu", joint_names.size(),
1009 for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
1011 const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]);
1020 Eigen::Affine3d cur_transform, start_transform;
1021 jm->computeTransform(current_state->getJointPositions(jm), cur_transform);
1023 Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation();
1024 Eigen::AngleAxisd rotation;
1025 rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear());
1030 <<
"': pos delta: " << offset.transpose()
1031 <<
" rot delta: " << rotation.angle());
1041 const moveit_msgs::RobotTrajectory& trajectory,
1042 const std::vector<std::string>& controllers)
1044 if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
1049 std::set<std::string> actuated_joints;
1051 auto isActuated = [
this](
const std::string& joint_name) ->
bool {
1052 const robot_model::JointModel* jm =
robot_model_->getJointModel(joint_name);
1053 return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != robot_model::JointModel::FIXED);
1055 for (
const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names)
1056 if (isActuated(joint_name))
1057 actuated_joints.insert(joint_name);
1058 for (
const std::string& joint_name : trajectory.joint_trajectory.joint_names)
1059 if (isActuated(joint_name))
1060 actuated_joints.insert(joint_name);
1062 if (actuated_joints.empty())
1068 if (controllers.empty())
1071 bool reloaded =
false;
1075 std::vector<std::string> all_controller_names;
1076 for (std::map<std::string, ControllerInformation>::const_iterator it =
known_controllers_.begin();
1078 all_controller_names.push_back(it->first);
1100 bool reloaded =
false;
1101 for (std::size_t i = 0; i < controllers.size(); ++i)
1109 for (std::size_t i = 0; i < controllers.size(); ++i)
1121 std::stringstream ss;
1122 for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
1124 ROS_ERROR_NAMED(
name_,
"Unable to identify any set of controllers that can actuate the specified joints: [ %s]",
1127 std::stringstream ss2;
1128 std::map<std::string, ControllerInformation>::const_iterator mi;
1131 ss2 <<
"controller '" << mi->second.name_ <<
"' controls joints:\n";
1133 std::set<std::string>::const_iterator ji;
1134 for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ji++)
1136 ss2 <<
" " << *ji << std::endl;
1157 catch (std::exception& ex)
1291 if (epart && part_callback)
1332 std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
1342 for (std::size_t i = 0; i < context.
controllers_.size(); ++i)
1344 moveit_controller_manager::MoveItControllerHandlePtr h;
1349 catch (std::exception& ex)
1372 catch (std::exception& ex)
1378 for (std::size_t j = 0; j < i; ++j)
1383 catch (std::exception& ex)
1403 int longest_part = -1;
1412 if (context.
trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp > current_time)
1413 d = std::max(d, context.
trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp - current_time);
1419 context.
trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start);
1421 if (longest_part < 0 ||
1424 std::max(context.
trajectory_parts_[longest_part].joint_trajectory.points.size(),
1425 context.
trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
1432 std::map<std::string, double>::const_iterator scaling_it =
1435 scaling_it->second :
1438 std::map<std::string, double>::const_iterator margin_it =
1445 expected_trajectory_duration =
1446 std::max(d * current_scaling +
ros::Duration(current_margin), expected_trajectory_duration);
1450 if (longest_part >= 0)
1455 context.
trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
1458 if (context.
trajectory_parts_[longest_part].joint_trajectory.header.stamp > current_time)
1459 d = context.
trajectory_parts_[longest_part].joint_trajectory.header.stamp - current_time;
1460 for (std::size_t j = 0; j < context.
trajectory_parts_[longest_part].joint_trajectory.points.size(); ++j)
1462 context.
trajectory_parts_[longest_part].joint_trajectory.points[j].time_from_start);
1467 if (context.
trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp > current_time)
1468 d = context.
trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp - current_time;
1469 for (std::size_t j = 0; j < context.
trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size();
1473 context.
trajectory_parts_[longest_part].multi_dof_joint_trajectory.points[j].time_from_start);
1478 for (std::size_t i = 0; i < handles.size(); ++i)
1485 ROS_ERROR_NAMED(
name_,
"Controller is taking too long to execute trajectory (the expected upper " 1486 "bound for the trajectory execution was %lf seconds). Stopping trajectory.",
1487 expected_trajectory_duration.
toSec());
1499 handles[i]->waitForExecution();
1546 double time_remaining = wait_time;
1548 robot_state::RobotStatePtr prev_state, cur_state;
1549 prev_state =
csm_->getCurrentState();
1550 prev_state->enforceBounds();
1553 unsigned int no_motion_count = 0;
1554 while (time_remaining > 0. && no_motion_count < 3)
1556 if (!
csm_->waitForCurrentState(
ros::Time::now(), time_remaining) || !(cur_state =
csm_->getCurrentState()))
1561 cur_state->enforceBounds();
1568 const std::vector<std::string>& joint_names =
trajectory.joint_trajectory.joint_names;
1569 const std::size_t n = joint_names.size();
1571 for (std::size_t i = 0; i < n && !moved; ++i)
1573 const robot_model::JointModel* jm = cur_state->getJointModel(joint_names[i]);
1580 no_motion_count = 0;
1589 std::swap(prev_state, cur_state);
1592 return time_remaining > 0;
1599 return std::make_pair(-1, -1);
1602 std::vector<ros::Time>::const_iterator time_index_it =
1608 const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>&
1621 const robot_model::JointModelGroup* joint_model_group =
robot_model_->getJointModelGroup(group);
1622 if (joint_model_group)
1630 std::vector<std::string> all_controller_names;
1631 for (std::map<std::string, ControllerInformation>::const_iterator it =
known_controllers_.begin();
1633 all_controller_names.push_back(it->first);
1634 std::vector<std::string> selected_controllers;
1635 std::set<std::string> jset;
1636 for (std::size_t i = 0; i < joints.size(); ++i)
1638 const robot_model::JointModel* jm =
robot_model_->getJointModel(joints[i]);
1641 if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
1643 jset.insert(joints[i]);
1664 std::vector<std::string> controllers_to_activate;
1665 std::vector<std::string> controllers_to_deactivate;
1666 std::set<std::string> joints_to_be_activated;
1667 std::set<std::string> joints_to_be_deactivated;
1668 for (std::size_t i = 0; i < controllers.size(); ++i)
1670 std::map<std::string, ControllerInformation>::const_iterator it =
known_controllers_.find(controllers[i]);
1676 if (!it->second.state_.active_)
1679 controllers_to_activate.push_back(controllers[i]);
1680 joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
1681 for (std::set<std::string>::iterator kt = it->second.overlapping_controllers_.begin();
1682 kt != it->second.overlapping_controllers_.end(); ++kt)
1687 controllers_to_deactivate.push_back(*kt);
1688 joints_to_be_deactivated.insert(ci.
joints_.begin(), ci.
joints_.end());
1695 std::set<std::string>
diff;
1696 std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
1697 joints_to_be_activated.begin(), joints_to_be_activated.end(), std::inserter(diff, diff.end()));
1701 std::vector<std::string> possible_additional_controllers;
1702 for (std::map<std::string, ControllerInformation>::const_iterator it =
known_controllers_.begin();
1706 for (std::size_t k = 0; k < controllers_to_activate.size(); ++k)
1707 if (it->second.overlapping_controllers_.find(controllers_to_activate[k]) !=
1708 it->second.overlapping_controllers_.end())
1714 possible_additional_controllers.push_back(it->first);
1718 std::vector<std::string> additional_controllers;
1719 if (
selectControllers(diff, possible_additional_controllers, additional_controllers))
1720 controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(),
1721 additional_controllers.end());
1725 if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
1730 for (std::size_t a = 0; a < controllers_to_activate.size(); ++a)
1736 for (std::size_t a = 0; a < controllers_to_deactivate.size(); ++a)
1738 return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
1748 std::set<std::string> originally_active;
1749 for (std::map<std::string, ControllerInformation>::const_iterator it =
known_controllers_.begin();
1751 if (it->second.state_.active_)
1752 originally_active.insert(it->first);
1753 return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
1763 for (
int i = 0; i < controller_list.
size(); ++i)
1768 if (controller.
hasMember(
"allowed_execution_duration_scaling"))
1770 controller[
"allowed_execution_duration_scaling"];
1771 if (controller.
hasMember(
"allowed_goal_duration_margin"))
1773 controller[
"allowed_goal_duration_margin"];
std::map< std::string, double > controller_allowed_execution_duration_scaling_
std::map< std::string, double > controller_allowed_goal_duration_margin_
moveit_controller_manager::ExecutionStatus last_execution_status_
static const char * DEPRECATION_WARNING
std::deque< TrajectoryExecutionContext * > continuous_execution_queue_
TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, const planning_scene_monitor::CurrentStateMonitorPtr &csm)
Load the controller manager plugin, start listening for events on a topic.
#define ROS_INFO_NAMED(name,...)
std::unique_ptr< pluginlib::ClassLoader< moveit_controller_manager::MoveItControllerManager > > controller_manager_loader_
moveit_controller_manager::ExecutionStatus waitForExecution()
ros::NodeHandle root_node_handle_
#define ROS_DEBUG_STREAM_NAMED(name, args)
std::vector< ros::Time > time_index_
void setExecutionVelocityScaling(double scaling)
#define ROS_ERROR_STREAM_NAMED(name, args)
#define ROS_WARN_NAMED(name,...)
bool distributeTrajectory(const moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &controllers, std::vector< moveit_msgs::RobotTrajectory > &parts)
bool ensureActiveControllersForJoints(const std::vector< std::string > &joints)
Make sure the active controllers are such that trajectories that actuate joints in the specified set ...
robot_model::RobotModelConstPtr robot_model_
bool ensureActiveController(const std::string &controller)
Make sure a particular controller is active.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
double allowed_goal_duration_margin_
boost::condition_variable execution_complete_condition_
void generateControllerCombination(std::size_t start_index, std::size_t controller_count, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers, std::vector< std::vector< std::string > > &selected_options, const std::set< std::string > &actuated_joints)
bool run_continuous_execution_thread_
ros::NodeHandle node_handle_
bool waitForRobotToStop(const TrajectoryExecutionContext &context, double wait_time=1.0)
bool validate(const TrajectoryExecutionContext &context) const
Validate first point of trajectory matches current robot state.
std::vector< std::size_t > nrdefault
std::unique_ptr< boost::thread > execution_thread_
std::vector< std::size_t > nractive
ros::Subscriber event_topic_subscriber_
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void receiveEvent(const std_msgs::StringConstPtr &event)
bool pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller="")
std::string getName(void *handle)
bool isManagingControllers() const
If this function returns true, then this instance of the manager is allowed to load/unload/switch con...
bool ensureActiveControllers(const std::vector< std::string > &controllers)
Make sure a particular set of controllers are active.
std::map< std::string, ControllerInformation > known_controllers_
#define ROS_INFO_STREAM_NAMED(name, args)
const moveit_controller_manager::MoveItControllerManagerPtr & getControllerManager() const
Get the instance of the controller manager used (this is the plugin instance loaded) ...
static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING
bool execution_duration_monitoring_
Type const & getType() const
double execution_velocity_scaling_
static const std::string EXECUTION_EVENT_TOPIC
DynamicReconfigureImpl * reconfigure_impl_
boost::mutex continuous_execution_mutex_
boost::function< void(const moveit_controller_manager::ExecutionStatus &)> ExecutionCompleteCallback
boost::function< void(std::size_t)> PathSegmentCompleteCallback
std::vector< TrajectoryExecutionContext * > trajectories_
void setWaitForTrajectoryCompletion(bool flag)
Enable or disable waiting for trajectory completion.
bool checkControllerCombination(std::vector< std::string > &controllers, const std::set< std::string > &actuated_joints)
#define ROS_DEBUG_NAMED(name,...)
std::vector< std::string > controllers_
The controllers to use for executing the different trajectory parts;.
moveit_controller_manager::MoveItControllerManagerPtr controller_manager_
std::unique_ptr< boost::thread > continuous_execution_thread_
#define ROS_FATAL_STREAM_NAMED(name, args)
std::vector< std::vector< std::string > > selected_options
double allowed_start_tolerance_
bool ensureActiveControllersForGroup(const std::string &group)
Make sure the active controllers are such that trajectories that actuate joints in the specified grou...
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
bool findControllers(const std::set< std::string > &actuated_joints, std::size_t controller_count, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers)
bool wait_for_trajectory_completion_
void setAllowedExecutionDurationScaling(double scaling)
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...
boost::mutex time_index_mutex_
static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN
void stopExecutionInternal()
void processEvent(const std::string &event)
Execute a named event (e.g., 'stop')
void setAllowedGoalDurationMargin(double margin)
void continuousExecutionThread()
std::vector< moveit_msgs::RobotTrajectory > trajectory_parts_
bool hasMember(const std::string &name) const
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
bool executePart(std::size_t part_index)
void executeThread(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear)
bool configure(TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &controllers)
std::vector< std::size_t > nrjoints
~TrajectoryExecutionManager()
Destructor. Cancels all running trajectories (if any)
void reloadControllerInformation()
bool getParam(const std::string &key, std::string &s) const
void clear()
Clear the trajectories to execute.
static const ros::Duration DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE(1.0)
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
moveit_controller_manager::ExecutionStatus executeAndWait(bool auto_clear=true)
bool areControllersActive(const std::vector< std::string > &controllers)
Check if a set of controllers are active.
std::vector< moveit_controller_manager::MoveItControllerHandlePtr > active_handles_
moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const
Return the controller status for the last attempted execution.
boost::condition_variable continuous_execution_condition_
void stopExecution(bool auto_clear=true)
Stop whatever executions are active, if any.
const std::vector< TrajectoryExecutionContext * > & getTrajectories() const
Get the trajectories to be executed.
void setAllowedStartTolerance(double tolerance)
Set joint-value tolerance for validating trajectory's start point against current robot state...
bool stop_continuous_execution_
void loadControllerParams()
std::pair< int, int > getCurrentExpectedTrajectoryIndex() const
boost::mutex execution_state_mutex_
void updateControllersState(const ros::Duration &age)
bool isControllerActive(const std::string &controller)
Check if a controller is active.
bool selectControllers(const std::set< std::string > &actuated_joints, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers)
planning_scene_monitor::CurrentStateMonitorPtr csm_
double allowed_execution_duration_scaling_
std::string asString() const
void enableExecutionDurationMonitoring(bool flag)
Data structure that represents information necessary to execute a trajectory.
void updateControllerState(const std::string &controller, const ros::Duration &age)
#define ROS_WARN_STREAM_NAMED(name, args)
bool push(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller="")