trajectory_execution_manager.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
39 #include <moveit_ros_planning/TrajectoryExecutionDynamicReconfigureConfig.h>
40 #include <dynamic_reconfigure/server.h>
41 #include <tf2_eigen/tf2_eigen.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& robot_model,
82  const planning_scene_monitor::CurrentStateMonitorPtr& csm)
83  : robot_model_(robot_model), 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& robot_model,
92  const planning_scene_monitor::CurrentStateMonitorPtr& csm,
93  bool manage_controllers)
94  : robot_model_(robot_model), csm_(csm), node_handle_("~"), manage_controllers_(manage_controllers)
95 {
96  initialize();
97 }
98 
100 {
102  stopExecution(true);
103  delete reconfigure_impl_;
104 }
105 
107 {
108  reconfigure_impl_ = nullptr;
109  verbose_ = false;
110  execution_complete_ = true;
112  current_context_ = -1;
118 
121 
122  // load controller-specific values for allowed_execution_duration_scaling and allowed_goal_duration_margin
124 
125  // load the controller manager plugin
126  try
127  {
129  "moveit_core", "moveit_controller_manager::MoveItControllerManager"));
130  }
132  {
133  ROS_FATAL_STREAM_NAMED(name_, "Exception while creating controller manager plugin loader: " << ex.what());
134  return;
135  }
136 
138  {
139  std::string controller;
140  if (!node_handle_.getParam("moveit_controller_manager", controller))
141  {
142  const std::vector<std::string>& classes = controller_manager_loader_->getDeclaredClasses();
143  if (classes.size() == 1)
144  {
145  controller = classes[0];
147  "Parameter '~moveit_controller_manager' is not specified but only one "
148  "matching plugin was found: '%s'. Using that one.",
149  controller.c_str());
150  }
151  else
152  ROS_FATAL_NAMED(name_, "Parameter '~moveit_controller_manager' not specified. This is needed to "
153  "identify the plugin to use for interacting with controllers. No paths can "
154  "be executed.");
155  }
156 
157  if (!controller.empty())
158  try
159  {
160  controller_manager_ = controller_manager_loader_->createUniqueInstance(controller);
161  }
163  {
165  "Exception while loading controller manager '" << controller << "': " << ex.what());
166  }
167  }
168 
169  // other configuration steps
171 
174 
176 
178  ROS_INFO_NAMED(name_, "Trajectory execution is managing controllers");
179  else
180  ROS_INFO_NAMED(name_, "Trajectory execution is not managing controllers");
181 }
182 
184 {
186 }
187 
189 {
191 }
192 
194 {
196 }
197 
199 {
200  execution_velocity_scaling_ = scaling;
201 }
202 
204 {
205  allowed_start_tolerance_ = tolerance;
206 }
207 
209 {
211 }
212 
214 {
215  return manage_controllers_;
216 }
217 
218 const moveit_controller_manager::MoveItControllerManagerPtr& TrajectoryExecutionManager::getControllerManager() const
219 {
220  return controller_manager_;
221 }
222 
223 void TrajectoryExecutionManager::processEvent(const std::string& event)
224 {
225  if (event == "stop")
226  stopExecution(true);
227  else
228  ROS_WARN_STREAM_NAMED(name_, "Unknown event type: '" << event << "'");
229 }
230 
231 void TrajectoryExecutionManager::receiveEvent(const std_msgs::StringConstPtr& event)
232 {
233  ROS_INFO_STREAM_NAMED(name_, "Received event '" << event->data << "'");
234  processEvent(event->data);
235 }
236 
237 bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory& trajectory, const std::string& controller)
238 {
239  if (controller.empty())
240  return push(trajectory, std::vector<std::string>());
241  else
242  return push(trajectory, std::vector<std::string>(1, controller));
243 }
244 
245 bool TrajectoryExecutionManager::push(const trajectory_msgs::JointTrajectory& trajectory, const std::string& controller)
246 {
247  if (controller.empty())
248  return push(trajectory, std::vector<std::string>());
249  else
250  return push(trajectory, std::vector<std::string>(1, controller));
251 }
252 
253 bool TrajectoryExecutionManager::push(const trajectory_msgs::JointTrajectory& trajectory,
254  const std::vector<std::string>& controllers)
255 {
256  moveit_msgs::RobotTrajectory traj;
257  traj.joint_trajectory = trajectory;
258  return push(traj, controllers);
259 }
260 
261 bool TrajectoryExecutionManager::push(const moveit_msgs::RobotTrajectory& trajectory,
262  const std::vector<std::string>& controllers)
263 {
264  if (!execution_complete_)
265  {
266  ROS_ERROR_NAMED(name_, "Cannot push a new trajectory while another is being executed");
267  return false;
268  }
269 
271  if (configure(*context, trajectory, controllers))
272  {
273  if (verbose_)
274  {
275  std::stringstream ss;
276  ss << "Pushed trajectory for execution using controllers [ ";
277  for (std::size_t i = 0; i < context->controllers_.size(); ++i)
278  ss << context->controllers_[i] << " ";
279  ss << "]:" << std::endl;
280  for (std::size_t i = 0; i < context->trajectory_parts_.size(); ++i)
281  ss << context->trajectory_parts_[i] << std::endl;
282  ROS_INFO_NAMED(name_, "%s", ss.str().c_str());
283  }
284  trajectories_.push_back(context);
285  return true;
286  }
287  else
288  {
289  delete context;
291  }
292 
293  return false;
294 }
295 
296 bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajectory& trajectory,
297  const std::string& controller)
298 {
299  if (controller.empty())
300  return pushAndExecute(trajectory, std::vector<std::string>());
301  else
302  return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
303 }
304 
305 bool TrajectoryExecutionManager::pushAndExecute(const trajectory_msgs::JointTrajectory& trajectory,
306  const std::string& controller)
307 {
308  if (controller.empty())
309  return pushAndExecute(trajectory, std::vector<std::string>());
310  else
311  return pushAndExecute(trajectory, std::vector<std::string>(1, controller));
312 }
313 
314 bool TrajectoryExecutionManager::pushAndExecute(const sensor_msgs::JointState& state, const std::string& controller)
315 {
316  if (controller.empty())
317  return pushAndExecute(state, std::vector<std::string>());
318  else
319  return pushAndExecute(state, std::vector<std::string>(1, controller));
320 }
321 
322 bool TrajectoryExecutionManager::pushAndExecute(const trajectory_msgs::JointTrajectory& trajectory,
323  const std::vector<std::string>& controllers)
324 {
325  moveit_msgs::RobotTrajectory traj;
326  traj.joint_trajectory = trajectory;
327  return pushAndExecute(traj, controllers);
328 }
329 
330 bool TrajectoryExecutionManager::pushAndExecute(const sensor_msgs::JointState& state,
331  const std::vector<std::string>& controllers)
332 {
333  moveit_msgs::RobotTrajectory traj;
334  traj.joint_trajectory.header = state.header;
335  traj.joint_trajectory.joint_names = state.name;
336  traj.joint_trajectory.points.resize(1);
337  traj.joint_trajectory.points[0].positions = state.position;
338  traj.joint_trajectory.points[0].velocities = state.velocity;
339  traj.joint_trajectory.points[0].effort = state.effort;
340  traj.joint_trajectory.points[0].time_from_start = ros::Duration(0, 0);
341  return pushAndExecute(traj, controllers);
342 }
343 
344 bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajectory& trajectory,
345  const std::vector<std::string>& controllers)
346 {
347  if (!execution_complete_)
348  {
349  ROS_ERROR_NAMED(name_, "Cannot push & execute a new trajectory while another is being executed");
350  return false;
351  }
352 
354  if (configure(*context, trajectory, controllers))
355  {
356  {
357  boost::mutex::scoped_lock slock(continuous_execution_mutex_);
358  continuous_execution_queue_.push_back(context);
361  new boost::thread(boost::bind(&TrajectoryExecutionManager::continuousExecutionThread, this)));
362  }
364  continuous_execution_condition_.notify_all();
365  return true;
366  }
367  else
368  {
369  delete context;
371  return false;
372  }
373 }
374 
376 {
377  std::set<moveit_controller_manager::MoveItControllerHandlePtr> used_handles;
379  {
381  {
382  boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
385  }
386 
388  {
389  for (std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin();
390  uit != used_handles.end(); ++uit)
391  if ((*uit)->getLastExecutionStatus() == moveit_controller_manager::ExecutionStatus::RUNNING)
392  (*uit)->cancelExecution();
393  used_handles.clear();
394  while (!continuous_execution_queue_.empty())
395  {
397  continuous_execution_queue_.pop_front();
398  delete context;
399  }
401  continue;
402  }
403 
404  while (!continuous_execution_queue_.empty())
405  {
406  TrajectoryExecutionContext* context = nullptr;
407  {
408  boost::mutex::scoped_lock slock(continuous_execution_mutex_);
409  if (continuous_execution_queue_.empty())
410  break;
411  context = continuous_execution_queue_.front();
412  continuous_execution_queue_.pop_front();
413  if (continuous_execution_queue_.empty())
414  continuous_execution_condition_.notify_all();
415  }
416 
417  // remove handles we no longer need
418  std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator uit = used_handles.begin();
419  while (uit != used_handles.end())
420  if ((*uit)->getLastExecutionStatus() != moveit_controller_manager::ExecutionStatus::RUNNING)
421  {
422  std::set<moveit_controller_manager::MoveItControllerHandlePtr>::iterator to_erase = uit;
423  ++uit;
424  used_handles.erase(to_erase);
425  }
426  else
427  ++uit;
428 
429  // now send stuff to controllers
430 
431  // first make sure desired controllers are active
432  if (areControllersActive(context->controllers_))
433  {
434  // get the controller handles needed to execute the new trajectory
435  std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles(context->controllers_.size());
436  for (std::size_t i = 0; i < context->controllers_.size(); ++i)
437  {
438  moveit_controller_manager::MoveItControllerHandlePtr h;
439  try
440  {
441  h = controller_manager_->getControllerHandle(context->controllers_[i]);
442  }
443  catch (std::exception& ex)
444  {
445  ROS_ERROR_NAMED(name_, "%s caught when retrieving controller handle", ex.what());
446  }
447  if (!h)
448  {
450  ROS_ERROR_NAMED(name_, "No controller handle for controller '%s'. Aborting.",
451  context->controllers_[i].c_str());
452  handles.clear();
453  break;
454  }
455  handles[i] = h;
456  }
457 
459  {
460  delete context;
461  break;
462  }
463 
464  // push all trajectories to all controllers simultaneously
465  if (!handles.empty())
466  for (std::size_t i = 0; i < context->trajectory_parts_.size(); ++i)
467  {
468  bool ok = false;
469  try
470  {
471  ok = handles[i]->sendTrajectory(context->trajectory_parts_[i]);
472  }
473  catch (std::exception& ex)
474  {
475  ROS_ERROR_NAMED(name_, "Caught %s when sending trajectory to controller", ex.what());
476  }
477  if (!ok)
478  {
479  for (std::size_t j = 0; j < i; ++j)
480  try
481  {
482  handles[j]->cancelExecution();
483  }
484  catch (std::exception& ex)
485  {
486  ROS_ERROR_NAMED(name_, "Caught %s when canceling execution", ex.what());
487  }
488  ROS_ERROR_NAMED(name_, "Failed to send trajectory part %zu of %zu to controller %s", i + 1,
489  context->trajectory_parts_.size(), handles[i]->getName().c_str());
490  if (i > 0)
491  ROS_ERROR_NAMED(name_, "Cancelling previously sent trajectory parts");
493  handles.clear();
494  break;
495  }
496  }
497  delete context;
498 
499  // remember which handles we used
500  for (std::size_t i = 0; i < handles.size(); ++i)
501  used_handles.insert(handles[i]);
502  }
503  else
504  {
505  ROS_ERROR_NAMED(name_, "Not all needed controllers are active. Cannot push and execute. You can try "
506  "calling ensureActiveControllers() before pushAndExecute()");
508  delete context;
509  }
510  }
511  }
512 }
513 
515 {
516  known_controllers_.clear();
518  {
519  std::vector<std::string> names;
520  controller_manager_->getControllersList(names);
521  for (std::size_t i = 0; i < names.size(); ++i)
522  {
523  std::vector<std::string> joints;
524  controller_manager_->getControllerJoints(names[i], joints);
526  ci.name_ = names[i];
527  ci.joints_.insert(joints.begin(), joints.end());
528  known_controllers_[ci.name_] = ci;
529  }
530 
531  for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
532  it != known_controllers_.end(); ++it)
533  for (std::map<std::string, ControllerInformation>::iterator jt = known_controllers_.begin();
534  jt != known_controllers_.end(); ++jt)
535  if (it != jt)
536  {
537  std::vector<std::string> intersect;
538  std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), jt->second.joints_.begin(),
539  jt->second.joints_.end(), std::back_inserter(intersect));
540  if (!intersect.empty())
541  {
542  it->second.overlapping_controllers_.insert(jt->first);
543  jt->second.overlapping_controllers_.insert(it->first);
544  }
545  }
546  }
547 }
548 
549 void TrajectoryExecutionManager::updateControllerState(const std::string& controller, const ros::Duration& age)
550 {
551  std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controller);
552  if (it != known_controllers_.end())
553  updateControllerState(it->second, age);
554  else
555  ROS_ERROR_NAMED(name_, "Controller '%s' is not known.", controller.c_str());
556 }
557 
559 {
560  if (ros::Time::now() - ci.last_update_ >= age)
561  {
563  {
564  if (verbose_)
565  ROS_INFO_NAMED(name_, "Updating information for controller '%s'.", ci.name_.c_str());
566  ci.state_ = controller_manager_->getControllerState(ci.name_);
568  }
569  }
570  else if (verbose_)
571  ROS_INFO_NAMED(name_, "Information for controller '%s' is assumed to be up to date.", ci.name_.c_str());
572 }
573 
575 {
576  for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
577  it != known_controllers_.end(); ++it)
578  updateControllerState(it->second, age);
579 }
580 
581 bool TrajectoryExecutionManager::checkControllerCombination(std::vector<std::string>& selected,
582  const std::set<std::string>& actuated_joints)
583 {
584  std::set<std::string> combined_joints;
585  for (std::size_t i = 0; i < selected.size(); ++i)
586  {
587  const ControllerInformation& ci = known_controllers_[selected[i]];
588  combined_joints.insert(ci.joints_.begin(), ci.joints_.end());
589  }
590 
591  if (verbose_)
592  {
593  std::stringstream ss, saj, sac;
594  for (std::size_t i = 0; i < selected.size(); ++i)
595  ss << selected[i] << " ";
596  for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
597  saj << *it << " ";
598  for (std::set<std::string>::const_iterator it = combined_joints.begin(); it != combined_joints.end(); ++it)
599  sac << *it << " ";
600  ROS_INFO_NAMED(name_, "Checking if controllers [ %s] operating on joints [ %s] cover joints [ %s]",
601  ss.str().c_str(), sac.str().c_str(), saj.str().c_str());
602  }
603 
604  return std::includes(combined_joints.begin(), combined_joints.end(), actuated_joints.begin(), actuated_joints.end());
605 }
606 
607 void TrajectoryExecutionManager::generateControllerCombination(std::size_t start_index, std::size_t controller_count,
608  const std::vector<std::string>& available_controllers,
609  std::vector<std::string>& selected_controllers,
610  std::vector<std::vector<std::string> >& selected_options,
611  const std::set<std::string>& actuated_joints)
612 {
613  if (selected_controllers.size() == controller_count)
614  {
615  if (checkControllerCombination(selected_controllers, actuated_joints))
616  selected_options.push_back(selected_controllers);
617  return;
618  }
619 
620  for (std::size_t i = start_index; i < available_controllers.size(); ++i)
621  {
622  bool overlap = false;
623  const ControllerInformation& ci = known_controllers_[available_controllers[i]];
624  for (std::size_t j = 0; j < selected_controllers.size() && !overlap; ++j)
625  {
626  if (ci.overlapping_controllers_.find(selected_controllers[j]) != ci.overlapping_controllers_.end())
627  overlap = true;
628  }
629  if (overlap)
630  continue;
631  selected_controllers.push_back(available_controllers[i]);
632  generateControllerCombination(i + 1, controller_count, available_controllers, selected_controllers,
633  selected_options, actuated_joints);
634  selected_controllers.pop_back();
635  }
636 }
637 
638 namespace
639 {
640 struct OrderPotentialControllerCombination
641 {
642  bool operator()(const std::size_t a, const std::size_t b) const
643  {
644  // preference is given to controllers marked as default
645  if (nrdefault[a] > nrdefault[b])
646  return true;
647  if (nrdefault[a] < nrdefault[b])
648  return false;
649 
650  // and then to ones that operate on fewer joints
651  if (nrjoints[a] < nrjoints[b])
652  return true;
653  if (nrjoints[a] > nrjoints[b])
654  return false;
655 
656  // and then to active ones
657  if (nractive[a] < nractive[b])
658  return true;
659  if (nractive[a] > nractive[b])
660  return false;
661 
662  return false;
663  }
664 
665  std::vector<std::vector<std::string> > selected_options;
666  std::vector<std::size_t> nrdefault;
667  std::vector<std::size_t> nrjoints;
668  std::vector<std::size_t> nractive;
669 };
670 } // namespace
671 
672 bool TrajectoryExecutionManager::findControllers(const std::set<std::string>& actuated_joints,
673  std::size_t controller_count,
674  const std::vector<std::string>& available_controllers,
675  std::vector<std::string>& selected_controllers)
676 {
677  // generate all combinations of controller_count controllers that operate on disjoint sets of joints
678  std::vector<std::string> work_area;
679  OrderPotentialControllerCombination order;
680  std::vector<std::vector<std::string> >& selected_options = order.selected_options;
681  generateControllerCombination(0, controller_count, available_controllers, work_area, selected_options,
682  actuated_joints);
683 
684  if (verbose_)
685  {
686  std::stringstream saj;
687  std::stringstream sac;
688  for (std::size_t i = 0; i < available_controllers.size(); ++i)
689  sac << available_controllers[i] << " ";
690  for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
691  saj << *it << " ";
692  ROS_INFO_NAMED(name_, "Looking for %zu controllers among [ %s] that cover joints [ %s]. Found %zd options.",
693  controller_count, sac.str().c_str(), saj.str().c_str(), selected_options.size());
694  }
695 
696  // if none was found, this is a problem
697  if (selected_options.empty())
698  return false;
699 
700  // if only one was found, return it
701  if (selected_options.size() == 1)
702  {
703  selected_controllers.swap(selected_options[0]);
704  return true;
705  }
706 
707  // if more options were found, evaluate them all and return the best one
708 
709  // count how many default controllers are used in each reported option, and how many joints are actuated in total by
710  // the selected controllers,
711  // to use that in the ranking of the options
712  order.nrdefault.resize(selected_options.size(), 0);
713  order.nrjoints.resize(selected_options.size(), 0);
714  order.nractive.resize(selected_options.size(), 0);
715  for (std::size_t i = 0; i < selected_options.size(); ++i)
716  {
717  for (std::size_t k = 0; k < selected_options[i].size(); ++k)
718  {
720  const ControllerInformation& ci = known_controllers_[selected_options[i][k]];
721 
722  if (ci.state_.default_)
723  order.nrdefault[i]++;
724  if (ci.state_.active_)
725  order.nractive[i]++;
726  order.nrjoints[i] += ci.joints_.size();
727  }
728  }
729 
730  // define a bijection to compute the raking of the found options
731  std::vector<std::size_t> bijection(selected_options.size(), 0);
732  for (std::size_t i = 0; i < selected_options.size(); ++i)
733  bijection[i] = i;
734 
735  // sort the options
736  std::sort(bijection.begin(), bijection.end(), order);
737 
738  // depending on whether we are allowed to load & unload controllers,
739  // we have different preference on deciding between options
740  if (!manage_controllers_)
741  {
742  // if we can't load different options at will, just choose one that is already loaded
743  for (std::size_t i = 0; i < selected_options.size(); ++i)
744  if (areControllersActive(selected_options[bijection[i]]))
745  {
746  selected_controllers.swap(selected_options[bijection[i]]);
747  return true;
748  }
749  }
750 
751  // otherwise, just use the first valid option
752  selected_controllers.swap(selected_options[bijection[0]]);
753  return true;
754 }
755 
756 bool TrajectoryExecutionManager::isControllerActive(const std::string& controller)
757 {
758  return areControllersActive(std::vector<std::string>(1, controller));
759 }
760 
761 bool TrajectoryExecutionManager::areControllersActive(const std::vector<std::string>& controllers)
762 {
763  for (std::size_t i = 0; i < controllers.size(); ++i)
764  {
766  std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
767  if (it == known_controllers_.end() || !it->second.state_.active_)
768  return false;
769  }
770  return true;
771 }
772 
773 bool TrajectoryExecutionManager::selectControllers(const std::set<std::string>& actuated_joints,
774  const std::vector<std::string>& available_controllers,
775  std::vector<std::string>& selected_controllers)
776 {
777  for (std::size_t i = 1; i <= available_controllers.size(); ++i)
778  if (findControllers(actuated_joints, i, available_controllers, selected_controllers))
779  {
780  // if we are not managing controllers, prefer to use active controllers even if there are more of them
781  if (!manage_controllers_ && !areControllersActive(selected_controllers))
782  {
783  std::vector<std::string> other_option;
784  for (std::size_t j = i + 1; j <= available_controllers.size(); ++j)
785  if (findControllers(actuated_joints, j, available_controllers, other_option))
786  {
787  if (areControllersActive(other_option))
788  {
789  selected_controllers = other_option;
790  break;
791  }
792  }
793  }
794  return true;
795  }
796  return false;
797 }
798 
799 bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::RobotTrajectory& trajectory,
800  const std::vector<std::string>& controllers,
801  std::vector<moveit_msgs::RobotTrajectory>& parts)
802 {
803  parts.clear();
804  parts.resize(controllers.size());
805 
806  std::set<std::string> actuated_joints_mdof;
807  actuated_joints_mdof.insert(trajectory.multi_dof_joint_trajectory.joint_names.begin(),
808  trajectory.multi_dof_joint_trajectory.joint_names.end());
809  std::set<std::string> actuated_joints_single;
810  for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i)
811  {
812  const robot_model::JointModel* jm = robot_model_->getJointModel(trajectory.joint_trajectory.joint_names[i]);
813  if (jm)
814  {
815  if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == robot_model::JointModel::FIXED)
816  continue;
817  actuated_joints_single.insert(jm->getName());
818  }
819  }
820 
821  for (std::size_t i = 0; i < controllers.size(); ++i)
822  {
823  std::map<std::string, ControllerInformation>::iterator it = known_controllers_.find(controllers[i]);
824  if (it == known_controllers_.end())
825  {
826  ROS_ERROR_STREAM_NAMED(name_, "Controller " << controllers[i] << " not found.");
827  return false;
828  }
829  std::vector<std::string> intersect_mdof;
830  std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_mdof.begin(),
831  actuated_joints_mdof.end(), std::back_inserter(intersect_mdof));
832  std::vector<std::string> intersect_single;
833  std::set_intersection(it->second.joints_.begin(), it->second.joints_.end(), actuated_joints_single.begin(),
834  actuated_joints_single.end(), std::back_inserter(intersect_single));
835  if (intersect_mdof.empty() && intersect_single.empty())
836  ROS_WARN_STREAM_NAMED(name_, "No joints to be distributed for controller " << controllers[i]);
837  {
838  if (!intersect_mdof.empty())
839  {
840  std::vector<std::string>& jnames = parts[i].multi_dof_joint_trajectory.joint_names;
841  jnames.insert(jnames.end(), intersect_mdof.begin(), intersect_mdof.end());
842  std::map<std::string, std::size_t> index;
843  for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
844  index[trajectory.multi_dof_joint_trajectory.joint_names[j]] = j;
845  std::vector<std::size_t> bijection(jnames.size());
846  for (std::size_t j = 0; j < jnames.size(); ++j)
847  bijection[j] = index[jnames[j]];
848 
849  parts[i].multi_dof_joint_trajectory.points.resize(trajectory.multi_dof_joint_trajectory.points.size());
850  for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.points.size(); ++j)
851  {
852  parts[i].multi_dof_joint_trajectory.points[j].time_from_start =
853  trajectory.multi_dof_joint_trajectory.points[j].time_from_start;
854  parts[i].multi_dof_joint_trajectory.points[j].transforms.resize(bijection.size());
855  for (std::size_t k = 0; k < bijection.size(); ++k)
856  {
857  parts[i].multi_dof_joint_trajectory.points[j].transforms[k] =
858  trajectory.multi_dof_joint_trajectory.points[j].transforms[bijection[k]];
859 
860  if (!trajectory.multi_dof_joint_trajectory.points[j].velocities.empty())
861  {
862  parts[i].multi_dof_joint_trajectory.points[j].velocities.resize(bijection.size());
863 
864  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.x =
865  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.x * execution_velocity_scaling_;
866 
867  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.y =
868  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.y * execution_velocity_scaling_;
869 
870  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].linear.z =
871  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].linear.z * execution_velocity_scaling_;
872 
873  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.x =
874  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.x * execution_velocity_scaling_;
875 
876  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.y =
877  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.y * execution_velocity_scaling_;
878 
879  parts[i].multi_dof_joint_trajectory.points[j].velocities[0].angular.z =
880  trajectory.multi_dof_joint_trajectory.points[j].velocities[0].angular.z * execution_velocity_scaling_;
881  }
882  }
883  }
884  }
885  if (!intersect_single.empty())
886  {
887  std::vector<std::string>& jnames = parts[i].joint_trajectory.joint_names;
888  jnames.insert(jnames.end(), intersect_single.begin(), intersect_single.end());
889  parts[i].joint_trajectory.header = trajectory.joint_trajectory.header;
890  std::map<std::string, std::size_t> index;
891  for (std::size_t j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j)
892  index[trajectory.joint_trajectory.joint_names[j]] = j;
893  std::vector<std::size_t> bijection(jnames.size());
894  for (std::size_t j = 0; j < jnames.size(); ++j)
895  bijection[j] = index[jnames[j]];
896  parts[i].joint_trajectory.points.resize(trajectory.joint_trajectory.points.size());
897  for (std::size_t j = 0; j < trajectory.joint_trajectory.points.size(); ++j)
898  {
899  parts[i].joint_trajectory.points[j].time_from_start = trajectory.joint_trajectory.points[j].time_from_start;
900  if (!trajectory.joint_trajectory.points[j].positions.empty())
901  {
902  parts[i].joint_trajectory.points[j].positions.resize(bijection.size());
903  for (std::size_t k = 0; k < bijection.size(); ++k)
904  parts[i].joint_trajectory.points[j].positions[k] =
905  trajectory.joint_trajectory.points[j].positions[bijection[k]];
906  }
907  if (!trajectory.joint_trajectory.points[j].velocities.empty())
908  {
909  parts[i].joint_trajectory.points[j].velocities.resize(bijection.size());
910  for (std::size_t k = 0; k < bijection.size(); ++k)
911  parts[i].joint_trajectory.points[j].velocities[k] =
912  trajectory.joint_trajectory.points[j].velocities[bijection[k]] * execution_velocity_scaling_;
913  }
914  if (!trajectory.joint_trajectory.points[j].accelerations.empty())
915  {
916  parts[i].joint_trajectory.points[j].accelerations.resize(bijection.size());
917  for (std::size_t k = 0; k < bijection.size(); ++k)
918  parts[i].joint_trajectory.points[j].accelerations[k] =
919  trajectory.joint_trajectory.points[j].accelerations[bijection[k]];
920  }
921  if (!trajectory.joint_trajectory.points[j].effort.empty())
922  {
923  parts[i].joint_trajectory.points[j].effort.resize(bijection.size());
924  for (std::size_t k = 0; k < bijection.size(); ++k)
925  parts[i].joint_trajectory.points[j].effort[k] =
926  trajectory.joint_trajectory.points[j].effort[bijection[k]];
927  }
928  }
929  }
930  }
931  }
932  return true;
933 }
934 
936 {
937  if (allowed_start_tolerance_ == 0) // skip validation on this magic number
938  return true;
939 
940  ROS_DEBUG_NAMED(name_, "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
941 
942  robot_state::RobotStatePtr current_state;
943  if (!csm_->waitForCurrentState(ros::Time::now()) || !(current_state = csm_->getCurrentState()))
944  {
945  ROS_WARN_NAMED(name_, "Failed to validate trajectory: couldn't receive full current joint state within 1s");
946  return false;
947  }
948 
949  for (const auto& trajectory : context.trajectory_parts_)
950  {
951  if (!trajectory.joint_trajectory.points.empty())
952  {
953  // Check single-dof trajectory
954  const std::vector<double>& positions = trajectory.joint_trajectory.points.front().positions;
955  const std::vector<std::string>& joint_names = trajectory.joint_trajectory.joint_names;
956  if (positions.size() != joint_names.size())
957  {
958  ROS_ERROR_NAMED(name_, "Wrong trajectory: #joints: %zu != #positions: %zu", joint_names.size(),
959  positions.size());
960  return false;
961  }
962 
963  for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
964  {
965  const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]);
966  if (!jm)
967  {
968  ROS_ERROR_STREAM_NAMED(name_, "Unknown joint in trajectory: " << joint_names[i]);
969  return false;
970  }
971 
972  double cur_position = current_state->getJointPositions(jm)[0];
973  double traj_position = positions[i];
974  // normalize positions and compare
975  jm->enforcePositionBounds(&cur_position);
976  jm->enforcePositionBounds(&traj_position);
977  if (jm->distance(&cur_position, &traj_position) > allowed_start_tolerance_)
978  {
980  "\nInvalid Trajectory: start point deviates from current robot state more than %g"
981  "\njoint '%s': expected: %g, current: %g",
982  allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position);
983  return false;
984  }
985  }
986  }
987  if (!trajectory.multi_dof_joint_trajectory.points.empty())
988  {
989  // Check multi-dof trajectory
990  const std::vector<geometry_msgs::Transform>& transforms =
991  trajectory.multi_dof_joint_trajectory.points.front().transforms;
992  const std::vector<std::string>& joint_names = trajectory.multi_dof_joint_trajectory.joint_names;
993  if (transforms.size() != joint_names.size())
994  {
995  ROS_ERROR_NAMED(name_, "Wrong trajectory: #joints: %zu != #transforms: %zu", joint_names.size(),
996  transforms.size());
997  return false;
998  }
999 
1000  for (std::size_t i = 0, end = joint_names.size(); i < end; ++i)
1001  {
1002  const robot_model::JointModel* jm = current_state->getJointModel(joint_names[i]);
1003  if (!jm)
1004  {
1005  ROS_ERROR_STREAM_NAMED(name_, "Unknown joint in trajectory: " << joint_names[i]);
1006  return false;
1007  }
1008 
1009  // compute difference (offset vector and rotation angle) between current transform
1010  // and start transform in trajectory
1011  Eigen::Isometry3d cur_transform, start_transform;
1012  jm->computeTransform(current_state->getJointPositions(jm), cur_transform);
1013  start_transform = tf2::transformToEigen(transforms[i]);
1014  Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation();
1015  Eigen::AngleAxisd rotation;
1016  rotation.fromRotationMatrix(cur_transform.rotation().transpose() * start_transform.rotation());
1017  if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_)
1018  {
1019  ROS_ERROR_STREAM_NAMED(name_, "\nInvalid Trajectory: start point deviates from current robot state more than "
1020  << allowed_start_tolerance_ << "\nmulti-dof joint '" << joint_names[i]
1021  << "': pos delta: " << offset.transpose()
1022  << " rot delta: " << rotation.angle());
1023  return false;
1024  }
1025  }
1026  }
1027  }
1028  return true;
1029 }
1030 
1032  const moveit_msgs::RobotTrajectory& trajectory,
1033  const std::vector<std::string>& controllers)
1034 {
1035  if (trajectory.multi_dof_joint_trajectory.points.empty() && trajectory.joint_trajectory.points.empty())
1036  {
1037  // empty trajectories don't need to configure anything
1038  return true;
1039  }
1040  std::set<std::string> actuated_joints;
1041 
1042  auto is_actuated = [this](const std::string& joint_name) -> bool {
1043  const robot_model::JointModel* jm = robot_model_->getJointModel(joint_name);
1044  return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != robot_model::JointModel::FIXED);
1045  };
1046  for (const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names)
1047  if (is_actuated(joint_name))
1048  actuated_joints.insert(joint_name);
1049  for (const std::string& joint_name : trajectory.joint_trajectory.joint_names)
1050  if (is_actuated(joint_name))
1051  actuated_joints.insert(joint_name);
1052 
1053  if (actuated_joints.empty())
1054  {
1055  ROS_WARN_NAMED(name_, "The trajectory to execute specifies no joints");
1056  return false;
1057  }
1058 
1059  if (controllers.empty())
1060  {
1061  bool retry = true;
1062  bool reloaded = false;
1063  while (retry)
1064  {
1065  retry = false;
1066  std::vector<std::string> all_controller_names;
1067  for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
1068  it != known_controllers_.end(); ++it)
1069  all_controller_names.push_back(it->first);
1070  if (selectControllers(actuated_joints, all_controller_names, context.controllers_))
1071  {
1072  if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1073  return true;
1074  }
1075  else
1076  {
1077  // maybe we failed because we did not have a complete list of controllers
1078  if (!reloaded)
1079  {
1081  reloaded = true;
1082  retry = true;
1083  }
1084  }
1085  }
1086  }
1087  else
1088  {
1089  // check if the specified controllers are valid names;
1090  // if they appear not to be, try to reload the controller information, just in case they are new in the system
1091  bool reloaded = false;
1092  for (std::size_t i = 0; i < controllers.size(); ++i)
1093  if (known_controllers_.find(controllers[i]) == known_controllers_.end())
1094  {
1096  reloaded = true;
1097  break;
1098  }
1099  if (reloaded)
1100  for (std::size_t i = 0; i < controllers.size(); ++i)
1101  if (known_controllers_.find(controllers[i]) == known_controllers_.end())
1102  {
1103  ROS_ERROR_NAMED(name_, "Controller '%s' is not known", controllers[i].c_str());
1104  return false;
1105  }
1106  if (selectControllers(actuated_joints, controllers, context.controllers_))
1107  {
1108  if (distributeTrajectory(trajectory, context.controllers_, context.trajectory_parts_))
1109  return true;
1110  }
1111  }
1112  std::stringstream ss;
1113  for (std::set<std::string>::const_iterator it = actuated_joints.begin(); it != actuated_joints.end(); ++it)
1114  ss << *it << " ";
1115  ROS_ERROR_NAMED(name_, "Unable to identify any set of controllers that can actuate the specified joints: [ %s]",
1116  ss.str().c_str());
1117 
1118  std::stringstream ss2;
1119  std::map<std::string, ControllerInformation>::const_iterator mi;
1120  for (mi = known_controllers_.begin(); mi != known_controllers_.end(); mi++)
1121  {
1122  ss2 << "controller '" << mi->second.name_ << "' controls joints:\n";
1123 
1124  std::set<std::string>::const_iterator ji;
1125  for (ji = mi->second.joints_.begin(); ji != mi->second.joints_.end(); ji++)
1126  {
1127  ss2 << " " << *ji << std::endl;
1128  }
1129  }
1130  ROS_ERROR_NAMED(name_, "Known controllers and their joints:\n%s", ss2.str().c_str());
1131  return false;
1132 }
1133 
1135 {
1136  execute(ExecutionCompleteCallback(), auto_clear);
1137  return waitForExecution();
1138 }
1139 
1141 {
1142  // execution_state_mutex_ needs to have been locked by the caller
1143  for (std::size_t i = 0; i < active_handles_.size(); ++i)
1144  try
1145  {
1146  active_handles_[i]->cancelExecution();
1147  }
1148  catch (std::exception& ex)
1149  {
1150  ROS_ERROR_NAMED(name_, "Caught %s when canceling execution.", ex.what());
1151  }
1152 }
1153 
1155 {
1157  continuous_execution_condition_.notify_all();
1158 
1159  if (!execution_complete_)
1160  {
1161  execution_state_mutex_.lock();
1162  if (!execution_complete_)
1163  {
1164  // we call cancel for all active handles; we know these are not being modified as we loop through them because of
1165  // the lock
1166  // we mark execution_complete_ as true ahead of time. Using this flag, executePart() will know that an external
1167  // trigger to stop has been received
1168  execution_complete_ = true;
1170 
1171  // we set the status here; executePart() will not set status when execution_complete_ is true ahead of time
1173  execution_state_mutex_.unlock();
1174  ROS_INFO_NAMED(name_, "Stopped trajectory execution.");
1175 
1176  // wait for the execution thread to finish
1177  boost::mutex::scoped_lock lock(execution_thread_mutex_);
1178  if (execution_thread_)
1179  {
1180  execution_thread_->join();
1181  execution_thread_.reset();
1182  }
1183 
1184  if (auto_clear)
1185  clear();
1186  }
1187  else
1188  execution_state_mutex_.unlock();
1189  }
1190  else if (execution_thread_) // just in case we have some thread waiting to be joined from some point in the past, we
1191  // join it now
1192  {
1193  boost::mutex::scoped_lock lock(execution_thread_mutex_);
1194  if (execution_thread_)
1195  {
1196  execution_thread_->join();
1197  execution_thread_.reset();
1198  }
1199  }
1200 }
1201 
1203 {
1204  execute(callback, PathSegmentCompleteCallback(), auto_clear);
1205 }
1206 
1208  const PathSegmentCompleteCallback& part_callback, bool auto_clear)
1209 {
1210  stopExecution(false);
1211 
1212  // check whether first trajectory starts at current robot state
1213  if (!trajectories_.empty() && !validate(*trajectories_.front()))
1214  {
1216  if (auto_clear)
1217  clear();
1218  if (callback)
1219  callback(last_execution_status_);
1220  return;
1221  }
1222 
1223  // start the execution thread
1224  execution_complete_ = false;
1225  execution_thread_.reset(
1226  new boost::thread(&TrajectoryExecutionManager::executeThread, this, callback, part_callback, auto_clear));
1227 }
1228 
1230 {
1231  {
1232  boost::unique_lock<boost::mutex> ulock(execution_state_mutex_);
1233  while (!execution_complete_)
1234  execution_complete_condition_.wait(ulock);
1235  }
1236  {
1237  boost::unique_lock<boost::mutex> ulock(continuous_execution_mutex_);
1238  while (!continuous_execution_queue_.empty())
1239  continuous_execution_condition_.wait(ulock);
1240  }
1241 
1242  // this will join the thread for executing sequences of trajectories
1243  stopExecution(false);
1244 
1245  return last_execution_status_;
1246 }
1247 
1249 {
1250  if (execution_complete_)
1251  {
1252  for (std::size_t i = 0; i < trajectories_.size(); ++i)
1253  delete trajectories_[i];
1254  trajectories_.clear();
1255  {
1256  boost::mutex::scoped_lock slock(continuous_execution_mutex_);
1257  while (!continuous_execution_queue_.empty())
1258  {
1259  delete continuous_execution_queue_.front();
1260  continuous_execution_queue_.pop_front();
1261  }
1262  }
1263  }
1264  else
1265  ROS_ERROR_NAMED(name_, "Cannot push a new trajectory while another is being executed");
1266 }
1267 
1269  const PathSegmentCompleteCallback& part_callback, bool auto_clear)
1270 {
1271  // if we already got a stop request before we even started anything, we abort
1272  if (execution_complete_)
1273  {
1275  if (callback)
1276  callback(last_execution_status_);
1277  return;
1278  }
1279 
1280  ROS_DEBUG_NAMED(name_, "Starting trajectory execution ...");
1281  // assume everything will be OK
1283 
1284  // execute each trajectory, one after the other (executePart() is blocking) or until one fails.
1285  // on failure, the status is set by executePart(). Otherwise, it will remain as set above (success)
1286  std::size_t i = 0;
1287  for (; i < trajectories_.size(); ++i)
1288  {
1289  bool epart = executePart(i);
1290  if (epart && part_callback)
1291  part_callback(i);
1292  if (!epart || execution_complete_)
1293  {
1294  ++i;
1295  break;
1296  }
1297  }
1298 
1299  // only report that execution finished successfully when the robot actually stopped moving
1302 
1303  ROS_INFO_NAMED(name_, "Completed trajectory execution with status %s ...", last_execution_status_.asString().c_str());
1304 
1305  // notify whoever is waiting for the event of trajectory completion
1306  execution_state_mutex_.lock();
1307  execution_complete_ = true;
1308  execution_state_mutex_.unlock();
1309  execution_complete_condition_.notify_all();
1310 
1311  // clear the paths just executed, if needed
1312  if (auto_clear)
1313  clear();
1314 
1315  // call user-specified callback
1316  if (callback)
1317  callback(last_execution_status_);
1318 }
1319 
1320 bool TrajectoryExecutionManager::executePart(std::size_t part_index)
1321 {
1322  TrajectoryExecutionContext& context = *trajectories_[part_index];
1323 
1324  // first make sure desired controllers are active
1326  {
1327  // stop if we are already asked to do so
1328  if (execution_complete_)
1329  return false;
1330 
1331  std::vector<moveit_controller_manager::MoveItControllerHandlePtr> handles;
1332  {
1333  boost::mutex::scoped_lock slock(execution_state_mutex_);
1334  if (!execution_complete_)
1335  {
1336  // time indexing uses this member too, so we lock this mutex as well
1337  time_index_mutex_.lock();
1338  current_context_ = part_index;
1339  time_index_mutex_.unlock();
1340  active_handles_.resize(context.controllers_.size());
1341  for (std::size_t i = 0; i < context.controllers_.size(); ++i)
1342  {
1343  moveit_controller_manager::MoveItControllerHandlePtr h;
1344  try
1345  {
1346  h = controller_manager_->getControllerHandle(context.controllers_[i]);
1347  }
1348  catch (std::exception& ex)
1349  {
1350  ROS_ERROR_NAMED(name_, "Caught %s when retrieving controller handle", ex.what());
1351  }
1352  if (!h)
1353  {
1354  active_handles_.clear();
1355  current_context_ = -1;
1357  ROS_ERROR_NAMED(name_, "No controller handle for controller '%s'. Aborting.",
1358  context.controllers_[i].c_str());
1359  return false;
1360  }
1361  active_handles_[i] = h;
1362  }
1363  handles = active_handles_; // keep a copy for later, to avoid thread safety issues
1364  for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1365  {
1366  bool ok = false;
1367  try
1368  {
1369  ok = active_handles_[i]->sendTrajectory(context.trajectory_parts_[i]);
1370  }
1371  catch (std::exception& ex)
1372  {
1373  ROS_ERROR_NAMED(name_, "Caught %s when sending trajectory to controller", ex.what());
1374  }
1375  if (!ok)
1376  {
1377  for (std::size_t j = 0; j < i; ++j)
1378  try
1379  {
1380  active_handles_[j]->cancelExecution();
1381  }
1382  catch (std::exception& ex)
1383  {
1384  ROS_ERROR_NAMED(name_, "Caught %s when canceling execution", ex.what());
1385  }
1386  ROS_ERROR_NAMED(name_, "Failed to send trajectory part %zu of %zu to controller %s", i + 1,
1387  context.trajectory_parts_.size(), active_handles_[i]->getName().c_str());
1388  if (i > 0)
1389  ROS_ERROR_NAMED(name_, "Cancelling previously sent trajectory parts");
1390  active_handles_.clear();
1391  current_context_ = -1;
1393  return false;
1394  }
1395  }
1396  }
1397  }
1398 
1399  // compute the expected duration of the trajectory and find the part of the trajectory that takes longest to execute
1400  ros::Time current_time = ros::Time::now();
1401  ros::Duration expected_trajectory_duration(0.0);
1402  int longest_part = -1;
1403  for (std::size_t i = 0; i < context.trajectory_parts_.size(); ++i)
1404  {
1405  ros::Duration d(0.0);
1406  if (!(context.trajectory_parts_[i].joint_trajectory.points.empty() &&
1407  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty()))
1408  {
1409  if (context.trajectory_parts_[i].joint_trajectory.header.stamp > current_time)
1410  d = context.trajectory_parts_[i].joint_trajectory.header.stamp - current_time;
1411  if (context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp > current_time)
1412  d = std::max(d, context.trajectory_parts_[i].multi_dof_joint_trajectory.header.stamp - current_time);
1413  d += std::max(context.trajectory_parts_[i].joint_trajectory.points.empty() ?
1414  ros::Duration(0.0) :
1415  context.trajectory_parts_[i].joint_trajectory.points.back().time_from_start,
1416  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.empty() ?
1417  ros::Duration(0.0) :
1418  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.back().time_from_start);
1419 
1420  if (longest_part < 0 ||
1421  std::max(context.trajectory_parts_[i].joint_trajectory.points.size(),
1422  context.trajectory_parts_[i].multi_dof_joint_trajectory.points.size()) >
1423  std::max(context.trajectory_parts_[longest_part].joint_trajectory.points.size(),
1424  context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size()))
1425  longest_part = i;
1426  }
1427 
1428  // prefer controller-specific values over global ones if defined
1429  // TODO: the controller-specific parameters are static, but override
1430  // the global ones are configurable via dynamic reconfigure
1431  std::map<std::string, double>::const_iterator scaling_it =
1433  const double current_scaling = scaling_it != controller_allowed_execution_duration_scaling_.end() ?
1434  scaling_it->second :
1436 
1437  std::map<std::string, double>::const_iterator margin_it =
1439  const double current_margin = margin_it != controller_allowed_goal_duration_margin_.end() ?
1440  margin_it->second :
1442 
1443  // expected duration is the duration of the longest part
1444  expected_trajectory_duration =
1445  std::max(d * current_scaling + ros::Duration(current_margin), expected_trajectory_duration);
1446  }
1447 
1448  // construct a map from expected time to state index, for easy access to expected state location
1449  if (longest_part >= 0)
1450  {
1451  boost::mutex::scoped_lock slock(time_index_mutex_);
1452 
1453  if (context.trajectory_parts_[longest_part].joint_trajectory.points.size() >=
1454  context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size())
1455  {
1456  ros::Duration d(0.0);
1457  if (context.trajectory_parts_[longest_part].joint_trajectory.header.stamp > current_time)
1458  d = context.trajectory_parts_[longest_part].joint_trajectory.header.stamp - current_time;
1459  for (std::size_t j = 0; j < context.trajectory_parts_[longest_part].joint_trajectory.points.size(); ++j)
1460  time_index_.push_back(current_time + d +
1461  context.trajectory_parts_[longest_part].joint_trajectory.points[j].time_from_start);
1462  }
1463  else
1464  {
1465  ros::Duration d(0.0);
1466  if (context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp > current_time)
1467  d = context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.header.stamp - current_time;
1468  for (std::size_t j = 0; j < context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points.size();
1469  ++j)
1470  time_index_.push_back(
1471  current_time + d +
1472  context.trajectory_parts_[longest_part].multi_dof_joint_trajectory.points[j].time_from_start);
1473  }
1474  }
1475 
1476  bool result = true;
1477  for (std::size_t i = 0; i < handles.size(); ++i)
1478  {
1480  {
1481  if (!handles[i]->waitForExecution(expected_trajectory_duration))
1482  if (!execution_complete_ && ros::Time::now() - current_time > expected_trajectory_duration)
1483  {
1485  "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() != nullptr || 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 } // namespace trajectory_execution_manager
d
moveit_controller_manager::ExecutionStatus last_execution_status_
std::deque< TrajectoryExecutionContext * > continuous_execution_queue_
#define ROS_INFO_NAMED(name,...)
std::unique_ptr< pluginlib::ClassLoader< moveit_controller_manager::MoveItControllerManager > > controller_manager_loader_
#define ROS_DEBUG_STREAM_NAMED(name, args)
ROSCPP_DECL void start()
#define ROS_ERROR_STREAM_NAMED(name, args)
#define ROS_WARN_NAMED(name,...)
bool validate(const TrajectoryExecutionContext &context) const
Validate first point of trajectory matches current robot state.
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 ...
bool ensureActiveController(const std::string &controller)
Make sure a particular controller is active.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void generateControllerCombination(std::size_t start_index, std::size_t controller_count, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers, std::vector< std::vector< std::string > > &selected_options, const std::set< std::string > &actuated_joints)
bool waitForRobotToStop(const TrajectoryExecutionContext &context, double wait_time=1.0)
std::vector< std::size_t > nrdefault
std::vector< std::size_t > nractive
bool isManagingControllers() const
If this function returns true, then this instance of the manager is allowed to load/unload/switch con...
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="")
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
bool ensureActiveControllers(const std::vector< std::string > &controllers)
Make sure a particular set of controllers are active.
std::map< std::string, ControllerInformation > known_controllers_
const std::vector< TrajectoryExecutionContext * > & getTrajectories() const
Get the trajectories to be executed.
#define ROS_INFO_STREAM_NAMED(name, args)
static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING
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
Type const & getType() const
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 getParam(const std::string &key, std::string &s) const
bool ensureActiveControllersForGroup(const std::string &group)
Make sure the active controllers are such that trajectories that actuate joints in the specified grou...
bool findControllers(const std::set< std::string > &actuated_joints, std::size_t controller_count, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers)
ROSCPP_DECL bool ok()
moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const
Return the controller status for the last attempted execution.
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...
TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &robot_model, const planning_scene_monitor::CurrentStateMonitorPtr &csm)
Load the controller manager plugin, start listening for events on a topic.
void dynamicReconfigureCallback(TrajectoryExecutionDynamicReconfigureConfig &config, uint32_t level)
static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN
void processEvent(const std::string &event)
Execute a named event (e.g., &#39;stop&#39;)
void executeThread(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear)
bool configure(TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &controllers)
std::vector< std::size_t > nrjoints
~TrajectoryExecutionManager()
Destructor. Cancels all running trajectories (if any)
static WallTime now()
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_
const moveit_controller_manager::MoveItControllerManagerPtr & getControllerManager() const
Get the instance of the controller manager used (this is the plugin instance loaded) ...
void stopExecution(bool auto_clear=true)
Stop whatever executions are active, if any.
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 hasMember(const std::string &name) const
bool selectControllers(const std::set< std::string > &actuated_joints, const std::vector< std::string > &available_controllers, std::vector< std::string > &selected_controllers)
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
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 Tue Sep 8 2020 04:15:10