def test_given_two_obstacles_when_find_two_closest_obstacles_then_return_obstacles(
self):
obstacles = [
Obstacle((10, 10), OBSTACLE_RADIUS),
Obstacle((20, 20), OBSTACLE_RADIUS)
]
real_world_environment = RealWorldEnvironment(obstacles=obstacles)
self.assertEqual(
obstacles,
real_world_environment.find_two_closest_obstacles(ANY_ROBOT))
def test_given_three_obstacles_when_find_two_closest_obstacles_then_return_two_closest_obstacles(
self):
expected_closest_obstacles = [
Obstacle((10, 10), OBSTACLE_RADIUS),
Obstacle((20, 10), OBSTACLE_RADIUS)
]
obstacles = list(expected_closest_obstacles)
obstacles.append(Obstacle((100, 100), OBSTACLE_RADIUS))
robot = Robot((15, 10), ROBOT_DIRECTION)
real_world_environment = RealWorldEnvironment(obstacles=obstacles)
closest_obstacles = real_world_environment.find_two_closest_obstacles(
robot)
self.assertEqual(expected_closest_obstacles, closest_obstacles)
def project_obstacle_from_pixel_to_real_world(
self, obstacle: Obstacle) -> Obstacle:
object_points = np.array([(0, 0, 41), (6.3, 0, 41), (-6.3, 0, 41),
(0, 6.3, 41), (0, -6.3, 41)], 'float32')
image_points = np.array([
obstacle.center,
(obstacle.center[0] + obstacle.radius, obstacle.center[1]),
(obstacle.center[0] - obstacle.radius, obstacle.center[1]),
(obstacle.center[0], obstacle.center[1] + obstacle.radius),
(obstacle.center[0], obstacle.center[1] - obstacle.radius)
])
_, rotation_vector, translation_vector = cv2.solvePnP(
object_points, image_points, self.camera_parameters.camera_matrix,
self.camera_parameters.distortion)
camera_to_obstacle = Transform.from_parameters(
np.asscalar(translation_vector[0]),
np.asscalar(translation_vector[1]),
np.asscalar(translation_vector[2]),
np.asscalar(rotation_vector[0]), np.asscalar(rotation_vector[1]),
np.asscalar(rotation_vector[2]))
world_to_obstacle = self.world_from_camera(camera_to_obstacle)
obstacle_information = world_to_obstacle.to_parameters(True)
return Obstacle((obstacle_information[0], obstacle_information[1]), 7)
def test_given_invalid_obstacle_then_exception_raised(self):
obstacle = Obstacle(INVALID_OBSTACLE_POSITION, NavigationEnvironment.OBSTACLE_RADIUS)
obs_list = [obstacle]
navigation_environment = NavigationEnvironment(MagicMock())
navigation_environment.create_grid()
self.assertRaises(NavigationEnvironmentDataError, navigation_environment.add_obstacles(obs_list))
def test_when_draw_real_environment_then_draw_each_cubes_and_obstacles(
self, cv2: Mock):
cubes = [FlagCube((0, 0), Color.BLACK), FlagCube((10, 10), Color.BLUE)]
obstacles = [Obstacle((50, 50), 10)]
target_zone = TargetZone(60)
real_environment = RealWorldEnvironment(obstacles, cubes, target_zone)
frame = MagicMock()
self.frame_drawer.draw_real_world_environment(frame, real_environment)
cv2.rectangle.assert_called()
cv2.circle.assert_called()
def test_when_draw_vision_environment_then_draw_each_cubes_and_obstacles(
self, cv2: Mock):
cubes = [
VisionCube(Color.RED, [(0, 0), (10, 10)]),
VisionCube(Color.RED, [(0, 0), (10, 10)])
]
obstacles = [Obstacle((50, 50), 10)]
vision_environment = VisionEnvironment(cubes, obstacles)
frame = MagicMock()
self.frame_drawer.draw_vision_environment(frame, vision_environment)
cv2.rectangle.assert_called()
cv2.circle.assert_called()
def test_given_obstacle_when_adding_obstacles_then_add_obstacle_to_navigation_environment(self):
obstacle = Obstacle(OBSTACLE_POSITION, NavigationEnvironment.OBSTACLE_RADIUS)
obs_list = [obstacle]
navigation_environment = NavigationEnvironment(MagicMock())
navigation_environment.create_grid()
navigation_environment.add_obstacles(obs_list)
for x in range(OBSTACLE_POSITION[0] - NavigationEnvironment.OBSTACLE_RADIUS,
OBSTACLE_POSITION[0] + NavigationEnvironment.OBSTACLE_RADIUS + 1):
for y in range(OBSTACLE_POSITION[1] - NavigationEnvironment.OBSTACLE_RADIUS,
OBSTACLE_POSITION[1] + NavigationEnvironment.OBSTACLE_RADIUS + 1):
self.assertEqual(Grid.OBSTACLE_VALUE,
navigation_environment.get_grid().get_vertex((x, y)).get_step_value())
def __project_and_draw_real_obstacle(self, frame,
obstacle: Obstacle) -> None:
radius_line = list(map(self.to_3d, obstacle.get_radius_line()))
real_positions = np.array(radius_line, 'float32')
image_positions = self.coordinate_converter.project_points_from_real_world_to_pixel(
real_positions)
cv2.circle(frame,
tuple(image_positions[0][0]),
image_positions[1][0][0] - image_positions[0][0][0],
Color.PINK2.bgr,
thickness=3,
lineType=cv2.LINE_AA)
def test_when_obstacle_then_goes_around_it(self):
environment = NavigationEnvironment(MagicMock())
environment.create_grid()
environment.add_obstacles([
Obstacle((SOME_OBSTACLE_LOCATION, SOME_OBSTACLE_LOCATION),
NavigationEnvironment.OBSTACLE_RADIUS)
])
starting_point_next_to_obstacle = (
SOME_OBSTACLE_LOCATION + Grid.DEFAULT_OFFSET -
NavigationEnvironment.OBSTACLE_RADIUS - 1,
SOME_OBSTACLE_LOCATION + 1)
ending_point_next_to_obstacle = (SOME_OBSTACLE_LOCATION -
Grid.DEFAULT_OFFSET +
NavigationEnvironment.OBSTACLE_RADIUS,
SOME_OBSTACLE_LOCATION)
path_calculator = PathCalculator(MagicMock())
path_calculator.calculate_path(starting_point_next_to_obstacle,
ending_point_next_to_obstacle,
environment.get_grid())
for position in path_calculator.get_calculated_path():
self.assertFalse(environment.get_grid().is_obstacle(position))
def __create_obstacle(self, contour) -> Obstacle:
center = (contour[0], contour[1])
radius = contour[2]
return Obstacle(center, radius)
'robot': {
'update_robot': False,
'use_mocked_robot_detector': False,
'mocked_robot_position': [15, 15],
'mocked_robot_orientation': 45
},
'cube_positions': CUBE_POSITION
}
}
SCENARIO_2 = {
'network_country_code': 31,
'infrared_signal_asked': True,
'real_world': {
'cubes': [FlagCube((152, -19), Color.WHITE)],
'obstacles': [Obstacle((97.45940399169922, 1.5616950988769531), 7)]
},
'config': {
'table_number': 4,
'resources_path': RESOURCES_PATH,
'camera': {
'mocked_camera_image_path':
"fig/saved_images/table2/00h02m31s.jpg",
'image_save_dir': "fig/saved_images/"
},
'robot': {
'update_robot': False,
'use_mocked_robot_detector': False,
'mocked_robot_position': [15, 15],
'mocked_robot_orientation': 45
},