plan_execution.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/plan_execution/plan_execution.h>
00038 #include <moveit/robot_state/conversions.h>
00039 #include <moveit/trajectory_processing/trajectory_tools.h>
00040 #include <moveit/collision_detection/collision_tools.h>
00041 #include <boost/algorithm/string/join.hpp>
00042 
00043 #include <dynamic_reconfigure/server.h>
00044 #include <moveit_ros_planning/PlanExecutionDynamicReconfigureConfig.h>
00045 
00046 namespace plan_execution
00047 {
00048 using namespace moveit_ros_planning;
00049 
00050 class PlanExecution::DynamicReconfigureImpl
00051 {
00052 public:
00053   DynamicReconfigureImpl(PlanExecution* owner)
00054     : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/plan_execution"))
00055   {
00056     dynamic_reconfigure_server_.setCallback(
00057         boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
00058   }
00059 
00060 private:
00061   void dynamicReconfigureCallback(PlanExecutionDynamicReconfigureConfig& config, uint32_t level)
00062   {
00063     owner_->setMaxReplanAttempts(config.max_replan_attempts);
00064     owner_->setTrajectoryStateRecordingFrequency(config.record_trajectory_state_frequency);
00065   }
00066 
00067   PlanExecution* owner_;
00068   dynamic_reconfigure::Server<PlanExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
00069 };
00070 }
00071 
00072 plan_execution::PlanExecution::PlanExecution(
00073     const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
00074     const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution)
00075   : node_handle_("~")
00076   , planning_scene_monitor_(planning_scene_monitor)
00077   , trajectory_execution_manager_(trajectory_execution)
00078 {
00079   if (!trajectory_execution_manager_)
00080     trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager(
00081         planning_scene_monitor_->getRobotModel(), planning_scene_monitor_->getStateMonitor()));
00082 
00083   default_max_replan_attempts_ = 5;
00084 
00085   preempt_requested_ = false;
00086   new_scene_update_ = false;
00087 
00088   // we want to be notified when new information is available
00089   planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, _1));
00090 
00091   // start the dynamic-reconfigure server
00092   reconfigure_impl_ = new DynamicReconfigureImpl(this);
00093 }
00094 
00095 plan_execution::PlanExecution::~PlanExecution()
00096 {
00097   delete reconfigure_impl_;
00098 }
00099 
00100 void plan_execution::PlanExecution::stop()
00101 {
00102   preempt_requested_ = true;
00103 }
00104 
00105 std::string plan_execution::PlanExecution::getErrorCodeString(const moveit_msgs::MoveItErrorCodes& error_code)
00106 {
00107   if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00108     return "Success";
00109   else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME)
00110     return "Invalid group name";
00111   else if (error_code.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED)
00112     return "Planning failed.";
00113   else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN)
00114     return "Invalid motion plan";
00115   else if (error_code.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
00116     return "Unable to aquire sensor data";
00117   else if (error_code.val == moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
00118     return "Motion plan invalidated by environment change";
00119   else if (error_code.val == moveit_msgs::MoveItErrorCodes::CONTROL_FAILED)
00120     return "Controller failed during execution";
00121   else if (error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT)
00122     return "Timeout reached";
00123   else if (error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
00124     return "Preempted";
00125   else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS)
00126     return "Invalid goal constraints";
00127   else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME)
00128     return "Invalid object name";
00129   else if (error_code.val == moveit_msgs::MoveItErrorCodes::FAILURE)
00130     return "Catastrophic failure";
00131   return "Unknown event";
00132 }
00133 
00134 void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan& plan, const Options& opt)
00135 {
00136   plan.planning_scene_monitor_ = planning_scene_monitor_;
00137   plan.planning_scene_ = planning_scene_monitor_->getPlanningScene();
00138   planAndExecuteHelper(plan, opt);
00139 }
00140 
00141 void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan& plan,
00142                                                    const moveit_msgs::PlanningScene& scene_diff, const Options& opt)
00143 {
00144   if (planning_scene::PlanningScene::isEmpty(scene_diff))
00145     planAndExecute(plan, opt);
00146   else
00147   {
00148     plan.planning_scene_monitor_ = planning_scene_monitor_;
00149     {
00150       planning_scene_monitor::LockedPlanningSceneRO lscene(planning_scene_monitor_);  // lock the scene so that it does
00151                                                                                       // not modify the world
00152                                                                                       // representation while diff() is
00153                                                                                       // called
00154       plan.planning_scene_ = lscene->diff(scene_diff);
00155     }
00156     planAndExecuteHelper(plan, opt);
00157   }
00158 }
00159 
00160 void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan& plan, const Options& opt)
00161 {
00162   // perform initial configuration steps & various checks
00163   preempt_requested_ = false;
00164 
00165   // run the actual motion plan & execution
00166   unsigned int max_replan_attempts =
00167       opt.replan_ ? (opt.replan_attempts_ > 0 ? opt.replan_attempts_ : default_max_replan_attempts_) : 1;
00168   unsigned int replan_attempts = 0;
00169   bool previously_solved = false;
00170 
00171   // run a planning loop for at most the maximum replanning attempts;
00172   // re-planning is executed only in case of known types of failures (e.g., environment changed)
00173   do
00174   {
00175     replan_attempts++;
00176     ROS_INFO("Planning attempt %u of at most %u", replan_attempts, max_replan_attempts);
00177 
00178     if (opt.before_plan_callback_)
00179       opt.before_plan_callback_();
00180 
00181     new_scene_update_ = false;  // we clear any scene updates to be evaluated because we are about to compute a new
00182                                 // plan, which should consider most recent updates already
00183 
00184     // if we never had a solved plan, or there is no specified way of fixing plans, just call the planner; otherwise,
00185     // try to repair the plan we previously had;
00186     bool solved =
00187         (!previously_solved || !opt.repair_plan_callback_) ?
00188             opt.plan_callback_(plan) :
00189             opt.repair_plan_callback_(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex());
00190 
00191     if (preempt_requested_)
00192       break;
00193 
00194     // if planning fails in a manner that is not recoverable, we exit the loop,
00195     // otherwise, we attempt to continue, if replanning attempts are left
00196     if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED ||
00197         plan.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN ||
00198         plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
00199     {
00200       if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA &&
00201           opt.replan_delay_ > 0.0)
00202       {
00203         ros::WallDuration d(opt.replan_delay_);
00204         d.sleep();
00205       }
00206       continue;
00207     }
00208 
00209     // abort if no plan was found
00210     if (solved)
00211       previously_solved = true;
00212     else
00213       break;
00214 
00215     if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00216     {
00217       if (opt.before_execution_callback_)
00218         opt.before_execution_callback_();
00219 
00220       if (preempt_requested_)
00221         break;
00222 
00223       // execute the trajectory, and monitor its executionm
00224       plan.error_code_ = executeAndMonitor(plan);
00225     }
00226 
00227     // if we are done, then we exit the loop
00228     if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00229       break;
00230 
00231     // if execution failed in a manner that we do not consider recoverable, we exit the loop (with failure)
00232     if (plan.error_code_.val != moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
00233       break;
00234     else
00235     {
00236       // othewrise, we wait (if needed)
00237       if (opt.replan_delay_ > 0.0)
00238       {
00239         ROS_INFO("Waiting for a %lf seconds before attempting a new plan ...", opt.replan_delay_);
00240         ros::WallDuration d(opt.replan_delay_);
00241         d.sleep();
00242         ROS_INFO("Done waiting");
00243       }
00244     }
00245   } while (!preempt_requested_ && max_replan_attempts > replan_attempts);
00246 
00247   if (preempt_requested_)
00248   {
00249     ROS_DEBUG("PlanExecution was preempted");
00250     plan.error_code_.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
00251   }
00252 
00253   if (opt.done_callback_)
00254     opt.done_callback_();
00255 
00256   if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00257     ROS_DEBUG("PlanExecution finished successfully.");
00258   else
00259     ROS_DEBUG("PlanExecution terminating with error code %d - '%s'", plan.error_code_.val,
00260               getErrorCodeString(plan.error_code_).c_str());
00261 }
00262 
00263 bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan& plan)
00264 {
00265   // check the validity of the currently executed path segment only, since there could be
00266   // changes in the world in between path segments
00267   return isRemainingPathValid(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex());
00268 }
00269 
00270 bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan& plan,
00271                                                          const std::pair<int, int>& path_segment)
00272 {
00273   if (path_segment.first >= 0 && path_segment.second >= 0 &&
00274       plan.plan_components_[path_segment.first].trajectory_monitoring_)
00275   {
00276     planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);  // lock the scene so that it
00277                                                                                          // does not modify the world
00278                                                                                          // representation while
00279                                                                                          // isStateValid() is called
00280     const robot_trajectory::RobotTrajectory& t = *plan.plan_components_[path_segment.first].trajectory_;
00281     const collision_detection::AllowedCollisionMatrix* acm =
00282         plan.plan_components_[path_segment.first].allowed_collision_matrix_.get();
00283     std::size_t wpc = t.getWayPointCount();
00284     collision_detection::CollisionRequest req;
00285     req.group_name = t.getGroupName();
00286     for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
00287     {
00288       collision_detection::CollisionResult res;
00289       if (acm)
00290         plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm);
00291       else
00292         plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i));
00293 
00294       if (res.collision || !plan.planning_scene_->isStateFeasible(t.getWayPoint(i), false))
00295       {
00296         // Dave's debacle
00297         ROS_INFO("Trajectory component '%s' is invalid",
00298                  plan.plan_components_[path_segment.first].description_.c_str());
00299 
00300         // call the same functions again, in verbose mode, to show what issues have been detected
00301         plan.planning_scene_->isStateFeasible(t.getWayPoint(i), true);
00302         req.verbose = true;
00303         res.clear();
00304         if (acm)
00305           plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm);
00306         else
00307           plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i));
00308         return false;
00309       }
00310     }
00311   }
00312   return true;
00313 }
00314 
00315 moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(const ExecutableMotionPlan& plan)
00316 {
00317   moveit_msgs::MoveItErrorCodes result;
00318 
00319   // try to execute the trajectory
00320   execution_complete_ = true;
00321 
00322   if (!trajectory_execution_manager_)
00323   {
00324     ROS_ERROR("No trajectory execution manager");
00325     result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00326     return result;
00327   }
00328 
00329   if (plan.plan_components_.empty())
00330   {
00331     result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00332     return result;
00333   }
00334 
00335   execution_complete_ = false;
00336 
00337   // push the trajectories we have slated for execution to the trajectory execution manager
00338   int prev = -1;
00339   for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
00340   {
00341     if (!plan.plan_components_[i].trajectory_ || plan.plan_components_[i].trajectory_->empty())
00342       continue;
00343 
00344     // \todo should this be in trajectory_execution ? Maybe. Then that will have to use kinematic_trajectory too;
00345     // spliting trajectories for controllers becomes interesting: tied to groups instead of joints. this could cause
00346     // some problems
00347     // in the meantime we do a hack:
00348 
00349     bool unwound = false;
00350     for (std::size_t j = 0; j < i; ++j)
00351       // if we ran unwind on a path for the same group
00352       if (plan.plan_components_[j].trajectory_ &&
00353           plan.plan_components_[j].trajectory_->getGroup() == plan.plan_components_[i].trajectory_->getGroup() &&
00354           !plan.plan_components_[j].trajectory_->empty())
00355       {
00356         plan.plan_components_[i].trajectory_->unwind(plan.plan_components_[j].trajectory_->getLastWayPoint());
00357         unwound = true;
00358         break;
00359       }
00360 
00361     if (!unwound)
00362     {
00363       // unwind the path to execute based on the current state of the system
00364       if (prev < 0)
00365         plan.plan_components_[i].trajectory_->unwind(
00366             plan.planning_scene_monitor_ && plan.planning_scene_monitor_->getStateMonitor() ?
00367                 *plan.planning_scene_monitor_->getStateMonitor()->getCurrentState() :
00368                 plan.planning_scene_->getCurrentState());
00369       else
00370         plan.plan_components_[i].trajectory_->unwind(plan.plan_components_[prev].trajectory_->getLastWayPoint());
00371     }
00372 
00373     prev = i;
00374 
00375     // convert to message, pass along
00376     moveit_msgs::RobotTrajectory msg;
00377     plan.plan_components_[i].trajectory_->getRobotTrajectoryMsg(msg);
00378     if (!trajectory_execution_manager_->push(msg))
00379     {
00380       trajectory_execution_manager_->clear();
00381       ROS_ERROR_STREAM("Apparently trajectory initialization failed");
00382       execution_complete_ = true;
00383       result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00384       return result;
00385     }
00386   }
00387 
00388   if (!trajectory_monitor_ && planning_scene_monitor_->getStateMonitor())
00389     trajectory_monitor_.reset(
00390         new planning_scene_monitor::TrajectoryMonitor(planning_scene_monitor_->getStateMonitor()));
00391 
00392   // start recording trajectory states
00393   if (trajectory_monitor_)
00394     trajectory_monitor_->startTrajectoryMonitor();
00395 
00396   // start a trajectory execution thread
00397   trajectory_execution_manager_->execute(
00398       boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1),
00399       boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, _1));
00400   // wait for path to be done, while checking that the path does not become invalid
00401   ros::Rate r(100);
00402   path_became_invalid_ = false;
00403   while (node_handle_.ok() && !execution_complete_ && !preempt_requested_ && !path_became_invalid_)
00404   {
00405     r.sleep();
00406     // check the path if there was an environment update in the meantime
00407     if (new_scene_update_)
00408     {
00409       new_scene_update_ = false;
00410       if (!isRemainingPathValid(plan))
00411       {
00412         path_became_invalid_ = true;
00413         break;
00414       }
00415     }
00416   }
00417 
00418   // stop execution if needed
00419   if (preempt_requested_)
00420   {
00421     ROS_INFO("Stopping execution due to preempt request");
00422     trajectory_execution_manager_->stopExecution();
00423   }
00424   else if (path_became_invalid_)
00425   {
00426     ROS_INFO("Stopping execution because the path to execute became invalid (probably the environment changed)");
00427     trajectory_execution_manager_->stopExecution();
00428   }
00429   else if (!execution_complete_)
00430   {
00431     ROS_WARN("Stopping execution due to unknown reason. Possibly the node is about to shut down.");
00432     trajectory_execution_manager_->stopExecution();
00433   }
00434 
00435   // stop recording trajectory states
00436   if (trajectory_monitor_)
00437     trajectory_monitor_->stopTrajectoryMonitor();
00438 
00439   // decide return value
00440   if (path_became_invalid_)
00441     result.val = moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE;
00442   else
00443   {
00444     if (preempt_requested_)
00445     {
00446       result.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
00447     }
00448     else
00449     {
00450       if (trajectory_execution_manager_->getLastExecutionStatus() ==
00451           moveit_controller_manager::ExecutionStatus::SUCCEEDED)
00452         result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00453       else if (trajectory_execution_manager_->getLastExecutionStatus() ==
00454                moveit_controller_manager::ExecutionStatus::TIMED_OUT)
00455         result.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
00456       else
00457         result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00458     }
00459   }
00460   return result;
00461 }
00462 
00463 void plan_execution::PlanExecution::planningSceneUpdatedCallback(
00464     const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
00465 {
00466   if (update_type & (planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY |
00467                      planning_scene_monitor::PlanningSceneMonitor::UPDATE_TRANSFORMS))
00468     new_scene_update_ = true;
00469 }
00470 
00471 void plan_execution::PlanExecution::doneWithTrajectoryExecution(
00472     const moveit_controller_manager::ExecutionStatus& status)
00473 {
00474   execution_complete_ = true;
00475 }
00476 
00477 void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const ExecutableMotionPlan* plan,
00478                                                                          std::size_t index)
00479 {
00480   ROS_DEBUG("Completed '%s'", plan->plan_components_[index].description_.c_str());
00481 
00482   // if any side-effects are associated to the trajectory part that just completed, execute them
00483   if (plan->plan_components_[index].effect_on_success_)
00484     if (!plan->plan_components_[index].effect_on_success_(plan))
00485     {
00486       // execution of side-effect failed
00487       ROS_ERROR("Execution of path-completion side-effect failed. Preempting.");
00488       preempt_requested_ = true;
00489       return;
00490     }
00491 
00492   // if there is a next trajectory, check it for validity, before we start execution
00493   std::size_t test_index = index;
00494   while (++test_index < plan->plan_components_.size())
00495     if (plan->plan_components_[test_index].trajectory_ && !plan->plan_components_[test_index].trajectory_->empty())
00496     {
00497       if (!isRemainingPathValid(*plan, std::make_pair<int>(test_index, 0)))
00498         path_became_invalid_ = true;
00499       break;
00500     }
00501 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:19