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