All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PlannerData.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, 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: Ryan Luna */
36 
37 #include "ompl/control/PlannerData.h"
38 
39 ompl::control::PlannerData::PlannerData(const SpaceInformationPtr &siC) : base::PlannerData(boost::static_pointer_cast<base::SpaceInformation>(siC)), siC_(siC)
40 {
41 }
42 
44 {
45  freeMemory();
46 }
47 
48 bool ompl::control::PlannerData::removeVertex (unsigned int vIndex)
49 {
51 }
52 
54 {
55  unsigned int index = vertexIndex (st);
56  if (index == INVALID_INDEX)
57  return false;
58 
59  std::map<unsigned int, const base::PlannerDataEdge*> edgeMap;
60  getEdges(index, edgeMap);
61 
62  for (std::map<unsigned int, const base::PlannerDataEdge*>::iterator edgemapit = edgeMap.begin(); edgemapit != edgeMap.end(); ++edgemapit)
63  {
64  // Before deleting the edge, free the control associated with it, if it was decoupled
65  Control* ctrl = const_cast<Control*>(static_cast<const PlannerDataEdgeControl*>(edgemapit->second)->getControl());
66  std::set<Control*>::iterator it = decoupledControls_.find(ctrl);
67  if (it != decoupledControls_.end())
68  {
69  siC_->freeControl(*it);
70  decoupledControls_.erase(it);
71  }
72  }
73 
75 }
76 
77 bool ompl::control::PlannerData::removeEdge (unsigned int v1, unsigned int v2)
78 {
80 }
81 
83 {
84  unsigned int index1, index2;
85  index1 = vertexIndex(v1);
86  index2 = vertexIndex(v2);
87 
88  if (index1 == INVALID_INDEX || index2 == INVALID_INDEX)
89  return false;
90 
91  // Before deleting the edge, free the control associated with it, if it was decoupled
92  PlannerDataEdgeControl &edge = static_cast<PlannerDataEdgeControl&>(getEdge(index1, index2));
93  Control* ctrl = const_cast<Control*>(edge.getControl());
94  std::set<Control*>::iterator it = decoupledControls_.find(ctrl);
95  if (it != decoupledControls_.end())
96  {
97  siC_->freeControl(*it);
98  decoupledControls_.erase(it);
99  }
100 
101  return ompl::base::PlannerData::removeEdge(index1, index2);
102 }
103 
105 {
107 
108  freeMemory();
109  decoupledControls_.clear();
110 }
111 
113 {
115 
116  for (unsigned int i = 0; i < numVertices(); ++i)
117  {
118  for (unsigned int j = 0; j < numVertices(); ++j)
119  {
120  if (edgeExists(i, j))
121  {
122  PlannerDataEdgeControl &edge = static_cast<PlannerDataEdgeControl&>(getEdge(i, j));
123  // If this edge's control is not in the decoupled list, clone it and add it
124  Control* ctrl = const_cast<Control*>(edge.getControl());
125  if (decoupledControls_.find(ctrl) == decoupledControls_.end())
126  {
127  Control *clone = siC_->cloneControl(ctrl);
128  decoupledControls_.insert(clone);
129  // Replacing the shallow control pointer with our shiny new clone
130  edge.c_ = clone;
131  }
132  }
133  }
134  }
135 }
136 
138 {
139  return siC_;
140 }
141 
143 {
144  return true;
145 }
146 
147 void ompl::control::PlannerData::freeMemory(void)
148 {
149  for (std::set<Control*>::iterator it = decoupledControls_.begin(); it != decoupledControls_.end(); ++it)
150  siC_->freeControl(*it);
151 }
Definition of an abstract control.
Definition: Control.h:48
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:111
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
virtual bool hasControls(void) const
Returns true if this PlannerData instance has controls associated with it.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
virtual bool removeVertex(const PlannerDataVertex &st)
Removes the vertex associated with the given data. If the vertex does not exist, false is returned...
const SpaceInformationPtr & getSpaceInformation(void) const
Return the instance of SpaceInformation used in this PlannerData.
const Control * getControl(void) const
Return the control associated with this edge.
Definition: PlannerData.h:76
virtual void decoupleFromPlanner(void)
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
virtual bool removeVertex(const base::PlannerDataVertex &st)
Removes the vertex associated with the given data. If the vertex does not exist, false is returned...
Definition: PlannerData.cpp:53
A boost shared pointer wrapper for ompl::control::SpaceInformation.
virtual ~PlannerData(void)
Destructor.
Definition: PlannerData.cpp:43
virtual bool removeEdge(unsigned int v1, unsigned int v2)
Removes the edge between vertex indexes v1 and v2. Success is returned.
Definition: PlannerData.cpp:77
virtual void clear(void)
Clears the entire data structure.
Definition: PlannerData.cpp:78
Space information containing necessary information for planning with controls. setup() needs to be ca...
virtual void decoupleFromPlanner(void)
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:84
virtual bool removeEdge(unsigned int v1, unsigned int v2)
Removes the edge between vertex indexes v1 and v2. Success is returned.
virtual void clear(void)
Clears the entire data structure.