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>
41 
43 {
44 const std::string TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC = "trajectory_execution_event";
45 
47 static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 0.5; // allow 0.5s more than the expected execution time
48  // before triggering a trajectory cancel (applied
49  // after scaling)
51  1.1; // allow the execution of a trajectory to take more time than expected (scaled by a value > 1)
52 
53 using namespace moveit_ros_planning;
54 
56 {
57 public:
59  : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/trajectory_execution"))
60  {
61  dynamic_reconfigure_server_.setCallback(
62  boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
63  }
64 
65 private:
66  void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig& config, uint32_t level)
67  {
68  owner_->enableExecutionDurationMonitoring(config.execution_duration_monitoring);
69  owner_->setAllowedExecutionDurationScaling(config.allowed_execution_duration_scaling);
70  owner_->setAllowedGoalDurationMargin(config.allowed_goal_duration_margin);
71  owner_->setExecutionVelocityScaling(config.execution_velocity_scaling);
72  owner_->setAllowedStartTolerance(config.allowed_start_tolerance);
73  }
74 
76  dynamic_reconfigure::Server<TrajectoryExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
77 };
78 
79 TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
80  const planning_scene_monitor::CurrentStateMonitorPtr& csm)
81  : robot_model_(kmodel), csm_(csm), node_handle_("~")
82 {
83  if (!node_handle_.getParam("moveit_manage_controllers", manage_controllers_))
84  manage_controllers_ = false;
85 
86  initialize();
87 }
88 
89 TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& kmodel,
90  const planning_scene_monitor::CurrentStateMonitorPtr& csm,
91  bool manage_controllers)
92  : robot_model_(kmodel), csm_(csm), node_handle_("~"), manage_controllers_(manage_controllers)
93 {
94  initialize();
95 }
96 
98 {
100  stopExecution(true);
101  delete reconfigure_impl_;
102 }
103 
104 static const char* DEPRECATION_WARNING =
105  "\nDeprecation warning: parameter '%s' moved into namespace 'trajectory_execution'."
106  "\nPlease, adjust file trajectory_execution.launch.xml!";
108 {
109  reconfigure_impl_ = NULL;
110  verbose_ = false;
111  execution_complete_ = true;
113  current_context_ = -1;
119 
120  // TODO: Reading from old param location should be removed in L-turtle. Handled by DynamicReconfigure.
121  if (node_handle_.getParam("allowed_execution_duration_scaling", allowed_execution_duration_scaling_))
122  ROS_WARN_NAMED("trajectory_execution_manager", DEPRECATION_WARNING, "allowed_execution_duration_scaling");
123  else
125 
126  if (node_handle_.getParam("allowed_goal_duration_margin", allowed_goal_duration_margin_))
127  ROS_WARN_NAMED("trajectory_execution_manager", DEPRECATION_WARNING, "allowed_goal_duration_margin");
128  else
130 
131  // load the controller manager plugin
132  try
133  {
135  "moveit_core", "moveit_controller_manager::MoveItControllerManager"));
136  }
138  {
139  ROS_FATAL_STREAM_NAMED("traj_execution",
140  "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("traj_execution", "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("traj_execution", "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("traj_execution", "Exception while loading controller manager '" << controller
171  << "': " << ex.what());
172  }
173  }
174 
175  // other configuration steps
177 
180 
182 
184  ROS_INFO_NAMED("traj_execution", "Trajectory execution is managing controllers");
185  else
186  ROS_INFO_NAMED("traj_execution", "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("traj_execution", "Unknown event type: '" << event << "'");
230 }
231 
232 void TrajectoryExecutionManager::receiveEvent(const std_msgs::StringConstPtr& event)
233 {
234  ROS_INFO_STREAM_NAMED("traj_execution", "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("traj_execution", "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("traj_execution", "%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("traj_execution", "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("traj_execution", "%s caught when retrieving controller handle", ex.what());
447  }
448  if (!h)
449  {
451  ROS_ERROR_NAMED("traj_execution", "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("traj_execution", "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("traj_execution", "Caught %s when canceling execution", ex.what());
488  }
489  ROS_ERROR_NAMED("traj_execution", "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("traj_execution", "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("traj_execution", "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("traj_execution", "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("traj_execution", "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("traj_execution", "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("traj_execution", "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("traj_execution",
694  "Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.",
695  controller_count, sac.str().c_str(), saj.str().c_str(), selected_options.size());
696  }
697 
698  // if none was found, this is a problem
699  if (selected_options.empty())
700  return false;
701 
702  // if only one was found, return it
703  if (selected_options.size() == 1)
704  {
705  selected_controllers.swap(selected_options[0]);
706  return true;
707  }
708 
709  // if more options were found, evaluate them all and return the best one
710 
711  // count how many default controllers are used in each reported option, and how many joints are actuated in total by
712  // the selected controllers,
713  // to use that in the ranking of the options
714  order.nrdefault.resize(selected_options.size(), 0);
715  order.nrjoints.resize(selected_options.size(), 0);
716  order.nractive.resize(selected_options.size(), 0);
717  for (std::size_t i = 0; i < selected_options.size(); ++i)
718  {
719  for (std::size_t k = 0; k < selected_options[i].size(); ++k)
720  {
722  const ControllerInformation& ci = known_controllers_[selected_options[i][k]];
723 
724  if (ci.state_.default_)
725  order.nrdefault[i]++;
726  if (ci.state_.active_)
727  order.nractive[i]++;
728  order.nrjoints[i] += ci.joints_.size();
729  }
730  }
731 
732  // define a bijection to compute the raking of the found options
733  std::vector<std::size_t> bijection(selected_options.size(), 0);
734  for (std::size_t i = 0; i < selected_options.size(); ++i)
735  bijection[i] = i;
736 
737  // sort the options
738  std::sort(bijection.begin(), bijection.end(), order);
739 
740  // depending on whether we are allowed to load & unload controllers,
741  // we have different preference on deciding between options
742  if (!manage_controllers_)
743  {
744  // if we can't load different options at will, just choose one that is already loaded
745  for (std::size_t i = 0; i < selected_options.size(); ++i)
746  if (areControllersActive(selected_options[bijection[i]]))
747  {
748  selected_controllers.swap(selected_options[bijection[i]]);
749  return true;
750  }
751  }
752 
753  // otherwise, just use the first valid option
754  selected_controllers.swap(selected_options[bijection[0]]);
755  return true;
756 }
757 
758 bool TrajectoryExecutionManager::isControllerActive(const std::string& controller)
759 {
760  return areControllersActive(std::vector<std::string>(1, controller));
761 }
762 
763 bool TrajectoryExecutionManager::areControllersActive(const std::vector<std::string>& controllers)
764 {
765  for (std::size_t i = 0; i < controllers.size(); ++i)
766  {
768  std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
769  if (it == known_controllers_.end() || !it->second.state_.active_)
770  return false;
771  }
772  return true;
773 }
774 
775 bool TrajectoryExecutionManager::selectControllers(const std::set<std::string>& actuated_joints,
776  const std::vector<std::string>& available_controllers,
777  std::vector<std::string>& selected_controllers)
778 {
779  for (std::size_t i = 1; i <= available_controllers.size(); ++i)
780  if (findControllers(actuated_joints, i, available_controllers, selected_controllers))
781  {
782  // if we are not managing controllers, prefer to use active controllers even if there are more of them
783  if (!manage_controllers_ && !areControllersActive(selected_controllers))
784  {
785  std::vector<std::string> other_option;
786  for (std::size_t j = i + 1; j <= available_controllers.size(); ++j)
787  if (findControllers(actuated_joints, j, available_controllers, other_option))
788  {
789  if (areControllersActive(other_option))
790  {
791  selected_controllers = other_option;
792  break;
793  }
794  }
795  }
796  return true;
797  }
798  return false;
799 }
800 
801 bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::RobotTrajectory& trajectory,
802  const std::vector<std::string>& controllers,
803  std::vector<moveit_msgs::RobotTrajectory>& parts)
804 {
805  parts.clear();
806  parts.resize(controllers.size());
807 
808  std::set<std::string> actuated_joints_mdof;
809  actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
810  trajectory.multi_dof_joint_trajectory.joint_names.end());
811  std::set<std::string> actuated_joints_single;
812  for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i)
813  {
814  const robot_model::JointModel* jm = robot_model_->getJointModel(trajectory.joint_trajectory.joint_names[i]);
815  if (jm)
816  {
817  if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
818  continue;
819  actuated_joints_single.insert(jm->getName());
820  }
821  }
822 
823  for (std::size_t i = 0; i < controllers.size(); ++i)
824  {
825  std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
826  if (it == known_controllers_.end())
827  {
828  ROS_ERROR_STREAM_NAMED("traj_execution", "Controller " << controllers[i] << " not found.");
829  return false;
830  }
831  std::vector<std::string> intersect_mdof;
832  std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_mdof.begin(),
833  actuated_joints_mdof.end(), std::back_inserter(intersect_mdof));
834  std::vector<std::string> intersect_single;
835  std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_single.begin(),
836  actuated_joints_single.end(), std::back_inserter(intersect_single));
837  if (intersect_mdof.empty() && intersect_single.empty())
838  ROS_WARN_STREAM_NAMED("traj_execution", "No joints to be distributed for controller " << controllers[i]);
839  {
840  if (!intersect_mdof.empty())
841  {
842  std::vector<std::string>& jnames = parts[i].multi_dof_joint_trajectory.joint_names;
843  jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
844  std::map<std::string, std::size_t> index;
845  for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
846  index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
847  std::vector<std::size_t> bijection(jnames.size());
848  for (std::size_t j = 0; j < jnames.size(); ++j)
849  bijection[j] = index[jnames[j]];
850 
851  parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
852  for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.points.size(); ++j)
853  {
854  parts[i].multi_dof_joint_trajectory.points[j].time_from_start =
855  trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
856  parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
857  for (std::size_t k = 0; k < bijection.size(); ++k)
858  {
859  parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
860  trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
861 
862  if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
863  {
864  parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());
865 
866  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
867  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.x * execution_velocity_scaling_;
868 
869  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
870  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.y * execution_velocity_scaling_;
871 
872  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
873  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.z * execution_velocity_scaling_;
874 
875  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
876  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.x * execution_velocity_scaling_;
877 
878  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
879  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.y * execution_velocity_scaling_;
880 
881  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
882  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.z * execution_velocity_scaling_;
883  }
884  }
885  }
886  }
887  if (!intersect_single.empty())
888  {
889  std::vector<std::string>& jnames = parts[i].joint_trajectory.joint_names;
890  jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
891  parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
892  std::map<std::string, std::size_t> index;
893  for (std::size_t j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j)
894  index[trajectory.joint_trajectory.joint_names[j]] = j;
895  std::vector<std::size_t> bijection(jnames.size());
896  for (std::size_t j = 0; j < jnames.size(); ++j)
897  bijection[j] = index[jnames[j]];
898  parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
899  for (std::size_t j = 0; j < trajectory.joint_trajectory.points.size(); ++j)
900  {
901  parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
902  if (!trajectory.joint_trajectory.points[j].positions.empty())
903  {
904  parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
905  for (std::size_t k = 0; k < bijection.size(); ++k)
906  parts[i].joint_trajectory.points[j].positions[k] =
907  trajectory.joint_trajectory.points[j].positions[bijection[k]];
908  }
909  if (!trajectory.joint_trajectory.points[j].velocities.empty())
910  {
911  parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
912  for (std::size_t k = 0; k < bijection.size(); ++k)
913  parts[i].joint_trajectory.points[j].velocities[k] =
914  trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
915  }
916  if (!trajectory.joint_trajectory.points[j].accelerations.empty())
917  {
918  parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
919  for (std::size_t k = 0; k < bijection.size(); ++k)
920  parts[i].joint_trajectory.points[j].accelerations[k] =
921  trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
922  }
923  if (!trajectory.joint_trajectory.points[j].effort.empty())
924  {
925  parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
926  for (std::size_t k = 0; k < bijection.size(); ++k)
927  parts[i].joint_trajectory.points[j].effort[k] =
928  trajectory.joint_trajectory.points[j].effort[bijection[k]];
929  }
930  }
931  }
932  }
933  }
934  return true;
935 }
936 
938 {
939  if (allowed_start_tolerance_ == 0) // skip validation on this magic number
940  return true;
941 
942  ROS_DEBUG_NAMED("traj_execution", "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
943 
944  robot_state::RobotStatePtr current_state;
945  if (!csm_->waitForCurrentState(ros::Time::now()) || !(current_state = csm_->getCurrentState()))
946  {
947  ROS_WARN_NAMED("traj_execution", "Failed to validate trajectory: couldn't receive full current joint state within "
948  "1s");
949  return false;
950  }
951 
952  for (const auto& trajectory : context.trajectory_parts_)
953  {
954  if (!trajectory.multi_dof_joint_trajectory.points.empty())
955  {
956  ROS_WARN_NAMED("traj_execution", "Validation of MultiDOFJointTrajectory is not implemented.");
957  // go on to check joint_trajectory component though
958  }
959  if (trajectory.joint_trajectory.points.empty())
960  {
961  // There is nothing to check
962  continue;
963  }
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  const std::size_t n = joint_names.size();
967  if (positions.size() != n)
968  {
969  ROS_ERROR_NAMED("traj_execution", "Wrong trajectory: #joints: %zu != #positions: %zu", n, positions.size());
970  return false;
971  }
972 
973  for (std::size_t i = 0; i < n; ++i)
974  {
975  const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]);
976  if (!jm)
977  {
978  ROS_ERROR_STREAM_NAMED("traj_execution", "Unknown joint in trajectory: " << joint_names[i]);
979  return false;
980  }
981 
982  double cur_position = current_state->getJointPositions(jm)[0];
983  double traj_position = positions[i];
984  // normalize positions and compare
985  jm->enforcePositionBounds(&cur_position);
986  jm->enforcePositionBounds(&traj_position);
987  if (fabs(cur_position - traj_position) > allowed_start_tolerance_)
988  {
989  ROS_ERROR_NAMED("traj_execution",
990  "\nInvalid Trajectory: start point deviates from current robot state more than %g"
991  "\njoint '%s': expected: %g, current: %g",
992  allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position);
993  return false;
994  }
995  }
996  }
997  return true;
998 }
999 
1001  const moveit_msgs::RobotTrajectory& trajectory,
1002  const std::vector<std::string>& controllers)
1003 {
1004  if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
1005  {
1006  ROS_WARN_NAMED("traj_execution", "The trajectory to execute is empty");
1007  return false;
1008  }
1009  std::set<std::string> actuated_joints;
1010  actuated_joints.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
1011  trajectory.multi_dof_joint_trajectory.joint_names.end());
1012  actuated_joints.insert(trajectory.joint_trajectory.joint_names.begin(),
1013  trajectory.joint_trajectory.joint_names.end());
1014  if (actuated_joints.empty())
1015  {
1016  ROS_WARN_NAMED("traj_execution", "The trajectory to execute specifies no joints");
1017  return false;
1018  }
1019 
1020  if (controllers.empty())
1021  {
1022  bool retry = true;
1023  bool reloaded = false;
1024  while (retry)
1025  {
1026  retry = false;
1027  std::vector<std::string> all_controller_names;
1028  for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1029  it != known_controllers_.end(); ++it)
1030  all_controller_names.push_back(it->first);
1031  if (selectControllers(actuated_joints, all_controller_names, context.controllers_))
1032  {
1033  if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1034  return true;
1035  }
1036  else
1037  {
1038  // maybe we failed because we did not have a complete list of controllers
1039  if (!reloaded)
1040  {
1042  reloaded = true;
1043  retry = true;
1044  }
1045  }
1046  }
1047  }
1048  else
1049  {
1050  // check if the specified controllers are valid names;
1051  // if they appear not to be, try to reload the controller information, just in case they are new in the system
1052  bool reloaded = false;
1053  for (std::size_t i = 0; i < controllers.size(); ++i)
1054  if (known_controllers_.find(controllers[i]) == known_controllers_.end())
1055  {
1057  reloaded = true;
1058  break;
1059  }
1060  if (reloaded)
1061  for (std::size_t i = 0; i < controllers.size(); ++i)
1062  if (known_controllers_.find(controllers[i]) == known_controllers_.end())
1063  {
1064  ROS_ERROR_NAMED("traj_execution", "Controller '%s' is not known", controllers[i].c_str());
1065  return false;
1066  }
1067  if (selectControllers(actuated_joints, controllers, context.controllers_))
1068  {
1069  if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1070  return true;
1071  }
1072  }
1073  std::stringstream ss;
1074  for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
1075  ss << *it << " ";
1076  ROS_ERROR_NAMED("traj_execution",
1077  "Unable to identify any set of controllers that can actuate the specified joints: [ %s]",
1078  ss.str().c_str());
1079 
1080  std::stringstream ss2;
1081  std::map<std::string, ControllerInformation>::const_iterator mi;
1082  for (mi = known_controllers_.begin(); mi != known_controllers_.end(); mi++)
1083  {
1084  ss2 << "controller '" << mi->second.name_ << "' controls joints:\n";
1085 
1086  std::set<std::string>::const_iterator ji;
1087  for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ji++)
1088  {
1089  ss2 << " " << *ji << std::endl;
1090  }
1091  }
1092  ROS_ERROR_NAMED("traj_execution", "Known controllers and their joints:\n%s", ss2.str().c_str());
1093  return false;
1094 }
1095 
1097 {
1098  execute(ExecutionCompleteCallback(), auto_clear);
1099  return waitForExecution();
1100 }
1101 
1103 {
1104  // execution_state_mutex_ needs to have been locked by the caller
1105  for (std::size_t i = 0; i < active_handles_.size(); ++i)
1106  try
1107  {
1108  active_handles_[i]->cancelExecution();
1109  }
1110  catch (std::exception& ex)
1111  {
1112  ROS_ERROR_NAMED("traj_execution", "Caught %s when canceling execution.", ex.what());
1113  }
1114 }
1115 
1117 {
1120 
1121  if (!execution_complete_)
1122  {
1123  execution_state_mutex_.lock();
1124  if (!execution_complete_)
1125  {
1126  // we call cancel for all active handles; we know these are not being modified as we loop through them because of
1127  // the lock
1128  // we mark execution_complete_ as true ahead of time. Using this flag, executePart() will know that an external
1129  // trigger to stop has been received
1130  execution_complete_ = true;
1132 
1133  // we set the status here; executePart() will not set status when execution_complete_ is true ahead of time
1135  execution_state_mutex_.unlock();
1136  ROS_INFO_NAMED("traj_execution", "Stopped trajectory execution.");
1137 
1138  // wait for the execution thread to finish
1139  execution_thread_->join();
1140  execution_thread_.reset();
1141 
1142  if (auto_clear)
1143  clear();
1144  }
1145  else
1146  execution_state_mutex_.unlock();
1147  }
1148  else if (execution_thread_) // just in case we have some thread waiting to be joined from some point in the past, we
1149  // join it now
1150  {
1151  execution_thread_->join();
1152  execution_thread_.reset();
1153  }
1154 }
1155 
1157 {
1158  execute(callback, PathSegmentCompleteCallback(), auto_clear);
1159 }
1160 
1162  const PathSegmentCompleteCallback& part_callback, bool auto_clear)
1163 {
1164  stopExecution(false);
1165 
1166  // check whether first trajectory starts at current robot state
1167  if (trajectories_.size() && !validate(*trajectories_.front()))
1168  {
1170  if (auto_clear)
1171  clear();
1172  if (callback)
1173  callback(last_execution_status_);
1174  return;
1175  }
1176 
1177  // start the execution thread
1178  execution_complete_ = false;
1179  execution_thread_.reset(
1180  new boost::thread(&TrajectoryExecutionManager::executeThread, this, callback, part_callback, auto_clear));
1181 }
1182 
1184 {
1185  {
1186  boost::unique_lock<boost::mutex> ulock(execution_state_mutex_);
1187  while (!execution_complete_)
1189  }
1190  {
1191  boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
1192  while (!continuous_execution_queue_.empty())
1194  }
1195 
1196  // this will join the thread for executing sequences of trajectories
1197  stopExecution(false);
1198 
1199  return last_execution_status_;
1200 }
1201 
1203 {
1204  if (execution_complete_)
1205  {
1206  for (std::size_t i = 0; i < trajectories_.size(); ++i)
1207  delete trajectories_[i];
1208  trajectories_.clear();
1209  {
1210  boost::mutex::scoped_lock slock(continuous_execution_mutex_);
1211  while (!continuous_execution_queue_.empty())
1212  {
1213  delete continuous_execution_queue_.front();
1214  continuous_execution_queue_.pop_front();
1215  }
1216  }
1217  }
1218  else
1219  ROS_ERROR_NAMED("traj_execution", "Cannot push a new trajectory while another is being executed");
1220 }
1221 
1223  const PathSegmentCompleteCallback& part_callback, bool auto_clear)
1224 {
1225  // if we already got a stop request before we even started anything, we abort
1226  if (execution_complete_)
1227  {
1229  if (callback)
1230  callback(last_execution_status_);
1231  return;
1232  }
1233 
1234  ROS_DEBUG_NAMED("traj_execution", "Starting trajectory execution ...");
1235  // assume everything will be OK
1237 
1238  // execute each trajectory, one after the other (executePart() is blocking) or until one fails.
1239  // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success)
1240  std::size_t i = 0;
1241  for (; i < trajectories_.size(); ++i)
1242  {
1243  bool epart = executePart(i);
1244  if (epart && part_callback)
1245  part_callback(i);
1246  if (!epart || execution_complete_)
1247  {
1248  ++i;
1249  break;
1250  }
1251  }
1252 
1253  // only report that execution finished when the robot stopped moving
1255 
1256  ROS_DEBUG_NAMED("traj_execution", "Completed trajectory execution with status %s ...",
1257  last_execution_status_.asString().c_str());
1258 
1259  // notify whoever is waiting for the event of trajectory completion
1260  execution_state_mutex_.lock();
1261  execution_complete_ = true;
1262  execution_state_mutex_.unlock();
1264 
1265  // clear the paths just executed, if needed
1266  if (auto_clear)
1267  clear();
1268 
1269  // call user-specified callback
1270  if (callback)
1271  callback(last_execution_status_);
1272 }
1273 
1274 bool TrajectoryExecutionManager::executePart(std::size_t part_index)
1275 {
1276  TrajectoryExecutionContext& context = *trajectories_[part_index];
1277 
1278  // first make sure desired controllers are active
1280  {
1281  // stop if we are already asked to do so
1282  if (execution_complete_)
1283  return false;
1284 
1285  std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
1286  {
1287  boost::mutex::scoped_lock slock(execution_state_mutex_);
1288  if (!execution_complete_)
1289  {
1290  // time indexing uses this member too, so we lock this mutex as well
1291  time_index_mutex_.lock();
1292  current_context_ = part_index;
1293  time_index_mutex_.unlock();
1294  active_handles_.resize(context.controllers_.size());
1295  for (std::size_t i = 0; i < context.controllers_.size(); ++i)
1296  {
1297  moveit_controller_manager::MoveItControllerHandlePtr h;
1298  try
1299  {
1300  h = controller_manager_->getControllerHandle(context.controllers_[i]);
1301  }
1302  catch (std::exception& ex)
1303  {
1304  ROS_ERROR_NAMED("traj_execution", "Caught %s when retrieving controller handle", ex.what());
1305  }
1306  if (!h)
1307  {
1308  active_handles_.clear();
1309  current_context_ = -1;
1311  ROS_ERROR_NAMED("traj_execution", "No controller handle for controller '%s'. Aborting.",
1312  context.controllers_[i].c_str());
1313  return false;
1314  }
1315  active_handles_[i] = h;
1316  }
1317  handles = active_handles_; // keep a copy for later, to avoid thread safety issues
1318  for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1319  {
1320  bool ok = false;
1321  try
1322  {
1323  ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
1324  }
1325  catch (std::exception& ex)
1326  {
1327  ROS_ERROR_NAMED("traj_execution", "Caught %s when sending trajectory to controller", ex.what());
1328  }
1329  if (!ok)
1330  {
1331  for (std::size_t j = 0; j < i; ++j)
1332  try
1333  {
1334  active_handles_[j]->cancelExecution();
1335  }
1336  catch (std::exception& ex)
1337  {
1338  ROS_ERROR_NAMED("traj_execution", "Caught %s when canceling execution", ex.what());
1339  }
1340  ROS_ERROR_NAMED("traj_execution", "Failed to send trajectory part %zu of %zu to controller %s", i + 1,
1341  context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
1342  if (i > 0)
1343  ROS_ERROR_NAMED("traj_execution", "Cancelling previously sent trajectory parts");
1344  active_handles_.clear();
1345  current_context_ = -1;
1347  return false;
1348  }
1349  }
1350  }
1351  }
1352 
1353  // compute the expected duration of the trajectory and find the part of the trajectory that takes longest to execute
1354  ros::Time current_time = ros::Time::now();
1355  ros::Duration expected_trajectory_duration(0.0);
1356  int longest_part = -1;
1357  for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1358  {
1359  ros::Duration d(0.0);
1360  if (!(context.trajectory_parts_[i].joint_trajectory.points.empty() &&
1361  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty()))
1362  {
1363  if (context.trajectory_parts_[i].joint_trajectory.header.stamp > current_time)
1364  d = context.trajectory_parts_[i].joint_trajectory.header.stamp - current_time;
1365  if (context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp > current_time)
1366  d = std::max(d, context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp - current_time);
1367  d += std::max(context.trajectory_parts_[i].joint_trajectory.points.empty() ?
1368  ros::Duration(0.0) :
1369  context.trajectory_parts_[i].joint_trajectory.points.back().time_from_start,
1370  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty() ?
1371  ros::Duration(0.0) :
1372  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start);
1373 
1374  if (longest_part < 0 ||
1375  std::max(context.trajectory_parts_[i].joint_trajectory.points.size(),
1376  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.size()) >
1377  std::max(context.trajectory_parts_[longest_part].joint_trajectory.points.size(),
1378  context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
1379  longest_part = i;
1380  }
1381  expected_trajectory_duration = std::max(d, expected_trajectory_duration);
1382  }
1383  // add 10% + 0.5s to the expected duration; this is just to allow things to finish propery
1384 
1385  expected_trajectory_duration = expected_trajectory_duration * allowed_execution_duration_scaling_ +
1387 
1388  if (longest_part >= 0)
1389  {
1390  boost::mutex::scoped_lock slock(time_index_mutex_);
1391 
1392  // construct a map from expected time to state index, for easy access to expected state location
1393  if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >=
1394  context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
1395  {
1396  ros::Duration d(0.0);
1397  if (context.trajectory_parts_[longest_part].joint_trajectory.header.stamp > current_time)
1398  d = context.trajectory_parts_[longest_part].joint_trajectory.header.stamp - current_time;
1399  for (std::size_t j = 0; j < context.trajectory_parts_[longest_part].joint_trajectory.points.size(); ++j)
1400  time_index_.push_back(current_time + d +
1401  context.trajectory_parts_[longest_part].joint_trajectory.points[j].time_from_start);
1402  }
1403  else
1404  {
1405  ros::Duration d(0.0);
1406  if (context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp > current_time)
1407  d = context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp - current_time;
1408  for (std::size_t j = 0; j < context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size();
1409  ++j)
1410  time_index_.push_back(
1411  current_time + d +
1412  context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points[j].time_from_start);
1413  }
1414  }
1415 
1416  bool result = true;
1417  for (std::size_t i = 0; i < handles.size(); ++i)
1418  {
1420  {
1421  if (!handles[i]->waitForExecution(expected_trajectory_duration))
1422  if (!execution_complete_ && ros::Time::now() - current_time > expected_trajectory_duration)
1423  {
1424  ROS_ERROR_NAMED("traj_execution", "Controller is taking too long to execute trajectory (the expected upper "
1425  "bound for the trajectory execution was %lf seconds). Stopping "
1426  "trajectory.",
1427  expected_trajectory_duration.toSec());
1428  {
1429  boost::mutex::scoped_lock slock(execution_state_mutex_);
1430  stopExecutionInternal(); // this is trally tricky. we can't call stopExecution() here, so we call the
1431  // internal function only
1432  }
1434  result = false;
1435  break;
1436  }
1437  }
1438  else
1439  handles[i]->waitForExecution();
1440 
1441  // if something made the trajectory stop, we stop this thread too
1442  if (execution_complete_)
1443  {
1444  result = false;
1445  break;
1446  }
1448  {
1449  ROS_WARN_STREAM_NAMED("traj_execution", "Controller handle "
1450  << handles[i]->getName() << " reports status "
1451  << handles[i]->getLastExecutionStatus().asString());
1452  last_execution_status_ = handles[i]->getLastExecutionStatus();
1453  result = false;
1454  }
1455  }
1456 
1457  // clear the active handles
1458  execution_state_mutex_.lock();
1459  active_handles_.clear();
1460 
1461  // clear the time index
1462  time_index_mutex_.lock();
1463  time_index_.clear();
1464  current_context_ = -1;
1465  time_index_mutex_.unlock();
1466 
1467  execution_state_mutex_.unlock();
1468  return result;
1469  }
1470  else
1471  {
1473  return false;
1474  }
1475 }
1476 
1478 {
1479  if (allowed_start_tolerance_ == 0) // skip validation on this magic number
1480  return true;
1481 
1483  double time_remaining = wait_time;
1484 
1485  robot_state::RobotStatePtr prev_state, cur_state;
1486  prev_state = csm_->getCurrentState();
1487  prev_state->enforceBounds();
1488 
1489  // assume robot stopped when 3 consecutive checks yield the same robot state
1490  unsigned int no_motion_count = 0; // count iterations with no motion
1491  while (time_remaining > 0. && no_motion_count < 3)
1492  {
1493  if (!csm_->waitForCurrentState(ros::Time::now(), time_remaining) || !(cur_state = csm_->getCurrentState()))
1494  {
1495  ROS_WARN_NAMED("traj_execution", "Failed to receive current joint state");
1496  return false;
1497  }
1498  cur_state->enforceBounds();
1499  time_remaining = wait_time - (ros::WallTime::now() - start).toSec(); // remaining wait_time
1500 
1501  // check for motion in effected joints of execution context
1502  bool moved = false;
1503  for (const auto& trajectory : context.trajectory_parts_)
1504  {
1505  const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
1506  const std::size_t n = joint_names.size();
1507 
1508  for (std::size_t i = 0; i < n && !moved; ++i)
1509  {
1510  const robot_model::JointModel* jm = cur_state->getJointModel(joint_names[i]);
1511  if (!jm)
1512  continue; // joint vanished from robot state (shouldn't happen), but we don't care
1513 
1514  if (fabs(cur_state->getJointPositions(jm)[0] - prev_state->getJointPositions(jm)[0]) > allowed_start_tolerance_)
1515  {
1516  moved = true;
1517  no_motion_count = 0;
1518  break;
1519  }
1520  }
1521  }
1522 
1523  if (!moved)
1524  ++no_motion_count;
1525 
1526  std::swap(prev_state, cur_state);
1527  }
1528 
1529  return time_remaining > 0;
1530 }
1531 
1533 {
1534  boost::mutex::scoped_lock slock(time_index_mutex_);
1535  if (current_context_ < 0)
1536  return std::make_pair(-1, -1);
1537  if (time_index_.empty())
1538  return std::make_pair((int)current_context_, -1);
1539  std::vector<ros::Time>::const_iterator it =
1540  std::lower_bound(time_index_.begin(), time_index_.end(), ros::Time::now());
1541  int pos = it - time_index_.begin();
1542  return std::make_pair((int)current_context_, pos);
1543 }
1544 
1545 const std::vector<TrajectoryExecutionManager::TrajectoryExecutionContext*>&
1547 {
1548  return trajectories_;
1549 }
1550 
1552 {
1553  return last_execution_status_;
1554 }
1555 
1557 {
1558  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group);
1559  if (joint_model_group)
1560  return ensureActiveControllersForJoints(joint_model_group->getJointModelNames());
1561  else
1562  return false;
1563 }
1564 
1565 bool TrajectoryExecutionManager::ensureActiveControllersForJoints(const std::vector<std::string>& joints)
1566 {
1567  std::vector<std::string> all_controller_names;
1568  for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1569  it != known_controllers_.end(); ++it)
1570  all_controller_names.push_back(it->first);
1571  std::vector<std::string> selected_controllers;
1572  std::set<std::string> jset;
1573  for (std::size_t i = 0; i < joints.size(); ++i)
1574  {
1575  const robot_model::JointModel* jm = robot_model_->getJointModel(joints[i]);
1576  if (jm)
1577  {
1578  if (jm->isPassive() || jm->getMimic() != NULL || jm->getType() == robot_model::JointModel::FIXED)
1579  continue;
1580  jset.insert(joints[i]);
1581  }
1582  }
1583 
1584  if (selectControllers(jset, all_controller_names, selected_controllers))
1585  return ensureActiveControllers(selected_controllers);
1586  else
1587  return false;
1588 }
1589 
1590 bool TrajectoryExecutionManager::ensureActiveController(const std::string& controller)
1591 {
1592  return ensureActiveControllers(std::vector<std::string>(1, controller));
1593 }
1594 
1595 bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector<std::string>& controllers)
1596 {
1598 
1599  if (manage_controllers_)
1600  {
1601  std::vector<std::string> controllers_to_activate;
1602  std::vector<std::string> controllers_to_deactivate;
1603  std::set<std::string> joints_to_be_activated;
1604  std::set<std::string> joints_to_be_deactivated;
1605  for (std::size_t i = 0; i < controllers.size(); ++i)
1606  {
1607  std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.find(controllers[i]);
1608  if (it == known_controllers_.end())
1609  {
1610  ROS_ERROR_STREAM_NAMED("traj_execution", "Controller " << controllers[i] << " is not known");
1611  return false;
1612  }
1613  if (!it->second.state_.active_)
1614  {
1615  ROS_DEBUG_STREAM_NAMED("traj_execution", "Need to activate " << controllers[i]);
1616  controllers_to_activate.push_back(controllers[i]);
1617  joints_to_be_activated.insert(it->second.joints_.begin(), it->second.joints_.end());
1618  for (std::set<std::string>::iterator kt = it->second.overlapping_controllers_.begin();
1619  kt != it->second.overlapping_controllers_.end(); ++kt)
1620  {
1621  const ControllerInformation& ci = known_controllers_[*kt];
1622  if (ci.state_.active_)
1623  {
1624  controllers_to_deactivate.push_back(*kt);
1625  joints_to_be_deactivated.insert(ci.joints_.begin(), ci.joints_.end());
1626  }
1627  }
1628  }
1629  else
1630  ROS_DEBUG_STREAM_NAMED("traj_execution", "Controller " << controllers[i] << " is already active");
1631  }
1632  std::set<std::string> diff;
1633  std::set_difference(joints_to_be_deactivated.begin(), joints_to_be_deactivated.end(),
1634  joints_to_be_activated.begin(), joints_to_be_activated.end(), std::inserter(diff, diff.end()));
1635  if (!diff.empty())
1636  {
1637  // find the set of controllers that do not overlap with the ones we want to activate so far
1638  std::vector<std::string> possible_additional_controllers;
1639  for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1640  it != known_controllers_.end(); ++it)
1641  {
1642  bool ok = true;
1643  for (std::size_t k = 0; k < controllers_to_activate.size(); ++k)
1644  if (it->second.overlapping_controllers_.find(controllers_to_activate[k]) !=
1645  it->second.overlapping_controllers_.end())
1646  {
1647  ok = false;
1648  break;
1649  }
1650  if (ok)
1651  possible_additional_controllers.push_back(it->first);
1652  }
1653 
1654  // out of the allowable controllers, try to find a subset of controllers that covers the joints to be actuated
1655  std::vector<std::string> additional_controllers;
1656  if (selectControllers(diff, possible_additional_controllers, additional_controllers))
1657  controllers_to_activate.insert(controllers_to_activate.end(), additional_controllers.begin(),
1658  additional_controllers.end());
1659  else
1660  return false;
1661  }
1662  if (!controllers_to_activate.empty() || !controllers_to_deactivate.empty())
1663  {
1664  if (controller_manager_)
1665  {
1666  // load controllers to be activated, if needed, and reset the state update cache
1667  for (std::size_t a = 0; a < controllers_to_activate.size(); ++a)
1668  {
1669  ControllerInformation& ci = known_controllers_[controllers_to_activate[a]];
1670  ci.last_update_ = ros::Time();
1671  }
1672  // reset the state update cache
1673  for (std::size_t a = 0; a < controllers_to_deactivate.size(); ++a)
1674  known_controllers_[controllers_to_deactivate[a]].last_update_ = ros::Time();
1675  return controller_manager_->switchControllers(controllers_to_activate, controllers_to_deactivate);
1676  }
1677  else
1678  return false;
1679  }
1680  else
1681  return true;
1682  }
1683  else
1684  {
1685  std::set<std::string> originally_active;
1686  for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1687  it != known_controllers_.end(); ++it)
1688  if (it->second.state_.active_)
1689  originally_active.insert(it->first);
1690  return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
1691  }
1692 }
1693 }
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...
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 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 Mon Jan 15 2018 03:51:18