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  {
60  class PlannerDataEdgeControl : public base::PlannerDataEdge
61  {
62  public:
64  PlannerDataEdgeControl(const Control *c, double duration) : c_(c), duration_(duration)
65  {
66  }
68  PlannerDataEdgeControl(const PlannerDataEdgeControl &rhs)
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 
121  class PlannerData : public base::PlannerData
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...
const Control * getControl() const
Return the control associated with this edge.
Definition: PlannerData.h:177
A shared pointer wrapper for ompl::base::SpaceInformation.
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
base::PlannerDataEdge * clone() const override
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:171
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
double getDuration() const
Return the duration associated with this edge.
Definition: PlannerData.h:182
~PlannerData() override
Destructor.
Definition: PlannerData.cpp:44
bool hasControls() const override
Returns true if this PlannerData instance has controls associated with it.
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
PlannerDataEdgeControl(const Control *c, double duration)
Constructor. Accepts a control pointer and a duration.
Definition: PlannerData.h:160
SpaceInformationPtr siC_
The instance of control::SpaceInformation associated with this data.
Definition: PlannerData.h:228
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:119
std::set< Control * > decoupledControls_
A list of controls that are allocated during the decoupleFromPlanner method. These controls are freed...
Definition: PlannerData.h:231
void clear() override
Clears the entire data structure.
PlannerData(const SpaceInformationPtr &siC)
Constructor. Accepts a SpaceInformationPtr for the space planned in.
Definition: PlannerData.cpp:39
Main namespace. Contains everything in this library.
Definition: AppBase.h:21