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/base/PlannerData.h"
00038
00039 void ompl::base::PlannerData::clear(void)
00040 {
00041 stateIndex.clear();
00042 states.clear();
00043 tags.clear();
00044 edges.clear();
00045 properties.clear();
00046 si.reset();
00047 }
00048
00049 void ompl::base::PlannerData::tagState(const State *s, int tag)
00050 {
00051 if (s != NULL)
00052 {
00053 std::map<const State*, unsigned int>::iterator it = stateIndex.find(s);
00054 if (it == stateIndex.end())
00055 {
00056 unsigned int p = states.size();
00057 states.push_back(s);
00058 tags.push_back(tag);
00059 stateIndex[s] = p;
00060 edges.resize(states.size());
00061 }
00062 else
00063 tags[it->second] = tag;
00064 }
00065 }
00066
00067 void ompl::base::PlannerData::recordEdge(const State *s1, const State *s2)
00068 {
00069 if (s1 == NULL || s2 == NULL)
00070 {
00071 const State *s = s1 == NULL ? s2 : s1;
00072 if (s != NULL)
00073 {
00074 std::map<const State*, unsigned int>::iterator it = stateIndex.find(s);
00075 if (it == stateIndex.end())
00076 {
00077 unsigned int p = states.size();
00078 states.push_back(s);
00079 tags.push_back(0);
00080 stateIndex[s] = p;
00081 edges.resize(states.size());
00082 }
00083 }
00084 }
00085 else
00086 {
00087 std::map<const State*, unsigned int>::iterator it1 = stateIndex.find(s1);
00088 std::map<const State*, unsigned int>::iterator it2 = stateIndex.find(s2);
00089
00090 bool newEdge = false;
00091
00092 unsigned int p1;
00093 if (it1 == stateIndex.end())
00094 {
00095 p1 = states.size();
00096 states.push_back(s1);
00097 tags.push_back(0);
00098 stateIndex[s1] = p1;
00099 edges.resize(states.size());
00100 newEdge = true;
00101 }
00102 else
00103 p1 = it1->second;
00104
00105 unsigned int p2;
00106 if (it2 == stateIndex.end())
00107 {
00108 p2 = states.size();
00109 states.push_back(s2);
00110 tags.push_back(0);
00111 stateIndex[s2] = p2;
00112 edges.resize(states.size());
00113 newEdge = true;
00114 }
00115 else
00116 p2 = it2->second;
00117
00118
00119 if (!newEdge)
00120 {
00121 newEdge = true;
00122 for (unsigned int i = 0 ; i < edges[p1].size() ; ++i)
00123 if (edges[p1][i] == p2)
00124 {
00125 newEdge = false;
00126 break;
00127 }
00128 }
00129
00130 if (newEdge)
00131 edges[p1].push_back(p2);
00132 }
00133 }
00134
00135 void ompl::base::PlannerData::print(std::ostream &out) const
00136 {
00137 out << states.size() << std::endl;
00138 for (unsigned int i = 0 ; i < states.size() ; ++i)
00139 {
00140 out << i << " (tag="<< tags[i] << "): ";
00141 if (si)
00142 si->printState(states[i], out);
00143 else
00144 out << states[i] << std::endl;
00145 }
00146
00147 for (unsigned int i = 0 ; i < edges.size() ; ++i)
00148 {
00149 if (edges[i].empty())
00150 continue;
00151 out << i << ": ";
00152 for (unsigned int j = 0 ; j < edges[i].size() ; ++j)
00153 out << edges[i][j] << ' ';
00154 out << std::endl;
00155 }
00156 }