plan_execution.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 
41 #include <boost/algorithm/string/join.hpp>
42 
43 #include <dynamic_reconfigure/server.h>
44 #include <moveit_ros_planning/PlanExecutionDynamicReconfigureConfig.h>
45 
46 namespace plan_execution
47 {
48 using namespace moveit_ros_planning;
49 
51 {
52 public:
54  : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/plan_execution"))
55  {
56  dynamic_reconfigure_server_.setCallback(
57  boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
58  }
59 
60 private:
61  void dynamicReconfigureCallback(PlanExecutionDynamicReconfigureConfig& config, uint32_t level)
62  {
63  owner_->setMaxReplanAttempts(config.max_replan_attempts);
64  owner_->setTrajectoryStateRecordingFrequency(config.record_trajectory_state_frequency);
65  }
66 
68  dynamic_reconfigure::Server<PlanExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
69 };
70 }
71 
73  const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
74  const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution)
75  : node_handle_("~")
76  , planning_scene_monitor_(planning_scene_monitor)
77  , trajectory_execution_manager_(trajectory_execution)
78 {
81  planning_scene_monitor_->getRobotModel(), planning_scene_monitor_->getStateMonitor()));
82 
84 
85  preempt_requested_ = false;
86  new_scene_update_ = false;
87 
88  // we want to be notified when new information is available
89  planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, _1));
90 
91  // start the dynamic-reconfigure server
93 }
94 
96 {
97  delete reconfigure_impl_;
98 }
99 
101 {
102  preempt_requested_ = true;
103 }
104 
105 std::string plan_execution::PlanExecution::getErrorCodeString(const moveit_msgs::MoveItErrorCodes& error_code)
106 {
107  if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
108  return "Success";
109  else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME)
110  return "Invalid group name";
111  else if (error_code.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED)
112  return "Planning failed.";
113  else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN)
114  return "Invalid motion plan";
115  else if (error_code.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
116  return "Unable to aquire sensor data";
117  else if (error_code.val == moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
118  return "Motion plan invalidated by environment change";
119  else if (error_code.val == moveit_msgs::MoveItErrorCodes::CONTROL_FAILED)
120  return "Controller failed during execution";
121  else if (error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT)
122  return "Timeout reached";
123  else if (error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
124  return "Preempted";
125  else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS)
126  return "Invalid goal constraints";
127  else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME)
128  return "Invalid object name";
129  else if (error_code.val == moveit_msgs::MoveItErrorCodes::FAILURE)
130  return "Catastrophic failure";
131  return "Unknown event";
132 }
133 
135 {
137  plan.planning_scene_ = planning_scene_monitor_->getPlanningScene();
138  planAndExecuteHelper(plan, opt);
139 }
140 
142  const moveit_msgs::PlanningScene& scene_diff, const Options& opt)
143 {
145  planAndExecute(plan, opt);
146  else
147  {
149  {
150  planning_scene_monitor::LockedPlanningSceneRO lscene(planning_scene_monitor_); // lock the scene so that it does
151  // not modify the world
152  // representation while diff() is
153  // called
154  plan.planning_scene_ = lscene->diff(scene_diff);
155  }
156  planAndExecuteHelper(plan, opt);
157  }
158 }
159 
161 {
162  // perform initial configuration steps & various checks
163  preempt_requested_ = false;
164 
165  // run the actual motion plan & execution
166  unsigned int max_replan_attempts =
168  unsigned int replan_attempts = 0;
169  bool previously_solved = false;
170 
171  // run a planning loop for at most the maximum replanning attempts;
172  // re-planning is executed only in case of known types of failures (e.g., environment changed)
173  do
174  {
175  replan_attempts++;
176  ROS_INFO_NAMED("plan_execution", "Planning attempt %u of at most %u", replan_attempts, max_replan_attempts);
177 
178  if (opt.before_plan_callback_)
179  opt.before_plan_callback_();
180 
181  new_scene_update_ = false; // we clear any scene updates to be evaluated because we are about to compute a new
182  // plan, which should consider most recent updates already
183 
184  // if we never had a solved plan, or there is no specified way of fixing plans, just call the planner; otherwise,
185  // try to repair the plan we previously had;
186  bool solved =
187  (!previously_solved || !opt.repair_plan_callback_) ?
188  opt.plan_callback_(plan) :
189  opt.repair_plan_callback_(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex());
190 
191  if (preempt_requested_)
192  break;
193 
194  // if planning fails in a manner that is not recoverable, we exit the loop,
195  // otherwise, we attempt to continue, if replanning attempts are left
196  if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED ||
197  plan.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN ||
198  plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
199  {
200  if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA &&
201  opt.replan_delay_ > 0.0)
202  {
204  d.sleep();
205  }
206  continue;
207  }
208 
209  // abort if no plan was found
210  if (solved)
211  previously_solved = true;
212  else
213  break;
214 
215  if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
216  {
219 
220  if (preempt_requested_)
221  break;
222 
223  // execute the trajectory, and monitor its executionm
224  plan.error_code_ = executeAndMonitor(plan);
225  }
226 
227  // if we are done, then we exit the loop
228  if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
229  break;
230 
231  // if execution failed in a manner that we do not consider recoverable, we exit the loop (with failure)
232  if (plan.error_code_.val != moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
233  break;
234  else
235  {
236  // othewrise, we wait (if needed)
237  if (opt.replan_delay_ > 0.0)
238  {
239  ROS_INFO_NAMED("plan_execution", "Waiting for a %lf seconds before attempting a new plan ...",
240  opt.replan_delay_);
242  d.sleep();
243  ROS_INFO_NAMED("plan_execution", "Done waiting");
244  }
245  }
246  } while (!preempt_requested_ && max_replan_attempts > replan_attempts);
247 
248  if (preempt_requested_)
249  {
250  ROS_DEBUG_NAMED("plan_execution", "PlanExecution was preempted");
251  plan.error_code_.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
252  }
253 
254  if (opt.done_callback_)
255  opt.done_callback_();
256 
257  if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
258  ROS_DEBUG_NAMED("plan_execution", "PlanExecution finished successfully.");
259  else
260  ROS_DEBUG_NAMED("plan_execution", "PlanExecution terminating with error code %d - '%s'", plan.error_code_.val,
261  getErrorCodeString(plan.error_code_).c_str());
262 }
263 
265 {
266  // check the validity of the currently executed path segment only, since there could be
267  // changes in the world in between path segments
268  return isRemainingPathValid(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex());
269 }
270 
272  const std::pair<int, int>& path_segment)
273 {
274  if (path_segment.first >= 0 &&
275  plan.plan_components_[path_segment.first].trajectory_monitoring_) // If path_segment.second <= 0, the function
276  // will fallback to check the entire trajectory
277  {
278  planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_); // lock the scene so that it
279  // does not modify the world
280  // representation while
281  // isStateValid() is called
282  const robot_trajectory::RobotTrajectory& t = *plan.plan_components_[path_segment.first].trajectory_;
284  plan.plan_components_[path_segment.first].allowed_collision_matrix_.get();
285  std::size_t wpc = t.getWayPointCount();
287  req.group_name = t.getGroupName();
288  for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
289  {
291  if (acm)
292  plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm);
293  else
294  plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i));
295 
296  if (res.collision || !plan.planning_scene_->isStateFeasible(t.getWayPoint(i), false))
297  {
298  // Dave's debacle
299  ROS_INFO_NAMED("plan_execution", "Trajectory component '%s' is invalid",
300  plan.plan_components_[path_segment.first].description_.c_str());
301 
302  // call the same functions again, in verbose mode, to show what issues have been detected
303  plan.planning_scene_->isStateFeasible(t.getWayPoint(i), true);
304  req.verbose = true;
305  res.clear();
306  if (acm)
307  plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm);
308  else
309  plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i));
310  return false;
311  }
312  }
313  }
314  return true;
315 }
316 
318 {
319  moveit_msgs::MoveItErrorCodes result;
320 
321  // try to execute the trajectory
322  execution_complete_ = true;
323 
325  {
326  ROS_ERROR_NAMED("plan_execution", "No trajectory execution manager");
327  result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
328  return result;
329  }
330 
331  if (plan.plan_components_.empty())
332  {
333  result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
334  return result;
335  }
336 
337  execution_complete_ = false;
338 
339  // push the trajectories we have slated for execution to the trajectory execution manager
340  int prev = -1;
341  for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
342  {
343  if (!plan.plan_components_[i].trajectory_ || plan.plan_components_[i].trajectory_->empty())
344  continue;
345 
346  // \todo should this be in trajectory_execution ? Maybe. Then that will have to use kinematic_trajectory too;
347  // spliting trajectories for controllers becomes interesting: tied to groups instead of joints. this could cause
348  // some problems
349  // in the meantime we do a hack:
350 
351  bool unwound = false;
352  for (std::size_t j = 0; j < i; ++j)
353  // if we ran unwind on a path for the same group
354  if (plan.plan_components_[j].trajectory_ &&
355  plan.plan_components_[j].trajectory_->getGroup() == plan.plan_components_[i].trajectory_->getGroup() &&
356  !plan.plan_components_[j].trajectory_->empty())
357  {
358  plan.plan_components_[i].trajectory_->unwind(plan.plan_components_[j].trajectory_->getLastWayPoint());
359  unwound = true;
360  break;
361  }
362 
363  if (!unwound)
364  {
365  // unwind the path to execute based on the current state of the system
366  if (prev < 0)
367  plan.plan_components_[i].trajectory_->unwind(
368  plan.planning_scene_monitor_ && plan.planning_scene_monitor_->getStateMonitor() ?
369  *plan.planning_scene_monitor_->getStateMonitor()->getCurrentState() :
370  plan.planning_scene_->getCurrentState());
371  else
372  plan.plan_components_[i].trajectory_->unwind(plan.plan_components_[prev].trajectory_->getLastWayPoint());
373  }
374 
375  prev = i;
376 
377  // convert to message, pass along
378  moveit_msgs::RobotTrajectory msg;
379  plan.plan_components_[i].trajectory_->getRobotTrajectoryMsg(msg);
380  if (!trajectory_execution_manager_->push(msg))
381  {
383  ROS_ERROR_STREAM_NAMED("plan_execution", "Apparently trajectory initialization failed");
384  execution_complete_ = true;
385  result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
386  return result;
387  }
388  }
389 
390  if (!trajectory_monitor_ && planning_scene_monitor_->getStateMonitor())
391  trajectory_monitor_.reset(
393 
394  // start recording trajectory states
396  trajectory_monitor_->startTrajectoryMonitor();
397 
398  // start a trajectory execution thread
400  boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1),
401  boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, _1));
402  // wait for path to be done, while checking that the path does not become invalid
403  ros::Rate r(100);
404  path_became_invalid_ = false;
406  {
407  r.sleep();
408  // check the path if there was an environment update in the meantime
409  if (new_scene_update_)
410  {
411  new_scene_update_ = false;
412  if (!isRemainingPathValid(plan))
413  {
414  path_became_invalid_ = true;
415  break;
416  }
417  }
418  }
419 
420  // stop execution if needed
421  if (preempt_requested_)
422  {
423  ROS_INFO_NAMED("plan_execution", "Stopping execution due to preempt request");
424  trajectory_execution_manager_->stopExecution();
425  }
426  else if (path_became_invalid_)
427  {
428  ROS_INFO_NAMED("plan_execution", "Stopping execution because the path to execute became invalid"
429  "(probably the environment changed)");
430  trajectory_execution_manager_->stopExecution();
431  }
432  else if (!execution_complete_)
433  {
434  ROS_WARN_NAMED("plan_execution", "Stopping execution due to unknown reason."
435  "Possibly the node is about to shut down.");
436  trajectory_execution_manager_->stopExecution();
437  }
438 
439  // stop recording trajectory states
441  trajectory_monitor_->stopTrajectoryMonitor();
442 
443  // decide return value
445  result.val = moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE;
446  else
447  {
448  if (preempt_requested_)
449  {
450  result.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
451  }
452  else
453  {
454  if (trajectory_execution_manager_->getLastExecutionStatus() ==
456  result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
457  else if (trajectory_execution_manager_->getLastExecutionStatus() ==
459  result.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
460  else
461  result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
462  }
463  }
464  return result;
465 }
466 
469 {
472  new_scene_update_ = true;
473 }
474 
477 {
478  execution_complete_ = true;
479 }
480 
482  std::size_t index)
483 {
484  if (plan->plan_components_.size() == 0)
485  {
486  ROS_WARN_NAMED("plan_execution", "Length of provided motion plan is zero.");
487  return;
488  }
489 
490  // if any side-effects are associated to the trajectory part that just completed, execute them
491  ROS_DEBUG_NAMED("plan_execution", "Completed '%s'", plan->plan_components_[index].description_.c_str());
492  if (plan->plan_components_[index].effect_on_success_)
493  if (!plan->plan_components_[index].effect_on_success_(plan))
494  {
495  // execution of side-effect failed
496  ROS_ERROR_NAMED("plan_execution", "Execution of path-completion side-effect failed. Preempting.");
497  preempt_requested_ = true;
498  return;
499  }
500 
501  // if there is a next trajectory, check it for validity, before we start execution
502  std::size_t test_index = index;
503  while (++test_index < plan->plan_components_.size())
504  if (plan->plan_components_[test_index].trajectory_ && !plan->plan_components_[test_index].trajectory_->empty())
505  {
506  if (!isRemainingPathValid(*plan, std::make_pair<int>(test_index, 0)))
507  path_became_invalid_ = true;
508  break;
509  }
510 }
d
#define ROS_INFO_NAMED(name,...)
#define ROS_ERROR_STREAM_NAMED(name, args)
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
The geometry of the scene was updated. This includes receiving new octomaps, collision objects...
#define ROS_WARN_NAMED(name,...)
const std::string & getGroupName() const
moveit_msgs::MoveItErrorCodes error_code_
An error code reflecting what went wrong (if anything)
void planAndExecuteHelper(ExecutableMotionPlan &plan, const Options &opt)
boost::function< bool(ExecutableMotionPlan &plan_to_update, const std::pair< int, int > &trajectory_index)> repair_plan_callback_
void dynamicReconfigureCallback(PlanExecutionDynamicReconfigureConfig &config, uint32_t level)
planning_scene_monitor::TrajectoryMonitorPtr trajectory_monitor_
trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
void planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
#define ROS_DEBUG_NAMED(name,...)
moveit_msgs::MoveItErrorCodes executeAndMonitor(const ExecutableMotionPlan &plan)
planning_scene::PlanningSceneConstPtr planning_scene_
bool sleep() const
void planAndExecute(ExecutableMotionPlan &plan, const Options &opt)
boost::function< void()> done_callback_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
ExecutableMotionPlanComputationFn plan_callback_
Callback for computing motion plans. This callback must always be specified.
boost::function< void()> before_plan_callback_
dynamic_reconfigure::Server< PlanExecutionDynamicReconfigureConfig > dynamic_reconfigure_server_
PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor, const trajectory_execution_manager::TrajectoryExecutionManagerPtr &trajectory_execution)
double replan_delay_
The amount of time to wait in between replanning attempts (in seconds)
bool isRemainingPathValid(const ExecutableMotionPlan &plan)
bool sleep()
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
bool replan_
Flag indicating whether replanning is allowed.
const robot_state::RobotState & getWayPoint(std::size_t index) const
std::size_t getWayPointCount() const
This namespace includes functionality specific to the execution and monitoring of motion plans...
Monitors the joint_states topic and tf to record the trajectory of the robot.
#define ROS_ERROR_NAMED(name,...)
The maintained set of fixed transforms in the monitored scene was updated.
A generic representation on what a computed motion plan looks like.
void successfulTrajectorySegmentExecution(const ExecutableMotionPlan *plan, std::size_t index)
unsigned int default_max_replan_attempts_
bool ok() const
boost::function< void()> before_execution_callback_
DynamicReconfigureImpl * reconfigure_impl_
std::string getErrorCodeString(const moveit_msgs::MoveItErrorCodes &error_code)
void doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus &status)
r
std::vector< ExecutableTrajectory > plan_components_
This is a convenience class for obtaining access to an instance of a locked PlanningScene.


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jan 15 2018 03:51:18