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  if (!plan.planning_scene_monitor_)
321  if (!plan.planning_scene_)
322  plan.planning_scene_ = planning_scene_monitor_->getPlanningScene();
323 
324  moveit_msgs::MoveItErrorCodes result;
325 
326  // try to execute the trajectory
327  execution_complete_ = true;
328 
330  {
331  ROS_ERROR_NAMED("plan_execution", "No trajectory execution manager");
332  result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
333  return result;
334  }
335 
336  if (plan.plan_components_.empty())
337  {
338  result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
339  return result;
340  }
341 
342  execution_complete_ = false;
343 
344  // push the trajectories we have slated for execution to the trajectory execution manager
345  int prev = -1;
346  for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
347  {
348  // \todo should this be in trajectory_execution ? Maybe. Then that will have to use kinematic_trajectory too;
349  // spliting trajectories for controllers becomes interesting: tied to groups instead of joints. this could cause
350  // some problems
351  // in the meantime we do a hack:
352 
353  bool unwound = false;
354  for (std::size_t j = 0; j < i; ++j)
355  // if we ran unwind on a path for the same group
356  if (plan.plan_components_[j].trajectory_ &&
357  plan.plan_components_[j].trajectory_->getGroup() == plan.plan_components_[i].trajectory_->getGroup() &&
358  !plan.plan_components_[j].trajectory_->empty())
359  {
360  plan.plan_components_[i].trajectory_->unwind(plan.plan_components_[j].trajectory_->getLastWayPoint());
361  unwound = true;
362  break;
363  }
364 
365  if (!unwound)
366  {
367  // unwind the path to execute based on the current state of the system
368  if (prev < 0)
369  plan.plan_components_[i].trajectory_->unwind(
370  plan.planning_scene_monitor_ && plan.planning_scene_monitor_->getStateMonitor() ?
371  *plan.planning_scene_monitor_->getStateMonitor()->getCurrentState() :
372  plan.planning_scene_->getCurrentState());
373  else
374  plan.plan_components_[i].trajectory_->unwind(plan.plan_components_[prev].trajectory_->getLastWayPoint());
375  }
376 
377  if (plan.plan_components_[i].trajectory_ && !plan.plan_components_[i].trajectory_->empty())
378  prev = i;
379 
380  // convert to message, pass along
381  moveit_msgs::RobotTrajectory msg;
382  plan.plan_components_[i].trajectory_->getRobotTrajectoryMsg(msg);
383  if (!trajectory_execution_manager_->push(msg))
384  {
386  ROS_ERROR_STREAM_NAMED("plan_execution", "Apparently trajectory initialization failed");
387  execution_complete_ = true;
388  result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
389  return result;
390  }
391  }
392 
393  if (!trajectory_monitor_ && planning_scene_monitor_->getStateMonitor())
394  trajectory_monitor_.reset(
396 
397  // start recording trajectory states
399  trajectory_monitor_->startTrajectoryMonitor();
400 
401  // start a trajectory execution thread
403  boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1),
404  boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, _1));
405  // wait for path to be done, while checking that the path does not become invalid
406  ros::Rate r(100);
407  path_became_invalid_ = false;
409  {
410  r.sleep();
411  // check the path if there was an environment update in the meantime
412  if (new_scene_update_)
413  {
414  new_scene_update_ = false;
415  if (!isRemainingPathValid(plan))
416  {
417  path_became_invalid_ = true;
418  break;
419  }
420  }
421  }
422 
423  // stop execution if needed
424  if (preempt_requested_)
425  {
426  ROS_INFO_NAMED("plan_execution", "Stopping execution due to preempt request");
427  trajectory_execution_manager_->stopExecution();
428  }
429  else if (path_became_invalid_)
430  {
431  ROS_INFO_NAMED("plan_execution", "Stopping execution because the path to execute became invalid"
432  "(probably the environment changed)");
433  trajectory_execution_manager_->stopExecution();
434  }
435  else if (!execution_complete_)
436  {
437  ROS_WARN_NAMED("plan_execution", "Stopping execution due to unknown reason."
438  "Possibly the node is about to shut down.");
439  trajectory_execution_manager_->stopExecution();
440  }
441 
442  // stop recording trajectory states
444  trajectory_monitor_->stopTrajectoryMonitor();
445 
446  // decide return value
448  result.val = moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE;
449  else
450  {
451  if (preempt_requested_)
452  {
453  result.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
454  }
455  else
456  {
457  if (trajectory_execution_manager_->getLastExecutionStatus() ==
459  result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
460  else if (trajectory_execution_manager_->getLastExecutionStatus() ==
462  result.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
463  else
464  result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
465  }
466  }
467  return result;
468 }
469 
472 {
475  new_scene_update_ = true;
476 }
477 
480 {
481  execution_complete_ = true;
482 }
483 
485  std::size_t index)
486 {
487  if (plan->plan_components_.size() == 0)
488  {
489  ROS_WARN_NAMED("plan_execution", "Length of provided motion plan is zero.");
490  return;
491  }
492 
493  // if any side-effects are associated to the trajectory part that just completed, execute them
494  ROS_DEBUG_NAMED("plan_execution", "Completed '%s'", plan->plan_components_[index].description_.c_str());
495  if (plan->plan_components_[index].effect_on_success_)
496  if (!plan->plan_components_[index].effect_on_success_(plan))
497  {
498  // execution of side-effect failed
499  ROS_ERROR_NAMED("plan_execution", "Execution of path-completion side-effect failed. Preempting.");
500  preempt_requested_ = true;
501  return;
502  }
503 
504  // if there is a next trajectory, check it for validity, before we start execution
505  ++index;
506  if (index < plan->plan_components_.size() && plan->plan_components_[index].trajectory_ &&
507  !plan->plan_components_[index].trajectory_->empty())
508  {
509  if (!isRemainingPathValid(*plan, std::make_pair<int>(index, 0)))
510  path_became_invalid_ = true;
511  }
512 }
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,...)
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.
moveit_msgs::MoveItErrorCodes executeAndMonitor(ExecutableMotionPlan &plan)
Execute and monitor a previously created plan.


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:32