import asyncio import json import random from ulab import numpy as np import arena import robot class DistanceSensorTracker: def __init__(self): robot.left_distance.distance_mode = 2 robot.right_distance.distance_mode = 2 robot.left_distance.timing_budget = 50 robot.right_distance.timing_budget = 50 self.left = 300 self.right = 300 async def main(self): robot.left_distance.start_ranging() robot.right_distance.start_ranging() while True: if robot.left_distance.data_ready and robot.left_distance.distance: self.left = robot.left_distance.distance * 10 # convert to mm robot.left_distance.clear_interrupt() if robot.right_distance.data_ready and robot.right_distance.distance: self.right = robot.right_distance.distance * 10 robot.right_distance.clear_interrupt() await asyncio.sleep(0.01) class CollisionAvoid: def __init__(self, distance_sensors): self.speed = 0.6 self.distance_sensors = distance_sensors async def main(self): while True: robot.set_right(self.speed) while self.distance_sensors.left < 300 or \ self.distance_sensors.right < 300: robot.set_left(-self.speed) await asyncio.sleep(0.3) robot.set_left(self.speed) await asyncio.sleep(0) def get_scaled_sample_around_mean(mean, scale): return mean + (random.uniform(-scale, scale) + random.uniform(-scale, scale)) / 2 def convert_to_standard_position(true_bearing): standard_position = 90 - true_bearing if standard_position > 180: standard_position -= 360 elif standard_position < -180: standard_position += 360 return standard_position def send_json(data): robot.uart.write((json.dumps(data) + "\n").encode()) def read_json(): try: data = robot.uart.readline() decoded = data.decode() return json.loads(decoded) except (UnicodeError, ValueError): print("Invalid data") return None def send_poses(samples): send_json({ "poses": np.array(samples[:,:2], dtype=np.int16).tolist(), }) class Simulation: def __init__(self): self.population_size = 200 self.imu_mix = 0.3 * 0.5 self.encoder_mix = 0.7 self.rotation_scale = 0.5 # degrees self.speed_scale = 3 # mm # Poses - each an array of [x, y, heading] self.poses = np.array( [( int(random.uniform(0, arena.width)), int(random.uniform(0, arena.height)), int(random.uniform(0, 360))) for _ in range(self.population_size)], dtype=np.float, ) self.distance_sensors = DistanceSensorTracker() self.collision_avoider = CollisionAvoid(self.distance_sensors) self.last_encoder_left = robot.left_encoder.read() self.last_encoder_right = robot.right_encoder.read() async def apply_sensor_model(self): # Based on vl53l1x sensor readings, create weight for each pose. # vl53l1x standard dev is +/- 5 mm. Each distance is a mean reading # we will first determine sensor positions based on poses # project forward based on distances sensed, introducing noise (based on standard dev) # then check this projected position against occupancy grid # and weight accordingly # distance sensor positions projected forward. x, y distance_sensor_left = np.zeros( (self.poses.shape[0], 2), dtype=np.float) distance_sensor_right = np.zeros( (self.poses.shape[0], 2), dtype=np.float) # sensors - they are facing forward, either side of the robot. Project them out to the sides # based on each poses heading and turn sensors into rays, # left sensor poses_left_90 = np.radians(self.poses[:, 2] + 90) distance_sensor_left[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.distance_sensor_from_middle distance_sensor_left[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_from_middle # now project forward by distance sensor range distance_sensor_left[:, 0] += np.cos(self.poses[:, 2]) * self.distance_sensors.left distance_sensor_left[:, 1] += np.sin(self.poses[:, 2]) * self.distance_sensors.left # right sensor poses_right_90 = np.radians(self.poses[:, 2] - 90) distance_sensor_right[:, 0] = self.poses[:, 0] + np.cos(poses_right_90) * robot.distance_sensor_from_middle distance_sensor_right[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_from_middle # now project forward by distance sensor range distance_sensor_right[:, 0] += np.cos(self.poses[:, 2]) * self.distance_sensors.right distance_sensor_right[:, 1] += np.sin(self.poses[:, 2]) * self.distance_sensors.right await asyncio.sleep(0) # weighted poses a numpy array of weights for each pose weights = np.zeros(self.poses.shape[0], dtype=np.float) for index in range(self.poses.shape[0]): # remove any that are outside the arena if not arena.point_is_inside_arena(self.poses[index,0], self.poses[index,1]): weights[index] = 0 continue # difference between this distance and the distance sensed is the error # weight is the inverse of the error weights[index] = arena.get_distance_grid_at_point(distance_sensor_left[index,0], distance_sensor_left[index,1]) weights[index] += arena.get_distance_grid_at_point(distance_sensor_right[index,0], distance_sensor_right[index,1]) await asyncio.sleep(0) #normalise the weights weights = weights / np.sum(weights) return weights def resample(self, weights, sample_count): """Return sample_count number of samples from the poses, based on the weights array. Uses low variance resampling""" samples = [] start = random.uniform(0, 1 / sample_count) cumulative_weights = weights[0] source_index = 0 for current_index in range(sample_count): weight_index = start + current_index / sample_count while weight_index > cumulative_weights: source_index += 1 cumulative_weights += weights[source_index] samples.append(source_index) return np.array([self.poses[n] for n in samples]) def convert_odometry_to_motion(self, left_encoder_delta, right_encoder_delta): """ left_encoder is the change in the left encoder right_encoder is the change in the right encoder returns rot1, trans, rot2 rot1 is the rotation of the robot in degrees before the translation trans is the distance the robot has moved in mm rot2 is the rotation of the robot in degrees """ left_mm = left_encoder_delta * robot.ticks_to_mm right_mm = right_encoder_delta * robot.ticks_to_mm if left_mm == right_mm: # no rotation return 0, left_mm, 0 # calculate the radius of the arc radius = (robot.wheelbase_mm / 2) * (left_mm + right_mm) / (right_mm - left_mm) ## angle = difference in steps / wheelbase d_theta = (right_mm - left_mm) / robot.wheelbase_mm # For a small enough motion, assume that the chord length = arc length arc_length = d_theta * radius rot1 = np.degrees(d_theta/2) rot2 = rot1 return rot1, arc_length, rot2 async def motion_model(self): """move forward, apply the motion model""" new_heading = robot.imu.euler[0] new_encoder_left = robot.left_encoder.read() new_encoder_right = robot.right_encoder.read() rot1, trans, rot2 = self.convert_odometry_to_motion( new_encoder_left - self.last_encoder_left, new_encoder_right - self.last_encoder_right) self.last_encoder_left = new_encoder_left self.last_encoder_right = new_encoder_right try: new_heading = robot.imu.euler[0] except OSError: new_heading = None if new_heading: heading_change = self.last_heading - new_heading self.last_heading = new_heading # convert heading from true bearing to standard position heading_change = convert_to_standard_position(heading_change) # blend with the encoder heading changes rot1 = rot1 * self.encoder_mix + heading_change * self.imu_mix rot2 = rot2 * self.encoder_mix + heading_change * self.imu_mix else: print("Failed to get heading") await asyncio.sleep(0) rot1_model = np.array([get_scaled_sample_around_mean(rot1, self.rotation_scale) for _ in range(self.poses.shape[0])]) trans_model = np.array([get_scaled_sample_around_mean(trans, self.speed_scale) for _ in range(self.poses.shape[0])]) rot2_model = np.array([get_scaled_sample_around_mean(rot2, self.rotation_scale) for _ in range(self.poses.shape[0])]) self.poses[:,2] += rot1_model rot1_radians = np.radians(self.poses[:,2]) self.poses[:,0] += trans_model * np.cos(rot1_radians) self.poses[:,1] += trans_model * np.sin(rot1_radians) self.poses[:,2] += rot2_model self.poses[:,2] = np.array([float(theta % 360) for theta in self.poses[:,2]]) async def main(self): asyncio.create_task(self.distance_sensors.main()) asyncio.create_task(self.collision_avoider.main()) self.last_heading = robot.imu.euler[0] try: while True: weights = await self.apply_sensor_model() send_poses(self.resample(weights, 20)) self.poses = self.resample(weights, self.population_size) await asyncio.sleep(0) await self.motion_model() finally: robot.stop() async def updater(simulation): print("starting updater") while True: sys_status, gyro, accel, mag = robot.imu.calibration_status if sys_status < 3: send_json( { "imu_calibration": { "gyro": gyro, "accel": accel, "mag": mag, "sys": sys_status, } } ) await asyncio.sleep(0.5) async def command_handler(simulation): print("Starting handler") update_task = None simulation_task = None while True: if robot.uart.in_waiting: print("Receiving data...") request = read_json() if not request: print("no request") continue print("Received: ", request) if request["command"] == "arena": send_json({ "arena": arena.boundary_lines, }) if not update_task: update_task = asyncio.create_task(updater(simulation)) elif request["command"] == "start": if not simulation_task: simulation_task = asyncio.create_task(simulation.main()) await asyncio.sleep(0.1) simulation = Simulation() asyncio.run(command_handler(simulation))