00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00089 planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, _1));
00090
00091
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_);
00151
00152
00153
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
00163 preempt_requested_ = false;
00164
00165
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
00172
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;
00182
00183
00184
00185
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
00195
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
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
00224 plan.error_code_ = executeAndMonitor(plan);
00225 }
00226
00227
00228 if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00229 break;
00230
00231
00232 if (plan.error_code_.val != moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
00233 break;
00234 else
00235 {
00236
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
00266
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_);
00277
00278
00279
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
00297 ROS_INFO("Trajectory component '%s' is invalid",
00298 plan.plan_components_[path_segment.first].description_.c_str());
00299
00300
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
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
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
00345
00346
00347
00348
00349 bool unwound = false;
00350 for (std::size_t j = 0; j < i; ++j)
00351
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
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
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
00393 if (trajectory_monitor_)
00394 trajectory_monitor_->startTrajectoryMonitor();
00395
00396
00397 trajectory_execution_manager_->execute(
00398 boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1),
00399 boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, _1));
00400
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
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
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
00436 if (trajectory_monitor_)
00437 trajectory_monitor_->stopTrajectoryMonitor();
00438
00439
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
00483 if (plan->plan_components_[index].effect_on_success_)
00484 if (!plan->plan_components_[index].effect_on_success_(plan))
00485 {
00486
00487 ROS_ERROR("Execution of path-completion side-effect failed. Preempting.");
00488 preempt_requested_ = true;
00489 return;
00490 }
00491
00492
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 }