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 int 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 return -1;
00085 }
00086 else
00087 {
00088 std::map<const State*, unsigned int>::iterator it1 = stateIndex.find(s1);
00089 std::map<const State*, unsigned int>::iterator it2 = stateIndex.find(s2);
00090
00091 bool newEdge = false;
00092
00093 unsigned int p1;
00094 if (it1 == stateIndex.end())
00095 {
00096 p1 = states.size();
00097 states.push_back(s1);
00098 tags.push_back(0);
00099 stateIndex[s1] = p1;
00100 edges.resize(states.size());
00101 newEdge = true;
00102 }
00103 else
00104 p1 = it1->second;
00105
00106 unsigned int p2;
00107 if (it2 == stateIndex.end())
00108 {
00109 p2 = states.size();
00110 states.push_back(s2);
00111 tags.push_back(0);
00112 stateIndex[s2] = p2;
00113 edges.resize(states.size());
00114 newEdge = true;
00115 }
00116 else
00117 p2 = it2->second;
00118
00119
00120 if (!newEdge)
00121 {
00122 newEdge = true;
00123 for (unsigned int i = 0 ; i < edges[p1].size() ; ++i)
00124 if (edges[p1][i] == p2)
00125 {
00126 newEdge = false;
00127 break;
00128 }
00129 }
00130
00131 if (newEdge)
00132 {
00133 edges[p1].push_back(p2);
00134 return p1;
00135 }
00136 else
00137 return -1;
00138 }
00139 }
00140
00141 void ompl::base::PlannerData::print(std::ostream &out) const
00142 {
00143 out << states.size() << std::endl;
00144 for (unsigned int i = 0 ; i < states.size() ; ++i)
00145 {
00146 out << i << " (tag="<< tags[i] << "): ";
00147 if (si)
00148 si->printState(states[i], out);
00149 else
00150 out << states[i] << std::endl;
00151 }
00152
00153 for (unsigned int i = 0 ; i < edges.size() ; ++i)
00154 {
00155 if (edges[i].empty())
00156 continue;
00157 out << i << ": ";
00158 for (unsigned int j = 0 ; j < edges[i].size() ; ++j)
00159 out << edges[i][j] << ' ';
00160 out << std::endl;
00161 }
00162 }