Esempio n. 1
0
int Point::directionAdjacent(Point &a, Point &b)
{
    int diff_col = a.GetCol() - b.GetCol();
    int diff_row = a.GetRow() - b.GetRow();
    char ret;
    
    char directionTable[3][3] = { {5,6,7}, {4,0,8}, {3,2,1}};
    if (diff_col > 1 || diff_row > 1) 
        return -1;
    else
    {  
        ret = directionTable[diff_row+1][diff_col+1];
        return ret;
    }
}
Esempio n. 2
0
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();
}