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