OrderedInfSampler.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Toronto
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 the University of Toronto 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 /* Authors: Jonathan Gammell */
36 
37 #include "ompl/base/samplers/informed/OrderedInfSampler.h"
38 #include "ompl/base/OptimizationObjective.h"
39 
40 namespace ompl
41 {
42  namespace base
43  {
44  // The default rejection-sampling class:
45  OrderedInfSampler::OrderedInfSampler(const InformedSamplerPtr &infSamplerPtr, unsigned int batchSize)
46  : InformedSampler(infSamplerPtr->getProblemDefn(), infSamplerPtr->getMaxNumberOfIters())
47  , infSampler_(infSamplerPtr)
48  , batchSize_(batchSize)
49  , orderedSamples_([this](const State *lhs, const State *rhs)
50  {
51  return queueComparator(lhs, rhs);
52  })
53  {
54  }
55 
56  bool OrderedInfSampler::sampleUniform(State *statePtr, const Cost &maxCost)
57  {
58  // Variables
59  // Whether a sampler has been found and returned
60  bool found = false;
61 
62  // Repeat until a valid pointer is found
63  while (!found)
64  {
65  // Check if the batch is empty
66  if (orderedSamples_.empty())
67  {
68  // It is, recreate:
69  createBatch(maxCost);
70  }
71 
72  // Does the front of the priority queue meet our requirement (as the requirement may have changed since
73  // the batch was generated)
74  if (InformedSampler::opt_->isCostBetterThan(InformedSampler::heuristicSolnCost(orderedSamples_.top()),
75  maxCost))
76  {
77  // Copy the front of the priority queue.
78  InformedSampler::space_->copyState(statePtr, orderedSamples_.top());
79 
80  // Free the pointer in the queue
81  InformedSampler::space_->freeState(orderedSamples_.top());
82 
83  // Pop it
84  orderedSamples_.pop();
85 
86  // And mark that we've found a sample
87  found = true;
88  }
89  else
90  {
91  // It does not, clear the queue
92  clearBatch();
93  }
94  }
95 
96  return found;
97  }
98 
99  bool OrderedInfSampler::sampleUniform(State *, const Cost &, const Cost &)
100  {
101  throw ompl::Exception("Not implemented");
102 
103  return false;
104  }
105 
107  {
108  return infSampler_->hasInformedMeasure();
109  }
110 
111  double OrderedInfSampler::getInformedMeasure(const Cost &currentCost) const
112  {
113  return infSampler_->getInformedMeasure(currentCost);
114  }
115 
116  bool OrderedInfSampler::queueComparator(const State *a, const State *b)
117  {
118  return InformedSampler::opt_->isCostBetterThan(InformedSampler::heuristicSolnCost(b),
120  }
121 
122  void OrderedInfSampler::createBatch(const Cost &maxCost)
123  {
124  // Allocate, create and store batchSize_ samples
125  for (unsigned int i = 0u; i < batchSize_; ++i)
126  {
127  // Allocate a state pointer
128  State *newStatePtr = InformedSampler::space_->allocState();
129 
130  // Sample the state pointer using the wrapped sampler
131  infSampler_->sampleUniform(newStatePtr, maxCost);
132 
133  // Store it into the queue
134  orderedSamples_.push(newStatePtr);
135  }
136  }
137 
138  void OrderedInfSampler::createBatch(const Cost &, const Cost &)
139  {
140  throw ompl::Exception("Not implemented");
141  }
142 
143  void OrderedInfSampler::clearBatch()
144  {
145  // Iterate through the entire queue, removing the element and freeing it.
146  while (!orderedSamples_.empty())
147  {
148  // Free the front state
149  InformedSampler::space_->freeState(orderedSamples_.top());
150 
151  // Pop the front state
152  orderedSamples_.pop();
153  }
154  }
155  }; // base
156 }; // ompl
Definition of an abstract state.
Definition: State.h:113
bool hasInformedMeasure() const override
Whether the wrapped sampler can provide a measure of the informed subset.
double getInformedMeasure(const Cost &currentCost) const override
The measure of the subset of the state space defined by the current solution cost that is being searc...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
OrderedInfSampler(const InformedSamplerPtr &infSamplerPtr, unsigned int batchSize)
Construct an ordering wrapper around the provided informed sampler.
OptimizationObjectivePtr opt_
A copy of the optimization objective.
bool sampleUniform(State *statePtr, const Cost &maxCost) override
Sample uniformly in the subset of the state space whose heuristic solution estimates are less than th...
virtual Cost heuristicSolnCost(const State *statePtr) const
A helper function to calculate the heuristic estimate of the solution cost for a given state using th...
StateSpacePtr space_
A copy of the state space.
The exception type for ompl.
Definition: Exception.h:78
Main namespace. Contains everything in this library.