model_based_planning_context.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 
37 #include <boost/algorithm/string/trim.hpp>
38 
49 
50 #include <ompl/base/samplers/UniformValidStateSampler.h>
51 #include <ompl/base/goals/GoalLazySamples.h>
52 #include <ompl/tools/config/SelfConfig.h>
53 #include <ompl/base/spaces/SE3StateSpace.h>
54 #include <ompl/datastructures/PDF.h>
55 
56 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
57 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
58 #include "ompl/base/objectives/MinimaxObjective.h"
59 #include "ompl/base/objectives/StateCostIntegralObjective.h"
60 #include "ompl/base/objectives/MaximizeMinClearanceObjective.h"
61 
64  : planning_interface::PlanningContext(name, spec.state_space_->getJointModelGroup()->getName())
65  , spec_(spec)
66  , complete_initial_robot_state_(spec.state_space_->getRobotModel())
67  , ompl_simple_setup_(spec.ompl_simple_setup_)
68  , ompl_benchmark_(*ompl_simple_setup_)
69  , ompl_parallel_plan_(ompl_simple_setup_->getProblemDefinition())
70  , ptc_(NULL)
71  , last_plan_time_(0.0)
72  , last_simplify_time_(0.0)
73  , max_goal_samples_(0)
74  , max_state_sampling_attempts_(0)
75  , max_goal_sampling_attempts_(0)
76  , max_planning_threads_(0)
77  , max_solution_segment_length_(0.0)
78  , minimum_waypoint_count_(0)
79  , use_state_validity_cache_(true)
80  , simplify_solutions_(true)
81 {
83  ompl_simple_setup_->getStateSpace()->computeSignature(space_signature_);
84  ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator(
86 }
87 
89 {
90  if (!spec_.state_space_)
91  {
92  ROS_ERROR_NAMED("model_based_planning_context", "No state space is configured yet");
93  return;
94  }
95  ob::ProjectionEvaluatorPtr pe = getProjectionEvaluator(peval);
96  if (pe)
97  spec_.state_space_->registerDefaultProjection(pe);
98 }
99 
100 ompl::base::ProjectionEvaluatorPtr
102 {
103  if (peval.find_first_of("link(") == 0 && peval[peval.length() - 1] == ')')
104  {
105  std::string link_name = peval.substr(5, peval.length() - 6);
106  if (getRobotModel()->hasLinkModel(link_name))
107  return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorLinkPose(this, link_name));
108  else
109  ROS_ERROR_NAMED("model_based_planning_context",
110  "Attempted to set projection evaluator with respect to position of link '%s', "
111  "but that link is not known to the kinematic model.",
112  link_name.c_str());
113  }
114  else if (peval.find_first_of("joints(") == 0 && peval[peval.length() - 1] == ')')
115  {
116  std::string joints = peval.substr(7, peval.length() - 8);
117  boost::replace_all(joints, ",", " ");
118  std::vector<unsigned int> j;
119  std::stringstream ss(joints);
120  while (ss.good() && !ss.eof())
121  {
122  std::string v;
123  ss >> v >> std::ws;
124  if (getJointModelGroup()->hasJointModel(v))
125  {
126  unsigned int vc = getJointModelGroup()->getJointModel(v)->getVariableCount();
127  if (vc > 0)
128  {
129  int idx = getJointModelGroup()->getVariableGroupIndex(v);
130  for (int q = 0; q < vc; ++q)
131  j.push_back(idx + q);
132  }
133  else
134  ROS_WARN_NAMED("model_based_planning_context", "%s: Ignoring joint '%s' in projection since it has 0 DOF",
135  name_.c_str(), v.c_str());
136  }
137  else
138  ROS_ERROR_NAMED("model_based_planning_context",
139  "%s: Attempted to set projection evaluator with respect to value of joint "
140  "'%s', but that joint is not known to the group '%s'.",
141  name_.c_str(), v.c_str(), getGroupName().c_str());
142  }
143  if (j.empty())
144  ROS_ERROR_NAMED("model_based_planning_context", "%s: No valid joints specified for joint projection",
145  name_.c_str());
146  else
147  return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorJointValue(this, j));
148  }
149  else
150  ROS_ERROR_NAMED("model_based_planning_context",
151  "Unable to allocate projection evaluator based on description: '%s'", peval.c_str());
152  return ob::ProjectionEvaluatorPtr();
153 }
154 
155 ompl::base::StateSamplerPtr
157 {
158  if (spec_.state_space_.get() != ss)
159  {
160  ROS_ERROR_NAMED("model_based_planning_context",
161  "%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str());
162  return ompl::base::StateSamplerPtr();
163  }
164 
165  ROS_DEBUG_NAMED("model_based_planning_context",
166  "%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str());
167 
168  if (path_constraints_)
169  {
171  {
172  const ConstraintApproximationPtr& ca =
173  spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_);
174  if (ca)
175  {
176  ompl::base::StateSamplerAllocator c_ssa = ca->getStateSamplerAllocator(path_constraints_msg_);
177  if (c_ssa)
178  {
179  ompl::base::StateSamplerPtr res = c_ssa(ss);
180  if (res)
181  {
182  ROS_INFO_NAMED("model_based_planning_context",
183  "%s: Using precomputed state sampler (approximated constraint space) for constraint '%s'",
184  name_.c_str(), path_constraints_msg_.name.c_str());
185  return res;
186  }
187  }
188  }
189  }
190 
191  constraint_samplers::ConstraintSamplerPtr cs;
194  path_constraints_->getAllConstraints());
195 
196  if (cs)
197  {
198  ROS_INFO_NAMED("model_based_planning_context", "%s: Allocating specialized state sampler for state space",
199  name_.c_str());
200  return ob::StateSamplerPtr(new ConstrainedSampler(this, cs));
201  }
202  }
203  ROS_DEBUG_NAMED("model_based_planning_context", "%s: Allocating default state sampler for state space",
204  name_.c_str());
205  return ss->allocDefaultStateSampler();
206 }
207 
209 {
210  // convert the input state to the corresponding OMPL state
211  ompl::base::ScopedState<> ompl_start_state(spec_.state_space_);
212  spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
213  ompl_simple_setup_->setStartState(ompl_start_state);
214  ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this)));
215 
217  {
218  const ConstraintApproximationPtr& ca =
219  spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_);
220  if (ca)
221  {
222  getOMPLStateSpace()->setInterpolationFunction(ca->getInterpolationFunction());
223  ROS_INFO_NAMED("model_based_planning_context", "Using precomputed interpolation states");
224  }
225  }
226 
227  useConfig();
228  if (ompl_simple_setup_->getGoal())
229  ompl_simple_setup_->setup();
230 }
231 
233 {
234  const std::map<std::string, std::string>& config = spec_.config_;
235  if (config.empty())
236  return;
237  std::map<std::string, std::string> cfg = config;
238 
239  // set the distance between waypoints when interpolating and collision checking.
240  std::map<std::string, std::string>::iterator it = cfg.find("longest_valid_segment_fraction");
241  // If one of the two variables is set.
242  if (it != cfg.end() || max_solution_segment_length_ != 0.0)
243  {
244  // clang-format off
245  double longest_valid_segment_fraction_config = (it != cfg.end())
246  ? boost::lexical_cast<double>(it->second) // value from config file if there
247  : 0.01; // default value in OMPL.
248  double longest_valid_segment_fraction_final = longest_valid_segment_fraction_config;
250  {
251  // If this parameter is specified too, take the most conservative of the two variables,
252  // i.e. the one that uses the shorter segment length.
253  longest_valid_segment_fraction_final = std::min(
254  longest_valid_segment_fraction_config,
255  max_solution_segment_length_ / spec_.state_space_->getMaximumExtent()
256  );
257  }
258  // clang-format on
259  cfg["longest_valid_segment_fraction"] = boost::lexical_cast<std::string>(longest_valid_segment_fraction_final);
260  }
261 
262  // set the projection evaluator
263  it = cfg.find("projection_evaluator");
264  if (it != cfg.end())
265  {
266  setProjectionEvaluator(boost::trim_copy(it->second));
267  cfg.erase(it);
268  }
269 
270  if (cfg.empty())
271  return;
272 
273  std::string optimizer;
274  ompl::base::OptimizationObjectivePtr objective;
275  it = cfg.find("optimization_objective");
276  if (it == cfg.end())
277  {
278  optimizer = "PathLengthOptimizationObjective";
279  ROS_DEBUG_NAMED("model_based_planning_context", "No optimization objective specified, defaulting to %s",
280  optimizer.c_str());
281  }
282  else
283  {
284  optimizer = it->second;
285  cfg.erase(it);
286  }
287 
288  if (optimizer == "PathLengthOptimizationObjective")
289  {
290  objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
291  }
292  else if (optimizer == "MinimaxObjective")
293  {
294  objective.reset(new ompl::base::MinimaxObjective(ompl_simple_setup_->getSpaceInformation()));
295  }
296  else if (optimizer == "StateCostIntegralObjective")
297  {
298  objective.reset(new ompl::base::StateCostIntegralObjective(ompl_simple_setup_->getSpaceInformation()));
299  }
300  else if (optimizer == "MechanicalWorkOptimizationObjective")
301  {
302  objective.reset(new ompl::base::MechanicalWorkOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
303  }
304  else if (optimizer == "MaximizeMinClearanceObjective")
305  {
306  objective.reset(new ompl::base::MaximizeMinClearanceObjective(ompl_simple_setup_->getSpaceInformation()));
307  }
308  else
309  {
310  objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
311  }
312 
313  ompl_simple_setup_->setOptimizationObjective(objective);
314 
315  // remove the 'type' parameter; the rest are parameters for the planner itself
316  it = cfg.find("type");
317  if (it == cfg.end())
318  {
319  if (name_ != getGroupName())
320  ROS_WARN_NAMED("model_based_planning_context", "%s: Attribute 'type' not specified in planner configuration",
321  name_.c_str());
322  }
323  else
324  {
325  std::string type = it->second;
326  cfg.erase(it);
327  ompl_simple_setup_->setPlannerAllocator(
328  boost::bind(spec_.planner_selector_(type), _1, name_ != getGroupName() ? name_ : "", spec_));
329  ROS_INFO_NAMED("model_based_planning_context",
330  "Planner configuration '%s' will use planner '%s'. "
331  "Additional configuration parameters will be set when the planner is constructed.",
332  name_.c_str(), type.c_str());
333  }
334 
335  // call the setParams() after setup(), so we know what the params are
336  ompl_simple_setup_->getSpaceInformation()->setup();
337  ompl_simple_setup_->getSpaceInformation()->params().setParams(cfg, true);
338  // call setup() again for possibly new param values
339  ompl_simple_setup_->getSpaceInformation()->setup();
340 }
341 
342 void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_msgs::WorkspaceParameters& wparams)
343 {
344  if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 &&
345  wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 &&
346  wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0)
347  ROS_WARN_NAMED("model_based_planning_context", "It looks like the planning volume was not specified.");
348 
349  ROS_DEBUG_NAMED("model_based_planning_context",
350  "%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = "
351  "[%f, %f], z = [%f, %f]",
352  name_.c_str(), wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y, wparams.max_corner.y,
353  wparams.min_corner.z, wparams.max_corner.z);
354 
355  spec_.state_space_->setPlanningVolume(wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y,
356  wparams.max_corner.y, wparams.min_corner.z, wparams.max_corner.z);
357 }
358 
360 {
361  ompl_simple_setup_->simplifySolution(timeout);
362  last_simplify_time_ = ompl_simple_setup_->getLastSimplificationTime();
363 }
364 
366 {
367  if (ompl_simple_setup_->haveSolutionPath())
368  {
369  og::PathGeometric& pg = ompl_simple_setup_->getSolutionPath();
370 
371  // Find the number of states that will be in the interpolated solution.
372  // This is what interpolate() does internally.
373  int eventual_states = 1;
374  std::vector<ompl::base::State*> states = pg.getStates();
375  for (size_t i = 0; i < states.size() - 1; i++)
376  {
377  eventual_states += ompl_simple_setup_->getStateSpace()->validSegmentCount(states[i], states[i + 1]);
378  }
379 
380  if (eventual_states < minimum_waypoint_count_)
381  {
382  // If that's not enough states, use the minimum amount instead.
383  pg.interpolate(minimum_waypoint_count_);
384  }
385  else
386  {
387  // Interpolate the path to have as the exact states that are checked when validating motions.
388  pg.interpolate();
389  }
390  }
391 }
392 
393 void ompl_interface::ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric& pg,
395 {
396  robot_state::RobotState ks = complete_initial_robot_state_;
397  for (std::size_t i = 0; i < pg.getStateCount(); ++i)
398  {
399  spec_.state_space_->copyToRobotState(ks, pg.getState(i));
400  traj.addSuffixWayPoint(ks, 0.0);
401  }
402 }
403 
405 {
406  traj.clear();
407  if (!ompl_simple_setup_->haveSolutionPath())
408  return false;
409  convertPath(ompl_simple_setup_->getSolutionPath(), traj);
410  return true;
411 }
412 
414 {
415  if (ompl_simple_setup_->getStateValidityChecker())
416  static_cast<StateValidityChecker*>(ompl_simple_setup_->getStateValidityChecker().get())->setVerbose(flag);
417 }
418 
420 {
421  // ******************* set up the goal representation, based on goal constraints
422 
423  std::vector<ob::GoalPtr> goals;
424  for (std::size_t i = 0; i < goal_constraints_.size(); ++i)
425  {
426  constraint_samplers::ConstraintSamplerPtr cs;
429  goal_constraints_[i]->getAllConstraints());
430  if (cs)
431  {
432  ob::GoalPtr g = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraints_[i], cs));
433  goals.push_back(g);
434  }
435  }
436 
437  if (!goals.empty())
438  return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals));
439  else
440  ROS_ERROR_NAMED("model_based_planning_context", "Unable to construct goal representation");
441 
442  return ob::GoalPtr();
443 }
444 
446  const robot_state::RobotState& complete_initial_robot_state)
447 {
448  complete_initial_robot_state_ = complete_initial_robot_state;
450 }
451 
453 {
454  ompl_simple_setup_->clear();
455  ompl_simple_setup_->clearStartStates();
456  ompl_simple_setup_->setGoal(ob::GoalPtr());
457  ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr());
458  path_constraints_.reset();
459  goal_constraints_.clear();
460  getOMPLStateSpace()->setInterpolationFunction(InterpolationFunction());
461 }
462 
463 bool ompl_interface::ModelBasedPlanningContext::setPathConstraints(const moveit_msgs::Constraints& path_constraints,
464  moveit_msgs::MoveItErrorCodes* error)
465 {
466  // ******************* set the path constraints to use
468  path_constraints_->add(path_constraints, getPlanningScene()->getTransforms());
469  path_constraints_msg_ = path_constraints;
470 
471  return true;
472 }
473 
475  const std::vector<moveit_msgs::Constraints>& goal_constraints, const moveit_msgs::Constraints& path_constraints,
476  moveit_msgs::MoveItErrorCodes* error)
477 {
478  // ******************* check if the input is correct
479  goal_constraints_.clear();
480  for (std::size_t i = 0; i < goal_constraints.size(); ++i)
481  {
482  moveit_msgs::Constraints constr = kinematic_constraints::mergeConstraints(goal_constraints[i], path_constraints);
483  kinematic_constraints::KinematicConstraintSetPtr kset(
485  kset->add(constr, getPlanningScene()->getTransforms());
486  if (!kset->empty())
487  goal_constraints_.push_back(kset);
488  }
489 
490  if (goal_constraints_.empty())
491  {
492  ROS_WARN_NAMED("model_based_planning_context", "%s: No goal constraints specified. There is no problem to solve.",
493  name_.c_str());
494  if (error)
495  error->val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
496  return false;
497  }
498 
499  ob::GoalPtr goal = constructGoal();
500  ompl_simple_setup_->setGoal(goal);
501  if (goal)
502  return true;
503  else
504  return false;
505 }
506 
507 bool ompl_interface::ModelBasedPlanningContext::benchmark(double timeout, unsigned int count,
508  const std::string& filename)
509 {
510  ompl_benchmark_.clearPlanners();
511  ompl_simple_setup_->setup();
512  ompl_benchmark_.addPlanner(ompl_simple_setup_->getPlanner());
513  ompl_benchmark_.setExperimentName(getRobotModel()->getName() + "_" + getGroupName() + "_" +
514  getPlanningScene()->getName() + "_" + name_);
515 
516  ot::Benchmark::Request req;
517  req.maxTime = timeout;
518  req.runCount = count;
519  req.displayProgress = true;
520  req.saveConsoleOutput = false;
521  ompl_benchmark_.benchmark(req);
522  return filename.empty() ? ompl_benchmark_.saveResultsToFile() : ompl_benchmark_.saveResultsToFile(filename.c_str());
523 }
524 
526 {
527  bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
528  if (gls)
529  static_cast<ob::GoalLazySamples*>(ompl_simple_setup_->getGoal().get())->startSampling();
530  else
531  // we know this is a GoalSampleableMux by elimination
532  static_cast<GoalSampleableRegionMux*>(ompl_simple_setup_->getGoal().get())->startSampling();
533 }
534 
536 {
537  bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
538  if (gls)
539  static_cast<ob::GoalLazySamples*>(ompl_simple_setup_->getGoal().get())->stopSampling();
540  else
541  // we know this is a GoalSampleableMux by elimination
542  static_cast<GoalSampleableRegionMux*>(ompl_simple_setup_->getGoal().get())->stopSampling();
543 }
544 
546 {
547  // clear previously computed solutions
548  ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths();
549  const ob::PlannerPtr planner = ompl_simple_setup_->getPlanner();
550  if (planner)
551  planner->clear();
552  startSampling();
553  ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
554 }
555 
557 {
558  stopSampling();
559  int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount();
560  int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount();
561  ROS_DEBUG_NAMED("model_based_planning_context", "There were %d valid motions and %d invalid motions.", v, iv);
562 
563  if (ompl_simple_setup_->getProblemDefinition()->hasApproximateSolution())
564  ROS_WARN_NAMED("model_based_planning_context", "Computed solution is approximate");
565 }
566 
568 {
569  if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
570  {
571  double ptime = getLastPlanTime();
573  {
574  simplifySolution(request_.allowed_planning_time - ptime);
575  ptime += getLastSimplifyTime();
576  }
578 
579  // fill the response
580  ROS_DEBUG_NAMED("model_based_planning_context", "%s: Returning successful solution with %lu states",
581  getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount());
582 
585  res.planning_time_ = ptime;
586  return true;
587  }
588  else
589  {
590  ROS_INFO_NAMED("model_based_planning_context", "Unable to solve the planning problem");
591  res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
592  return false;
593  }
594 }
595 
597 {
598  if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
599  {
600  res.trajectory_.reserve(3);
601 
602  // add info about planned solution
603  double ptime = getLastPlanTime();
604  res.processing_time_.push_back(ptime);
605  res.description_.push_back("plan");
606  res.trajectory_.resize(res.trajectory_.size() + 1);
608  getSolutionPath(*res.trajectory_.back());
609 
610  // simplify solution if time remains
612  {
613  simplifySolution(request_.allowed_planning_time - ptime);
614  res.processing_time_.push_back(getLastSimplifyTime());
615  res.description_.push_back("simplify");
616  res.trajectory_.resize(res.trajectory_.size() + 1);
618  getSolutionPath(*res.trajectory_.back());
619  }
620 
621  ompl::time::point start_interpolate = ompl::time::now();
623  res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate));
624  res.description_.push_back("interpolate");
625  res.trajectory_.resize(res.trajectory_.size() + 1);
627  getSolutionPath(*res.trajectory_.back());
628 
629  // fill the response
630  ROS_DEBUG_NAMED("model_based_planning_context", "%s: Returning successful solution with %lu states",
631  getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount());
632  return true;
633  }
634  else
635  {
636  ROS_INFO_NAMED("model_based_planning_context", "Unable to solve the planning problem");
637  res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
638  return false;
639  }
640 }
641 
642 bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned int count)
643 {
644  moveit::tools::Profiler::ScopedBlock sblock("PlanningContext:Solve");
645  ompl::time::point start = ompl::time::now();
646  preSolve();
647 
648  bool result = false;
649  if (count <= 1)
650  {
651  ROS_DEBUG_NAMED("model_based_planning_context", "%s: Solving the planning problem once...", name_.c_str());
652  ob::PlannerTerminationCondition ptc =
653  ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
655  result = ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION;
656  last_plan_time_ = ompl_simple_setup_->getLastPlanComputationTime();
658  }
659  else
660  {
661  ROS_DEBUG_NAMED("model_based_planning_context", "%s: Solving the planning problem %u times...", name_.c_str(),
662  count);
663  ompl_parallel_plan_.clearHybridizationPaths();
664  if (count <= max_planning_threads_)
665  {
666  ompl_parallel_plan_.clearPlanners();
667  if (ompl_simple_setup_->getPlannerAllocator())
668  for (unsigned int i = 0; i < count; ++i)
669  ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
670  else
671  for (unsigned int i = 0; i < count; ++i)
672  ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
673 
674  ob::PlannerTerminationCondition ptc =
675  ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
677  result = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
678  last_plan_time_ = ompl::time::seconds(ompl::time::now() - start);
680  }
681  else
682  {
683  ob::PlannerTerminationCondition ptc =
684  ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
686  int n = count / max_planning_threads_;
687  result = true;
688  for (int i = 0; i < n && ptc() == false; ++i)
689  {
690  ompl_parallel_plan_.clearPlanners();
691  if (ompl_simple_setup_->getPlannerAllocator())
692  for (unsigned int i = 0; i < max_planning_threads_; ++i)
693  ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
694  else
695  for (unsigned int i = 0; i < max_planning_threads_; ++i)
696  ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
697  bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
698  result = result && r;
699  }
700  n = count % max_planning_threads_;
701  if (n && ptc() == false)
702  {
703  ompl_parallel_plan_.clearPlanners();
704  if (ompl_simple_setup_->getPlannerAllocator())
705  for (int i = 0; i < n; ++i)
706  ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
707  else
708  for (int i = 0; i < n; ++i)
709  ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
710  bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
711  result = result && r;
712  }
713  last_plan_time_ = ompl::time::seconds(ompl::time::now() - start);
715  }
716  }
717 
718  postSolve();
719 
720  return result;
721 }
722 
723 void ompl_interface::ModelBasedPlanningContext::registerTerminationCondition(const ob::PlannerTerminationCondition& ptc)
724 {
725  boost::mutex::scoped_lock slock(ptc_lock_);
726  ptc_ = &ptc;
727 }
728 
730 {
731  boost::mutex::scoped_lock slock(ptc_lock_);
732  ptc_ = NULL;
733 }
734 
736 {
737  boost::mutex::scoped_lock slock(ptc_lock_);
738  if (ptc_)
739  ptc_->terminate();
740  return true;
741 }
double last_simplify_time_
the time spent simplifying the last plan
robot_trajectory::RobotTrajectoryPtr trajectory_
ot::Benchmark ompl_benchmark_
the OMPL tool for benchmarking planners
#define ROS_INFO_NAMED(name,...)
#define ROS_WARN_NAMED(name,...)
std::string getName(void *handle)
const og::SimpleSetupPtr & getOMPLSimpleSetup() const
const robot_model::JointModelGroup * getJointModelGroup() const
void setPlanningVolume(const moveit_msgs::WorkspaceParameters &wparams)
const robot_state::RobotState & getCompleteInitialRobotState() const
const planning_scene::PlanningSceneConstPtr & getPlanningScene() const
moveit_msgs::MoveItErrorCodes error_code_
unsigned int max_planning_threads_
when planning in parallel, this is the maximum number of threads to use at one time ...
#define ROS_DEBUG_NAMED(name,...)
kinematic_constraints::KinematicConstraintSetPtr path_constraints_
ModelBasedPlanningContext(const std::string &name, const ModelBasedPlanningContextSpecification &spec)
double last_plan_time_
the time spent computing the last plan
const ob::PlannerTerminationCondition * ptc_
ot::ParallelPlan ompl_parallel_plan_
tool used to compute multiple plans in parallel; this uses the problem definition maintained by ompl_...
moveit_msgs::MoveItErrorCodes error_code_
virtual ob::ProjectionEvaluatorPtr getProjectionEvaluator(const std::string &peval) const
virtual ob::StateSamplerPtr allocPathConstrainedSampler(const ompl::base::StateSpace *ss) const
og::SimpleSetupPtr ompl_simple_setup_
the OMPL planning context; this contains the problem definition and the planner used ...
ModelBasedPlanningContextSpecification spec_
const std::string & getGroupName() const
bool setGoalConstraints(const std::vector< moveit_msgs::Constraints > &goal_constraints, const moveit_msgs::Constraints &path_constraints, moveit_msgs::MoveItErrorCodes *error)
void registerTerminationCondition(const ob::PlannerTerminationCondition &ptc)
constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_
bool benchmark(double timeout, unsigned int count, const std::string &filename="")
std::vector< kinematic_constraints::KinematicConstraintSetPtr > goal_constraints_
void convertPath(const og::PathGeometric &pg, robot_trajectory::RobotTrajectory &traj) const
std::vector< robot_trajectory::RobotTrajectoryPtr > trajectory_
const std::string & getName() const
bool setPathConstraints(const moveit_msgs::Constraints &path_constraints, moveit_msgs::MoveItErrorCodes *error)
#define ROS_ERROR_NAMED(name,...)
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
An interface for a OMPL state validity checker.
bool getSolutionPath(robot_trajectory::RobotTrajectory &traj) const
void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
const robot_model::RobotModelConstPtr & getRobotModel() const
r
virtual bool solve(planning_interface::MotionPlanResponse &res)
boost::function< bool(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state)> InterpolationFunction
void setCompleteInitialState(const robot_state::RobotState &complete_initial_robot_state)
const ModelBasedStateSpacePtr & getOMPLStateSpace() const


ompl
Author(s): Ioan Sucan
autogenerated on Thu Jul 12 2018 02:54:29