int main()
{
// Read the configuration file
ConfigManager::ReadParameters();
// Read and inflate map
const char* mapurl = ConfigManager::GetMapUrl().erase(ConfigManager::GetMapUrl().size() - 1).c_str();
Map* currMap = LoadMap(mapurl);
int robotSize = MAX(ConfigManager::GetRobotHeight(),ConfigManager::GetRobotWidth());
double mapResulotion = ConfigManager::GetMapResolution();
int InflateAddition = ceil((robotSize / 2) / mapResulotion);
Map* inflateMap = currMap->Inflate(InflateAddition);
Map* Maparticle = currMap->Inflate(5);
// Calculate the ratio between the grid resolution and map resolution
int res = ConfigManager::GetGridResolution() / ConfigManager::GetMapResolution();
Grid* grid = new Grid(inflateMap->GetMatrix(),inflateMap->GetWidth(),inflateMap->GetHeight(), res);
// A*
PathPlanner* p = new PathPlanner();
Point* start = ConfigManager::GetStartLocationMapResolution();
Point* end = ConfigManager::GetGoalMapResolution();
int startGridPointX,startGridPointY, endGridPointX, endGridPointY;
startGridPointX = start->GetRow() / res;
startGridPointY = start->GetCol() / res;
endGridPointX = end->GetRow() / res;
endGridPointY = end->GetCol() / res;
std::string route = p->pathFind(startGridPointX ,startGridPointY,endGridPointX,endGridPointY,grid);
// WayPoint calculation
Point* startWithRes = new Point(start->GetRow() / 4, start->GetCol() / 4);
vector<Point *> waypointsArr = WayPoints::CalculateByDirectionalPath(route, startWithRes);
Robot robot("localhost",6665);
PlnObstacleAvoid plnOA(&robot);
Manager manager(&robot, &plnOA, waypointsArr, Maparticle);
manager.run();
}
string WaypointManager::Path()
{
ConfigManager* cm = new ConfigManager();
int const xStart=cm->getStartLocationX();
int const yStart=cm->getStartLocationY();
int const xEnd=cm->getGoalLocationX();
int const yEnd=cm->getGoalLocationY();
PathPlanner* Pathp = new PathPlanner();
vector < pair<int,int> > Points;
string FinalDir = "";
int NumOfSteps = 0;
int NumOfDir = 0;
string Directions = Pathp->pathFind(xStart,yStart,xEnd,yEnd,&Points);
pair<int,int> pnt;
pnt.first = xStart;
pnt.second = yStart;
FinalPoints.push_back(pnt);
for (int NumOfPnt=(Points.size()-1);NumOfPnt>1;NumOfPnt--)
{
if (NumOfSteps==3 || Directions[NumOfDir]!=Directions[NumOfDir+1])
{
pnt.first = Points[NumOfPnt].first;
pnt.second = Points[NumOfPnt].second;
FinalPoints.push_back(pnt);
FinalDir+=Directions[NumOfDir];
NumOfSteps=0;
}
NumOfSteps++;
NumOfDir++;
}
FinalDir+=Directions[NumOfDir];
pnt.first = xEnd;
pnt.second = yEnd;
FinalPoints.push_back(pnt);
return FinalDir;
}