def __init__(self):
self.prev_easting = 0.0
self.prev_northing = 0.0
self.yaw = 0.0
self.q = np.empty((4, ), dtype=np.float64)
# Get parameters
plot_pose_pos = rospy.get_param("~plot_pose_track", False)
plot_gnss = rospy.get_param("~plot_gnss_track", False)
plot_odometry = rospy.get_param("~plot_odometry_track", False)
plot_yaw = rospy.get_param("~plot_pose_yaw", False)
offset_e = rospy.get_param("~easting_offset", 0.0) # [m]
offset_n = rospy.get_param("~northing_offset", 0.0) # [m]
self.trkpt_threshold = rospy.get_param("~trackpoint_threshold",
0.1) # [m]
map_update_frequency = rospy.get_param("~map_update_frequency", 1.0)
map_title = rospy.get_param("~map_title", "Track")
map_window_size = rospy.get_param("~map_window_size", 5.0) # [inches]
rospy.loginfo(rospy.get_name() + " Coordinate offset: E%.3f N%.3f" %
(offset_e, offset_n))
rospy.loginfo(rospy.get_name() + " Trackpoint threshold: %.3f m" %
(self.trkpt_threshold))
# setup map plot
self.plot = track_map(plot_pose_pos, plot_gnss, plot_odometry,
plot_yaw, map_title, map_window_size, offset_e,
offset_n)
self.plot.set_trackpoint_threshold(self.trkpt_threshold)
# Get topic names
pose_topic = rospy.get_param("~pose_sub", '/fmKnowledge/pose')
gnss_topic = rospy.get_param("~gnss_sub",
'/fmInformation/gpgga_tranmerc')
odom_topic = rospy.get_param("~odom_sub", '/fmKnowledge/encoder_odom')
wptnav_status_topic = rospy.get_param("~wptnav_status_sub",
'/fmData/wptnav_status')
# Setup subscription topic callbacks
rospy.Subscriber(pose_topic, Odometry, self.on_pose_topic)
rospy.Subscriber(gnss_topic, gpgga_tranmerc, self.on_gnss_topic)
rospy.Subscriber(odom_topic, Odometry, self.on_odom_topic)
rospy.Subscriber(wptnav_status_topic, waypoint_navigation_status,
self.on_wptnav_status_topic)
# Call updater function
self.r = rospy.Rate(map_update_frequency) # set updater frequency
self.updater()
def __init__(self):
self.prev_easting = 0.0
self.prev_northing = 0.0
self.yaw = 0.0
self.q = np.empty((4, ), dtype=np.float64)
# Get parameters
plot_pose_pos = rospy.get_param("~plot_pose_track",False)
plot_gnss = rospy.get_param("~plot_gnss_track",False)
plot_odometry = rospy.get_param("~plot_odometry_track",False)
plot_yaw = rospy.get_param("~plot_pose_yaw",False)
offset_e = rospy.get_param("~easting_offset",0.0) # [m]
offset_n = rospy.get_param("~northing_offset",0.0) # [m]
self.trkpt_threshold = rospy.get_param("~trackpoint_threshold",0.1) # [m]
map_update_frequency = rospy.get_param("~map_update_frequency", 1.0)
map_title = rospy.get_param("~map_title", "Track")
map_window_size = rospy.get_param("~map_window_size",5.0) # [inches]
rospy.loginfo(rospy.get_name() + " Coordinate offset: E%.3f N%.3f" % (offset_e, offset_n))
rospy.loginfo(rospy.get_name() + " Trackpoint threshold: %.3f m" % (self.trkpt_threshold))
# setup map plot
self.plot = track_map(plot_pose_pos, plot_gnss, plot_odometry, plot_yaw, map_title, map_window_size, offset_e, offset_n)
self.plot.set_trackpoint_threshold (self.trkpt_threshold)
# Get topic names
pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
gnss_topic = rospy.get_param("~gnss_sub",'/fmInformation/gpgga_tranmerc')
odom_topic = rospy.get_param("~odom_sub",'/fmKnowledge/encoder_odom')
wptnav_status_topic = rospy.get_param("~wptnav_status_sub",'/fmData/wptnav_status')
# Setup subscription topic callbacks
rospy.Subscriber(pose_topic, Odometry, self.on_pose_topic)
rospy.Subscriber(gnss_topic, gpgga_tranmerc, self.on_gnss_topic)
rospy.Subscriber(odom_topic, Odometry, self.on_odom_topic)
rospy.Subscriber(wptnav_status_topic, waypoint_navigation_status, self.on_wptnav_status_topic)
# Call updater function
self.r = rospy.Rate(map_update_frequency) # set updater frequency
self.updater()
ctrl_c = 0
def signal_handler(signal, frame):
global ctrl_c
ctrl_c = 1
print 'Ctrl-C pressed'
print 'Quit'
signal.signal(signal.SIGINT, signal_handler)
# setup plotting
if plot_relative_coordinates == False: # define absolute or relative plotting coordinates
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
global ctrl_c
ctrl_c = 1
print 'Ctrl-C pressed'
print 'Quit'
signal.signal(signal.SIGINT, signal_handler)
# setup plotting
if plot_relative_coordinates == False: # define absolute or relative plotting coordinates
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, \
"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
global ctrl_c
ctrl_c = 1
print 'Ctrl-C pressed'
print 'Quit'
signal.signal(signal.SIGINT, signal_handler)
# setup plotting
if plot_relative_coordinates == False: # define absolute or relative plotting coordinates
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
ctrl_c = 0
def signal_handler(signal, frame):
global ctrl_c
ctrl_c = 1
print 'Ctrl-C pressed'
print 'Quit'
signal.signal(signal.SIGINT, signal_handler)
# setup plotting
if plot_relative_coordinates == False: # define absolute or relative plotting coordinates
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, \
"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