def test_calculate_angle_difference_negative():
pose = Pose(Point(1, 2), math.pi / 4)
goal = Point(1, 3)
angle_difference = pose.calculate_angle_difference(goal)
assert math.isclose(angle_difference, -math.pi / 4)
def _init_pose(self) -> None:
area = random.choice(self._spawn_area)
x_coordinate = random.uniform(area[0][0], area[0][1])
y_coordinate = random.uniform(area[1][0], area[1][1])
position = Point(x_coordinate, y_coordinate)
yaw = random.uniform(-math.pi, math.pi)
self._pose = Pose(position, yaw)
def _create_scan_poses(self) -> np.ndarray:
scan_poses = np.empty(self._N_MEASUREMENTS, dtype=Pose)
for i, scan_angle in enumerate(self._SCAN_ANGLES):
scan_poses[i] = Pose(copy.copy(self._pose.position),
self._pose.yaw + scan_angle)
return scan_poses
def test_do_update_observation():
env = NavigationTrack()
env._pose = Pose(Point(0, 8.5), 0.78539816339)
env._do_update_observation()
assert np.allclose(env._observation,
[2.12132034356, 1.5, 2.12132034356, 10, 2.12132034356],
atol=0.06)
def test_yaw_right_action():
env = NavigationTrack()
env._pose = Pose(Point(0, 8.5), 0)
env._do_perform_action(env._YAW_RIGHT)
assert math.isclose(env._pose.position.x_coordinate, 0)
assert math.isclose(env._pose.position.y_coordinate, 8.54, abs_tol=0.06)
assert math.isclose(env._pose.yaw, 0.2, abs_tol=0.06)
def test_forward_action():
env = NavigationTrack()
env._pose = Pose(Point(0, 8.5), 0)
env._do_perform_action(env._FORWARD)
assert math.isclose(env._pose.position.x_coordinate, 0)
assert math.isclose(env._pose.position.y_coordinate, 8.7, abs_tol=0.06)
assert math.isclose(env._pose.yaw, 0, abs_tol=0.06)
def test_do_update_observation():
env = NavigationGoal()
env._pose = Pose(Point(0, 8.5), 0.78539816339)
env._goal = Point(1.5, 8.5)
env._do_update_observation()
expected_observation = [
2.12132034356, 1.5, 2.12132034356, 10, 14.1421356237, 1.5,
0.78539816339
]
assert np.allclose(env._observation, expected_observation, atol=0.06)
def test_rotate():
pose = Pose(Point(1, 2), math.pi / 4)
pose.rotate(math.pi / 4)
assert pose == Pose(Point(1, 2), math.pi / 2)
def test_shift():
pose = Pose(Point(1, 2), math.pi / 4)
pose.shift(math.sqrt(2), math.pi / 4)
assert pose == Pose(Point(2, 3), math.pi / 2)
def test_pose_equality():
pose1 = Pose(Point(1, 2), math.pi / 4)
pose2 = Pose(Point(1, 2), math.pi / 4)
assert pose1 == pose2
def test_move_with_negative_yaw():
pose = Pose(Point(1, 2), -math.pi / 4)
pose.move(math.sqrt(2))
assert pose == Pose(Point(0, 3), -math.pi / 4)
def test_move_with_positive_yaw():
pose = Pose(Point(1, 2), math.pi / 4)
pose.move(math.sqrt(2))
assert pose == Pose(Point(2, 3), math.pi / 4)
def test_move_with_yaw_pi():
pose = Pose(Point(1, 2), math.pi)
pose.move(1)
assert pose == Pose(Point(1, 1), math.pi)
def test_move_with_yaw_zero():
pose = Pose(Point(1, 2), 0)
pose.move(1)
assert pose == Pose(Point(1, 3), 0)
def test_valid_yaw_less_than_negative_pi():
pose = Pose(Point(1, 2), -3 * math.pi / 2)
assert pose.yaw == math.pi / 2
def test_valid_yaw_greater_than_pi():
pose = Pose(Point(1, 2), 3 * math.pi / 2)
assert pose.yaw == -math.pi / 2