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