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