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
00054 DynamicReconfigureImpl(PlanExecution *owner) : owner_(owner),
00055 dynamic_reconfigure_server_(ros::NodeHandle("~/plan_execution"))
00056 {
00057 dynamic_reconfigure_server_.setCallback(boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
00058 }
00059
00060 private:
00061
00062 void dynamicReconfigureCallback(PlanExecutionDynamicReconfigureConfig &config, uint32_t level)
00063 {
00064 owner_->setMaxReplanAttempts(config.max_replan_attempts);
00065 owner_->setTrajectoryStateRecordingFrequency(config.record_trajectory_state_frequency);
00066 }
00067
00068 PlanExecution *owner_;
00069 dynamic_reconfigure::Server<PlanExecutionDynamicReconfigureConfig> dynamic_reconfigure_server_;
00070 };
00071
00072 }
00073
00074 plan_execution::PlanExecution::PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor,
00075 const trajectory_execution_manager::TrajectoryExecutionManagerPtr& trajectory_execution) :
00076 node_handle_("~"), 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(planning_scene_monitor_->getRobotModel()));
00081
00082 default_max_replan_attempts_ = 5;
00083
00084 preempt_requested_ = false;
00085 new_scene_update_ = false;
00086
00087
00088 planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, _1));
00089
00090
00091 reconfigure_impl_ = new DynamicReconfigureImpl(this);
00092 }
00093
00094 plan_execution::PlanExecution::~PlanExecution()
00095 {
00096 delete reconfigure_impl_;
00097 }
00098
00099 void plan_execution::PlanExecution::stop()
00100 {
00101 preempt_requested_ = true;
00102 }
00103
00104 std::string plan_execution::PlanExecution::getErrorCodeString(const moveit_msgs::MoveItErrorCodes& error_code)
00105 {
00106 if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00107 return "Success";
00108 else
00109 if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME)
00110 return "Invalid group name";
00111 else
00112 if (error_code.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED)
00113 return "Planning failed.";
00114 else
00115 if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN)
00116 return "Invalid motion plan";
00117 else
00118 if (error_code.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
00119 return "Unable to aquire sensor data";
00120 else
00121 if (error_code.val == moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
00122 return "Motion plan invalidated by environment change";
00123 else
00124 if (error_code.val == moveit_msgs::MoveItErrorCodes::CONTROL_FAILED)
00125 return "Controller failed during execution";
00126 else
00127 if (error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT)
00128 return "Timeout reached";
00129 else
00130 if (error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
00131 return "Preempted";
00132 else
00133 if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS)
00134 return "Invalid goal constraints";
00135 else
00136 if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME)
00137 return "Invalid object name";
00138 else
00139 if (error_code.val == moveit_msgs::MoveItErrorCodes::FAILURE)
00140 return "Catastrophic failure";
00141 return "Unknown event";
00142 }
00143
00144 void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan &plan, const Options &opt)
00145 {
00146 plan.planning_scene_monitor_ = planning_scene_monitor_;
00147 plan.planning_scene_ = planning_scene_monitor_->getPlanningScene();
00148 planAndExecuteHelper(plan, opt);
00149 }
00150
00151 void plan_execution::PlanExecution::planAndExecute(ExecutableMotionPlan &plan, const moveit_msgs::PlanningScene &scene_diff, const Options &opt)
00152 {
00153 if (planning_scene::PlanningScene::isEmpty(scene_diff))
00154 planAndExecute(plan, opt);
00155 else
00156 {
00157 plan.planning_scene_monitor_ = planning_scene_monitor_;
00158 {
00159 planning_scene_monitor::LockedPlanningSceneRO lscene(planning_scene_monitor_);
00160 plan.planning_scene_ = lscene->diff(scene_diff);
00161 }
00162 planAndExecuteHelper(plan, opt);
00163 }
00164 }
00165
00166 void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan &plan, const Options &opt)
00167 {
00168
00169 preempt_requested_ = false;
00170
00171
00172 unsigned int max_replan_attempts = opt.replan_ ? (opt.replan_attempts_ > 0 ? opt.replan_attempts_ : default_max_replan_attempts_) : 1;
00173 unsigned int replan_attempts = 0;
00174 bool previously_solved = false;
00175
00176
00177
00178 do
00179 {
00180 replan_attempts++;
00181 ROS_INFO("Planning attempt %u of at most %u", replan_attempts, max_replan_attempts);
00182
00183 if (opt.before_plan_callback_)
00184 opt.before_plan_callback_();
00185
00186 new_scene_update_ = false;
00187
00188
00189 bool solved = (!previously_solved || !opt.repair_plan_callback_) ?
00190 opt.plan_callback_(plan) :
00191 opt.repair_plan_callback_(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex());
00192
00193 if (preempt_requested_)
00194 break;
00195
00196
00197
00198 if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED ||
00199 plan.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN ||
00200 plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
00201 {
00202 if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA && opt.replan_delay_ > 0.0)
00203 {
00204 ros::WallDuration d(opt.replan_delay_);
00205 d.sleep();
00206 }
00207 continue;
00208 }
00209
00210
00211 if (solved)
00212 previously_solved = true;
00213 else
00214 break;
00215
00216 if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00217 {
00218 if (opt.before_execution_callback_)
00219 opt.before_execution_callback_();
00220
00221 if (preempt_requested_)
00222 break;
00223
00224
00225 plan.error_code_ = executeAndMonitor(plan);
00226 }
00227
00228
00229 if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00230 break;
00231
00232
00233 if (plan.error_code_.val != moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
00234 break;
00235 else
00236 {
00237
00238 if (opt.replan_delay_ > 0.0)
00239 {
00240 ROS_INFO("Waiting for a %lf seconds before attempting a new plan ...", opt.replan_delay_);
00241 ros::WallDuration d(opt.replan_delay_);
00242 d.sleep();
00243 ROS_INFO("Done waiting");
00244 }
00245 }
00246 } while (!preempt_requested_ && max_replan_attempts > replan_attempts);
00247
00248 if (preempt_requested_)
00249 {
00250 ROS_DEBUG("PlanExecution was preempted");
00251 plan.error_code_.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
00252 }
00253
00254 if (opt.done_callback_)
00255 opt.done_callback_();
00256
00257 if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00258 ROS_DEBUG("PlanExecution finished successfully.");
00259 else
00260 ROS_DEBUG("PlanExecution terminating with error code %d - '%s'", plan.error_code_.val,
00261 getErrorCodeString(plan.error_code_).c_str());
00262 }
00263
00264 bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan &plan)
00265 {
00266
00267
00268 return isRemainingPathValid(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex());
00269 }
00270
00271 bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionPlan &plan, const std::pair<int, int> &path_segment)
00272 {
00273 if (path_segment.first >= 0 && path_segment.second >= 0 && plan.plan_components_[path_segment.first].trajectory_monitoring_)
00274 {
00275 planning_scene_monitor::LockedPlanningSceneRO lscene(plan.planning_scene_monitor_);
00276 const robot_trajectory::RobotTrajectory &t = *plan.plan_components_[path_segment.first].trajectory_;
00277 const collision_detection::AllowedCollisionMatrix *acm = plan.plan_components_[path_segment.first].allowed_collision_matrix_.get();
00278 std::size_t wpc = t.getWayPointCount();
00279 collision_detection::CollisionRequest req;
00280 req.group_name = t.getGroupName();
00281 for (std::size_t i = std::max(path_segment.second - 1, 0) ; i < wpc ; ++i)
00282 {
00283 collision_detection::CollisionResult res;
00284 if (acm)
00285 plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm);
00286 else
00287 plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i));
00288
00289 if (res.collision || !plan.planning_scene_->isStateFeasible(t.getWayPoint(i), false))
00290 {
00291
00292 ROS_INFO("Trajectory component '%s' is invalid", plan.plan_components_[path_segment.first].description_.c_str());
00293
00294
00295 plan.planning_scene_->isStateFeasible(t.getWayPoint(i), true);
00296 req.verbose = true;
00297 res.clear();
00298 if (acm)
00299 plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i), *acm);
00300 else
00301 plan.planning_scene_->checkCollisionUnpadded(req, res, t.getWayPoint(i));
00302 return false;
00303 }
00304 }
00305 }
00306 return true;
00307 }
00308
00309 moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(const ExecutableMotionPlan &plan)
00310 {
00311 moveit_msgs::MoveItErrorCodes result;
00312
00313
00314 execution_complete_ = true;
00315
00316 if (!trajectory_execution_manager_)
00317 {
00318 ROS_ERROR("No trajectory execution manager");
00319 result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00320 return result;
00321 }
00322
00323 if (plan.plan_components_.empty())
00324 {
00325 result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00326 return result;
00327 }
00328
00329 execution_complete_ = false;
00330
00331
00332 int prev = -1;
00333 for (std::size_t i = 0 ; i < plan.plan_components_.size() ; ++i)
00334 {
00335 if (!plan.plan_components_[i].trajectory_ || plan.plan_components_[i].trajectory_->empty())
00336 continue;
00337
00338
00339
00340
00341
00342 bool unwound = false;
00343 for (std::size_t j = 0 ; j < i ; ++j)
00344
00345 if (plan.plan_components_[j].trajectory_ && plan.plan_components_[j].trajectory_->getGroup() == plan.plan_components_[i].trajectory_->getGroup() && !plan.plan_components_[j].trajectory_->empty())
00346 {
00347 plan.plan_components_[i].trajectory_->unwind(plan.plan_components_[j].trajectory_->getLastWayPoint());
00348 unwound = true;
00349 break;
00350 }
00351
00352 if (!unwound)
00353 {
00354
00355 if (prev < 0)
00356 plan.plan_components_[i].trajectory_->unwind(plan.planning_scene_monitor_ && plan.planning_scene_monitor_->getStateMonitor() ?
00357 *plan.planning_scene_monitor_->getStateMonitor()->getCurrentState() :
00358 plan.planning_scene_->getCurrentState());
00359 else
00360 plan.plan_components_[i].trajectory_->unwind(plan.plan_components_[prev].trajectory_->getLastWayPoint());
00361 }
00362
00363 prev = i;
00364
00365
00366 moveit_msgs::RobotTrajectory msg;
00367 plan.plan_components_[i].trajectory_->getRobotTrajectoryMsg(msg);
00368 if (!trajectory_execution_manager_->push(msg))
00369 {
00370 trajectory_execution_manager_->clear();
00371 ROS_ERROR_STREAM("Apparently trajectory initialization failed");
00372 execution_complete_ = true;
00373 result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00374 return result;
00375 }
00376 }
00377
00378 if (!trajectory_monitor_ && planning_scene_monitor_->getStateMonitor())
00379 trajectory_monitor_.reset(new planning_scene_monitor::TrajectoryMonitor(planning_scene_monitor_->getStateMonitor()));
00380
00381
00382 if (trajectory_monitor_)
00383 trajectory_monitor_->startTrajectoryMonitor();
00384
00385
00386 trajectory_execution_manager_->execute(boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1),
00387 boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, _1));
00388
00389 ros::Rate r(100);
00390 path_became_invalid_ = false;
00391 while (node_handle_.ok() && !execution_complete_ && !preempt_requested_ && !path_became_invalid_)
00392 {
00393 r.sleep();
00394
00395 if (new_scene_update_)
00396 {
00397 new_scene_update_ = false;
00398 if (!isRemainingPathValid(plan))
00399 {
00400 path_became_invalid_ = true;
00401 break;
00402 }
00403 }
00404 }
00405
00406
00407 if (preempt_requested_)
00408 {
00409 ROS_INFO("Stopping execution due to preempt request");
00410 trajectory_execution_manager_->stopExecution();
00411 }
00412 else
00413 if (path_became_invalid_)
00414 {
00415 ROS_INFO("Stopping execution because the path to execute became invalid (probably the environment changed)");
00416 trajectory_execution_manager_->stopExecution();
00417 }
00418 else
00419 if (!execution_complete_)
00420 {
00421 ROS_WARN("Stopping execution due to unknown reason. Possibly the node is about to shut down.");
00422 trajectory_execution_manager_->stopExecution();
00423 }
00424
00425
00426 if (trajectory_monitor_)
00427 trajectory_monitor_->stopTrajectoryMonitor();
00428
00429
00430 if (path_became_invalid_)
00431 result.val = moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE;
00432 else
00433 {
00434 if (preempt_requested_)
00435 {
00436 result.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
00437 }
00438 else
00439 {
00440 if (trajectory_execution_manager_->getLastExecutionStatus() == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
00441 result.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00442 else
00443 if (trajectory_execution_manager_->getLastExecutionStatus() == moveit_controller_manager::ExecutionStatus::TIMED_OUT)
00444 result.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
00445 else
00446 result.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
00447 }
00448 }
00449 return result;
00450 }
00451
00452 void plan_execution::PlanExecution::planningSceneUpdatedCallback(const planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
00453 {
00454 if (update_type & (planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY | planning_scene_monitor::PlanningSceneMonitor::UPDATE_TRANSFORMS))
00455 new_scene_update_ = true;
00456 }
00457
00458 void plan_execution::PlanExecution::doneWithTrajectoryExecution(const moveit_controller_manager::ExecutionStatus &status)
00459 {
00460 execution_complete_ = true;
00461 }
00462
00463 void plan_execution::PlanExecution::successfulTrajectorySegmentExecution(const ExecutableMotionPlan *plan, std::size_t index)
00464 {
00465 ROS_DEBUG("Completed '%s'", plan->plan_components_[index].description_.c_str());
00466
00467
00468 if (plan->plan_components_[index].effect_on_success_)
00469 if (!plan->plan_components_[index].effect_on_success_(plan))
00470 {
00471
00472 ROS_ERROR("Execution of path-completion side-effect failed. Preempting.");
00473 preempt_requested_ = true;
00474 return;
00475 }
00476
00477
00478 std::size_t test_index = index;
00479 while (++test_index < plan->plan_components_.size())
00480 if (plan->plan_components_[test_index].trajectory_ && !plan->plan_components_[test_index].trajectory_->empty())
00481 {
00482 if (!isRemainingPathValid(*plan, std::make_pair<int>(test_index, 0)))
00483 path_became_invalid_ = true;
00484 break;
00485 }
00486 }