def test_did_not_move(controller):
"""
Robot did not move.
"""
pos, angle = controller.odometry(10, 10, Vector2(0, 0), 0)
assert pos == Vector2(0, 0)
assert angle == 0
def test_negation():
"""
Test negation.
"""
vec = Vector2(42, -10)
assert Vector2(-42, 10) == -vec
def test_multiplication():
"""
Test multiplication.
"""
vec = Vector2(42, -10)
assert Vector2(84, -20) == vec * 2
def test_division():
"""
Test division.
"""
vec = Vector2(42, -10)
assert Vector2(21, -5) == vec / 2
def test_move_straight(configuration):
"""
Robot moved in a straight line.
"""
pos, angle = odometry_arc(10, 10, Vector2(0, 0), 0, configuration)
assert pos == Vector2(10, 0)
assert angle == 0
def test_did_not_move(configuration):
"""
Robot did not move an inch, with a non zero initial position.
"""
pos, angle = odometry_arc(0, 0, Vector2(123, 321), 456, configuration)
assert pos == Vector2(123, 321)
assert angle == 456
def test_addition():
"""
Test +.
"""
vec1 = Vector2(42, 0)
vec2 = Vector2(-40, 12)
assert Vector2(2, 12) == (vec1 + vec2)
def test_subtraction():
"""
Test -.
"""
vec1 = Vector2(10, -10)
vec2 = Vector2(20, -20)
assert Vector2(-10, 10) == (vec1 - vec2)
def test_move_straight_at_angle(configuration):
"""
Robot moved in a straight line but with a non zero initial angle.
"""
pos, angle = odometry_arc(10, 10, Vector2(0, 0), math.pi / 2,
configuration)
assert pos == Vector2(0, 10)
assert angle == math.pi / 2
def test_rotate_without_translating(configuration):
"""
Robot rotated without any linear movement.
"""
angle_rotated = math.pi / 2
distance = angle_rotated * DISTANCE_BETWEEN_WHEELS / 2
pos, angle = odometry_arc(-distance, distance, Vector2(1, 2), 3,
configuration)
assert pos == Vector2(1, 2)
assert angle == math.pi / 2 + 3
def test_odometry_position(localization_controller,
odometry_controller_mock):
"""
Test the functions related to odometry pos/angle.
"""
odometry_controller_mock.odometry = MagicMock(
return_value=(Vector2(1, 2), 3))
localization_controller.update_odometry_position(10, 10)
assert localization_controller.get_position() == Vector2(1, 2)
assert localization_controller.get_angle() == 3
def odometry_controller(configuration):
"""
Odometry controller.
"""
controller = OdometryController(configuration=configuration)
# Set initial position.
pos, angle = controller.odometry(10, 10, Vector2(0, 0), 0)
assert pos == Vector2(0, 0)
assert angle == 0
return controller
def test_move_straight(controller):
"""
Robot moved in a straight line.
"""
pos, angle = controller.odometry(20, 20, Vector2(0, 0), 0)
assert pos == Vector2(
2 * math.pi * WHEEL_RADIUS * 10 / TICK_PER_REVOLUTION,
0,
)
assert angle == 0
# Move backward in a straight line.
pos, angle = controller.odometry(10, 10, Vector2(0, 0), math.pi / 2)
assert pos.x < 1e-10
assert pos.y == -2 * math.pi * WHEEL_RADIUS * 10 / TICK_PER_REVOLUTION
assert angle == math.pi / 2
def set_detection(
self, seen_polar: Tuple[Tuple[Radian, Millimeter], ...]) -> None:
""" Set the detected obstacles to the desired value in the lidar controller. """
self.seen_polar = seen_polar
self.seen_cartesian = tuple(
Vector2(radius * cos(angle), radius * sin(angle))
for radius, angle in seen_polar)
def test_rotate_and_move_left(controller):
"""
Robot rotated AND translated. Right wheel travelled more than left wheel.
"""
pos, angle = controller.odometry(10, 11, Vector2(0, 0), 0)
assert pos.x > 0 # Moved forward.
assert pos.y > 0 # Went a bit up.
assert angle > 0 # Turned left.
def test_rotate_and_move_right(controller):
"""
Robot rotated AND translated. Left wheel travelled more than right wheel.
"""
pos, angle = controller.odometry(11, 10, Vector2(0, 0), 0)
assert pos.x > 0 # Moved forward.
assert pos.y < 0 # Went a bit down.
assert angle < 0 # Turned right.
def test_rotate_and_move_left(configuration):
"""
Robot rotated AND translated. Right wheel travelled more than left wheel.
"""
pos, angle = odometry_arc(0, 1, Vector2(0, 0), 0, configuration)
assert pos.x > 0 # Moved forward.
assert pos.y > 0 # Went a bit up.
assert angle > 0 # Turned left.
def configuration_stub(configuration_test: Configuration) -> Configuration:
"""
Configuration for tests.
"""
return dataclasses.replace(configuration_test,
initial_angle=0,
initial_position=Vector2(0, 0),
wheel_radius=1 / (2 * math.pi))
def test_properties():
"""
Test X and Y properties.
"""
vec = Vector2(10, -42)
assert vec.x == 10
assert vec.y == -42
def symmetries_position(self, position: Vector2) -> Vector2:
"""
Symmetries a position if necessary
"""
if self.configuration.color == Color.YELLOW:
symmetric_position = Vector2(-position.x, position.y)
return symmetric_position
return position
def right(angle: Radian) -> Vector2:
"""
Get the right direction relative to the direction.
"""
return Vector2(
math.cos(angle - math.pi / 2),
math.sin(angle - math.pi / 2),
)
def forward(angle: Radian) -> Vector2:
"""
Get the front direction relative to the direction.
"""
return Vector2(
math.cos(angle),
math.sin(angle),
)
def test_rotate_and_move_right(configuration):
"""
Robot rotated AND translated. Left wheel travelled more than right wheel.
"""
pos, angle = odometry_arc(1, 0, Vector2(0, 0), 0, configuration)
assert pos.x > 0 # Moved forward.
assert pos.y < 0 # Went a bit down.
assert angle < 0 # Turned right.
def test_json_encoder_vector2():
"""
Happy path for Vector2.
"""
to_encode = {'a': Vector2(2, 3)}
dump = json.dumps(to_encode, cls=RobotJSONEncoder, indent=None)
assert dump == '{"a": {"x": 2.0, "y": 3.0}}'
def test_vector_sym2():
"""
Test vector symmetry
"""
vec = Vector2(-167, 124)
symmetry_controller = get_symmetry_controller(color=Color.BLUE)
sym_vec = symmetry_controller.symmetries_position(vec)
assert sym_vec.x == -167
assert sym_vec.y == 124
def test_vector_sym1():
"""
Test vector symmetry
"""
vec = Vector2(-148, 29)
symmetry_controller = get_symmetry_controller(color=Color.YELLOW)
sym_vec = symmetry_controller.symmetries_position(vec)
assert sym_vec.x == 148
assert sym_vec.y == 29
def _odometry(self, left_tick: int, right_tick: int, pos: Vector2, angle: Radian) -> \
Tuple[Vector2, Radian]:
if not self.intialized:
# Cannot calculate the delta of the ticks.
self.intialized = True
return pos, angle
d_left = self._tick_to_millimeter(left_tick - self.previous_left_tick)
d_right = self._tick_to_millimeter(right_tick -
self.previous_right_tick)
if d_left == d_right == 0:
# Robot did not move.
return pos, angle
d_distance = (d_right + d_left) / 2
if d_distance == 0:
# Robot did not translate, rotate only.
wheel_dist = self.configuration.distance_between_wheels / 2
d_theta = d_right / wheel_dist
return pos, angle + d_theta
if d_right == d_left:
# Trajectory is straight, translate only.
return pos + Vector2(
math.cos(angle) * d_distance,
math.sin(angle) * d_distance), angle
# Trajectory is both translation and rotation.
# 1. Calculate the radius of curvature.
wheel_dist = self.configuration.distance_between_wheels
curvature_radius = (d_right + d_left) / \
(d_right - d_left) * wheel_dist / 2
# 2. Calculate the angle delta.
d_theta = d_distance / curvature_radius
# 3. Calculate the center of curvature.
center_pos = pos + Vector2(-curvature_radius * math.sin(angle),
curvature_radius * math.cos(angle))
# 4. Calculate the "new" position, approximating the trajectory to a circular arc.
return center_pos + Vector2(
curvature_radius * math.sin(angle + d_theta),
-curvature_radius * math.cos(angle + d_theta)), angle + d_theta
async def run(self) -> None:
"""
Run the strategy.
"""
for vec, reverse in PATH:
LOGGER.get().info("move robot", destination=vec)
await self.motion_controller.move_to(Vector2(vec.x, 2000 - vec.y),
reverse)
LOGGER.get().info("Strategy algorithm finished running") # lol
def ray_segment_intersection(
ray: Ray,
segment: Segment) -> Tuple[Optional[Vector2], Optional[float]]:
"""
Get the intersection point between a ray and a segment.
More precisely, will find the intersection point between [a,b) and ]c,d[ (thus a 0 length
segment will never intersect).
Returns None if they do not intersect.
Return the intersection coordinates and the distance from the origin of the ray otherwise.
"""
origin = numpy.array([ray.origin.x, ray.origin.y])
direction = numpy.array([ray.direction.x, ray.direction.y])
start = numpy.array([segment.start.x, segment.start.y])
end = numpy.array([segment.end.x, segment.end.y])
# Find intersection point:
# origin + direction * x = start + (end - start) * y
# origin ^ (end-start) + direction ^ (end-start) * x = start ^ (end-start)
# direction ^ (end-start) * x = start ^ (end-start) - origin ^ (end-start)
# direction ^ (end-start) * x = (start - origin) ^ (end-start)
# A * x = B
# x = B / A
segment_length2 = (end - start).dot(end - start)
if segment_length2 == 0:
return None, None # Segment is 0 length, will never be hit by a ray.
coef_a = numpy.cross(direction, (end - start))
if coef_a == 0:
return None, None # Collinear, will never intersect.
coef_b = numpy.cross(start - origin, end - start)
v_x = coef_b / coef_a
if v_x < 0:
return None, None # Ray hit behind.
# Then...
# intersection_point = origin + direction * x
intersection_point = origin + direction * v_x
# To find Y:
# intersection_point = start + (end - start) * y
# intersection_point - start = (end - start) * y
# (intersection_point - start) . (end - start) = (end - start) . (end - start) * y
# (intersection_point - start) . (end - start) = || end - start || ** 2 * y
# y = (intersection_point - start) . (end - start) / || end - start || ** 2
# y = (intersection_point - start) . (end - start) / || end - start || ** 2
coef_a = (intersection_point - start).dot(end - start)
v_y = coef_a / segment_length2
if v_y <= 0 or v_y >= 1:
return None, None # Ray did not hit segment.
return Vector2(*intersection_point), v_x
def odometry_arc(delta_left: Millimeter, delta_right: Millimeter,
current_position: Vector2, current_angle: Radian,
configuration: Configuration) -> Tuple[Vector2, Radian]:
"""
Computes the current position and angle of the robot using the movements of its wheels.
Uses a curvature radius to compute the new position.
"""
if math.isclose(delta_right, 0) and math.isclose(delta_left, 0):
# The robot did not move.
return current_position, current_angle
delta_distance = (delta_left + delta_right) / 2
if math.isclose(delta_distance, 0):
# Robot did not translate, rotation only.
wheel_dist = configuration.distance_between_wheels / 2
d_theta = delta_right / wheel_dist
return current_position, current_angle + d_theta
if math.isclose(delta_left, delta_right):
# Linear Translation.
return current_position + Vector2(
math.cos(current_angle) * delta_distance,
math.sin(current_angle) * delta_distance), current_angle
# Trajectory is both translation and rotation.
# 1. Calculate the radius of curvature.
wheel_dist = configuration.distance_between_wheels
curvature_radius = (delta_right + delta_left) / \
(delta_right - delta_left) * wheel_dist / 2
# 2. Calculate the angle delta.
d_theta = delta_distance / curvature_radius
# 3. Calculate the center of curvature.
center_pos = current_position + Vector2(
-curvature_radius * math.sin(current_angle),
curvature_radius * math.cos(current_angle))
# 4. Calculate the "new" position, approximating the trajectory to a circular arc.
return center_pos + Vector2(
curvature_radius * math.sin(current_angle + d_theta), -curvature_radius
* math.cos(current_angle + d_theta)), current_angle + d_theta
