def test_turn_location_position(self): # Human assisted test # Ensure that the car returns to the same location if we drive around in circles. import visualizer pose_estimator = PoseEstimator((0, 0, 0), 0, 0, 0, 0) d = 4 steering_angle_radians = np.pi / d outer_turn_radius_meters = pose_estimator.calc_outer_turn_radius( steering_angle_radians) front_wheel_turn_circumference = 2 * np.pi * config.FRONT_WHEEL_RADIUS_METERS turn_circle_circumference = 2 * np.pi * outer_turn_radius_meters ticks_required = config.ENCODER_RESOLUTION_FRONT_WHEEL * turn_circle_circumference / front_wheel_turn_circumference result_loc = [] ticks = 0 for i in range(2 * d): result_loc.append( pose_estimator.estimate(time=i, steering_angle=steering_angle_radians, encoder_ticks=ticks, angular_velocity=0)) ticks = ticks + ticks_required / (2 * d) for loc in result_loc: visualizer.draw_car(loc[0], loc[1], loc[2]) visualizer.show()
def test_outer_radius(self): pose_estimator = PoseEstimator((0, 0, 0), 0, 0, 0, 0) steering_angle_radians = np.pi / 2 self.assertEqual( config.DISTANCE_FROM_FRONT_WHEEL_BACK_AXIS, pose_estimator.calc_outer_turn_radius(steering_angle_radians)) steering_angle_radians = np.pi / 4 self.assertEqual( config.DISTANCE_FROM_FRONT_WHEEL_BACK_AXIS / np.sin(steering_angle_radians), pose_estimator.calc_outer_turn_radius(steering_angle_radians)) steering_angle_radians = 0 self.assertEqual( np.inf, pose_estimator.calc_outer_turn_radius(steering_angle_radians))