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