Vertex.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Toronto
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 Toronto 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 /* Authors: Jonathan Gammell */
36 
37 // My definition:
38 #include "ompl/geometric/planners/bitstar/datastructures/Vertex.h"
39 
40 // For std::move
41 #include <utility>
42 // For std::swap
43 #include <algorithm>
44 
45 // The ID generator class
46 #include "ompl/geometric/planners/bitstar/datastructures/IdGenerator.h"
47 
48 namespace ompl
49 {
50  namespace geometric
51  {
53  // Public functions:
55  bool root /*= false*/)
56  : vId_(getIdGenerator().getNewId())
57  , si_(std::move(si))
58  , opt_(std::move(opt))
59  , state_(si_->allocState())
60  , isRoot_(root)
61  , edgeCost_(opt_->infiniteCost())
62  , cost_(opt_->infiniteCost())
63  {
64  if (this->isRoot())
65  {
66  cost_ = opt_->identityCost();
67  }
68  // No else, infinite by default
69  }
70 
72  {
73  // Free the state on destruction
74  si_->freeState(state_);
75  }
76 
78  {
79  this->assertNotPruned();
80  return vId_;
81  }
82 
84  {
85  this->assertNotPruned();
86 
87  return state_;
88  }
89 
91  {
92  this->assertNotPruned();
93 
94  return state_;
95  }
96 
98  {
99  this->assertNotPruned();
100 
101  return isRoot_;
102  }
103 
105  {
106  this->assertNotPruned();
107 
108  return static_cast<bool>(parentSPtr_);
109  }
110 
112  {
113  // No need to assert, as the two other functions both do
114 
115  return this->isRoot() || this->hasParent();
116  }
117 
118  unsigned int BITstar::Vertex::getDepth() const
119  {
120  this->assertNotPruned();
121 
122 #ifdef BITSTAR_DEBUG
123  if (this->isRoot() == false && this->hasParent() == false)
124  {
125  throw ompl::Exception("Attempting to get the depth of a vertex that does not have a parent yet is not "
126  "root.");
127  }
128 #endif // BITSTAR_DEBUG
129 
130  return depth_;
131  }
132 
134  {
135  this->assertNotPruned();
136 
137 #ifdef BITSTAR_DEBUG
138  if (this->hasParent() == false)
139  {
140  if (this->isRoot() == true)
141  {
142  throw ompl::Exception("Attempting to access the parent of the root vertex.");
143  }
144  else
145  {
146  throw ompl::Exception("Attempting to access the parent of a vertex that does not have one.");
147  }
148  }
149 #endif // BITSTAR_DEBUG
150 
151  return parentSPtr_;
152  }
153 
155  {
156  this->assertNotPruned();
157 
158 #ifdef BITSTAR_DEBUG
159  if (this->hasParent() == false)
160  {
161  if (this->isRoot() == true)
162  {
163  throw ompl::Exception("Attempting to access the parent of the root vertex.");
164  }
165  else
166  {
167  throw ompl::Exception("Attempting to access the parent of a vertex that does not have one.");
168  }
169  }
170 #endif // BITSTAR_DEBUG
171 
172  return parentSPtr_;
173  }
174 
175  void BITstar::Vertex::addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost,
176  bool updateChildCosts /*= true*/)
177  {
178  this->assertNotPruned();
179 
180 #ifdef BITSTAR_DEBUG
181  if (this->hasParent() == true)
182  {
183  throw ompl::Exception("Attempting to add a parent to a vertex that already has one.");
184  }
185  if (this->isRoot() == true)
186  {
187  throw ompl::Exception("Attempting to add a parent to the root vertex, which cannot have a parent.");
188  }
189 #endif // BITSTAR_DEBUG
190 
191  // Store the parent
192  parentSPtr_ = newParent;
193 
194  // Store the edge cost
195  edgeCost_ = edgeInCost;
196 
197  // Update my cost
198  this->updateCostAndDepth(updateChildCosts);
199  }
200 
201  void BITstar::Vertex::removeParent(bool updateChildCosts /*= true*/)
202  {
203  this->assertNotPruned();
204 
205 #ifdef BITSTAR_DEBUG
206  if (this->hasParent() == false)
207  {
208  throw ompl::Exception("Attempting to remove the parent of a vertex that does not have a parent.");
209  }
210  if (this->isRoot() == true)
211  {
212  throw ompl::Exception("Attempting to remove the parent of the root vertex, which cannot have a "
213  "parent.");
214  }
215 #endif // BITSTAR_DEBUG
216 
217  // Clear my parent
218  parentSPtr_.reset();
219 
220  // Update costs:
221  this->updateCostAndDepth(updateChildCosts);
222  }
223 
225  {
226  this->assertNotPruned();
227 
228  return !childWPtrs_.empty();
229  }
230 
232  {
233  this->assertNotPruned();
234 
235  children->clear();
236 
237  for (const auto &childWPtr : childWPtrs_)
238  {
239 #ifdef BITSTAR_DEBUG
240  // Check that the weak pointer hasn't expired
241  if (childWPtr.expired() == true)
242  {
243  throw ompl::Exception("A (weak) pointer to a child was found to have expired while collecting the "
244  "children of a vertex.");
245  }
246 #endif // BITSTAR_DEBUG
247 
248  // Lock and push back
249  children->push_back(childWPtr.lock());
250  }
251  }
252 
254  {
255  this->assertNotPruned();
256 
257  children->clear();
258 
259  for (const auto &childWPtr : childWPtrs_)
260  {
261 #ifdef BITSTAR_DEBUG
262  // Check that the weak pointer hasn't expired
263  if (childWPtr.expired() == true)
264  {
265  throw ompl::Exception("A (weak) pointer to a child was found to have expired while collecting the "
266  "children of a vertex.");
267  }
268 #endif // BITSTAR_DEBUG
269 
270  // Lock and push back
271  children->push_back(childWPtr.lock());
272  }
273  }
274 
275  void BITstar::Vertex::addChild(const VertexPtr &newChild, bool updateChildCosts /*= true*/)
276  {
277  this->assertNotPruned();
278 
279  // Push back the shared_ptr into the vector of weak_ptrs, this makes a weak_ptr copy
280  childWPtrs_.push_back(newChild);
281 
282  if (updateChildCosts)
283  {
284  newChild->updateCostAndDepth(true);
285  }
286  // No else, leave the costs out of date.
287  }
288 
289  void BITstar::Vertex::removeChild(const VertexPtr &oldChild, bool updateChildCosts /*= true*/)
290  {
291  this->assertNotPruned();
292 
293  // Variables
294  // Whether the child has been found (and then deleted);
295  bool foundChild;
296 
297  // Iterate over the vector of children pointers until the child is found. Iterators make erase easier
298  foundChild = false;
299  for (auto childIter = childWPtrs_.begin(); childIter != childWPtrs_.end() && !foundChild;
300  ++childIter)
301  {
302 #ifdef BITSTAR_DEBUG
303  // Check that the weak pointer hasn't expired
304  if (childIter->expired() == true)
305  {
306  throw ompl::Exception("A (weak) pointer to a child was found to have expired while removing a "
307  "child from a vertex.");
308  }
309 #endif // BITSTAR_DEBUG
310 
311  // Check if this is the child we're looking for
312  if (childIter->lock()->getId() == oldChild->getId())
313  {
314  // It is, mark as found
315  foundChild = true;
316 
317  // Remove the child from the vector
318  // Swap to the end
319  if (childIter != (childWPtrs_.end() - 1))
320  {
321  std::swap(*childIter, childWPtrs_.back());
322  }
323 
324  // Pop it off the end
325  childWPtrs_.pop_back();
326  }
327  // No else, move on
328  }
329 
330  // Update the child cost if appropriate
331  if (updateChildCosts)
332  {
333  oldChild->updateCostAndDepth(true);
334  }
335 // No else, leave the costs out of date.
336 
337 #ifdef BITSTAR_DEBUG
338  // Throw if we did not find the child
339  if (foundChild == false)
340  {
341  throw ompl::Exception("Attempting to remove a child vertex not present in the vector of children "
342  "stored in the (supposed) parent vertex.");
343  }
344 #endif // BITSTAR_DEBUG
345  }
346 
348  {
349  this->assertNotPruned();
350 
351  return cost_;
352  }
353 
355  {
356  this->assertNotPruned();
357 
358 #ifdef BITSTAR_DEBUG
359  if (this->hasParent() == false)
360  {
361  throw ompl::Exception("Attempting to access the incoming-edge cost of a vertex without a parent.");
362  }
363 #endif // BITSTAR_DEBUG
364 
365  return edgeCost_;
366  }
367 
369  {
370  this->assertNotPruned();
371 
372  return isNew_;
373  }
374 
376  {
377  this->assertNotPruned();
378 
379  isNew_ = true;
380  }
381 
383  {
384  this->assertNotPruned();
385 
386  isNew_ = false;
387  }
388 
390  {
391  this->assertNotPruned();
392 
393  return hasBeenExpandedToSamples_;
394  }
395 
397  {
398  this->assertNotPruned();
399 
400  hasBeenExpandedToSamples_ = true;
401  }
402 
404  {
405  this->assertNotPruned();
406 
407  hasBeenExpandedToSamples_ = false;
408  }
409 
411  {
412  this->assertNotPruned();
413 
414  return hasBeenExpandedToVertices_;
415  }
416 
418  {
419  this->assertNotPruned();
420 
421  hasBeenExpandedToVertices_ = true;
422  }
423 
425  {
426  this->assertNotPruned();
427 
428  hasBeenExpandedToVertices_ = false;
429  }
430 
432  {
433  return isPruned_;
434  }
435 
437  {
438  this->assertNotPruned();
439 
440  isPruned_ = true;
441  }
442 
444  {
445  isPruned_ = false;
446  }
448 
450  // Protected functions:
451  void BITstar::Vertex::updateCostAndDepth(bool cascadeUpdates /*= true*/)
452  {
453  this->assertNotPruned();
454 
455  if (this->isRoot())
456  {
457  // Am I root? -- I don't really know how this would ever be called, but ok.
458  cost_ = opt_->identityCost();
459  depth_ = 0u;
460  }
461  else if (!this->hasParent())
462  {
463  // Am I disconnected?
464  cost_ = opt_->infiniteCost();
465 
466  // Set the depth to 0u, getDepth will throw in this condition
467  depth_ = 0u;
468 
469 #ifdef BITSTAR_DEBUG
470  // Assert that I have not been asked to cascade this bad data to my children:
471  if (this->hasChildren() == true && cascadeUpdates == true)
472  {
473  throw ompl::Exception("Attempting to update descendants' costs and depths of a vertex that does "
474  "not have a parent and is not root. This information would therefore be "
475  "gibberish.");
476  }
477 #endif // BITSTAR_DEBUG
478  }
479  else
480  {
481  // I have a parent, so my cost is my parent cost + my edge cost to the parent
482  cost_ = opt_->combineCosts(parentSPtr_->getCost(), edgeCost_);
483 
484  // I am one more than my parent's depth:
485  depth_ = (parentSPtr_->getDepth() + 1u);
486  }
487 
488  // Am I updating my children?
489  if (cascadeUpdates)
490  {
491  // Now, iterate over my vector of children and tell each one to update its own damn cost:
492  for (auto &childWPtr : childWPtrs_)
493  {
494 #ifdef BITSTAR_DEBUG
495  // Check that it hasn't expired
496  if (childWPtr.expired() == true)
497  {
498  throw ompl::Exception("A (weak) pointer to a child has was found to have expired while "
499  "updating the costs and depths of descendant vertices.");
500  }
501 #endif // BITSTAR_DEBUG
502 
503  // Get a lock and tell the child to update:
504  childWPtr.lock()->updateCostAndDepth(true);
505  }
506  }
507  // No else, do not update the children. I hope the caller knows what they're doing.
508  }
510 
512  // Private functions:
513  void BITstar::Vertex::assertNotPruned() const
514  {
515 #ifdef BITSTAR_DEBUG
516  if (isPruned_ == true)
517  {
518  std::cout << std::endl
519  << "vId: " << vId_ << std::endl;
520  throw ompl::Exception("Attempting to access a pruned vertex.");
521  }
522 #endif // BITSTAR_DEBUG
523  }
525  } // geometric
526 } // ompl
bool hasBeenExpandedToVertices() const
Returns true if the vertex has been expanded towards vertices.
Definition: Vertex.cpp:410
bool hasBeenExpandedToSamples() const
Returns true if the vertex has been expanded towards samples.
Definition: Vertex.cpp:389
unsigned int getDepth() const
Get the "depth" of the vertex from the root. A root vertex is at depth 0, a direct descendent of the ...
Definition: Vertex.cpp:118
void markNew()
Mark the vertex as new.
Definition: Vertex.cpp:375
bool hasParent() const
Get whether this vertex has a parent.
Definition: Vertex.cpp:104
void markExpandedToSamples()
Mark the vertex as expanded towards samples.
Definition: Vertex.cpp:396
STL namespace.
bool isPruned() const
Whether the vertex has been pruned.
Definition: Vertex.cpp:431
VertexPtr getParent()
Get the parent of a vertex as a mutable pointer.
Definition: Vertex.cpp:154
void updateCostAndDepth(bool cascadeUpdates=true)
Calculates the updated cost and depth of the current state, as well as calling all children&#39;s updateC...
Definition: Vertex.cpp:451
ompl::base::Cost getEdgeInCost() const
Get the incremental cost-to-come of a vertex.
Definition: Vertex.cpp:354
bool isNew() const
Returns true if the vertex is marked as new. Vertices are new until marked old.
Definition: Vertex.cpp:368
ompl::base::State * state()
The state of a vertex as a mutable pointer.
Definition: Vertex.cpp:90
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
Vertex(ompl::base::SpaceInformationPtr si, ompl::base::OptimizationObjectivePtr opt, bool root=false)
Constructor.
Definition: Vertex.cpp:54
void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost, bool updateChildCosts=true)
Set the parent of a vertex, cannot be used to replace a previous parent. Will update this vertex&#39;s co...
Definition: Vertex.cpp:175
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
ompl::base::State const * stateConst() const
The state of a vertex as a constant pointer.
Definition: Vertex.cpp:83
void removeChild(const VertexPtr &oldChild, bool updateChildCosts=true)
Remove a child vertex. Does not change this vertex&#39;s cost, and can update the child and its descenden...
Definition: Vertex.cpp:289
void removeParent(bool updateChildCosts=true)
Remove the parent edge. Will update this vertex&#39;s cost, and can update the descendent costs...
Definition: Vertex.cpp:201
void markUnexpandedToVertices()
Mark the vertex as not expanded towards vertices.
Definition: Vertex.cpp:424
A shared pointer wrapper for ompl::base::SpaceInformation.
bool isRoot() const
Whether the vertex is root.
Definition: Vertex.cpp:97
ompl::base::Cost getCost() const
Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.
Definition: Vertex.cpp:347
void markOld()
Mark the vertex as old.
Definition: Vertex.cpp:382
Definition of an abstract state.
Definition: State.h:49
void addChild(const VertexPtr &newChild, bool updateChildCosts=true)
Add a child vertex. Does not change this vertex&#39;s cost, and can update the child and its descendent c...
Definition: Vertex.cpp:275
BITstar::VertexId getId() const
The (unique) vertex ID.
Definition: Vertex.cpp:77
void getChildrenConst(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition: Vertex.cpp:231
The exception type for ompl.
Definition: Exception.h:46
A shared pointer wrapper for ompl::base::OptimizationObjective.
void getChildren(VertexPtrVector *children)
Get the children of a vertex as mutable pointers.
Definition: Vertex.cpp:253
void markUnexpandedToSamples()
Mark the vertex as not expanded towards samples.
Definition: Vertex.cpp:403
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers.
Definition: BITstar.h:130
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:132
bool hasChildren() const
Get whether this vertex has any children.
Definition: Vertex.cpp:224
void markUnpruned()
Mark the vertex as unpruned.
Definition: Vertex.cpp:443
std::shared_ptr< Vertex > VertexPtr
A vertex shared pointer.
Definition: BITstar.h:121
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void markExpandedToVertices()
Mark the vertex as expanded towards vertices.
Definition: Vertex.cpp:417
VertexConstPtr getParentConst() const
Get the parent of a vertex as a constant pointer.
Definition: Vertex.cpp:133
void markPruned()
Mark the vertex as pruned.
Definition: Vertex.cpp:436
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:134
bool isInTree() const
Get whether a vertex is "in the graph" or not. This returns true if the vertex is the graph root or i...
Definition: Vertex.cpp:111