trajectory_execution_manager.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
39 #include <moveit_ros_planning/TrajectoryExecutionDynamicReconfigureConfig.h>
40 #include <dynamic_reconfigure/server.h>
42 
44 {
45 const std::string TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC = "trajectory_execution_event";
46 
48 static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 0.5; // allow 0.5s more than the expected execution time
49  // before triggering a trajectory cancel (applied
50  // after scaling)
52  1.1; // allow the execution of a trajectory to take more time than expected (scaled by a value > 1)
53 
54 using namespace moveit_ros_planning;
55 
57 {
58 public:
60  : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/trajectory_execution"))
61  {
62  dynamic_reconfigure_server_.setCallback(
63  boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
64  }
65 
66 private:
67  void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig& config, uint32_t level)
68  {
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  }
75 
77  dynamic_reconfigure::Server<TrajectoryExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
78 };
79 
80 TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
81  const planning_scene_monitor::CurrentStateMonitorPtr& csm)
82  : robot_model_(kmodel), csm_(csm), node_handle_("~")
83 {
84  if (!node_handle_.getParam("moveit_manage_controllers", manage_controllers_))
85  manage_controllers_ = false;
86 
87  initialize();
88 }
89 
90 TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
91  const planning_scene_monitor::CurrentStateMonitorPtr& csm,
92  bool manage_controllers)
93  : robot_model_(kmodel), csm_(csm), node_handle_("~"), manage_controllers_(manage_controllers)
94 {
95  initialize();
96 }
97 
99 {
101  stopExecution(true);
102  delete reconfigure_impl_;
103 }
104 
105 static const char* DEPRECATION_WARNING =
106  "\nDeprecation warning: parameter '%s' moved into namespace 'trajectory_execution'."
107  "\nPlease, adjust file trajectory_execution.launch.xml!";
109 {
110  reconfigure_impl_ = NULL;
111  verbose_ = false;
112  execution_complete_ = true;
114  current_context_ = -1;
120 
121  // TODO: Reading from old param location should be removed in L-turtle. Handled by DynamicReconfigure.
122  if (node_handle_.getParam("allowed_execution_duration_scaling", allowed_execution_duration_scaling_))
123  ROS_WARN_NAMED(name_, DEPRECATION_WARNING, "allowed_execution_duration_scaling");
124  else
126 
127  if (node_handle_.getParam("allowed_goal_duration_margin", allowed_goal_duration_margin_))
128  ROS_WARN_NAMED(name_, DEPRECATION_WARNING, "allowed_goal_duration_margin");
129  else
131 
132  // load the controller manager plugin
133  try
134  {
136  "moveit_core", "moveit_controller_manager::MoveItControllerManager"));
137  }
139  {
140  ROS_FATAL_STREAM_NAMED(name_, "Exception while creating controller manager plugin loader: " << ex.what());
141  return;
142  }
143 
145  {
146  std::string controller;
147  if (!node_handle_.getParam("moveit_controller_manager", controller))
148  {
149  const std::vector<std::string>& classes = controller_manager_loader_->getDeclaredClasses();
150  if (classes.size() == 1)
151  {
152  controller = classes[0];
153  ROS_WARN_NAMED(name_, "Parameter '~moveit_controller_manager' is not specified but only one "
154  "matching plugin was found: '%s'. Using that one.",
155  controller.c_str());
156  }
157  else
158  ROS_FATAL_NAMED(name_, "Parameter '~moveit_controller_manager' not specified. This is needed to "
159  "identify the plugin to use for interacting with controllers. No paths can "
160  "be executed.");
161  }
162 
163  if (!controller.empty())
164  try
165  {
166  controller_manager_.reset(controller_manager_loader_->createUnmanagedInstance(controller));
167  }
169  {
170  ROS_FATAL_STREAM_NAMED(name_, "Exception while loading controller manager '" << controller
171  << "': " << ex.what());
172  }
173  }
174 
175  // other configuration steps
177 
180 
182 
184  ROS_INFO_NAMED(name_, "Trajectory execution is managing controllers");
185  else
186  ROS_INFO_NAMED(name_, "Trajectory execution is not managing controllers");
187 }
188 
190 {
192 }
193 
195 {
197 }
198 
200 {
202 }
203 
205 {
206  execution_velocity_scaling_ = scaling;
207 }
208 
210 {
211  allowed_start_tolerance_ = tolerance;
212 }
213 
215 {
216  return manage_controllers_;
217 }
218 
219 const moveit_controller_manager::MoveItControllerManagerPtr& TrajectoryExecutionManager::getControllerManager() const
220 {
221  return controller_manager_;
222 }
223 
224 void TrajectoryExecutionManager::processEvent(const std::string& event)
225 {
226  if (event == "stop")
227  stopExecution(true);
228  else
229  ROS_WARN_STREAM_NAMED(name_, "Unknown event type: '" << event << "'");
230 }
231 
232 void TrajectoryExecutionManager::receiveEvent(const std_msgs::StringConstPtr& event)
233 {
234  ROS_INFO_STREAM_NAMED(name_, "Received event '" << event->data << "'");
235  processEvent(event->data);
236 }
237 
238 bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory& trajectory, const std::string& controller)
239 {
240  if (controller.empty())
241  return push(trajectory, std::vector<std::string>());
242  else
243  return push(trajectory, std::vector<std::string>(1, controller));
244 }
245 
246 bool TrajectoryExecutionManager::push(const trajectory_msgs::JointTrajectory& trajectory, const std::string& controller)
247 {
248  if (controller.empty())
249  return push(trajectory, std::vector<std::string>());
250  else
251  return push(trajectory, std::vector<std::string>(1, controller));
252 }
253 
254 bool TrajectoryExecutionManager::push(const trajectory_msgs::JointTrajectory& trajectory,
255  const std::vector<std::string>& controllers)
256 {
257  moveit_msgs::RobotTrajectory traj;
258  traj.joint_trajectory = trajectory;
259  return push(traj, controllers);
260 }
261 
262 bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory& trajectory,
263  const std::vector<std::string>& controllers)
264 {
265  if (!execution_complete_)
266  {
267  ROS_ERROR_NAMED(name_, "Cannot push a new trajectory while another is being executed");
268  return false;
269  }
270 
272  if (configure(*context, trajectory, controllers))
273  {
274  if (verbose_)
275  {
276  std::stringstream ss;
277  ss << "Pushed trajectory for execution using controllers [ ";
278  for (std::size_t i = 0; i < context->controllers_.size(); ++i)
279  ss << context->controllers_[i] << " ";
280  ss << "]:" << std::endl;
281  for (std::size_t i = 0; i < context->trajectory_parts_.size(); ++i)
282  ss << context->trajectory_parts_[i] << std::endl;
283  ROS_INFO_NAMED(name_, "%s", ss.str().c_str());
284  }
285  trajectories_.push_back(context);
286  return true;
287  }
288  else
289  {
290  delete context;
292  }
293 
294  return false;
295 }
296 
297 bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajectory& trajectory,
298  const std::string& controller)
299 {
300  if (controller.empty())
301  return pushAndExecute(trajectory, std::vector<std::string>());
302  else
303  return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
304 }
305 
306 bool TrajectoryExecutionManager::pushAndExecute(const trajectory_msgs::JointTrajectory& trajectory,
307  const std::string& controller)
308 {
309  if (controller.empty())
310  return pushAndExecute(trajectory, std::vector<std::string>());
311  else
312  return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
313 }
314 
315 bool TrajectoryExecutionManager::pushAndExecute(const sensor_msgs::JointState& state, const std::string& controller)
316 {
317  if (controller.empty())
318  return pushAndExecute(state, std::vector<std::string>());
319  else
320  return pushAndExecute(state, std::vector<std::string>(1, controller));
321 }
322 
323 bool TrajectoryExecutionManager::pushAndExecute(const trajectory_msgs::JointTrajectory& trajectory,
324  const std::vector<std::string>& controllers)
325 {
326  moveit_msgs::RobotTrajectory traj;
327  traj.joint_trajectory = trajectory;
328  return pushAndExecute(traj, controllers);
329 }
330 
331 bool TrajectoryExecutionManager::pushAndExecute(const sensor_msgs::JointState& state,
332  const std::vector<std::string>& controllers)
333 {
334  moveit_msgs::RobotTrajectory traj;
335  traj.joint_trajectory.header = state.header;
336  traj.joint_trajectory.joint_names = state.name;
337  traj.joint_trajectory.points.resize(1);
338  traj.joint_trajectory.points[0].positions = state.position;
339  traj.joint_trajectory.points[0].velocities = state.velocity;
340  traj.joint_trajectory.points[0].effort = state.effort;
341  traj.joint_trajectory.points[0].time_from_start = ros::Duration(0, 0);
342  return pushAndExecute(traj, controllers);
343 }
344 
345 bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajectory& trajectory,
346  const std::vector<std::string>& controllers)
347 {
348  if (!execution_complete_)
349  {
350  ROS_ERROR_NAMED(name_, "Cannot push & execute a new trajectory while another is being executed");
351  return false;
352  }
353 
355  if (configure(*context, trajectory, controllers))
356  {
357  {
358  boost::mutex::scoped_lock slock(continuous_execution_mutex_);
359  continuous_execution_queue_.push_back(context);
362  new boost::thread(boost::bind(&TrajectoryExecutionManager::continuousExecutionThread, this)));
363  }
366  return true;
367  }
368  else
369  {
370  delete context;
372  return false;
373  }
374 }
375 
377 {
378  std::set<moveit_controller_manager::MoveItControllerHandlePtr> used_handles;
380  {
382  {
383  boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
386  }
387 
389  {
390  for (std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin();
391  uit != used_handles.end(); ++uit)
392  if ((*uit)->getLastExecutionStatus() == moveit_controller_manager::ExecutionStatus::RUNNING)
393  (*uit)->cancelExecution();
394  used_handles.clear();
395  while (!continuous_execution_queue_.empty())
396  {
398  continuous_execution_queue_.pop_front();
399  delete context;
400  }
402  continue;
403  }
404 
405  while (!continuous_execution_queue_.empty())
406  {
407  TrajectoryExecutionContext* context = NULL;
408  {
409  boost::mutex::scoped_lock slock(continuous_execution_mutex_);
410  if (continuous_execution_queue_.empty())
411  break;
412  context = continuous_execution_queue_.front();
413  continuous_execution_queue_.pop_front();
414  if (continuous_execution_queue_.empty())
416  }
417 
418  // remove handles we no longer need
419  std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin();
420  while (uit != used_handles.end())
421  if ((*uit)->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::RUNNING)
422  {
423  std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator toErase = uit;
424  ++uit;
425  used_handles.erase(toErase);
426  }
427  else
428  ++uit;
429 
430  // now send stuff to controllers
431 
432  // first make sure desired controllers are active
433  if (areControllersActive(context->controllers_))
434  {
435  // get the controller handles needed to execute the new trajectory
436  std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles(context->controllers_.size());
437  for (std::size_t i = 0; i < context->controllers_.size(); ++i)
438  {
439  moveit_controller_manager::MoveItControllerHandlePtr h;
440  try
441  {
442  h = controller_manager_->getControllerHandle(context->controllers_[i]);
443  }
444  catch (std::exception& ex)
445  {
446  ROS_ERROR_NAMED(name_, "%s caught when retrieving controller handle", ex.what());
447  }
448  if (!h)
449  {
451  ROS_ERROR_NAMED(name_, "No controller handle for controller '%s'. Aborting.",
452  context->controllers_[i].c_str());
453  handles.clear();
454  break;
455  }
456  handles[i] = h;
457  }
458 
460  {
461  delete context;
462  break;
463  }
464 
465  // push all trajectories to all controllers simultaneously
466  if (!handles.empty())
467  for (std::size_t i = 0; i < context->trajectory_parts_.size(); ++i)
468  {
469  bool ok = false;
470  try
471  {
472  ok = handles[i]->sendTrajectory(context->trajectory_parts_[i]);
473  }
474  catch (std::exception& ex)
475  {
476  ROS_ERROR_NAMED(name_, "Caught %s when sending trajectory to controller", ex.what());
477  }
478  if (!ok)
479  {
480  for (std::size_t j = 0; j < i; ++j)
481  try
482  {
483  handles[j]->cancelExecution();
484  }
485  catch (std::exception& ex)
486  {
487  ROS_ERROR_NAMED(name_, "Caught %s when canceling execution", ex.what());
488  }
489  ROS_ERROR_NAMED(name_, "Failed to send trajectory part %zu of %zu to controller %s", i + 1,
490  context->trajectory_parts_.size(), handles[i]->getName().c_str());
491  if (i > 0)
492  ROS_ERROR_NAMED(name_, "Cancelling previously sent trajectory parts");
494  handles.clear();
495  break;
496  }
497  }
498  delete context;
499 
500  // remember which handles we used
501  for (std::size_t i = 0; i < handles.size(); ++i)
502  used_handles.insert(handles[i]);
503  }
504  else
505  {
506  ROS_ERROR_NAMED(name_, "Not all needed controllers are active. Cannot push and execute. You can try "
507  "calling ensureActiveControllers() before pushAndExecute()");
509  delete context;
510  }
511  }
512  }
513 }
514 
516 {
517  known_controllers_.clear();
519  {
520  std::vector<std::string> names;
521  controller_manager_->getControllersList(names);
522  for (std::size_t i = 0; i < names.size(); ++i)
523  {
524  std::vector<std::string> joints;
525  controller_manager_->getControllerJoints(names[i], joints);
527  ci.name_ = names[i];
528  ci.joints_.insert(joints.begin(), joints.end());
529  known_controllers_[ci.name_] = ci;
530  }
531 
532  for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
533  it != known_controllers_.end(); ++it)
534  for (std::map<std::string, ControllerInformation>::iterator jt = known_controllers_.begin();
535  jt != known_controllers_.end(); ++jt)
536  if (it != jt)
537  {
538  std::vector<std::string> intersect;
539  std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), jt->second.joints_.begin(),
540  jt->second.joints_.end(), std::back_inserter(intersect));
541  if (!intersect.empty())
542  {
543  it->second.overlapping_controllers_.insert(jt->first);
544  jt->second.overlapping_controllers_.insert(it->first);
545  }
546  }
547  }
548 }
549 
550 void TrajectoryExecutionManager::updateControllerState(const std::string& controller, const ros::Duration& age)
551 {
552  std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
553  if (it != known_controllers_.end())
554  updateControllerState(it->second, age);
555  else
556  ROS_ERROR_NAMED(name_, "Controller '%s' is not known.", controller.c_str());
557 }
558 
560 {
561  if (ros::Time::now() - ci.last_update_ >= age)
562  {
564  {
565  if (verbose_)
566  ROS_INFO_NAMED(name_, "Updating information for controller '%s'.", ci.name_.c_str());
567  ci.state_ = controller_manager_->getControllerState(ci.name_);
569  }
570  }
571  else if (verbose_)
572  ROS_INFO_NAMED(name_, "Information for controller '%s' is assumed to be up to date.", ci.name_.c_str());
573 }
574 
576 {
577  for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
578  it != known_controllers_.end(); ++it)
579  updateControllerState(it->second, age);
580 }
581 
582 bool TrajectoryExecutionManager::checkControllerCombination(std::vector<std::string>& selected,
583  const std::set<std::string>& actuated_joints)
584 {
585  std::set<std::string> combined_joints;
586  for (std::size_t i = 0; i < selected.size(); ++i)
587  {
588  const ControllerInformation& ci = known_controllers_[selected[i]];
589  combined_joints.insert(ci.joints_.begin(), ci.joints_.end());
590  }
591 
592  if (verbose_)
593  {
594  std::stringstream ss, saj, sac;
595  for (std::size_t i = 0; i < selected.size(); ++i)
596  ss << selected[i] << " ";
597  for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
598  saj << *it << " ";
599  for (std::set<std::string>::const_iterator it = combined_joints.begin(); it != combined_joints.end(); ++it)
600  sac << *it << " ";
601  ROS_INFO_NAMED(name_, "Checking if controllers [ %s] operating on joints [ %s] cover joints [ %s]",
602  ss.str().c_str(), sac.str().c_str(), saj.str().c_str());
603  }
604 
605  return std::includes(combined_joints.begin(), combined_joints.end(), actuated_joints.begin(), actuated_joints.end());
606 }
607 
608 void TrajectoryExecutionManager::generateControllerCombination(std::size_t start_index, std::size_t controller_count,
609  const std::vector<std::string>& available_controllers,
610  std::vector<std::string>& selected_controllers,
611  std::vector<std::vector<std::string> >& selected_options,
612  const std::set<std::string>& actuated_joints)
613 {
614  if (selected_controllers.size() == controller_count)
615  {
616  if (checkControllerCombination(selected_controllers, actuated_joints))
617  selected_options.push_back(selected_controllers);
618  return;
619  }
620 
621  for (std::size_t i = start_index; i < available_controllers.size(); ++i)
622  {
623  bool overlap = false;
624  const ControllerInformation& ci = known_controllers_[available_controllers[i]];
625  for (std::size_t j = 0; j < selected_controllers.size() && !overlap; ++j)
626  {
627  if (ci.overlapping_controllers_.find(selected_controllers[j]) != ci.overlapping_controllers_.end())
628  overlap = true;
629  }
630  if (overlap)
631  continue;
632  selected_controllers.push_back(available_controllers[i]);
633  generateControllerCombination(i + 1, controller_count, available_controllers, selected_controllers,
634  selected_options, actuated_joints);
635  selected_controllers.pop_back();
636  }
637 }
638 
639 namespace
640 {
641 struct OrderPotentialControllerCombination
642 {
643  bool operator()(const std::size_t a, const std::size_t b) const
644  {
645  // preference is given to controllers marked as default
646  if (nrdefault[a] > nrdefault[b])
647  return true;
648  if (nrdefault[a] < nrdefault[b])
649  return false;
650 
651  // and then to ones that operate on fewer joints
652  if (nrjoints[a] < nrjoints[b])
653  return true;
654  if (nrjoints[a] > nrjoints[b])
655  return false;
656 
657  // and then to active ones
658  if (nractive[a] < nractive[b])
659  return true;
660  if (nractive[a] > nractive[b])
661  return false;
662 
663  return false;
664  }
665 
666  std::vector<std::vector<std::string> > selected_options;
667  std::vector<std::size_t> nrdefault;
668  std::vector<std::size_t> nrjoints;
669  std::vector<std::size_t> nractive;
670 };
671 }
672 
673 bool TrajectoryExecutionManager::findControllers(const std::set<std::string>& actuated_joints,
674  std::size_t controller_count,
675  const std::vector<std::string>& available_controllers,
676  std::vector<std::string>& selected_controllers)
677 {
678  // generate all combinations of controller_count controllers that operate on disjoint sets of joints
679  std::vector<std::string> work_area;
680  OrderPotentialControllerCombination order;
681  std::vector<std::vector<std::string> >& selected_options = order.selected_options;
682  generateControllerCombination(0, controller_count, available_controllers, work_area, selected_options,
683  actuated_joints);
684 
685  if (verbose_)
686  {
687  std::stringstream saj;
688  std::stringstream sac;
689  for (std::size_t i = 0; i < available_controllers.size(); ++i)
690  sac << available_controllers[i] << " ";
691  for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
692  saj << *it << " ";
693  ROS_INFO_NAMED(name_, "Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.",
694  controller_count, sac.str().c_str(), saj.str().c_str(), selected_options.size());
695  }
696 
697  // if none was found, this is a problem
698  if (selected_options.empty())
699  return false;
700 
701  // if only one was found, return it
702  if (selected_options.size() == 1)
703  {
704  selected_controllers.swap(selected_options[0]);
705  return true;
706  }
707 
708  // if more options were found, evaluate them all and return the best one
709 
710  // count how many default controllers are used in each reported option, and how many joints are actuated in total by
711  // the selected controllers,
712  // to use that in the ranking of the options
713  order.nrdefault.resize(selected_options.size(), 0);
714  order.nrjoints.resize(selected_options.size(), 0);
715  order.nractive.resize(selected_options.size(), 0);
716  for (std::size_t i = 0; i < selected_options.size(); ++i)
717  {
718  for (std::size_t k = 0; k < selected_options[i].size(); ++k)
719  {
721  const ControllerInformation& ci = known_controllers_[selected_options[i][k]];
722 
723  if (ci.state_.default_)
724  order.nrdefault[i]++;
725  if (ci.state_.active_)
726  order.nractive[i]++;
727  order.nrjoints[i] += ci.joints_.size();
728  }
729  }
730 
731  // define a bijection to compute the raking of the found options
732  std::vector<std::size_t> bijection(selected_options.size(), 0);
733  for (std::size_t i = 0; i < selected_options.size(); ++i)
734  bijection[i] = i;
735 
736  // sort the options
737  std::sort(bijection.begin(), bijection.end(), order);
738 
739  // depending on whether we are allowed to load & unload controllers,
740  // we have different preference on deciding between options
741  if (!manage_controllers_)
742  {
743  // if we can't load different options at will, just choose one that is already loaded
744  for (std::size_t i = 0; i < selected_options.size(); ++i)
745  if (areControllersActive(selected_options[bijection[i]]))
746  {
747  selected_controllers.swap(selected_options[bijection[i]]);
748  return true;
749  }
750  }
751 
752  // otherwise, just use the first valid option
753  selected_controllers.swap(selected_options[bijection[0]]);
754  return true;
755 }
756 
757 bool TrajectoryExecutionManager::isControllerActive(const std::string& controller)
758 {
759  return areControllersActive(std::vector<std::string>(1, controller));
760 }
761 
762 bool TrajectoryExecutionManager::areControllersActive(const std::vector<std::string>& controllers)
763 {
764  for (std::size_t i = 0; i < controllers.size(); ++i)
765  {
767  std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
768  if (it == known_controllers_.end() || !it->second.state_.active_)
769  return false;
770  }
771  return true;
772 }
773 
774 bool TrajectoryExecutionManager::selectControllers(const std::set<std::string>& actuated_joints,
775  const std::vector<std::string>& available_controllers,
776  std::vector<std::string>& selected_controllers)
777 {
778  for (std::size_t i = 1; i <= available_controllers.size(); ++i)
779  if (findControllers(actuated_joints, i, available_controllers, selected_controllers))
780  {
781  // if we are not managing controllers, prefer to use active controllers even if there are more of them
782  if (!manage_controllers_ && !areControllersActive(selected_controllers))
783  {
784  std::vector<std::string> other_option;
785  for (std::size_t j = i + 1; j <= available_controllers.size(); ++j)
786  if (findControllers(actuated_joints, j, available_controllers, other_option))
787  {
788  if (areControllersActive(other_option))
789  {
790  selected_controllers = other_option;
791  break;
792  }
793  }
794  }
795  return true;
796  }
797  return false;
798 }
799 
800 bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::RobotTrajectory& trajectory,
801  const std::vector<std::string>& controllers,
802  std::vector<moveit_msgs::RobotTrajectory>& parts)
803 {
804  parts.clear();
805  parts.resize(controllers.size());
806 
807  std::set<std::string> actuated_joints_mdof;
808  actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
809  trajectory.multi_dof_joint_trajectory.joint_names.end());
810  std::set<std::string> actuated_joints_single;
811  for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i)
812  {
813  const robot_model::JointModel* jm = robot_model_->getJointModel(trajectory.joint_trajectory.joint_names[i]);
814  if (jm)
815  {
816  if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
817  continue;
818  actuated_joints_single.insert(jm->getName());
819  }
820  }
821 
822  for (std::size_t i = 0; i < controllers.size(); ++i)
823  {
824  std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
825  if (it == known_controllers_.end())
826  {
827  ROS_ERROR_STREAM_NAMED(name_, "Controller " << controllers[i] << " not found.");
828  return false;
829  }
830  std::vector<std::string> intersect_mdof;
831  std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_mdof.begin(),
832  actuated_joints_mdof.end(), std::back_inserter(intersect_mdof));
833  std::vector<std::string> intersect_single;
834  std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_single.begin(),
835  actuated_joints_single.end(), std::back_inserter(intersect_single));
836  if (intersect_mdof.empty() && intersect_single.empty())
837  ROS_WARN_STREAM_NAMED(name_, "No joints to be distributed for controller " << controllers[i]);
838  {
839  if (!intersect_mdof.empty())
840  {
841  std::vector<std::string>& jnames = parts[i].multi_dof_joint_trajectory.joint_names;
842  jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
843  std::map<std::string, std::size_t> index;
844  for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
845  index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
846  std::vector<std::size_t> bijection(jnames.size());
847  for (std::size_t j = 0; j < jnames.size(); ++j)
848  bijection[j] = index[jnames[j]];
849 
850  parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
851  for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.points.size(); ++j)
852  {
853  parts[i].multi_dof_joint_trajectory.points[j].time_from_start =
854  trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
855  parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
856  for (std::size_t k = 0; k < bijection.size(); ++k)
857  {
858  parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
859  trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
860 
861  if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
862  {
863  parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());
864 
865  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
866  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.x * execution_velocity_scaling_;
867 
868  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
869  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.y * execution_velocity_scaling_;
870 
871  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
872  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.z * execution_velocity_scaling_;
873 
874  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
875  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.x * execution_velocity_scaling_;
876 
877  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
878  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.y * execution_velocity_scaling_;
879 
880  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
881  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.z * execution_velocity_scaling_;
882  }
883  }
884  }
885  }
886  if (!intersect_single.empty())
887  {
888  std::vector<std::string>& jnames = parts[i].joint_trajectory.joint_names;
889  jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
890  parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
891  std::map<std::string, std::size_t> index;
892  for (std::size_t j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j)
893  index[trajectory.joint_trajectory.joint_names[j]] = j;
894  std::vector<std::size_t> bijection(jnames.size());
895  for (std::size_t j = 0; j < jnames.size(); ++j)
896  bijection[j] = index[jnames[j]];
897  parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
898  for (std::size_t j = 0; j < trajectory.joint_trajectory.points.size(); ++j)
899  {
900  parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
901  if (!trajectory.joint_trajectory.points[j].positions.empty())
902  {
903  parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
904  for (std::size_t k = 0; k < bijection.size(); ++k)
905  parts[i].joint_trajectory.points[j].positions[k] =
906  trajectory.joint_trajectory.points[j].positions[bijection[k]];
907  }
908  if (!trajectory.joint_trajectory.points[j].velocities.empty())
909  {
910  parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
911  for (std::size_t k = 0; k < bijection.size(); ++k)
912  parts[i].joint_trajectory.points[j].velocities[k] =
913  trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
914  }
915  if (!trajectory.joint_trajectory.points[j].accelerations.empty())
916  {
917  parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
918  for (std::size_t k = 0; k < bijection.size(); ++k)
919  parts[i].joint_trajectory.points[j].accelerations[k] =
920  trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
921  }
922  if (!trajectory.joint_trajectory.points[j].effort.empty())
923  {
924  parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
925  for (std::size_t k = 0; k < bijection.size(); ++k)
926  parts[i].joint_trajectory.points[j].effort[k] =
927  trajectory.joint_trajectory.points[j].effort[bijection[k]];
928  }
929  }
930  }
931  }
932  }
933  return true;
934 }
935 
937 {
938  if (allowed_start_tolerance_ == 0) // skip validation on this magic number
939  return true;
940 
941  ROS_DEBUG_NAMED(name_, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
942 
943  robot_state::RobotStatePtr current_state;
944  if (!csm_->waitForCurrentState(ros::Time::now()) || !(current_state = csm_->getCurrentState()))
945  {
946  ROS_WARN_NAMED(name_, "Failed to validate trajectory: couldn't receive full current joint state within 1s");
947  return false;
948  }
949 
950  for (const auto& trajectory : context.trajectory_parts_)
951  {
952  if (!trajectory.joint_trajectory.points.empty())
953  {
954  // Check single-dof trajectory
955  const std::vector<double>& positions = trajectory.joint_trajectory.points.front().positions;
956  const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
957  if (positions.size() != joint_names.size())
958  {
959  ROS_ERROR_NAMED(name_, "Wrong trajectory: #joints: %zu != #positions: %zu", joint_names.size(),
960  positions.size());
961  return false;
962  }
963 
964  for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
965  {
966  const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]);
967  if (!jm)
968  {
969  ROS_ERROR_STREAM_NAMED(name_, "Unknown joint in trajectory: " << joint_names[i]);
970  return false;
971  }
972 
973  double cur_position = current_state->getJointPositions(jm)[0];
974  double traj_position = positions[i];
975  // normalize positions and compare
976  jm->enforcePositionBounds(&cur_position);
977  jm->enforcePositionBounds(&traj_position);
978  if (fabs(cur_position - traj_position) > allowed_start_tolerance_)
979  {
980  ROS_ERROR_NAMED(name_, "\nInvalid Trajectory: start point deviates from current robot state more than %g"
981  "\njoint '%s': expected: %g, current: %g",
982  allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position);
983  return false;
984  }
985  }
986  }
987  if (!trajectory.multi_dof_joint_trajectory.points.empty())
988  {
989  // Check multi-dof trajectory
990  const std::vector<geometry_msgs::Transform>& transforms =
991  trajectory.multi_dof_joint_trajectory.points.front().transforms;
992  const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
993  if (transforms.size() != joint_names.size())
994  {
995  ROS_ERROR_NAMED(name_, "Wrong trajectory: #joints: %zu != #transforms: %zu", joint_names.size(),
996  transforms.size());
997  return false;
998  }
999 
1000  for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
1001  {
1002  const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]);
1003  if (!jm)
1004  {
1005  ROS_ERROR_STREAM_NAMED(name_, "Unknown joint in trajectory: " << joint_names[i]);
1006  return false;
1007  }
1008 
1009  // compute difference (offset vector and rotation angle) between current transform
1010  // and start transform in trajectory
1011  Eigen::Affine3d cur_transform, start_transform;
1012  jm->computeTransform(current_state->getJointPositions(jm), cur_transform);
1013  tf::transformMsgToEigen(transforms[i], start_transform);
1014  Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation();
1015  Eigen::AngleAxisd rotation;
1016  rotation.fromRotationMatrix(cur_transform.rotation().inverse() * start_transform.rotation());
1017  if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_)
1018  {
1019  ROS_ERROR_STREAM_NAMED(name_, "\nInvalid Trajectory: start point deviates from current robot state more than "
1020  << allowed_start_tolerance_ << "\nmulti-dof joint '" << joint_names[i]
1021  << "': pos delta: " << offset.transpose()
1022  << " rot delta: " << rotation.angle());
1023  return false;
1024  }
1025  }
1026  }
1027  }
1028  return true;
1029 }
1030 
1032  const moveit_msgs::RobotTrajectory& trajectory,
1033  const std::vector<std::string>& controllers)
1034 {
1035  if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
1036  {
1037  ROS_WARN_NAMED(name_, "The trajectory to execute is empty");
1038  return false;
1039  }
1040  std::set<std::string> actuated_joints;
1041 
1042  auto isActuated = [this](const std::string& joint_name) -> bool {
1043  const robot_model::JointModel* jm = robot_model_->getJointModel(joint_name);
1044  return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != robot_model::JointModel::FIXED);
1045  };
1046  for (const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names)
1047  if (isActuated(joint_name))
1048  actuated_joints.insert(joint_name);
1049  for (const std::string& joint_name : trajectory.joint_trajectory.joint_names)
1050  if (isActuated(joint_name))
1051  actuated_joints.insert(joint_name);
1052 
1053  if (actuated_joints.empty())
1054  {
1055  ROS_WARN_NAMED(name_, "The trajectory to execute specifies no joints");
1056  return false;
1057  }
1058 
1059  if (controllers.empty())
1060  {
1061  bool retry = true;
1062  bool reloaded = false;
1063  while (retry)
1064  {
1065  retry = false;
1066  std::vector<std::string> all_controller_names;
1067  for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1068  it != known_controllers_.end(); ++it)
1069  all_controller_names.push_back(it->first);
1070  if (selectControllers(actuated_joints, all_controller_names, context.controllers_))
1071  {
1072  if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1073  return true;
1074  }
1075  else
1076  {
1077  // maybe we failed because we did not have a complete list of controllers
1078  if (!reloaded)
1079  {
1081  reloaded = true;
1082  retry = true;
1083  }
1084  }
1085  }
1086  }
1087  else
1088  {
1089  // check if the specified controllers are valid names;
1090  // if they appear not to be, try to reload the controller information, just in case they are new in the system
1091  bool reloaded = false;
1092  for (std::size_t i = 0; i < controllers.size(); ++i)
1093  if (known_controllers_.find(controllers[i]) == known_controllers_.end())
1094  {
1096  reloaded = true;
1097  break;
1098  }
1099  if (reloaded)
1100  for (std::size_t i = 0; i < controllers.size(); ++i)
1101  if (known_controllers_.find(controllers[i]) == known_controllers_.end())
1102  {
1103  ROS_ERROR_NAMED(name_, "Controller '%s' is not known", controllers[i].c_str());
1104  return false;
1105  }
1106  if (selectControllers(actuated_joints, controllers, context.controllers_))
1107  {
1108  if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1109  return true;
1110  }
1111  }
1112  std::stringstream ss;
1113  for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
1114  ss << *it << " ";
1115  ROS_ERROR_NAMED(name_, "Unable to identify any set of controllers that can actuate the specified joints: [ %s]",
1116  ss.str().c_str());
1117 
1118  std::stringstream ss2;
1119  std::map<std::string, ControllerInformation>::const_iterator mi;
1120  for (mi = known_controllers_.begin(); mi != known_controllers_.end(); mi++)
1121  {
1122  ss2 << "controller '" << mi->second.name_ << "' controls joints:\n";
1123 
1124  std::set<std::string>::const_iterator ji;
1125  for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ji++)
1126  {
1127  ss2 << " " << *ji << std::endl;
1128  }
1129  }
1130  ROS_ERROR_NAMED(name_, "Known controllers and their joints:\n%s", ss2.str().c_str());
1131  return false;
1132 }
1133 
1135 {
1136  execute(ExecutionCompleteCallback(), auto_clear);
1137  return waitForExecution();
1138 }
1139 
1141 {
1142  // execution_state_mutex_ needs to have been locked by the caller
1143  for (std::size_t i = 0; i < active_handles_.size(); ++i)
1144  try
1145  {
1146  active_handles_[i]->cancelExecution();
1147  }
1148  catch (std::exception& ex)
1149  {
1150  ROS_ERROR_NAMED(name_, "Caught %s when canceling execution.", ex.what());
1151  }
1152 }
1153 
1155 {
1158 
1159  if (!execution_complete_)
1160  {
1161  execution_state_mutex_.lock();
1162  if (!execution_complete_)
1163  {
1164  // we call cancel for all active handles; we know these are not being modified as we loop through them because of
1165  // the lock
1166  // we mark execution_complete_ as true ahead of time. Using this flag, executePart() will know that an external
1167  // trigger to stop has been received
1168  execution_complete_ = true;
1170 
1171  // we set the status here; executePart() will not set status when execution_complete_ is true ahead of time
1173  execution_state_mutex_.unlock();
1174  ROS_INFO_NAMED(name_, "Stopped trajectory execution.");
1175 
1176  // wait for the execution thread to finish
1177  execution_thread_->join();
1178  execution_thread_.reset();
1179 
1180  if (auto_clear)
1181  clear();
1182  }
1183  else
1184  execution_state_mutex_.unlock();
1185  }
1186  else if (execution_thread_) // just in case we have some thread waiting to be joined from some point in the past, we
1187  // join it now
1188  {
1189  execution_thread_->join();
1190  execution_thread_.reset();
1191  }
1192 }
1193 
1195 {
1196  execute(callback, PathSegmentCompleteCallback(), auto_clear);
1197 }
1198 
1200  const PathSegmentCompleteCallback& part_callback, bool auto_clear)
1201 {
1202  stopExecution(false);
1203 
1204  // check whether first trajectory starts at current robot state
1205  if (trajectories_.size() && !validate(*trajectories_.front()))
1206  {
1208  if (auto_clear)
1209  clear();
1210  if (callback)
1211  callback(last_execution_status_);
1212  return;
1213  }
1214 
1215  // start the execution thread
1216  execution_complete_ = false;
1217  execution_thread_.reset(
1218  new boost::thread(&TrajectoryExecutionManager::executeThread, this, callback, part_callback, auto_clear));
1219 }
1220 
1222 {
1223  {
1224  boost::unique_lock<boost::mutex> ulock(execution_state_mutex_);
1225  while (!execution_complete_)
1227  }
1228  {
1229  boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
1230  while (!continuous_execution_queue_.empty())
1232  }
1233 
1234  // this will join the thread for executing sequences of trajectories
1235  stopExecution(false);
1236 
1237  return last_execution_status_;
1238 }
1239 
1241 {
1242  if (execution_complete_)
1243  {
1244  for (std::size_t i = 0; i < trajectories_.size(); ++i)
1245  delete trajectories_[i];
1246  trajectories_.clear();
1247  {
1248  boost::mutex::scoped_lock slock(continuous_execution_mutex_);
1249  while (!continuous_execution_queue_.empty())
1250  {
1251  delete continuous_execution_queue_.front();
1252  continuous_execution_queue_.pop_front();
1253  }
1254  }
1255  }
1256  else
1257  ROS_ERROR_NAMED(name_, "Cannot push a new trajectory while another is being executed");
1258 }
1259 
1261  const PathSegmentCompleteCallback& part_callback, bool auto_clear)
1262 {
1263  // if we already got a stop request before we even started anything, we abort
1264  if (execution_complete_)
1265  {
1267  if (callback)
1268  callback(last_execution_status_);
1269  return;
1270  }
1271 
1272  ROS_DEBUG_NAMED(name_, "Starting trajectory execution ...");
1273  // assume everything will be OK
1275 
1276  // execute each trajectory, one after the other (executePart() is blocking) or until one fails.
1277  // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success)
1278  std::size_t i = 0;
1279  for (; i < trajectories_.size(); ++i)
1280  {
1281  bool epart = executePart(i);
1282  if (epart && part_callback)
1283  part_callback(i);
1284  if (!epart || execution_complete_)
1285  {
1286  ++i;
1287  break;
1288  }
1289  }
1290 
1291  // only report that execution finished when the robot stopped moving
1293 
1294  ROS_INFO_NAMED(name_, "Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str());
1295 
1296  // notify whoever is waiting for the event of trajectory completion
1297  execution_state_mutex_.lock();
1298  execution_complete_ = true;
1299  execution_state_mutex_.unlock();
1301 
1302  // clear the paths just executed, if needed
1303  if (auto_clear)
1304  clear();
1305 
1306  // call user-specified callback
1307  if (callback)
1308  callback(last_execution_status_);
1309 }
1310 
1311 bool TrajectoryExecutionManager::executePart(std::size_t part_index)
1312 {
1313  TrajectoryExecutionContext& context = *trajectories_[part_index];
1314 
1315  // first make sure desired controllers are active
1317  {
1318  // stop if we are already asked to do so
1319  if (execution_complete_)
1320  return false;
1321 
1322  std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
1323  {
1324  boost::mutex::scoped_lock slock(execution_state_mutex_);
1325  if (!execution_complete_)
1326  {
1327  // time indexing uses this member too, so we lock this mutex as well
1328  time_index_mutex_.lock();
1329  current_context_ = part_index;
1330  time_index_mutex_.unlock();
1331  active_handles_.resize(context.controllers_.size());
1332  for (std::size_t i = 0; i < context.controllers_.size(); ++i)
1333  {
1334  moveit_controller_manager::MoveItControllerHandlePtr h;
1335  try
1336  {
1337  h = controller_manager_->getControllerHandle(context.controllers_[i]);
1338  }
1339  catch (std::exception& ex)
1340  {
1341  ROS_ERROR_NAMED(name_, "Caught %s when retrieving controller handle", ex.what());
1342  }
1343  if (!h)
1344  {
1345  active_handles_.clear();
1346  current_context_ = -1;
1348  ROS_ERROR_NAMED(name_, "No controller handle for controller '%s'. Aborting.",
1349  context.controllers_[i].c_str());
1350  return false;
1351  }
1352  active_handles_[i] = h;
1353  }
1354  handles = active_handles_; // keep a copy for later, to avoid thread safety issues
1355  for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1356  {
1357  bool ok = false;
1358  try
1359  {
1360  ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
1361  }
1362  catch (std::exception& ex)
1363  {
1364  ROS_ERROR_NAMED(name_, "Caught %s when sending trajectory to controller", ex.what());
1365  }
1366  if (!ok)
1367  {
1368  for (std::size_t j = 0; j < i; ++j)
1369  try
1370  {
1371  active_handles_[j]->cancelExecution();
1372  }
1373  catch (std::exception& ex)
1374  {
1375  ROS_ERROR_NAMED(name_, "Caught %s when canceling execution", ex.what());
1376  }
1377  ROS_ERROR_NAMED(name_, "Failed to send trajectory part %zu of %zu to controller %s", i + 1,
1378  context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
1379  if (i > 0)
1380  ROS_ERROR_NAMED(name_, "Cancelling previously sent trajectory parts");
1381  active_handles_.clear();
1382  current_context_ = -1;
1384  return false;
1385  }
1386  }
1387  }
1388  }
1389 
1390  // compute the expected duration of the trajectory and find the part of the trajectory that takes longest to execute
1391  ros::Time current_time = ros::Time::now();
1392  ros::Duration expected_trajectory_duration(0.0);
1393  int longest_part = -1;
1394  for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1395  {
1396  ros::Duration d(0.0);
1397  if (!(context.trajectory_parts_[i].joint_trajectory.points.empty() &&
1398  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty()))
1399  {
1400  if (context.trajectory_parts_[i].joint_trajectory.header.stamp > current_time)
1401  d = context.trajectory_parts_[i].joint_trajectory.header.stamp - current_time;
1402  if (context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp > current_time)
1403  d = std::max(d, context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp - current_time);
1404  d += std::max(context.trajectory_parts_[i].joint_trajectory.points.empty() ?
1405  ros::Duration(0.0) :
1406  context.trajectory_parts_[i].joint_trajectory.points.back().time_from_start,
1407  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty() ?
1408  ros::Duration(0.0) :
1409  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start);
1410 
1411  if (longest_part < 0 ||
1412  std::max(context.trajectory_parts_[i].joint_trajectory.points.size(),
1413  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.size()) >
1414  std::max(context.trajectory_parts_[longest_part].joint_trajectory.points.size(),
1415  context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
1416  longest_part = i;
1417  }
1418  expected_trajectory_duration = std::max(d, expected_trajectory_duration);
1419  }
1420  // add 10% + 0.5s to the expected duration; this is just to allow things to finish propery
1421 
1422  expected_trajectory_duration = expected_trajectory_duration * allowed_execution_duration_scaling_ +
1424 
1425  // construct a map from expected time to state index, for easy access to expected state location
1426  if (longest_part >= 0)
1427  {
1428  boost::mutex::scoped_lock slock(time_index_mutex_);
1429 
1430  if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >=
1431  context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
1432  {
1433  ros::Duration d(0.0);
1434  if (context.trajectory_parts_[longest_part].joint_trajectory.header.stamp > current_time)
1435  d = context.trajectory_parts_[longest_part].joint_trajectory.header.stamp - current_time;
1436  for (std::size_t j = 0; j < context.trajectory_parts_[longest_part].joint_trajectory.points.size(); ++j)
1437  time_index_.push_back(current_time + d +
1438  context.trajectory_parts_[longest_part].joint_trajectory.points[j].time_from_start);
1439  }
1440  else
1441  {
1442  ros::Duration d(0.0);
1443  if (context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp > current_time)
1444  d = context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp - current_time;
1445  for (std::size_t j = 0; j < context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size();
1446  ++j)
1447  time_index_.push_back(
1448  current_time + d +
1449  context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points[j].time_from_start);
1450  }
1451  }
1452 
1453  bool result = true;
1454  for (std::size_t i = 0; i < handles.size(); ++i)
1455  {
1457  {
1458  if (!handles[i]->waitForExecution(expected_trajectory_duration))
1459  if (!execution_complete_ && ros::Time::now() - current_time > expected_trajectory_duration)
1460  {
1461  ROS_ERROR_NAMED(name_, "Controller is taking too long to execute trajectory (the expected upper "
1462  "bound for the trajectory execution was %lf seconds). Stopping trajectory.",
1463  expected_trajectory_duration.toSec());
1464  {
1465  boost::mutex::scoped_lock slock(execution_state_mutex_);
1466  stopExecutionInternal(); // this is really tricky. we can't call stopExecution() here, so we call the
1467  // internal function only
1468  }
1470  result = false;
1471  break;
1472  }
1473  }
1474  else
1475  handles[i]->waitForExecution();
1476 
1477  // if something made the trajectory stop, we stop this thread too
1478  if (execution_complete_)
1479  {
1480  result = false;
1481  break;
1482  }
1484  {
1485  ROS_WARN_STREAM_NAMED(name_, "Controller handle " << handles[i]->getName() << " reports status "
1486  << handles[i]->getLastExecutionStatus().asString());
1487  last_execution_status_ = handles[i]->getLastExecutionStatus();
1488  result = false;
1489  }
1490  }
1491 
1492  // clear the active handles
1493  execution_state_mutex_.lock();
1494  active_handles_.clear();
1495 
1496  // clear the time index
1497  time_index_mutex_.lock();
1498  time_index_.clear();
1499  current_context_ = -1;
1500  time_index_mutex_.unlock();
1501 
1502  execution_state_mutex_.unlock();
1503  return result;
1504  }
1505  else
1506  {
1508  return false;
1509  }
1510 }
1511 
1513 {
1514  if (allowed_start_tolerance_ == 0) // skip validation on this magic number
1515  return true;
1516 
1518  double time_remaining = wait_time;
1519 
1520  robot_state::RobotStatePtr prev_state, cur_state;
1521  prev_state = csm_->getCurrentState();
1522  prev_state->enforceBounds();
1523 
1524  // assume robot stopped when 3 consecutive checks yield the same robot state
1525  unsigned int no_motion_count = 0; // count iterations with no motion
1526  while (time_remaining > 0. && no_motion_count < 3)
1527  {
1528  if (!csm_->waitForCurrentState(ros::Time::now(), time_remaining) || !(cur_state = csm_->getCurrentState()))
1529  {
1530  ROS_WARN_NAMED(name_, "Failed to receive current joint state");
1531  return false;
1532  }
1533  cur_state->enforceBounds();
1534  time_remaining = wait_time - (ros::WallTime::now() - start).toSec(); // remaining wait_time
1535 
1536  // check for motion in effected joints of execution context
1537  bool moved = false;
1538  for (const auto& trajectory : context.trajectory_parts_)
1539  {
1540  const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
1541  const std::size_t n = joint_names.size();
1542 
1543  for (std::size_t i = 0; i < n && !moved; ++i)
1544  {
1545  const robot_model::JointModel* jm = cur_state->getJointModel(joint_names[i]);
1546  if (!jm)
1547  continue; // joint vanished from robot state (shouldn't happen), but we don't care
1548 
1549  if (fabs(cur_state->getJointPositions(jm)[0] - prev_state->getJointPositions(jm)[0]) > allowed_start_tolerance_)
1550  {
1551  moved = true;
1552  no_motion_count = 0;
1553  break;
1554  }
1555  }
1556  }
1557 
1558  if (!moved)
1559  ++no_motion_count;
1560 
1561  std::swap(prev_state, cur_state);
1562  }
1563 
1564  return time_remaining > 0;
1565 }
1566 
1568 {
1569  boost::mutex::scoped_lock slock(time_index_mutex_);
1570  if (current_context_ < 0)
1571  return std::make_pair(-1, -1);
1572  if (time_index_.empty())
1573  return std::make_pair((int)current_context_, -1);
1574  std::vector<ros::Time>::const_iterator time_index_it =
1575  std::lower_bound(time_index_.begin(), time_index_.end(), ros::Time::now());
1576  int pos = time_index_it - time_index_.begin();
1577  return std::make_pair((int)current_context_, pos);
1578 }
1579 
1580 const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>&
1582 {
1583  return trajectories_;
1584 }
1585 
1587 {
1588  return last_execution_status_;
1589 }
1590 
1592 {
1593  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group);
1594  if (joint_model_group)
1595  return ensureActiveControllersForJoints(joint_model_group->getJointModelNames());
1596  else
1597  return false;
1598 }
1599 
1600 bool TrajectoryExecutionManager::ensureActiveControllersForJoints(const std::vector<std::string>& joints)
1601 {
1602  std::vector<std::string> all_controller_names;
1603  for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1604  it != known_controllers_.end(); ++it)
1605  all_controller_names.push_back(it->first);
1606  std::vector<std::string> selected_controllers;
1607  std::set<std::string> jset;
1608  for (std::size_t i = 0; i < joints.size(); ++i)
1609  {
1610  const robot_model::JointModel* jm = robot_model_->getJointModel(joints[i]);
1611  if (jm)
1612  {
1613  if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
1614  continue;
1615  jset.insert(joints[i]);
1616  }
1617  }
1618 
1619  if (selectControllers(jset, all_controller_names, selected_controllers))
1620  return ensureActiveControllers(selected_controllers);
1621  else
1622  return false;
1623 }
1624 
1625 bool TrajectoryExecutionManager::ensureActiveController(const std::string& controller)
1626 {
1627  return ensureActiveControllers(std::vector<std::string>(1, controller));
1628 }
1629 
1630 bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector<std::string>& controllers)
1631 {
1633 
1634  if (manage_controllers_)
1635  {
1636  std::vector<std::string> controllers_to_activate;
1637  std::vector<std::string> controllers_to_deactivate;
1638  std::set<std::string> joints_to_be_activated;
1639  std::set<std::string> joints_to_be_deactivated;
1640  for (std::size_t i = 0; i < controllers.size(); ++i)
1641  {
1642  std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.find(controllers[i]);
1643  if (it == known_controllers_.end())
1644  {
1645  ROS_ERROR_STREAM_NAMED(name_, "Controller " << controllers[i] << " is not known");
1646  return false;
1647  }
1648  if (!it->second.state_.active_)
1649  {
1650  ROS_DEBUG_STREAM_NAMED(name_, "Need to activate " << controllers[i]);
1651  controllers_to_activate.push_back(controllers[i]);
1652  joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
1653  for (std::set<std::string>::iterator kt = it->second.overlapping_controllers_.begin();
1654  kt != it->second.overlapping_controllers_.end(); ++kt)
1655  {
1656  const ControllerInformation& ci = known_controllers_[*kt];
1657  if (ci.state_.active_)
1658  {
1659  controllers_to_deactivate.push_back(*kt);
1660  joints_to_be_deactivated.insert(ci.joints_.begin(), ci.joints_.end());
1661  }
1662  }
1663  }
1664  else
1665  ROS_DEBUG_STREAM_NAMED(name_, "Controller " << controllers[i] << " is already active");
1666  }
1667  std::set<std::string> diff;
1668  std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
1669  joints_to_be_activated.begin(), joints_to_be_activated.end(), std::inserter(diff, diff.end()));
1670  if (!diff.empty())
1671  {
1672  // find the set of controllers that do not overlap with the ones we want to activate so far
1673  std::vector<std::string> possible_additional_controllers;
1674  for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1675  it != known_controllers_.end(); ++it)
1676  {
1677  bool ok = true;
1678  for (std::size_t k = 0; k < controllers_to_activate.size(); ++k)
1679  if (it->second.overlapping_controllers_.find(controllers_to_activate[k]) !=
1680  it->second.overlapping_controllers_.end())
1681  {
1682  ok = false;
1683  break;
1684  }
1685  if (ok)
1686  possible_additional_controllers.push_back(it->first);
1687  }
1688 
1689  // out of the allowable controllers, try to find a subset of controllers that covers the joints to be actuated
1690  std::vector<std::string> additional_controllers;
1691  if (selectControllers(diff, possible_additional_controllers, additional_controllers))
1692  controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(),
1693  additional_controllers.end());
1694  else
1695  return false;
1696  }
1697  if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
1698  {
1699  if (controller_manager_)
1700  {
1701  // load controllers to be activated, if needed, and reset the state update cache
1702  for (std::size_t a = 0; a < controllers_to_activate.size(); ++a)
1703  {
1704  ControllerInformation& ci = known_controllers_[controllers_to_activate[a]];
1705  ci.last_update_ = ros::Time();
1706  }
1707  // reset the state update cache
1708  for (std::size_t a = 0; a < controllers_to_deactivate.size(); ++a)
1709  known_controllers_[controllers_to_deactivate[a]].last_update_ = ros::Time();
1710  return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
1711  }
1712  else
1713  return false;
1714  }
1715  else
1716  return true;
1717  }
1718  else
1719  {
1720  std::set<std::string> originally_active;
1721  for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1722  it != known_controllers_.end(); ++it)
1723  if (it->second.state_.active_)
1724  originally_active.insert(it->first);
1725  return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
1726  }
1727 }
1728 }
d
moveit_controller_manager::ExecutionStatus last_execution_status_
void notify_all() BOOST_NOEXCEPT
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_
#define ROS_DEBUG_STREAM_NAMED(name, args)
#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 ...
void wait(unique_lock< mutex > &m)
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())
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 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::vector< std::size_t > nractive
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
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
boost::function< void(const moveit_controller_manager::ExecutionStatus &)> ExecutionCompleteCallback
bool checkControllerCombination(std::vector< std::string > &controllers, const std::set< std::string > &actuated_joints)
moveit_controller_manager::MoveItControllerManager::ControllerState state_
#define ROS_DEBUG_NAMED(name,...)
std::vector< std::string > controllers_
The controllers to use for executing the different trajectory parts;.
unsigned int index
moveit_controller_manager::MoveItControllerManagerPtr controller_manager_
dynamic_reconfigure::Server< TrajectoryExecutionDynamicReconfigureConfig > dynamic_reconfigure_server_
#define ROS_FATAL_STREAM_NAMED(name, args)
std::vector< std::vector< std::string > > selected_options
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)
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 dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig &config, uint32_t level)
static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN
void processEvent(const std::string &event)
Execute a named event (e.g., &#39;stop&#39;)
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
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)
static WallTime now()
bool getParam(const std::string &key, std::string &s) const
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)
static Time now()
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.
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&#39;s start point against current robot state...
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)
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="")


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sat Apr 21 2018 02:55:20