path = planning.a_star((pf.centroid[0], pf.centroid[1]), goal) smoothed_path = planning.smooth_path(path, data_weight=0.3, smooth_weight=0.1) path_followed = smoothed_path.copy() planning.show(smoothed_path, block=False) print("localized at ") print(path_followed[0]) #path_followed.pop(0) #delete the start node, as it has been already reached if not localized: #if we are not localized yet, we explore the map and try to get our particle filter converged v, w = navigation.explore(z_us, z_v, z_w) robot.move(v, w) z_us, z_v, z_w = robot.sense() pf.move(z_v, z_w, dt) if resample_count >= 25: #we only resample every 25 steps, so more info is gathered and less time is lost pf.resample(z_us) resample_count = 0 #pf.show('Sense', save_figure=False) else: #once we are localized, we follow the path created previously, resampling this time at every simulation step if path_followed: v, w = navigation.move_control(pf.centroid, path_followed[0], z_us) robot.move(v, w) z_us, z_v, z_w = robot.sense() pf.move(z_v, z_w, dt)
class Run: def __init__(self, factory): """Constructor. Args: factory (factory.FactoryCreate) """ self.create = factory.create_create() self.time = factory.create_time_helper() self.servo = factory.create_servo() self.sonar = factory.create_sonar() # Add the IP-address of your computer here if you run on the robot self.virtual_create = factory.create_virtual_create() self.map = lab8_map.Map("lab8_map.json") self.odometry = Odometry() self.odometry.x = 1 self.odometry.y = 0.5 self.pidTheta = PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True) # constants self.base_speed = 100 self.variance_sensor = 0.1 self.variance_distance = 0.01 self.variance_direction = 0.05 self.world_width = 3.0 # the x self.world_height = 3.0 # the y self.travel_dist = 0.25 self.min_dist_to_wall = self.travel_dist + 0.2 self.min_dist_to_localize = 0.2 self.min_theta_to_localize = math.pi / 4 self.n_threshold = math.log(0.09) self.filter = ParticleFilter(self.virtual_create, self.variance_sensor, self.variance_distance, self.variance_direction, num_particles=100, world_width=self.world_width, world_height=self.world_height, input_map=self.map, n_threshold=self.n_threshold) def run(self): self.create.start() self.create.safe() # request sensors self.create.start_stream([ create2.Sensor.LeftEncoderCounts, create2.Sensor.RightEncoderCounts, ]) isLocalized = False angle_counter = 0 # This is an example on how to detect that a button was pressed in V-REP while not isLocalized: self.draw_particles() # generate random num command = random.choice([c for c in Command]) if command is Command.FORWARD: if self.sonar.get_distance() < self.min_dist_to_wall: continue self.filter.move(0, self.travel_dist) # 100 mm/s = 0.1 m/s self.create.drive_direct(self.base_speed, self.base_speed) self.sleep(abs(self.travel_dist / 0.1)) # stop self.create.drive_direct(0, 0) elif command is Command.TURN_LEFT: # turn_angle = math.pi / 2 + self.odometry.theta turn_angle = math.pi / 2 + angle_counter turn_angle %= 2 * math.pi angle_counter += math.pi / 2 angle_counter %= 2 * math.pi self.filter.move(turn_angle, 0) # turn left by 90 degree self.go_to_angle(turn_angle) elif command is Command.TURN_RIGHT: # turn_angle = -math.pi / 2 + self.odometry.theta turn_angle = -math.pi / 2 + angle_counter turn_angle %= 2 * math.pi angle_counter -= math.pi / 2 angle_counter %= 2 * math.pi self.filter.move(turn_angle, 0) # turn right by 90 degree self.go_to_angle(turn_angle) self.filter.sense(self.sonar.get_distance()) # check if localized # distance between odometry and estimated positions dist_position_to_goal = math.sqrt( (self.odometry.x - self.filter.x)**2 + (self.odometry.y - self.filter.y)**2) diff_theta_to_goal = abs(self.odometry.theta - self.filter.theta) isLocalized = dist_position_to_goal < self.min_dist_to_localize and diff_theta_to_goal < self.min_theta_to_localize def go_to_angle(self, goal_theta): while abs( math.atan2(math.sin(goal_theta - self.odometry.theta), math.cos(goal_theta - self.odometry.theta))) > 0.1: # print("Go TO: " + str(math.degrees(goal_theta)) + " " + str(math.degrees(self.odometry.theta))) output_theta = self.pidTheta.update(self.odometry.theta, goal_theta, self.time.time()) self.create.drive_direct(int(+output_theta), int(-output_theta)) self.sleep(0.01) self.create.drive_direct(0, 0) def sleep(self, time_in_sec): """Sleeps for the specified amount of time while keeping odometry up-to-date Args: time_in_sec (float): time to sleep in seconds """ start = self.time.time() while True: state = self.create.update() if state is not None: self.odometry.update(state.leftEncoderCounts, state.rightEncoderCounts) # print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta))) t = self.time.time() if start + time_in_sec <= t: break def draw_particles(self): data = [] # get position data from all particles for particle in self.filter.particles: data.extend([particle.x, particle.y, 0.1, particle.theta]) # paint all particles in simulation self.virtual_create.set_point_cloud(data)
class Run: def __init__(self, factory): """Constructor. Args: factory (factory.FactoryCreate) """ self.create = factory.create_create() self.time = factory.create_time_helper() self.servo = factory.create_servo() self.sonar = factory.create_sonar() # Add the IP-address of your computer here if you run on the robot self.virtual_create = factory.create_virtual_create() self.map = lab8_map.Map("lab8_map.json") self.odometry = Odometry() self.pidTheta = PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True) # constants self.base_speed = 100 self.variance_sensor = 0.1 self.variance_distance = 0.01 self.variance_direction = 0.05 self.world_width = 3.0 # the x self.world_height = 3.0 # the y self.filter = ParticleFilter(self.virtual_create, self.variance_sensor, self.variance_distance, self.variance_direction, num_particles=100, world_width=self.world_width, world_height=self.world_height) def run(self): # # This is an example on how to visualize the pose of our estimated position # # where our estimate is that the robot is at (x,y,z)=(0.5,0.5,0.1) with heading pi # self.virtual_create.set_pose((0.5, 0.5, 0.1), 0) # # # This is an example on how to show particles # # the format is x,y,z,theta,x,y,z,theta,... # data = [0.5, 0.5, 0.1, math.pi/2, 1.5, 1, 0.1, 0] # self.virtual_create.set_point_cloud(data) # # # This is an example on how to estimate the distance to a wall for the given # # map, assuming the robot is at (0, 0) and has heading math.pi # print(self.map.closest_distance((0.5,0.5), 0)) # # # This is an example on how to detect that a button was pressed in V-REP # while True: # b = self.virtual_create.get_last_button() # if b == self.virtual_create.Button.MoveForward: # print("Forward pressed!") # elif b == self.virtual_create.Button.TurnLeft: # print("Turn Left pressed!") # elif b == self.virtual_create.Button.TurnRight: # print("Turn Right pressed!") # elif b == self.virtual_create.Button.Sense: # print("Sense pressed!") # # self.time.sleep(0.01) self.create.start() self.create.safe() # request sensors self.create.start_stream([ create2.Sensor.LeftEncoderCounts, create2.Sensor.RightEncoderCounts, ]) # This is an example on how to detect that a button was pressed in V-REP while True: self.draw_particles() b = self.virtual_create.get_last_button() if b == self.virtual_create.Button.MoveForward: self.filter.move(0, 0.5) # 100 mm/s = 0.1 m/s self.create.drive_direct(self.base_speed, self.base_speed) self.sleep(abs(0.5 / 0.1)) # stop self.create.drive_direct(0, 0) elif b == self.virtual_create.Button.TurnLeft: turn_angle = math.pi / 2 + self.odometry.theta turn_angle %= 2 * math.pi self.filter.move(turn_angle, 0) # turn left by 90 degree self.go_to_angle(turn_angle) elif b == self.virtual_create.Button.TurnRight: turn_angle = -math.pi / 2 + self.odometry.theta turn_angle %= 2 * math.pi self.filter.move(turn_angle, 0) # turn right by 90 degree self.go_to_angle(turn_angle) elif b == self.virtual_create.Button.Sense: self.filter.sense(self.sonar.get_distance()) self.sleep(0.01) def go_to_angle(self, goal_theta): while abs( math.atan2(math.sin(goal_theta - self.odometry.theta), math.cos(goal_theta - self.odometry.theta))) > 0.01: print("Go TO: " + str(math.degrees(goal_theta)) + " " + str(math.degrees(self.odometry.theta))) output_theta = self.pidTheta.update(self.odometry.theta, goal_theta, self.time.time()) self.create.drive_direct(int(+output_theta), int(-output_theta)) self.sleep(0.01) self.create.drive_direct(0, 0) def sleep(self, time_in_sec): """Sleeps for the specified amount of time while keeping odometry up-to-date Args: time_in_sec (float): time to sleep in seconds """ start = self.time.time() while True: state = self.create.update() if state is not None: self.odometry.update(state.leftEncoderCounts, state.rightEncoderCounts) # print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta))) t = self.time.time() if start + time_in_sec <= t: break def draw_particles(self): data = [] # get position data from all particles for particle in self.filter.particles: data.extend([particle.x, particle.y, 0.1, particle.theta]) # paint all particles in simulation self.virtual_create.set_point_cloud(data)