ompl/control/src/PlannerData.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2012, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ryan Luna */ 00036 00037 #include "ompl/control/PlannerData.h" 00038 00039 ompl::control::PlannerData::PlannerData(const SpaceInformationPtr &siC) : base::PlannerData(boost::static_pointer_cast<base::SpaceInformation>(siC)), siC_(siC) 00040 { 00041 } 00042 00043 ompl::control::PlannerData::~PlannerData() 00044 { 00045 freeMemory(); 00046 } 00047 00048 bool ompl::control::PlannerData::removeVertex (unsigned int vIndex) 00049 { 00050 return ompl::base::PlannerData::removeVertex(vIndex); 00051 } 00052 00053 bool ompl::control::PlannerData::removeVertex (const ompl::base::PlannerDataVertex &st) 00054 { 00055 unsigned int index = vertexIndex (st); 00056 if (index == INVALID_INDEX) 00057 return false; 00058 00059 std::map<unsigned int, const base::PlannerDataEdge*> edgeMap; 00060 getEdges(index, edgeMap); 00061 00062 for (std::map<unsigned int, const base::PlannerDataEdge*>::iterator edgemapit = edgeMap.begin(); edgemapit != edgeMap.end(); ++edgemapit) 00063 { 00064 // Before deleting the edge, free the control associated with it, if it was decoupled 00065 Control *ctrl = const_cast<Control*>(static_cast<const PlannerDataEdgeControl*>(edgemapit->second)->getControl()); 00066 std::set<Control*>::iterator it = decoupledControls_.find(ctrl); 00067 if (it != decoupledControls_.end()) 00068 { 00069 siC_->freeControl(*it); 00070 decoupledControls_.erase(it); 00071 } 00072 } 00073 00074 return ompl::base::PlannerData::removeVertex(index); 00075 } 00076 00077 bool ompl::control::PlannerData::removeEdge (unsigned int v1, unsigned int v2) 00078 { 00079 return ompl::base::PlannerData::removeEdge(v1, v2); 00080 } 00081 00082 bool ompl::control::PlannerData::removeEdge (const ompl::base::PlannerDataVertex &v1, const ompl::base::PlannerDataVertex &v2) 00083 { 00084 unsigned int index1, index2; 00085 index1 = vertexIndex(v1); 00086 index2 = vertexIndex(v2); 00087 00088 if (index1 == INVALID_INDEX || index2 == INVALID_INDEX) 00089 return false; 00090 00091 // Before deleting the edge, free the control associated with it, if it was decoupled 00092 PlannerDataEdgeControl &edge = static_cast<PlannerDataEdgeControl&>(getEdge(index1, index2)); 00093 Control *ctrl = const_cast<Control*>(edge.getControl()); 00094 std::set<Control*>::iterator it = decoupledControls_.find(ctrl); 00095 if (it != decoupledControls_.end()) 00096 { 00097 siC_->freeControl(*it); 00098 decoupledControls_.erase(it); 00099 } 00100 00101 return ompl::base::PlannerData::removeEdge(index1, index2); 00102 } 00103 00104 void ompl::control::PlannerData::clear () 00105 { 00106 ompl::base::PlannerData::clear(); 00107 00108 freeMemory(); 00109 decoupledControls_.clear(); 00110 } 00111 00112 void ompl::control::PlannerData::decoupleFromPlanner() 00113 { 00114 ompl::base::PlannerData::decoupleFromPlanner(); 00115 00116 for (unsigned int i = 0; i < numVertices(); ++i) 00117 { 00118 for (unsigned int j = 0; j < numVertices(); ++j) 00119 { 00120 if (edgeExists(i, j)) 00121 { 00122 PlannerDataEdgeControl &edge = static_cast<PlannerDataEdgeControl&>(getEdge(i, j)); 00123 // If this edge's control is not in the decoupled list, clone it and add it 00124 Control *ctrl = const_cast<Control*>(edge.getControl()); 00125 if (decoupledControls_.find(ctrl) == decoupledControls_.end()) 00126 { 00127 Control *clone = siC_->cloneControl(ctrl); 00128 decoupledControls_.insert(clone); 00129 // Replacing the shallow control pointer with our shiny new clone 00130 edge.c_ = clone; 00131 } 00132 } 00133 } 00134 } 00135 } 00136 00137 const ompl::control::SpaceInformationPtr& ompl::control::PlannerData::getSpaceInformation() const 00138 { 00139 return siC_; 00140 } 00141 00142 bool ompl::control::PlannerData::hasControls() const 00143 { 00144 return true; 00145 } 00146 00147 void ompl::control::PlannerData::freeMemory() 00148 { 00149 for (std::set<Control*>::iterator it = decoupledControls_.begin(); it != decoupledControls_.end(); ++it) 00150 siC_->freeControl(*it); 00151 }