void WalkFromObstacle(Robot& r) {
r.SetSpeed(0.0, 0.0);
r.Refresh();
int density_right = 0;
int density_left = 0;
// Check what is more clear - right or left - for every obstacle in ray the direction gets a point to the counter
for (int i = 0; i < ConfigurationManager::GetMiddleLaserIndex(); i++) {
// Left side
if (r.GetLaser(i) < ConfigurationManager::GetObstacleAheadDistace()) {
density_right++;
}
// Right side
if (r.GetLaser(ConfigurationManager::GetLaserSamples() - i) < ConfigurationManager::GetObstacleAheadDistace()) {
density_left++;
}
}
// Turn to the cleanest side
if (density_right > density_left) {
r.SetSpeed(0.0, ConfigurationManager::GetTurnRightSpeed());
}
else {
r.SetSpeed(0.0, ConfigurationManager::GetTurnLeftSpeed());
}
// Keep turn in place until there is no obstacle ahead
while (true) {
bool obstacle_found = false;
r.Refresh();
for (int i = 0; i < ConfigurationManager::GetLastLaserIndex(); i++) {
obstacle_found |= (r.GetLaser(i) < ConfigurationManager::GetObstacleAheadDistace());
}
if (!obstacle_found) {
break;
}
}
// If there's no more obstacle ahead, stop turning and continue moving forward for a bit
r.SetSpeed(ConfigurationManager::GetMoveForawrdSpeed(), 0);
// Some minor movements in new selected path
for (int i = 0; i < ConfigurationManager::GetMinorMovementLoops(); i++) {
r.Refresh();
}
// Stop walking!
r.SetSpeed(0.0, 0.0);
}
bool WalkToWaypoint(Robot& r, Map& grid) {
bool arrivedToWayoint = false;
bool on_waypoint = false;
bool on_a_star = false;
bool obstacle_found = false;
r.SetSpeed(ConfigurationManager::GetMoveForawrdSpeed(), 0);
Point current_location;
// Some minor movements in new selected path
for (int i = 0; i < ConfigurationManager::GetMinorMovementLoops(); i++) {
r.Refresh();
}
Point p;
while (true) {
r.Refresh();
if (ConfigurationManager::USE_PARTICLES) {
Location estimated = r.GetEstimateLocationForGrid();
current_location = Point(floor((estimated.x_)), floor(estimated.y_));
}
else {
current_location = Point(floor((r.GetRealLocationForGrid().x_)), floor(r.GetRealLocationForGrid().y_ ));
}
if(ConfigurationManager::DEBUG_BEHAIVOURS)
{
cout << "(" << current_location.x_
<< "," << current_location.y_
<< ")" << endl;
}
// Check if arrived to the waypoint in radius of 1
for (int i = -1; i <= 1; i++) {
for (int j = -1; j <= 1; j++) {
p = Point(current_location.x_ + j, current_location.y_ + i);
on_waypoint |=
(grid[p.y_][p.x_].cell_state ==
GridCellState::WAY_POINT);
}
}
// If arrived, break
if (on_waypoint) {
arrivedToWayoint = true;
break;
}
// If didn't arrive check if obstacle found ahead - If in the radius ahead the laser ray is smaller then 0.3m
for (int i = ConfigurationManager::GetObstacleStartIndex(); i < ConfigurationManager::GetObstacleEndIndex(); i++) {
obstacle_found |= (r.GetLaser(i) < ConfigurationManager::GetObstacleAheadDistace());
}
// If obstacle found, avoid it
if (obstacle_found) {
// if(ConfigurationManager::DEBUG_BEHAIVOURS)
// {
cout << "Obtstacle found ahead" << endl;
// }
WalkFromObstacle(r);
break;
}
// Check if in the A* path
on_a_star = false;
for (int i = -1; i <= 1; i++) {
for (int j = -1; j <= 1; j++) {
p = Point(current_location.x_ + j, current_location.y_ + i);
on_a_star |=
(grid[p.y_][p.x_].cell_state ==
GridCellState::A_STAR_PATH);
}
}
if (!on_a_star) {
// if(ConfigurationManager::DEBUG_BEHAIVOURS)
// {
cout << "lost A* path" << endl;
// }
break;
}
}
r.SetSpeed(0.0, 0.0);
return arrivedToWayoint;
}
