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