Python msg.NavSatFix方法代码示例

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import NavSatFix [as 别名]
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) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import NavSatFix [as 别名]
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) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import NavSatFix [as 别名]
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) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import NavSatFix [as 别名]
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) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import NavSatFix [as 别名]
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) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import NavSatFix [as 别名]
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) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import NavSatFix [as 别名]
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) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import NavSatFix [as 别名]
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() 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import NavSatFix [as 别名]
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()