model_based_planning_context.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 the 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/ompl_interface/model_based_planning_context.h>
00038 #include <moveit/ompl_interface/detail/state_validity_checker.h>
00039 #include <moveit/ompl_interface/detail/constrained_sampler.h>
00040 #include <moveit/ompl_interface/detail/constrained_goal_sampler.h>
00041 #include <moveit/ompl_interface/detail/goal_union.h>
00042 #include <moveit/ompl_interface/detail/projection_evaluators.h>
00043 #include <moveit/ompl_interface/constraints_library.h>
00044 #include <moveit/kinematic_constraints/utils.h>
00045 #include <moveit/profiler/profiler.h>
00046 #include <eigen_conversions/eigen_msg.h>
00047 
00048 #include <ompl/base/samplers/UniformValidStateSampler.h>
00049 #include <ompl/base/goals/GoalLazySamples.h>
00050 #include <ompl/tools/config/SelfConfig.h>
00051 #include <ompl/base/spaces/SE3StateSpace.h>
00052 #include <ompl/datastructures/PDF.h>
00053 
00054 ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std::string &name, const ModelBasedPlanningContextSpecification &spec) :
00055   planning_interface::PlanningContext(name, spec.state_space_->getJointModelGroup()->getName()),
00056   spec_(spec),
00057   complete_initial_robot_state_(spec.state_space_->getRobotModel()),
00058   ompl_simple_setup_(spec.state_space_),
00059   ompl_benchmark_(ompl_simple_setup_),
00060   ompl_parallel_plan_(ompl_simple_setup_.getProblemDefinition()),
00061   last_plan_time_(0.0),
00062   last_simplify_time_(0.0),
00063   max_goal_samples_(0),
00064   max_state_sampling_attempts_(0),
00065   max_goal_sampling_attempts_(0),
00066   max_planning_threads_(0),
00067   max_solution_segment_length_(0.0),
00068   minimum_waypoint_count_(0),
00069   use_state_validity_cache_(true),
00070   simplify_solutions_(true)
00071 {
00072   ompl_simple_setup_.getStateSpace()->computeSignature(space_signature_);
00073   ompl_simple_setup_.getStateSpace()->setStateSamplerAllocator(boost::bind(&ModelBasedPlanningContext::allocPathConstrainedSampler, this, _1));
00074 }
00075 
00076 void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator(const std::string &peval)
00077 {
00078   if (!spec_.state_space_)
00079   {
00080     logError("No state space is configured yet");
00081     return;
00082   }
00083   ob::ProjectionEvaluatorPtr pe = getProjectionEvaluator(peval);
00084   if (pe)
00085     spec_.state_space_->registerDefaultProjection(pe);
00086 }
00087 
00088 ompl::base::ProjectionEvaluatorPtr ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::string &peval) const
00089 {
00090   if (peval.find_first_of("link(") == 0 && peval[peval.length() - 1] == ')')
00091   {
00092     std::string link_name = peval.substr(5, peval.length() - 6);
00093     if (getRobotModel()->hasLinkModel(link_name))
00094       return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorLinkPose(this, link_name));
00095     else
00096       logError("Attempted to set projection evaluator with respect to position of link '%s', but that link is not known to the kinematic model.", link_name.c_str());
00097   }
00098   else
00099     if (peval.find_first_of("joints(") == 0 && peval[peval.length() - 1] == ')')
00100     {
00101       std::string joints = peval.substr(7, peval.length() - 8);
00102       boost::replace_all(joints, ",", " ");
00103       std::vector<std::pair<std::string, unsigned int> > j;
00104       std::stringstream ss(joints);
00105       while (ss.good() && !ss.eof())
00106       {
00107     std::string v; ss >> v >> std::ws;
00108     if (getJointModelGroup()->hasJointModel(v))
00109     {
00110       unsigned int vc = getJointModelGroup()->getJointModel(v)->getVariableCount();
00111       if (vc > 0)
00112         j.push_back(std::make_pair(v, vc));
00113       else
00114         logWarn("%s: Ignoring joint '%s' in projection since it has 0 DOF", name_.c_str(), v.c_str());
00115     }
00116     else
00117       logError("%s: Attempted to set projection evaluator with respect to value of joint '%s', but that joint is not known to the group '%s'.",
00118                    name_.c_str(), v.c_str(), getGroupName().c_str());
00119       }
00120       if (j.empty())
00121     logError("%s: No valid joints specified for joint projection", name_.c_str());
00122       else
00123     return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorJointValue(this, j));
00124     }
00125     else
00126       logError("Unable to allocate projection evaluator based on description: '%s'", peval.c_str());
00127   return ob::ProjectionEvaluatorPtr();
00128 }
00129 
00130 ompl::base::StateSamplerPtr ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const ompl::base::StateSpace *ss) const
00131 {
00132   if (spec_.state_space_.get() != ss)
00133   {
00134     logError("%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str());
00135     return ompl::base::StateSamplerPtr();
00136   }
00137 
00138   logDebug("%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str());
00139 
00140   if (path_constraints_)
00141   {
00142     if (spec_.constraints_library_)
00143     {
00144       const ConstraintApproximationPtr &ca = spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_);
00145       if (ca)
00146       {
00147         ompl::base::StateSamplerAllocator c_ssa = ca->getStateSamplerAllocator(path_constraints_msg_);
00148         if (c_ssa)
00149         {
00150           ompl::base::StateSamplerPtr res = c_ssa(ss);
00151           if (res)
00152           {
00153             logInform("%s: Using precomputed state sampler (approximated constraint space) for constraint '%s'", name_.c_str(), path_constraints_msg_.name.c_str());
00154             return res;
00155           }
00156         }
00157       }
00158     }
00159 
00160     constraint_samplers::ConstraintSamplerPtr cs;
00161     if (spec_.constraint_sampler_manager_)
00162       cs = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), path_constraints_->getAllConstraints());
00163 
00164     if (cs)
00165     {
00166       logInform("%s: Allocating specialized state sampler for state space", name_.c_str());
00167       return ob::StateSamplerPtr(new ConstrainedSampler(this, cs));
00168     }
00169   }
00170   logDebug("%s: Allocating default state sampler for state space", name_.c_str());
00171   return ss->allocDefaultStateSampler();
00172 }
00173 
00174 void ompl_interface::ModelBasedPlanningContext::configure()
00175 {
00176   // convert the input state to the corresponding OMPL state
00177   ompl::base::ScopedState<> ompl_start_state(spec_.state_space_);
00178   spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
00179   ompl_simple_setup_.setStartState(ompl_start_state);
00180   ompl_simple_setup_.setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this)));
00181 
00182   if (path_constraints_ && spec_.constraints_library_)
00183   {
00184     const ConstraintApproximationPtr &ca = spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_);
00185     if (ca)
00186     {
00187       getOMPLStateSpace()->setInterpolationFunction(ca->getInterpolationFunction());
00188       logInform("Using precomputed interpolation states");
00189     }
00190   }
00191 
00192   useConfig();
00193   if (ompl_simple_setup_.getGoal())
00194     ompl_simple_setup_.setup();
00195 }
00196 
00197 void ompl_interface::ModelBasedPlanningContext::useConfig()
00198 {
00199   const std::map<std::string, std::string> &config = spec_.config_;
00200   if (config.empty())
00201     return;
00202   std::map<std::string, std::string> cfg = config;
00203 
00204   // set the projection evaluator
00205   std::map<std::string, std::string>::iterator it = cfg.find("projection_evaluator");
00206   if (it != cfg.end())
00207   {
00208     setProjectionEvaluator(boost::trim_copy(it->second));
00209     cfg.erase(it);
00210   }
00211 
00212   if (cfg.empty())
00213     return;
00214 
00215   // remove the 'type' parameter; the rest are parameters for the planner itself
00216   it = cfg.find("type");
00217   if (it == cfg.end())
00218   {
00219     if (name_ != getGroupName())
00220       logWarn("%s: Attribute 'type' not specified in planner configuration", name_.c_str());
00221   }
00222   else
00223   {
00224     std::string type = it->second;
00225     cfg.erase(it);
00226     ompl_simple_setup_.setPlannerAllocator(boost::bind(spec_.planner_selector_(type), _1,
00227                                name_ != getGroupName() ? name_ : "", spec_));
00228     logInform("Planner configuration '%s' will use planner '%s'. Additional configuration parameters will be set when the planner is constructed.",
00229               name_.c_str(), type.c_str());
00230   }
00231 
00232   // call the setParams() after setup(), so we know what the params are
00233   ompl_simple_setup_.getSpaceInformation()->setup();
00234   ompl_simple_setup_.getSpaceInformation()->params().setParams(cfg, true);
00235   // call setup() again for possibly new param values
00236   ompl_simple_setup_.getSpaceInformation()->setup();
00237 }
00238 
00239 void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_msgs::WorkspaceParameters &wparams)
00240 {
00241   if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 &&
00242       wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 &&
00243       wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0)
00244     logWarn("It looks like the planning volume was not specified.");
00245 
00246   logDebug("%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = [%f, %f], z = [%f, %f]", name_.c_str(),
00247            wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y, wparams.max_corner.y, wparams.min_corner.z, wparams.max_corner.z);
00248 
00249   spec_.state_space_->setPlanningVolume(wparams.min_corner.x, wparams.max_corner.x,
00250                                         wparams.min_corner.y, wparams.max_corner.y,
00251                                         wparams.min_corner.z, wparams.max_corner.z);
00252 }
00253 
00254 void ompl_interface::ModelBasedPlanningContext::simplifySolution(double timeout)
00255 {
00256   ompl_simple_setup_.simplifySolution(timeout);
00257   last_simplify_time_ = ompl_simple_setup_.getLastSimplificationTime();
00258 }
00259 
00260 void ompl_interface::ModelBasedPlanningContext::interpolateSolution()
00261 {
00262   if (ompl_simple_setup_.haveSolutionPath())
00263   {
00264     og::PathGeometric &pg = ompl_simple_setup_.getSolutionPath();
00265     pg.interpolate(std::max((unsigned int)floor(0.5 + pg.length() / max_solution_segment_length_), minimum_waypoint_count_));
00266   }
00267 }
00268 
00269 void ompl_interface::ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric &pg, robot_trajectory::RobotTrajectory &traj) const
00270 {
00271   robot_state::RobotState ks = complete_initial_robot_state_;
00272   for (std::size_t i = 0 ; i < pg.getStateCount() ; ++i)
00273   {
00274     spec_.state_space_->copyToRobotState(ks, pg.getState(i));
00275     traj.addSuffixWayPoint(ks, 0.0);
00276   }
00277 }
00278 
00279 bool ompl_interface::ModelBasedPlanningContext::getSolutionPath(robot_trajectory::RobotTrajectory &traj) const
00280 {
00281   traj.clear();
00282   if (!ompl_simple_setup_.haveSolutionPath())
00283     return false;
00284   convertPath(ompl_simple_setup_.getSolutionPath(), traj);
00285   return true;
00286 }
00287 
00288 void ompl_interface::ModelBasedPlanningContext::setVerboseStateValidityChecks(bool flag)
00289 {
00290   if (ompl_simple_setup_.getStateValidityChecker())
00291     static_cast<StateValidityChecker*>(ompl_simple_setup_.getStateValidityChecker().get())->setVerbose(flag);
00292 }
00293 
00294 ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal()
00295 {
00296   // ******************* set up the goal representation, based on goal constraints
00297 
00298   std::vector<ob::GoalPtr> goals;
00299   for (std::size_t i = 0 ; i < goal_constraints_.size() ; ++i)
00300   {
00301     constraint_samplers::ConstraintSamplerPtr cs;
00302     if (spec_.constraint_sampler_manager_)
00303       cs = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(), goal_constraints_[i]->getAllConstraints());
00304     if (cs)
00305     {
00306       ob::GoalPtr g = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraints_[i], cs));
00307       goals.push_back(g);
00308     }
00309   }
00310 
00311   if (!goals.empty())
00312     return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals));
00313   else
00314     logError("Unable to construct goal representation");
00315 
00316   return ob::GoalPtr();
00317 }
00318 
00319 void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState(const robot_state::RobotState &complete_initial_robot_state)
00320 {
00321   complete_initial_robot_state_ = complete_initial_robot_state;
00322 }
00323 
00324 void ompl_interface::ModelBasedPlanningContext::clear()
00325 {
00326   ompl_simple_setup_.clear();
00327   ompl_simple_setup_.clearStartStates();
00328   ompl_simple_setup_.setGoal(ob::GoalPtr());
00329   ompl_simple_setup_.setStateValidityChecker(ob::StateValidityCheckerPtr());
00330   path_constraints_.reset();
00331   goal_constraints_.clear();
00332   getOMPLStateSpace()->setInterpolationFunction(InterpolationFunction());
00333 }
00334 
00335 bool ompl_interface::ModelBasedPlanningContext::setPathConstraints(const moveit_msgs::Constraints &path_constraints,
00336                                    moveit_msgs::MoveItErrorCodes *error)
00337 {
00338   // ******************* set the path constraints to use
00339   path_constraints_.reset(new kinematic_constraints::KinematicConstraintSet(getRobotModel()));
00340   path_constraints_->add(path_constraints, getPlanningScene()->getTransforms());
00341   path_constraints_msg_ = path_constraints;
00342 
00343   return true;
00344 }
00345 
00346 bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints(const std::vector<moveit_msgs::Constraints> &goal_constraints,
00347                                    const moveit_msgs::Constraints &path_constraints,
00348                                    moveit_msgs::MoveItErrorCodes *error)
00349 {
00350 
00351   // ******************* check if the input is correct
00352   goal_constraints_.clear();
00353   for (std::size_t i = 0 ; i < goal_constraints.size() ; ++i)
00354   {
00355     moveit_msgs::Constraints constr = kinematic_constraints::mergeConstraints(goal_constraints[i], path_constraints);
00356     kinematic_constraints::KinematicConstraintSetPtr kset(new kinematic_constraints::KinematicConstraintSet(getRobotModel()));
00357     kset->add(constr, getPlanningScene()->getTransforms());
00358     if (!kset->empty())
00359       goal_constraints_.push_back(kset);
00360   }
00361 
00362   if (goal_constraints_.empty())
00363   {
00364     logWarn("%s: No goal constraints specified. There is no problem to solve.", name_.c_str());
00365     if (error)
00366       error->val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
00367     return false;
00368   }
00369 
00370   ob::GoalPtr goal = constructGoal();
00371   ompl_simple_setup_.setGoal(goal);
00372   if (goal)
00373     return true;
00374   else
00375     return false;
00376 }
00377 
00378 bool ompl_interface::ModelBasedPlanningContext::benchmark(double timeout, unsigned int count, const std::string &filename)
00379 {
00380   ompl_benchmark_.clearPlanners();
00381   ompl_simple_setup_.setup();
00382   ompl_benchmark_.addPlanner(ompl_simple_setup_.getPlanner());
00383   ompl_benchmark_.setExperimentName(getRobotModel()->getName() + "_" + getGroupName() + "_" +
00384                     getPlanningScene()->getName() + "_" + name_);
00385 
00386   ot::Benchmark::Request req;
00387   req.maxTime = timeout;
00388   req.runCount = count;
00389   req.displayProgress = true;
00390   req.saveConsoleOutput = false;
00391   ompl_benchmark_.benchmark(req);
00392   return filename.empty() ? ompl_benchmark_.saveResultsToFile() : ompl_benchmark_.saveResultsToFile(filename.c_str());
00393 }
00394 
00395 void ompl_interface::ModelBasedPlanningContext::preSolve()
00396 {
00397   // clear previously computed solutions
00398   ompl_simple_setup_.getProblemDefinition()->clearSolutionPaths();
00399   const ob::PlannerPtr planner = ompl_simple_setup_.getPlanner();
00400   if (planner)
00401     planner->clear();
00402   bool gls = ompl_simple_setup_.getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
00403   // just in case sampling is not started
00404   if (gls)
00405     static_cast<ob::GoalLazySamples*>(ompl_simple_setup_.getGoal().get())->startSampling();
00406 
00407   ompl_simple_setup_.getSpaceInformation()->getMotionValidator()->resetMotionCounter();
00408 }
00409 
00410 void ompl_interface::ModelBasedPlanningContext::postSolve()
00411 {
00412   bool gls = ompl_simple_setup_.getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
00413   if (gls)
00414     // just in case we need to stop sampling
00415     static_cast<ob::GoalLazySamples*>(ompl_simple_setup_.getGoal().get())->stopSampling();
00416 
00417   int v = ompl_simple_setup_.getSpaceInformation()->getMotionValidator()->getValidMotionCount();
00418   int iv = ompl_simple_setup_.getSpaceInformation()->getMotionValidator()->getInvalidMotionCount();
00419   logDebug("There were %d valid motions and %d invalid motions.", v, iv);
00420 
00421   if (ompl_simple_setup_.getProblemDefinition()->hasApproximateSolution())
00422     logWarn("Computed solution is approximate");
00423 }
00424 
00425 bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse &res)
00426 {
00427   if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
00428   {
00429     double ptime = getLastPlanTime();
00430     if (simplify_solutions_ && ptime < request_.allowed_planning_time)
00431     {
00432       simplifySolution(request_.allowed_planning_time - ptime);
00433       ptime += getLastSimplifyTime();
00434     }
00435     interpolateSolution();
00436 
00437     // fill the response
00438     logDebug("%s: Returning successful solution with %lu states", getName().c_str(),
00439              getOMPLSimpleSetup().getSolutionPath().getStateCount());
00440 
00441     res.trajectory_.reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName()));
00442     getSolutionPath(*res.trajectory_);
00443     res.planning_time_ = ptime;
00444     return true;
00445   }
00446   else
00447   {
00448     logInform("Unable to solve the planning problem");
00449     res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00450     return false;
00451   }
00452 }
00453 
00454 bool ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse &res)
00455 {
00456   if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
00457   {
00458     res.trajectory_.reserve(3);
00459 
00460     // add info about planned solution
00461     double ptime = getLastPlanTime();
00462     res.processing_time_.push_back(ptime);
00463     res.description_.push_back("plan");
00464     res.trajectory_.resize(res.trajectory_.size() + 1);
00465     res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName()));
00466     getSolutionPath(*res.trajectory_.back());
00467 
00468     // simplify solution if time remains
00469     if (simplify_solutions_ && ptime < request_.allowed_planning_time)
00470     {
00471       simplifySolution(request_.allowed_planning_time - ptime);
00472       res.processing_time_.push_back(getLastSimplifyTime());
00473       res.description_.push_back("simplify");
00474       res.trajectory_.resize(res.trajectory_.size() + 1);
00475       res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName()));
00476       getSolutionPath(*res.trajectory_.back());
00477     }
00478 
00479     ompl::time::point start_interpolate = ompl::time::now();
00480     interpolateSolution();
00481     res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate));
00482     res.description_.push_back("interpolate");
00483     res.trajectory_.resize(res.trajectory_.size() + 1);
00484     res.trajectory_.back().reset(new robot_trajectory::RobotTrajectory(getRobotModel(), getGroupName()));
00485     getSolutionPath(*res.trajectory_.back());
00486 
00487     // fill the response
00488     logDebug("%s: Returning successful solution with %lu states", getName().c_str(),
00489              getOMPLSimpleSetup().getSolutionPath().getStateCount());
00490     return true;
00491   }
00492   else
00493   {
00494     logInform("Unable to solve the planning problem");
00495     res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00496     return false;
00497   }
00498 }
00499 
00500 bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned int count)
00501 {
00502   moveit::Profiler::ScopedBlock sblock("PlanningContext:Solve");
00503   ompl::time::point start = ompl::time::now();
00504   preSolve();
00505 
00506   bool result = false;
00507   if (count <= 1)
00508   {
00509     logDebug("%s: Solving the planning problem once...", name_.c_str());
00510     ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
00511     registerTerminationCondition(ptc);
00512     result = ompl_simple_setup_.solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION;
00513     last_plan_time_ = ompl_simple_setup_.getLastPlanComputationTime();
00514     unregisterTerminationCondition();
00515   }
00516   else
00517   {
00518     logDebug("%s: Solving the planning problem %u times...", name_.c_str(), count);
00519     ompl_parallel_plan_.clearHybridizationPaths();
00520     if (count <= max_planning_threads_)
00521     {
00522       ompl_parallel_plan_.clearPlanners();
00523       if (ompl_simple_setup_.getPlannerAllocator())
00524     for (unsigned int i = 0 ; i < count ; ++i)
00525       ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_.getPlannerAllocator());
00526       else
00527     for (unsigned int i = 0 ; i < count ; ++i)
00528       ompl_parallel_plan_.addPlanner(ompl::geometric::getDefaultPlanner(ompl_simple_setup_.getGoal()));
00529 
00530       ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
00531       registerTerminationCondition(ptc);
00532       result = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
00533       last_plan_time_ = ompl::time::seconds(ompl::time::now() - start);
00534       unregisterTerminationCondition();
00535     }
00536     else
00537     {
00538       ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
00539       registerTerminationCondition(ptc);
00540       int n = count / max_planning_threads_;
00541       result = true;
00542       for (int i = 0 ; i < n && ptc() == false ; ++i)
00543       {
00544     ompl_parallel_plan_.clearPlanners();
00545     if (ompl_simple_setup_.getPlannerAllocator())
00546       for (unsigned int i = 0 ; i < max_planning_threads_ ; ++i)
00547         ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_.getPlannerAllocator());
00548     else
00549       for (unsigned int i = 0 ; i < max_planning_threads_ ; ++i)
00550         ompl_parallel_plan_.addPlanner(og::getDefaultPlanner(ompl_simple_setup_.getGoal()));
00551     bool r = ompl_parallel_plan_.solve(ptc, 1, max_planning_threads_, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
00552     result = result && r;
00553       }
00554       n = count % max_planning_threads_;
00555       if (n && ptc() == false)
00556       {
00557     ompl_parallel_plan_.clearPlanners();
00558     if (ompl_simple_setup_.getPlannerAllocator())
00559       for (int i = 0 ; i < n ; ++i)
00560         ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_.getPlannerAllocator());
00561     else
00562       for (int i = 0 ; i < n ; ++i)
00563         ompl_parallel_plan_.addPlanner(og::getDefaultPlanner(ompl_simple_setup_.getGoal()));
00564     bool r = ompl_parallel_plan_.solve(ptc, 1, n, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
00565     result = result && r;
00566       }
00567       last_plan_time_ = ompl::time::seconds(ompl::time::now() - start);
00568       unregisterTerminationCondition();
00569     }
00570   }
00571 
00572   postSolve();
00573 
00574   return result;
00575 }
00576 
00577 void ompl_interface::ModelBasedPlanningContext::registerTerminationCondition(const ob::PlannerTerminationCondition &ptc)
00578 {
00579   boost::mutex::scoped_lock slock(ptc_lock_);
00580   ptc_ = &ptc;
00581 }
00582 
00583 void ompl_interface::ModelBasedPlanningContext::unregisterTerminationCondition()
00584 {
00585   boost::mutex::scoped_lock slock(ptc_lock_);
00586   ptc_ = NULL;
00587 }
00588 
00589 bool ompl_interface::ModelBasedPlanningContext::terminate()
00590 {
00591   boost::mutex::scoped_lock slock(ptc_lock_);
00592   if (ptc_)
00593     ptc_->terminate();
00594   return true;
00595 }


ompl
Author(s): Ioan Sucan
autogenerated on Mon Oct 6 2014 11:12:03