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