void TurnRobotToWaypoint(Robot& r, const double angle) { // The mistake in the robot static const double deg_miss = (2.0 * M_PI / 180.0); // 0.0349 // static const double deg_miss = 0.0; double robot_angle; // Get the robot's yaw if (ConfigurationManager::USE_PARTICLES) { robot_angle = r.GetEstimateLocationForGrid().yaw_; } else { robot_angle = r.GetRealLocationForGrid().yaw_; robot_angle = ((robot_angle > 0) ? robot_angle : M_PI + M_PI + robot_angle); } // If the robot need to move with clockwise // Means, the current angle is bigger than the walking angle if (((robot_angle > angle) && (robot_angle - angle < M_PI)) || ((robot_angle < angle) && (angle - robot_angle > M_PI))) { // While we havn't reach the desired walking angel yet // Keep turn the robot clockwise while (angle < robot_angle - deg_miss) { // Turn the robot r.SetSpeed(0.0, ConfigurationManager::GetTurnRightSpeed()); // Refresh the particles r.Refresh(); if (ConfigurationManager::USE_PARTICLES) { robot_angle = r.GetEstimateLocationForGrid().yaw_; } else { robot_angle = r.GetRealLocationForGrid().yaw_; robot_angle = ((robot_angle > 0) ? robot_angle : M_PI + M_PI + robot_angle); } } } else {// turn against clockwise while (angle > robot_angle + deg_miss) { // Turn the robot r.SetSpeed(0.0, ConfigurationManager::GetTurnLeftSpeed()); // Refresh the particles r.Refresh(); if (ConfigurationManager::USE_PARTICLES) { robot_angle = r.GetEstimateLocationForGrid().yaw_; } else { robot_angle = r.GetRealLocationForGrid().yaw_; robot_angle = ((robot_angle > 0) ? robot_angle : M_PI + M_PI + robot_angle); } } } r.SetSpeed(0.0, 0.0); // Refresh the particles r.Refresh(); }
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; }