GoalSpace.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Oxford.
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 Oxford 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: Jonathan Gammell */
36 
37 #include "ompl/base/goals/GoalSpace.h"
38 #include <limits>
39 
40 ompl::base::GoalSpace::~GoalSpace()
41 {
42 }
43 
45 {
46  goalSampler_->sampleUniform(st);
47 }
48 
50 {
51  return std::numeric_limits<unsigned int>::max();
52 }
53 
55 {
56  // Short circuit distance calculation if satisfied.
57  if (goalSpace_->satisfiesBounds(st))
58  {
59  return 0.0;
60  }
61  // Else, do the work
62 
63  // Allocate a new state as a copy of the query
64  State *proj = si_->allocState();
65  si_->copyState(proj, st);
66 
67  // Bring the copy into the bounds of the goal
68  goalSpace_->enforceBounds(proj);
69 
70  // Calculate the distance between those two states
71  double dist = si_->distance(st, proj);
72 
73  // Free the copied projection
74  si_->freeState(proj);
75 
76  // Return the distance
77  return dist;
78 }
79 
80 void ompl::base::GoalSpace::print(std::ostream &out) const
81 {
82  out << "Goal space, threshold = " << threshold_ << ", memory address = " << this << ", volume = " << goalSpace_->getMeasure() << std::endl;
83 }
84 
86 {
87  if (space->getType() != si_->getStateSpace()->getType())
88  {
89  OMPL_ERROR("GoalSpace: The specified goal space must be the same type as the problem space.");
90  }
91 
92  goalSpace_ = space;
93  goalSampler_ = goalSpace_->allocStateSampler();
94 }
95 
97 {
98  return goalSpace_;
99 }
Definition of an abstract state.
Definition: State.h:114
StateSpacePtr getSpace() const
Get the goal space.
Definition: GoalSpace.cpp:96
void sampleGoal(State *st) const override
Sample a state in the goal region.
Definition: GoalSpace.cpp:44
unsigned int maxSampleCount() const override
Return the maximum number of samples that can be asked for before repeating.
Definition: GoalSpace.cpp:49
A shared pointer wrapper for ompl::base::StateSpace.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void print(std::ostream &out=std::cout) const override
Print information about the goal data structure to a stream.
Definition: GoalSpace.cpp:80
void setSpace(const StateSpacePtr space)
Set the goal space.
Definition: GoalSpace.cpp:85
double distanceGoal(const State *st) const override
Compute the distance to the goal (heuristic)
Definition: GoalSpace.cpp:54