void PlannerPRM::initialize(Sampler* sampler_, const RoboCompCommonBehavior::ParameterList ¶ms)
{
sampler = sampler_;
////////////////////////
/// Initialize RRTplaner
////////////////////////
plannerRRT.initialize(sampler);
////////////////////////
/// Check if graph already exists
////////////////////////
if( QFile(graphFileName).exists())
{
qDebug() << __FUNCTION__ << "Graph file exits. Loading";
readGraphFromFile(graphFileName);
}
else
{
try
{
nPointsInGraph = std::stoi(params.at("PlannerGraphPoints").value);
nNeighboursInGraph = std::stoi(params.at("PlannerGraphNeighbours").value);
maxDistToSearchmm = std::stof(params.at("PlannerGraphMaxDistanceToSearch").value);
robotRadiusmm = std::stof(params.at("RobotRadius").value);
}
catch(...)
{ qFatal("Planner-Initialize. Aborting. Some Planner graph parameters not found in config file"); }
qDebug() << __FUNCTION__ << "No graph file found. Creating with " << nPointsInGraph << "nodes and " << nNeighboursInGraph << "neighboors";
QList<QVec> pointList = sampler->sampleFreeSpaceR2(nPointsInGraph);
if( pointList.size() < nNeighboursInGraph )
qFatal("Planner-Initialize. Aborting. Could not find enough free points to build de graph");
qDebug() << __FUNCTION__ << "Creating with " << nPointsInGraph << "nodes and " << nNeighboursInGraph << "neighboors";
constructGraph(pointList, nNeighboursInGraph, maxDistToSearchmm, robotRadiusmm); ///GET From IM ----------------------------------
qDebug() << __FUNCTION__ << "Graph constructed with " << pointList.size() << "points";
writeGraphToFile(graphFileName);
}
graphDirtyBit = true;
}
void TopologicalMapper::writePointGraphToFile(std::string &filename) {
writeGraphToFile(filename, point_graph_, map_resp_.map.info);
}
void TopologicalMapper::writeRegionGraphToFile(std::string &filename) {
writeGraphToFile(filename, region_graph_, map_resp_.map.info);
}
