Ejemplo n.º 1
0
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);
}
Ejemplo n.º 2
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;
}