00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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(void)
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
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
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 (void)
00105 {
00106 ompl::base::PlannerData::clear();
00107
00108 freeMemory();
00109 decoupledControls_.clear();
00110 }
00111
00112 void ompl::control::PlannerData::decoupleFromPlanner(void)
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
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
00130 edge.c_ = clone;
00131 }
00132 }
00133 }
00134 }
00135 }
00136
00137 const ompl::control::SpaceInformationPtr& ompl::control::PlannerData::getSpaceInformation(void) const
00138 {
00139 return siC_;
00140 }
00141
00142 bool ompl::control::PlannerData::hasControls(void) const
00143 {
00144 return true;
00145 }
00146
00147 void ompl::control::PlannerData::freeMemory(void)
00148 {
00149 for (std::set<Control*>::iterator it = decoupledControls_.begin(); it != decoupledControls_.end(); ++it)
00150 siC_->freeControl(*it);
00151 }