All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
LazyPRM.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Willow Garage
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, Ryan Luna */
36 
37 #include "ompl/geometric/planners/prm/LazyPRM.h"
38 #include "ompl/base/OptimizationObjective.h"
39 #include <boost/graph/incremental_components.hpp>
40 #include <boost/graph/lookup_edge.hpp>
41 #include <boost/foreach.hpp>
42 
43 #define foreach BOOST_FOREACH
44 
46  PRM(si, starStrategy),
47  vertexValidityProperty_(boost::get(vertex_flags_t(), g_)),
48  edgeValidityProperty_(boost::get(edge_flags_t(), g_))
49 {
50  setName("LazyPRM");
51 }
52 
53 ompl::geometric::LazyPRM::~LazyPRM()
54 {
55 }
56 
57 ompl::geometric::PRM::Vertex ompl::geometric::LazyPRM::addMilestone(base::State *state)
58 {
59  boost::mutex::scoped_lock _(graphMutex_);
60 
61  Vertex m = boost::add_vertex(g_);
62  stateProperty_[m] = state;
63  vertexValidityProperty_[m] = VALIDITY_UNKNOWN;
64 
65  // Initialize to its own (dis)connected component.
66  disjointSets_.make_set(m);
67 
68  nn_->add(m);
69 
70  // Which milestones will we attempt to connect to?
71 
72  const std::vector<Vertex>& neighbors = connectionStrategy_(m);
73 
74  foreach (Vertex n, neighbors)
75  if (connectionFilter_(m, n))
76  {
77  const base::Cost weight = opt_->motionCost(stateProperty_[m], stateProperty_[n]);
78  const unsigned int id = maxEdgeID_++;
79  const Graph::edge_property_type properties(weight, id);
80  const Edge &e = boost::add_edge(m, n, properties, g_).first;
81  edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
82  uniteComponents(n, m);
83  }
84 
85  return m;
86 }
87 
89  base::State *workState)
90 {
91  /* grow roadmap in lazy fashion -- add vertices and edges without checking validity */
92  while (ptc == false)
93  {
94  simpleSampler_->sampleUniform(workState);
95  addMilestone(si_->cloneState(workState));
96  }
97 }
98 
100 {
101  if (!isSetup())
102  setup();
103  if (!simpleSampler_)
104  simpleSampler_ = si_->allocStateSampler();
105 
106  growRoadmap(ptc);
107 }
108 
109 ompl::base::PathPtr ompl::geometric::LazyPRM::constructGeometricPath(const boost::vector_property_map<Vertex> &prev, const Vertex &start, const Vertex &goal)
110 {
111  // first, get the solution states without copying them
112  std::vector<const base::State*> states;
113  Vertex prevVertex = goal;
114  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
115  {
116  const base::State *st = stateProperty_[pos];
117  unsigned int &vd = vertexValidityProperty_[pos];
118  if ((vd & VALIDITY_TRUE) == 0)
119  if (si_->isValid(st))
120  vd |= VALIDITY_TRUE;
121  if ((vd & VALIDITY_TRUE) == 0)
122  {
123  states.clear();
124  // remove vertex from graph
125  nn_->remove(pos);
126  si_->freeState(stateProperty_[pos]);
127  boost::clear_vertex(pos, g_);
128  boost::remove_vertex(pos, g_);
129  break;
130  }
131  else
132  {
133  // check the edge too, if the vertex was valid
134  if (prevVertex != pos)
135  {
136  Edge e = boost::lookup_edge(prevVertex, pos, g_).first;
137  unsigned int &evd = edgeValidityProperty_[e];
138  if ((evd & VALIDITY_TRUE) == 0)
139  if (si_->checkMotion(states.back(), st))
140  evd |= VALIDITY_TRUE;
141  if ((evd & VALIDITY_TRUE) == 0)
142  {
143  states.clear();
144  boost::remove_edge(e, g_);
145  break;
146  }
147  }
148  }
149  prevVertex = pos;
150  states.push_back(st);
151  }
152 
153  if (states.empty())
154  return base::PathPtr();
155  else
156  // start is checked for validity already
157  states.push_back(stateProperty_[start]);
158 
159  PathGeometric *p = new PathGeometric(si_);
160  for (std::size_t i = 0 ; i < states.size() ; ++i)
161  p->append(states[i]);
162  p->reverse();
163  return base::PathPtr(p);
164 }
virtual Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
Definition: LazyPRM.cpp:57
LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: LazyPRM.cpp:45
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
virtual void growRoadmap(const base::PlannerTerminationCondition &ptc, base::State *workState)
Randomly sample the state space, add and connect milestones in the roadmap. Stop this process when th...
Definition: LazyPRM.cpp:88
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:60
virtual base::PathPtr constructGeometricPath(const boost::vector_property_map< Vertex > &prev, const Vertex &start, const Vertex &goal)
Given a solution represented as a vector of predecesors in the roadmap, construct a geometric path...
Definition: LazyPRM.cpp:109
virtual void constructRoadmap(const base::PlannerTerminationCondition &ptc)
For LazyPRM, this simply calls growRoadmap()
Definition: LazyPRM.cpp:99
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void reverse(void)
Reverse the path.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
Probabilistic RoadMap planner.
Definition: PRM.h:83
Definition of a geometric path.
Definition: PathGeometric.h:55
A boost shared pointer wrapper for ompl::base::Path.