Beispiel #1
0
    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()
Beispiel #3
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, \
	"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
Beispiel #4
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
Beispiel #5
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, \
 "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
Beispiel #6
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