plot_northing_offset = 0.0 else: plot_easting_offset = -ekf_easting_init plot_northing_offset = -ekf_northing_init plot = track_map(plot_pose, plot_gnss, plot_odometry, plot_yaw, \ "Simulation", 8.0, plot_easting_offset, plot_northing_offset) latest_pose_yaw = 0.0 latest_absolute_yaw = 0.0 latest_ahrs_yaw = 0.0 latest_odo_yaw = 0.0 gnss_fix_msg_count = 0 # import simulation data odo_sim = odometry_data(odo_file, odo_skip_lines, odo_max_lines) imu_sim = imu_data(imu_file, imu_skip_lines, imu_max_lines) gnss_sim = gnss_data(gnss_file, gnss_skip_lines, gnss_max_lines) # define simulation time based on odometry data sim_offset = odo_sim.data[0][0] sim_len = odo_sim.data[-1][0] - sim_offset sim_steps = sim_len / sim_step_interval sim_time = 0 print('Simulation') print(' Step interval: %.2fs' % sim_step_interval) print(' Total: %.2fs (%.0f steps)' % (sim_len, sim_steps)) # initialize estimator (preprocessing) robot_max_speed = 3.0 # [m/s] pp = odometry_gnss_pose_preprocessor(robot_max_speed) # initialize estimator (EKF)
plot_easting_offset = 0.0 plot_northing_offset = 0.0 else: plot_easting_offset = -ekf_easting_init plot_northing_offset = -ekf_northing_init plot = track_map(plot_pose, plot_gnss, plot_odometry, plot_yaw, \ "Robot track", 5.0, plot_easting_offset, plot_northing_offset) latest_pose_yaw = 0.0 latest_gnss_yaw = 0.0 latest_ahrs_yaw = 0.0 latest_odo_yaw = 0.0 # import simulation data odo_sim = odometry_data(odo_file, odo_max_lines) imu_sim = imu_data(imu_file, imu_max_lines) gnss_sim = gnss_data(gnss_file, gnss_max_lines) # define simulation time based on odometry data sim_offset = odo_sim.data[0][0] sim_len = odo_sim.data[-1][0] - sim_offset sim_steps = sim_len/sim_step_interval sim_time = 0 print ('Simulation') print (' Step interval: %.2fs' % sim_step_interval) print (' Total: %.2fs (%.0f steps)' % (sim_len, sim_steps)) # initialize estimator (preprocessing) robot_max_speed = 3.0 # [m/s] pp = pose_2d_preprocessor (robot_max_speed) # initialize estimator (EKF)
global ctrl_c ctrl_c = 1 print 'Ctrl-C pressed' print 'Quit' signal.signal(signal.SIGINT, signal_handler) # load plot functions plot = estimator_plot() plot.enable_odometry (True) plot.enable_gnss (True) plot.enable_pose (True) # import simulation data odo_sim = odometry_data(odo_file, odo_max_lines) odometry = [] gnss_sim = gnss_data(gnss_file, gnss_max_lines, relative_coordinates) gnss = [] # define simulation time based on gnss data sim_offset = gnss_sim.data[0][0] sim_len = gnss_sim.data[-1][0] - sim_offset sim_steps = sim_len/sim_step_interval sim_time = 0 print ('Simulation') print (' Step interval: %.2fs' % sim_step_interval) print (' Total: %.2fs (%.0f steps)' % (sim_len, sim_steps)) # initialize estimator (gnss preprocessing) gp = pose_2d_gnss_preprocessor() # initialize estimator (EKF)