GoalState.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, 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 the 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 "ompl/base/goals/GoalState.h"
38 #include "ompl/base/SpaceInformation.h"
39 
40 ompl::base::GoalState::~GoalState()
41 {
42  if (state_ != nullptr)
43  si_->freeState(state_);
44 }
45 
47 {
48  return si_->distance(st, state_);
49 }
50 
51 void ompl::base::GoalState::print(std::ostream &out) const
52 {
53  out << "Goal state, threshold = " << threshold_ << ", memory address = " << this << ", state = " << std::endl;
54  si_->printState(state_, out);
55 }
56 
58 {
59  si_->copyState(st, state_);
60 }
61 
63 {
64  return 1;
65 }
66 
68 {
69  if (state_ != nullptr)
70  si_->freeState(state_);
71  state_ = si_->cloneState(st);
72 }
73 
75 {
76  setState(st.get());
77 }
78 
80 {
81  return state_;
82 }
83 
85 {
86  return state_;
87 }
void setState(const ScopedState<> &st)
Set the goal state.
Definition: GoalState.cpp:74
Definition of a scoped state.
Definition: ScopedState.h:56
void sampleGoal(State *st) const override
Sample a state in the goal region.
Definition: GoalState.cpp:57
unsigned int maxSampleCount() const override
Return the maximum number of samples that can be asked for before repeating.
Definition: GoalState.cpp:62
void print(std::ostream &out=std::cout) const override
Print information about the goal data structure to a stream.
Definition: GoalState.cpp:51
const State * getState() const
Get the goal state.
Definition: GoalState.cpp:79
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:394
void setState(const State *st)
Set the goal state.
Definition: GoalState.cpp:67
State * state_
The goal state.
Definition: GoalState.h:86
Definition of an abstract state.
Definition: State.h:49
SpaceInformationPtr si_
The space information for this goal.
Definition: Goal.h:149
double distanceGoal(const State *st) const override
Compute the distance to the goal (heuristic)
Definition: GoalState.cpp:46
double threshold_
The maximum distance that is allowed to the goal. By default, this is initialized to the minimum epsi...
Definition: GoalRegion.h:91