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)
