Python sensor_msgs.msg.NavSatFix() Examples

The following are 10 code examples of sensor_msgs.msg.NavSatFix(). You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example. You may also want to check out all available functions/classes of the module sensor_msgs.msg , or try the search function .
Example #1
Source File: ros_agent.py    From scenario_runner with MIT License 5 votes vote down vote up
def publish_gnss(self, sensor_id, data):
        """
        Function to publish gnss data
        """
        msg = NavSatFix()
        msg.header = self.get_header()
        msg.header.frame_id = 'gps'
        msg.latitude = data[0]
        msg.longitude = data[1]
        msg.altitude = data[2]
        msg.status.status = NavSatStatus.STATUS_SBAS_FIX
        # pylint: disable=line-too-long
        msg.status.service = NavSatStatus.SERVICE_GPS | NavSatStatus.SERVICE_GLONASS | NavSatStatus.SERVICE_COMPASS | NavSatStatus.SERVICE_GALILEO
        # pylint: enable=line-too-long
        self.publisher_map[sensor_id].publish(msg) 
Example #2
Source File: control.py    From uctf with Apache License 2.0 5 votes vote down vote up
def _subscribe(self):
        assert self.state_subscriber is None
        assert self.position_subscriber is None
        self.state_subscriber = rospy.Subscriber(
            '%s/mavros/state' % self.namespace, State,
            callback=self._state_callback)
        self.position_subscriber = rospy.Subscriber(
            '%s/mavros/global_position/global' % self.namespace, NavSatFix,
            callback=self._position_callback) 
Example #3
Source File: kitti2bag.py    From kitti2bag with MIT License 5 votes vote down vote up
def save_gps_fix_data(bag, kitti, gps_frame_id, topic):
    for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
        navsatfix_msg = NavSatFix()
        navsatfix_msg.header.frame_id = gps_frame_id
        navsatfix_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
        navsatfix_msg.latitude = oxts.packet.lat
        navsatfix_msg.longitude = oxts.packet.lon
        navsatfix_msg.altitude = oxts.packet.alt
        navsatfix_msg.status.service = 1
        bag.write(topic, navsatfix_msg, t=navsatfix_msg.header.stamp) 
Example #4
Source File: kitti2bag.py    From SARosPerceptionKitti with MIT License 5 votes vote down vote up
def save_gps_fix_data(bag, kitti, gps_frame_id, topic):
    for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
        navsatfix_msg = NavSatFix()
        navsatfix_msg.header.frame_id = gps_frame_id
        navsatfix_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
        navsatfix_msg.latitude = oxts.packet.lat
        navsatfix_msg.longitude = oxts.packet.lon
        navsatfix_msg.altitude = oxts.packet.alt
        navsatfix_msg.status.service = 1
        bag.write(topic, navsatfix_msg, t=navsatfix_msg.header.stamp) 
Example #5
Source File: gnss.py    From ros-bridge with MIT License 5 votes vote down vote up
def sensor_data_updated(self, carla_gnss_measurement):
        """
        Function to transform a received gnss event into a ROS NavSatFix message

        :param carla_gnss_measurement: carla gnss measurement object
        :type carla_gnss_measurement: carla.GnssMeasurement
        """
        navsatfix_msg = NavSatFix()
        navsatfix_msg.header = self.get_msg_header(timestamp=carla_gnss_measurement.timestamp)
        navsatfix_msg.latitude = carla_gnss_measurement.latitude
        navsatfix_msg.longitude = carla_gnss_measurement.longitude
        navsatfix_msg.altitude = carla_gnss_measurement.altitude
        self.publish_message(
            self.get_topic_prefix() + "/fix", navsatfix_msg) 
Example #6
Source File: ros_bridge_client.py    From ros-bridge with MIT License 5 votes vote down vote up
def test_gnss(self):
        """
        Tests Gnss
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/gnss/gnss1/fix", NavSatFix, timeout=TIMEOUT)
        self.assertEqual(msg.header.frame_id, "ego_vehicle/gnss/gnss1")
        self.assertNotEqual(msg.latitude, 0.0)
        self.assertNotEqual(msg.longitude, 0.0)
        self.assertNotEqual(msg.altitude, 0.0) 
Example #7
Source File: odom_to_lla.py    From lawn_tractor with MIT License 5 votes vote down vote up
def callback(data):
    #  Set Map Origin
    #  30.174885, -96.512199
    olat = 30.174885
    olon = -96.512199

    # Odom to lat/lon
    xg2, yg2 = gc.xy2ll(data.pose.pose.position.x,
                        data.pose.pose.position.y, olat, olon)

    # build navsat message
    fake_gps = NavSatFix()
    fake_gps.header.frame_id = rospy.get_param('~frame_id', 'gps')
    fake_gps.header.stamp = rospy.Time.now()
    fake_gps.status.status = 1
    fake_gps.status.service = 1
    fake_gps.latitude = xg2
    fake_gps.longitude = yg2
    fake_gps.altitude = 0
    #compass = Float(90.0)

    pub = rospy.Publisher('fake_gps', NavSatFix, queue_size=10)
    pub.publish(fake_gps)

    # Odom to UTM
    utmy, utmx, utmzone = gc.LLtoUTM(xg2, yg2)

    # build navsat message
    fake_UTM = NavSatFix()
    fake_UTM.header.frame_id = rospy.get_param('~frame_id', 'utm')
    fake_UTM.header.stamp = rospy.Time.now()
    fake_UTM.status.status = 1
    fake_UTM.status.service = 1
    fake_UTM.latitude = utmy
    fake_UTM.longitude = utmx
    fake_UTM.altitude = 0
    #compass = Float(90.0)

    pub2 = rospy.Publisher('fake_utm', NavSatFix, queue_size=10)
    pub2.publish(fake_UTM) 
Example #8
Source File: collect_gps_points.py    From lawn_tractor with MIT License 5 votes vote down vote up
def collect_gps():
    global gps_file

    rospy.init_node('collect_gps_points', anonymous=True)
    rospy.Subscriber("/fake_gps", NavSatFix, callback)
    rospy.Subscriber("/status/gps_waypoint_collection",Bool, collection_status_CB )

    try:
        gps_file = open(location_collect, "a")
        rospy.loginfo(gps_file)
    except IOError:
        print "Could not open gps_points_file.txt file."
    
    rospy.spin() 
Example #9
Source File: state_publisher.py    From genesis_path_follower with MIT License 4 votes vote down vote up
def pub_loop():
	rospy.init_node('state_publisher', anonymous=True)	
	rospy.Subscriber('/gps/fix', NavSatFix, parse_gps_fix, queue_size=1)
	rospy.Subscriber('/gps/vel', TwistWithCovarianceStamped, parse_gps_vel, queue_size=1)
	rospy.Subscriber('/imu/data', Imu, parse_imu_data, queue_size=1)
	rospy.Subscriber('/vehicle/steering', SteeringReport, parse_steering_angle, queue_size=1)

	if not (rospy.has_param('lat0') and rospy.has_param('lon0') and rospy.has_param('yaw0')):
		raise ValueError('Invalid rosparam global origin provided!')

	if not rospy.has_param('time_check_on'):
		raise ValueError('Did not specify if time validity should be checked!')

	LAT0 = rospy.get_param('lat0')
	LON0 = rospy.get_param('lon0')
	YAW0 = rospy.get_param('yaw0')
	time_check_on = rospy.get_param('time_check_on')
	
	state_pub = rospy.Publisher('state_est', state_est, queue_size=1)

	r = rospy.Rate(100)
	while not rospy.is_shutdown():		
		
		if None in (lat, lon, psi, vel, acc_filt, df): 
			r.sleep() # If the vehicle state info has not been received.
			continue

		curr_state = state_est()
		curr_state.header.stamp = rospy.Time.now()
		
		# TODO: time validity check, only publish if data is fresh
		#if time_check_on and not time_valid(curr_state.header.stamp,[tm_vel, tm_df, tm_imu, tm_gps]):
		#	r.sleep()
		#	continue

		curr_state.lat = lat
		curr_state.lon = lon

		X,Y = latlon_to_XY(LAT0, LON0, lat, lon)

		curr_state.x   = X
		curr_state.y   = Y
		curr_state.psi = psi
		curr_state.v   = vel
		
		curr_state.a   = acc_filt
		curr_state.df  = df

		state_pub.publish(curr_state)
		r.sleep() 
Example #10
Source File: carla_manual_control.py    From ros-bridge with MIT License 4 votes vote down vote up
def __init__(self, role_name, width, height):
        self.role_name = role_name
        self.dim = (width, height)
        font = pygame.font.Font(pygame.font.get_default_font(), 20)
        fonts = [x for x in pygame.font.get_fonts() if 'mono' in x]
        default_font = 'ubuntumono'
        mono = default_font if default_font in fonts else fonts[0]
        mono = pygame.font.match_font(mono)
        self._font_mono = pygame.font.Font(mono, 14)
        self._notifications = FadingText(font, (width, 40), (0, height - 40))
        self.help = HelpText(pygame.font.Font(mono, 24), width, height)
        self._show_info = True
        self._info_text = []
        self.vehicle_status = CarlaEgoVehicleStatus()
        self.tf_listener = tf.TransformListener()
        self.vehicle_status_subscriber = rospy.Subscriber(
            "/carla/{}/vehicle_status".format(self.role_name),
            CarlaEgoVehicleStatus, self.vehicle_status_updated)
        self.vehicle_info = CarlaEgoVehicleInfo()
        self.vehicle_info_subscriber = rospy.Subscriber(
            "/carla/{}/vehicle_info".format(self.role_name),
            CarlaEgoVehicleInfo, self.vehicle_info_updated)
        self.latitude = 0
        self.longitude = 0
        self.manual_control = False
        self.gnss_subscriber = rospy.Subscriber(
            "/carla/{}/gnss/gnss1/fix".format(self.role_name), NavSatFix, self.gnss_updated)
        self.manual_control_subscriber = rospy.Subscriber(
            "/carla/{}/vehicle_control_manual_override".format(self.role_name),
            Bool, self.manual_control_override_updated)

        self.carla_status = CarlaStatus()
        self.status_subscriber = rospy.Subscriber(
            "/carla/status", CarlaStatus, self.carla_status_updated)