PlannerData.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
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 Rice University 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: Mark Moll */
36 
37 #ifndef OMPL_CONTROL_PLANNER_DATA_
38 #define OMPL_CONTROL_PLANNER_DATA_
39 
40 #include "ompl/base/PlannerData.h"
41 #include "ompl/control/SpaceInformation.h"
42 #include "ompl/control/Control.h"
43 #include <boost/serialization/base_object.hpp>
44 
45 namespace ompl
46 {
47  namespace control
48  {
61  {
62  public:
64  PlannerDataEdgeControl(const Control *c, double duration) : c_(c), duration_(duration)
65  {
66  }
69  : c_(rhs.c_), duration_(rhs.duration_)
70  {
71  }
72 
73  ~PlannerDataEdgeControl() override = default;
74 
75  base::PlannerDataEdge *clone() const override
76  {
77  return static_cast<base::PlannerDataEdge *>(new PlannerDataEdgeControl(*this));
78  }
79 
81  const Control *getControl() const
82  {
83  return c_;
84  }
86  double getDuration() const
87  {
88  return duration_;
89  }
90 
91  bool operator==(const PlannerDataEdge &rhs) const override
92  {
93  const auto *rhsc = static_cast<const PlannerDataEdgeControl *>(&rhs);
94  if (c_ == rhsc->c_)
95  return static_cast<const PlannerDataEdge>(*this) == rhs;
96  return false;
97  }
98 
99  protected:
100  friend class boost::serialization::access;
101  friend class PlannerDataStorage;
102  friend class PlannerData;
103 
104  PlannerDataEdgeControl() = default;
105 
106  template <class Archive>
107  void serialize(Archive &ar, const unsigned int /*version*/)
108  {
109  ar &boost::serialization::base_object<base::PlannerDataEdge>(*this);
110  ar &duration_;
111  // Serializing the control is handled by control::PlannerDataStorage
112  }
113 
114  const Control *c_{nullptr};
115  double duration_;
116  };
117 
122  {
123  public:
125  PlannerData(const SpaceInformationPtr &siC);
127  ~PlannerData() override;
128 
132  bool removeVertex(const base::PlannerDataVertex &st) override;
136  bool removeVertex(unsigned int vIndex) override;
137 
139  bool removeEdge(unsigned int v1, unsigned int v2) override;
142  bool removeEdge(const base::PlannerDataVertex &v1, const base::PlannerDataVertex &v2) override;
143 
145  void clear() override;
146 
154  void decoupleFromPlanner() override;
155 
158 
160  bool hasControls() const override;
161 
162  protected:
167  std::set<Control *> decoupledControls_;
168 
169  private:
170  void freeMemory();
171  };
172  }
173 }
174 
175 #endif
void decoupleFromPlanner() override
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
Definition of an abstract control.
Definition: Control.h:47
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:60
PlannerData(const SpaceInformationPtr &siC)
Constructor. Accepts a SpaceInformationPtr for the space planned in.
Definition: PlannerData.cpp:39
bool hasControls() const override
Returns true if this PlannerData instance has controls associated with it.
base::PlannerDataEdge * clone() const override
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:75
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
SpaceInformationPtr siC_
The instance of control::SpaceInformation associated with this data.
Definition: PlannerData.h:164
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool removeVertex(const base::PlannerDataVertex &st) override
Removes the vertex associated with the given data. If the vertex does not exist, false is returned...
Definition: PlannerData.cpp:54
PlannerDataEdgeControl(const Control *c, double duration)
Constructor. Accepts a control pointer and a duration.
Definition: PlannerData.h:64
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
std::set< Control * > decoupledControls_
A list of controls that are allocated during the decoupleFromPlanner method. These controls are freed...
Definition: PlannerData.h:167
~PlannerData() override
Destructor.
Definition: PlannerData.cpp:44
A shared pointer wrapper for ompl::control::SpaceInformation.
const Control * getControl() const
Return the control associated with this edge.
Definition: PlannerData.h:81
void clear() override
Clears the entire data structure.
double getDuration() const
Return the duration associated with this edge.
Definition: PlannerData.h:86
Base class for a PlannerData edge.
Definition: PlannerData.h:126
PlannerDataEdgeControl(const PlannerDataEdgeControl &rhs)
Copy constructor.
Definition: PlannerData.h:68
bool removeEdge(unsigned int v1, unsigned int v2) override
Removes the edge between vertex indexes v1 and v2. Success is returned.
Definition: PlannerData.cpp:79