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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines