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; }