def __move_out_of_obstacles(self):
obstacle = self._model.real_world_environment.find_closest_obstacle(
self._model.robot)
dodge_direction = get_normalized_direction(obstacle.center,
self._model.robot.center)
dodge_angle = get_angle(dodge_direction)
delta_angle = (dodge_angle - self._model.robot.orientation) % 360
cardinal_angle = (round(delta_angle / 90) * 90) % 360
distance_to_move = abs(self.__navigation_environment.BIGGEST_ROBOT_RADIUS + \
self.__navigation_environment.OBSTACLE_RADIUS - \
distance_between(obstacle.center, self._model.robot.center) + 3)
if distance_to_move < 1:
distance_to_move = 1
self.__logger.info('Moving away from obstacle {} of {} cm.'.format(
obstacle, distance_to_move))
if cardinal_angle == 0:
self.__movements_to_destination.insert(0,
Forward(distance_to_move))
elif cardinal_angle == 90:
self.__movements_to_destination.insert(0, Left(distance_to_move))
elif cardinal_angle == 180:
self.__movements_to_destination.insert(0,
Backward(distance_to_move))
elif cardinal_angle == 270:
self.__movements_to_destination.insert(0, Right(distance_to_move))
else:
self.__logger.warning(
'Supposed to move at {} degree...'.format(cardinal_angle))
self.__movements_to_destination.append(Forward(0))
def test_when_convert_example_path_then_received_corresponding_commands(self):
self.path = [(0, 0), (5, 5), (5, 9), (4, 9), (3, 8)]
result_movements, result_path = self.path_converter.convert_path(self.path, self.robot, 130)
print(result_path)
expected_movements = [Rotate(45), Forward((round((2 * 5 ** 2) ** (1 / 2), 1))),
Rotate(45), Forward(4.0),
Rotate(90), Forward(1.0),
Rotate(45), Forward(FORTY_FIVE_DEGREES_MOVE_LENGTH),
Rotate(-95)]
expected_path = [((0, 0), (5, 5)), ((5, 5), (5, 9)), ((5, 9), (4, 9)), ((4, 9), (3, 8))]
self.assertEqual(expected_path, result_path)
self.assertEqual(expected_movements, result_movements)
def interactive_testing(self):
while True:
command = input(
'enter something:ir, grab, drop, light, forward, backward, rotate'
)
command = command.split(" ")
self.__logger.info('You entered : {}'.format(command[0]))
if command[0] == 'ir':
self.__network.send_actions([IR()])
self.__check_infrared_signal()
elif command[0] == 'grab':
self.__network.send_actions([Grab()])
elif command[0] == 'drop':
self.__network.send_actions([Drop()])
elif command[0] == 'led':
self.__network.send_actions([LightItUp()])
elif command[0] == 'f':
self.__network.send_actions([Forward(float(command[1]))])
elif command[0] == 'r':
self.__network.send_actions([Right(float(command[1]))])
elif command[0] == 'l':
self.__network.send_actions([Left(float(command[1]))])
elif command[0] == 'b':
self.__network.send_actions([Backward(float(command[1]))])
elif command[0] == 'ro':
self.__network.send_actions([Rotate(float(command[1]))])
def __move_robot_to_grab_cube(self):
if self._model.target_cube.wall == Wall.UP:
robot_pos = self._model.robot.center[1]
target_position = self._model.target_cube.center[1] - self.__config[
'distance_between_robot_center_and_cube_center']
elif self._model.target_cube.wall == Wall.DOWN:
robot_pos = self._model.robot.center[1]
target_position = self._model.target_cube.center[1] + self.__config[
'distance_between_robot_center_and_cube_center']
elif self._model.target_cube.wall == Wall.MIDDLE:
robot_pos = self._model.robot.center[0]
target_position = self._model.target_cube.center[0] - self.__config[
'distance_between_robot_center_and_cube_center']
else:
self.__logger.info("Where tf is the cube? {}".format(
self._model.target_cube.wall.name))
return
distance_to_travel = ceil(abs(target_position - robot_pos))
self.__logger.info("Moving to grab cube by : {} cm".format(
str(distance_to_travel)))
self.__destination = None
self.__todo_when_arrived_at_destination = [
Forward(distance_to_travel),
CanIGrab()
]
self.__update_path(force=True)
self.__send_next_actions_commands()
def __aligning_for_cube_drop(self):
robot_pos = (self._model.robot.center[0], self._model.robot.center[1])
target_position = (
self._model.country.stylized_flag.flag_cubes[
self._model.current_cube_index - 1].center[0] +
self.__config['distance_between_robot_center_and_cube_center'],
self._model.country.stylized_flag.flag_cubes[
self._model.current_cube_index - 1].center[1])
distance_to_travel = distance_between(robot_pos, target_position)
self.__logger.info("Moving to drop target : {} cm".format(
str(distance_to_travel)))
self.__destination = None
if robot_pos[0] < target_position[0]:
self.__todo_when_arrived_at_destination = [
Backward(distance_to_travel)
]
else:
self.__todo_when_arrived_at_destination = [
Forward(distance_to_travel)
]
self.__update_path(force=True)
self.__send_next_actions_commands()
def __approach_for_cube_grab(self):
robot_pos_x = self._model.robot.center[0]
robot_pos_y = self._model.robot.center[1]
if self._model.target_cube.wall == Wall.UP:
target_position_y = self._model.target_cube.center[1] - self.SAFE_DISTANCE_FROM_CUBE \
- self.__config['distance_between_robot_center_and_cube_center']
distance = target_position_y - robot_pos_y
elif self._model.target_cube.wall == Wall.DOWN:
target_position_y = self._model.target_cube.center[1] + self.SAFE_DISTANCE_FROM_CUBE \
+ self.__config['distance_between_robot_center_and_cube_center']
distance = target_position_y - robot_pos_y
elif self._model.target_cube.wall == Wall.MIDDLE:
target_position_x = self._model.target_cube.center[0] - self.SAFE_DISTANCE_FROM_CUBE \
- self.__config['distance_between_robot_center_and_cube_center']
distance = target_position_x - robot_pos_x
else:
self.__logger.info(
"Wall_of_next_cube is not correctly set:\n{}".format(
str(self._model.target_cube.wall)))
return
self.__destination = None
self.__todo_when_arrived_at_destination = [Forward(abs(distance))]
self.__update_path(force=True)
self.__send_next_actions_commands()
def test_when_convert_south_west_direction_then_command_length_is_radical_two(self):
path = [(0, 0), (-1, -1)]
movements, segments = self.path_converter.convert_path(path, self.robot)
expected_movements = [Rotate(-135), Forward(FORTY_FIVE_DEGREES_MOVE_LENGTH)]
expected_segments = [((0, 0), (-1, -1))]
self.assertEqual(expected_movements, movements)
self.assertEqual(expected_segments, segments)
def test_when_convert_north_direction_then_command_length_is_one(self):
path = [(0, 0), (0, 1)]
movements, segments = self.path_converter.convert_path(path, self.robot)
expected_movements = [Rotate(90), Forward(1)]
expected_segments = [((0, 0), (0, 1))]
self.assertEqual(expected_movements, movements)
self.assertEqual(expected_segments, segments)
def test_when_convert_south_east_direction_then_command_length_is_radical_two(self):
path = [(0, 0), (1, -1)]
movements, segments = self.path_converter.convert_path(path, self.robot)
expected_movements = [Rotate(-45), Forward(FORTY_FIVE_DEGREES_MOVE_LENGTH)]
expected_segments = [((0, 0), (1, -1))]
print(', '.join(str(mouv) for mouv in movements))
self.assertEqual(expected_movements, movements)
self.assertEqual(expected_segments, segments)
def test_when_convert_west_direction_then_command_length_is_one(self):
path = [(0, 0), (-1, 0)]
movements, segments = self.path_converter.convert_path(path, self.robot)
expected_movements = [Rotate(-180), Forward(1)]
expected_segments = [((0, 0), (-1, 0))]
print(', '.join(str(mouv) for mouv in movements))
self.assertEqual(expected_movements, movements)
self.assertEqual(expected_segments, segments)