Loading...
Searching...
No Matches
PlannerDataVertexAnnotated.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Stuttgart
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 Stuttgart nor the names
18 * of its contributors may be used to endorse or promote products
19 * derived from this software without specific prior written
20 * permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Andreas Orthey */
37
38#include <ompl/multilevel/datastructures/PlannerDataVertexAnnotated.h>
39
40using namespace ompl::multilevel;
41
43 : PlannerDataVertex(state), stateBase_(state), stateTotal_(nullptr), si_(nullptr)
44{
45 totalStateIsSet = false;
46}
47
49 : PlannerDataVertex(rhs.getBaseState(), rhs.tag_)
50{
51 level_ = rhs.getLevel();
52 maxLevel_ = rhs.getMaxLevel();
53 component_ = rhs.getComponent();
56 si_ = rhs.getSpaceInformationPtr();
57}
58
59PlannerDataVertexAnnotated::~PlannerDataVertexAnnotated()
60{
62 {
63 si_->freeState(stateTotal_);
64 }
65}
66
71
72void PlannerDataVertexAnnotated::setComponent(unsigned int component)
73{
74 component_ = component;
75}
76
77ompl::base::SpaceInformationPtr PlannerDataVertexAnnotated::getSpaceInformationPtr() const
78{
79 return si_;
80}
81
82unsigned int PlannerDataVertexAnnotated::getComponent() const
83{
84 return component_;
85}
86
88{
89 level_ = level;
90}
91
92unsigned int PlannerDataVertexAnnotated::getLevel() const
93{
94 return level_;
95}
96
98{
99 maxLevel_ = level;
100}
101
102unsigned int PlannerDataVertexAnnotated::getMaxLevel() const
103{
104 return maxLevel_;
105}
106
108{
109 if (totalStateIsSet)
110 return stateTotal_;
111 else
112 return state_;
113}
114
119
124
129
136
137bool PlannerDataVertexAnnotated::operator==(const PlannerDataVertex &rhs) const
138{
139 const PlannerDataVertexAnnotated &rhsA = static_cast<const PlannerDataVertexAnnotated &>(rhs);
140 bool equiv = (getLevel() == rhsA.getLevel() && getBaseState() == rhsA.getBaseState());
141 return equiv;
142}
143
144std::ostream &operator<<(std::ostream &out, const PlannerDataVertexAnnotated &v)
145{
146 out << "AnnotatedVertex";
147 out << " ->level " << v.getLevel() << "/" << v.getMaxLevel();
148 out << " ->component " << v.getComponent();
149 out << std::endl;
150 return out;
151}
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
const State * state_
The state represented by this vertex.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
An annotated vertex, adding information about its level in the multilevel hierarchy....
virtual PlannerDataVertex * clone() const override
Return a clone of this object, allocated from the heap.
void setBaseState(const ompl::base::State *s)
Explicitly changes base state (does not change mode)
ompl::base::SpaceInformationPtr si_
Pointer to total space (to free total space element upon deletion)
void setTotalState(ompl::base::State *s, ompl::base::SpaceInformationPtr si)
Set total state, i.e. the lift of the base state to the total space (last Spaceinformationptr in sequ...
ompl::base::State * getStateNonConst() const
Same as getState(), but state can be changed.
bool totalStateIsSet
There are two modes. Mode 1 is the normal mode where this class contains a reference to the base stat...
unsigned int maxLevel_
How many spaces exists in the multilevel structure.
virtual const ompl::base::State * getState() const override
Returns base state in baseMode and total state in totalMode. The total space here is the last element...
unsigned int component_
(Optional:) which component in roadmap does vertex belong to
PlannerDataVertexAnnotated(const ompl::base::State *state)
Constructor for base state. Set mode to baseMode.
void setComponent(unsigned int component_)
The component of vertex in the graph (start, goal or other component)
const ompl::base::State * getBaseState() const
Returns base state, indepent of mode.
const ompl::base::State * stateBase_
Internal reference to base state. Same as state_ in normal Mode to avoid confusion.
ompl::base::State * stateTotal_
Storage of total state.
virtual bool operator==(const PlannerDataVertex &rhs) const override
Verifies equality by checking level and base state (mode independent)
void setMaxLevel(unsigned int level_)
The maximum level in the bundle space hierarchy.
void setLevel(unsigned int level_)
The level of vertex in the bundle space hierarchy.
unsigned int level_
The level for the base state.
This namespace contains datastructures and planners to exploit multilevel abstractions,...