def __init__(self, file_name, directory, map_name, map_dir, FilterClass, **plot_info): # project.set_project_dir("rccar") self.checkpoints = get_map("checkpoints.txt") self.waypoints = Waypoints( map_name, map_dir=map_dir ) initial_long, initial_lat = self.checkpoints[0] second_long, second_lat = self.checkpoints[1] initial_heading = FilterClass.get_gps_bearing( second_long, second_lat, initial_long, initial_lat ) filter = FilterClass( standard_params['counts_per_rotation'], standard_params['wheel_radius'], standard_params['front_back_dist'], standard_params['max_speed'], standard_params['left_angle_limit'], standard_params['right_angle_limit'], standard_params['left_servo_limit'], standard_params['right_servo_limit'], initial_long, initial_lat, initial_heading ) super(FilterTest, self).__init__( file_name, directory, filter, plot_info )
class FilterTest(Simulator): def __init__(self, file_name, directory, map_name, map_dir, FilterClass, **plot_info): # project.set_project_dir("rccar") self.checkpoints = get_map("checkpoints.txt") self.waypoints = Waypoints( map_name, map_dir=map_dir ) initial_long, initial_lat = self.checkpoints[0] second_long, second_lat = self.checkpoints[1] initial_heading = FilterClass.get_gps_bearing( second_long, second_lat, initial_long, initial_lat ) filter = FilterClass( standard_params['counts_per_rotation'], standard_params['wheel_radius'], standard_params['front_back_dist'], standard_params['max_speed'], standard_params['left_angle_limit'], standard_params['right_angle_limit'], standard_params['left_servo_limit'], standard_params['right_servo_limit'], initial_long, initial_lat, initial_heading ) super(FilterTest, self).__init__( file_name, directory, filter, plot_info ) def fill_data_array(self, plot_option, data_array): if plot_option == "map_plot": gps_map = np.array(self.waypoints.map) data_array[0] = gps_map[:, 0] data_array[1] = gps_map[:, 1] elif plot_option == "checkpoints_plot": checkpoints_map = np.array(self.checkpoints) data_array[0] = checkpoints_map[:, 0] data_array[1] = checkpoints_map[:, 1] def step(self, index, timestamp, name, values): if name == "imu": self.record_imu(timestamp, values) elif name == "gps": self.record_gps(timestamp, values) elif name == "encoder": self.record_enc(timestamp, values) elif name == "motors": self.record_motors(values) elif name == "servo": self.record_servo(values) elif name == "state": self.record_state(values, "recorded_state_plot", "recorded_heading_lines_plot") elif name == "checkpoint": self.record_checkpoint(values) def record_checkpoint(self, values): check_long, check_lat = self.checkpoints[values["num"]] self.xy_line_segment( "checkpoint_lines_plot", self.filter.state["x"], self.filter.state["y"], check_long, check_lat) def record_waypoint(self, state): goal_x, goal_y = self.waypoints.get_goal(state) self.xy_line_segment( "waypoints_plot", state["x"], state["y"], goal_x, goal_y) def record_state(self, state, state_plot, heading_lines_plot): if self.plot_enabled[state_plot]: self.plot_data[state_plot][0].append(state["x"]) self.plot_data[state_plot][1].append(state["y"]) self.angled_line_segment( heading_lines_plot, state["x"], state["y"], state["angle"], 0.0001, state["vx"], state["vy"], 0.75) def record_imu(self, timestamp, values): state = self.filter.update_imu(timestamp, values["yaw"]) if self.plot_enabled["calculated_state_plot"]: self.record_state(state, "calculated_state_plot", "heading_lines_plot") self.record_waypoint(state) def record_gps(self, timestamp, values): state = self.filter.update_gps(timestamp, values["long"], values["lat"]) if self.plot_enabled["calculated_state_plot"]: self.record_state(state, "calculated_state_plot", "heading_lines_plot") self.record_waypoint(state) if self.plot_enabled["gps_plot"]: self.plot_data["gps_plot"][0].append(values["long"]) self.plot_data["gps_plot"][1].append(values["lat"]) def record_enc(self, timestamp, values): state = self.filter.update_encoder(timestamp, values["counts"]) if self.plot_enabled["calculated_state_plot"]: self.record_state(state, "calculated_state_plot", "heading_lines_plot") self.record_waypoint(state) if self.plot_enabled["encoder_position_plot"]: x, y = self.filter.xy_meters_to_gps( self.filter.enc_x, self.filter.enc_y) self.plot_data["encoder_position_plot"][0].append(math.degrees(x)) self.plot_data["encoder_position_plot"][1].append(math.degrees(y)) def record_motors(self, values): if self.plot_enabled["plot_calculated_state"]: self.filter.update_motors(values) def record_servo(self, values): if self.plot_enabled["plot_calculated_state"]: self.filter.update_servo(values)