Python NavSatFix.altitude方法代码示例

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
    def parse_gps(self, data):
        '''
        Given raw data from Jaguar GPS socket, parse and return NavSatFix message.
        If bad/incomplete data, return None
        '''
        # use regex to parse
        gpgga_hit = re.search('^\$(GPGGA.+?)\r\n', data)
        gprmc_hit = re.search('^\$(GPRMC.+?)\r\n', data)

        if gprmc_hit:
            # Get estimated heading (not part of standard ROS navsatfix message)
            gprmc = gprmc_hit.group(0).split(",")
            try:
                heading = float(gprmc[8])
            except:
                heading = float("NaN")
            else:
                while heading < -180.0:
                    heading += 360.0
                while heading > 180.0:
                    heading -= 360.0
            finally:
                self.current_est_heading = heading
        
        if gpgga_hit:
            gpgga = gpgga_hit.group(0).split(",")
            
            nav_msg = NavSatFix()
            # Set header information 
            time = gpgga[1]
            hrs = float(time[0:1])
            mins = float(time[2:3])
            secs = float(time[4:5])
            nav_msg.header.stamp = rospy.Time.from_sec(hrs * 3600 + mins * 60 + secs)
            nav_msg.header.frame_id = "gps"
            # Set GPS Status
            status = NavSatStatus()
            status.status = -1 if int(gpgga[6]) == 0 else 0
            nav_msg.status = status
            # Set longitude and latitude
            lat_str = gpgga[2]
            lon_str = gpgga[4]
            lat_degs = float(lat_str[:2]) + (float(lat_str[2:]) / 60.0)
            lon_degs = float(lon_str[:3]) + (float(lon_str[3:]) / 60.0)
            nav_msg.latitude = -1 * lat_degs if gpgga[3] == "S" else lat_degs
            nav_msg.longitude = -1 * lon_degs if gpgga[5] == "W" else lon_degs
            # Set altitude (Positive is above the WGS 84 ellipsoid)
            try:
                nav_msg.altitude = float(gpgga[9])
            except:
                nav_msg.altitude = float("NaN")
            # Set covariance type to unknown
            nav_msg.position_covariance_type = 0

            return nav_msg
        else:
            return None

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
def callback(sensorstr):
	data = sensorstr.data
	if data[0] == 'Z':
		data = data.replace("Z","").replace("\n","").replace("\r","")
		data_list = data.split(',')
		if len(data_list) == 4:
			try:
				##data_list structure: lat, lon, heading, vel 
				float_list = [float(i) for i in data_list]
				poseGPS = NavSatFix()
				poseGPS.header.frame_id = "world"
				poseGPS.header.stamp = rospy.Time.now()
				poseGPS.status.status = 0
				poseGPS.status.service = 1
				poseGPS.latitude = float_list[0]
				poseGPS.longitude = float_list[1]
				poseGPS.altitude = 0.0
				poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0]
				poseGPS.position_covariance_type = 2
				try: 
					pubGPS.publish(poseGPS)
					log = "GPS Pose Data: %f %f Publish at Time: %s" % (float_list[0], float_list[1], rospy.get_time())
				except: 
					log = "poseGPS Publisher Error"

			except: 
				log = "GPS Data Error! Data :  %s" % data
				rospy.loginfo(log)
		rospy.loginfo(log)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
def got_position(xx, yy, aa, stamp):
    global EP

    fix = NavSatFix()
    fix.header.stamp = stamp
    fix.header.frame_id = "sim_gps"
    fix.status.status = 0
    fix.status.service = 1

    utm_e = base_xx + base_utm_e + xx + CONFIG["SIM_ERROR"] * cos((yy + time()) / EP)
    utm_n = base_yy + base_utm_n + yy + CONFIG["SIM_ERROR"] * cos((utm_e / 3.14) / EP)
    (lon, lat) = conv(utm_e, utm_n, inverse=True)

    fix.latitude = lat
    fix.longitude = lon
    fix.altitude = 1.3

    # print "got_position: %f %f" % (lat, lon)

    gps.publish(fix)
    pos.publish(Pose2D(utm_e, utm_n, aa))
    tfBr.sendTransform(
        (utm_e - base_xx, utm_n - base_yy, 0),
        tf.transformations.quaternion_from_euler(0, 0, 0),
        rospy.Time.now(),
        "/map",
        "/gps",
    )

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
    def bestpos_handler(self, bestpos):
        navsat = NavSatFix()

        # TODO: The timestamp here should come from SPAN, not the ROS system time.
        navsat.header.stamp = rospy.Time.now()
        navsat.header.frame_id = self.odom_frame

        # Assume GPS - this isn't exposed
        navsat.status.service = NavSatStatus.SERVICE_GPS

        position_type_to_status = {
            BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX,
            BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX,
            BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX,
            BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
            BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX,
        }
        navsat.status.status = position_type_to_status.get(bestpos.position_type, NavSatStatus.STATUS_NO_FIX)

        # Position in degrees.
        navsat.latitude = bestpos.latitude
        navsat.longitude = bestpos.longitude

        # Altitude in metres.
        navsat.altitude = bestpos.altitude
        navsat.position_covariance[0] = pow(2, bestpos.latitude_std)
        navsat.position_covariance[4] = pow(2, bestpos.longitude_std)
        navsat.position_covariance[8] = pow(2, bestpos.altitude_std)
        navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN

        # Ship ito
        self.pub_navsatfix.publish(navsat)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
	def callback(self):
		
		#Publish data at 1 Hz
		rate = rospy.Rate(1)
		
		#Define the RTKRCV server ports corresponding to each of the processing
		#Modes
		port = 5801
		
		sock = socket.socket()
		host = socket.gethostname()
		sock.connect((host, port))
		
		#WGS84 Parameters
		e2 = 6.69437999014e-3
		a = 6378137.0
		
		#At a rate of 1 hz, create an rtklib message for each socket and publish
		#the data over 
		while not rospy.is_shutdown():
		
			#Create and RTKLIB and a NavSatFix data structure
			rtk = rtklib()
			navsat = NavSatFix()
					
			#Set the data structure header time to the current system time
			navsat.header.stamp = rospy.Time.now()
			rtk.header.stamp = rospy.Time.now()	
					
			#Get the position message from the RTKRCV server
			msgStr = socket.recv(1024)
				
			#Split the message
			msg = msgStr.split()
					
			#Save the latitude, longitude and ellipsoid height
			navsat.latitude = float(msg[2])
			navsat.longitude = float(msg[3])
			navsat.altitude = float(msg[4])
					
			#Save the position covariance
			navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])]
			navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
					
			#Compute the radius of curvature in the 
			N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2))
					
			#Compute and store the ECEF position
			rtk.x =  (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0)
			rtk.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0)
			rtk.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0)
			rtk.delay = float(msg[13])
			rtk.ftest = float(msg[14])
					
			#Store the NavSatFix data
			rtk.state = navsat
				
			pubs[i].publish(rtk)
				
			rate.sleep()

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
def talker():
	
	print 'in talker'
	pub = rospy.Publisher('GPS', NavSatFix)
	rospy.init_node('GPStalker')
	while not rospy.is_shutdown():
		#Assuming that parse will return these values
		time.sleep(1)
		parse()
		msg = NavSatFix()
		Fix = NavSatStatus()
		Fix.status = GPS.mode
		Fix.service = GPS.numSat
		
		msg.header.stamp = rospy.Time.now()
		msg.status = Fix
		msg.latitude = GPS.lat
		msg.longitude = GPS.lon
		msg.altitude = GPS.alt
		msg.position_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
		msg.position_covariance_type = 0
		#covariance_type = 0 unknown
		#                = 1 approximated
		#                = 2 diagonal known
		#                = 3 known
		pub.publish(msg)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
 def publish_gps(self):
     if self.drone.positionGPS!=None:
        gps_data = NavSatFix()
        gps_data.latitude = self.drone.positionGPS[0]
        gps_data.longitude = self.drone.positionGPS[1]#maybe reversed?
        gps_data.altitude = self.drone.positionGPS[2]
        gps_data.header.frame_id = self.unique_id
        self.gps_pub.publish(gps_data)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
def gps():
	print utm.conversion.R
	import roslib; roslib.load_manifest('ardrone_control')
	import rospy;  global rospy 
	
	

	from sensor_msgs.msg import NavSatFix
	
	
	# rospy.init_node('test')

	gps = GPS() 
	#print gps 

	m = NavSatFix()
	m.latitude = 45.0
	m.altitude = utm.conversion.R
	m.longitude = 20.0

	gps.set_zero(m) 

	gps.measure(m)
	print gps.get_state()
	print gps.Z 

	m.latitude = 45.1
	m.altitude = utm.conversion.R
	m.longitude = 20.0

	gps.measure(m)
	print gps.get_state()
	print gps.Z 

	gps.set_zero_yaw( 30 * pi/180 )
	gps.measure(m)
	print gps.get_state()
	print gps.get_measurement()
	print gps.Z 

	for i in range(10000):
		gps.measure(m)
		gps.Broadcast()

	rospy.spin()

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
    def default(self, ci='unused'):
        gps = NavSatFix()
        gps.header = self.get_ros_header()

        gps.latitude = self.data['x']
        gps.longitude = self.data['y']
        gps.altitude = self.data['z']

        self.publish(gps)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
def getMsg():
	
	pub_navsat = rospy.Publisher('/rtklib/rover', NavSatFix)
	pub_pose = rospy.Publisher('/rtklib/pose', PoseStamped)
	
	#Initialize the RTKLIB ROS node	
	rospy.init_node('rtklib_messages', anonymous=True)
	
	#Define the publishing frequency of the node
	rate = rospy.Rate(10)
	
	#Create a socket
	sock = socket.socket()
	
	#Get the address of the local host
	host = socket.gethostname()
	
	#Connect to the RTKRCV server that is bound to port xxxx
	port = 5801
	sock.connect((host,port))
	
	e2 = 6.69437999014e-3
	a = 6378137.0
	
	while not rospy.is_shutdown():
		
		navsat = NavSatFix()
		ecef_xyz = Point()
		ecef_pose = Pose()
		ecef_stampedPose = PoseStamped()
		
		ecef_stampedPose = 
		navsat.header.stamp = rospy.Time.now()
		
		#Get the position message from the RTKRCV server
		msgStr = sock.recv(1024)
		
		#Split the message
		msg = msgStr.split()
		
		navsat.latitude = float(msg[2])
		navsat.longitude = float(msg[3])
		navsat.altitude = float(msg[4])
		
		N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2))
		
		ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0)
		ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0)
		ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0)
		
		ecef_pose.position = ecef_xyz
		ecef_stampedPose.pose = ecef_pose
		
		pub_navsat.publish(navsat)
		pub_pose.publish(ecef_stampedPose)
		
		rate.sleep()

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
    def callback_sbp_pos(self, msg, **metadata):
        if self.debug:
            rospy.loginfo("Received SBP_MSG_POS_LLH (Sender: %d): %s" % (msg.sender, repr(msg)))

        out = NavSatFix()
        out.header.frame_id = self.frame_id
        out.header.stamp = rospy.Time.now()

        out.status.service = NavSatStatus.SERVICE_GPS

        out.latitude = msg.lat
        out.longitude = msg.lon
        out.altitude = msg.height

        out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED

        if msg.flags & 0x03:
            self.last_rtk_time = time.time()
            self.rtk_fix_mode = msg.flags & 0x03

            out.status.status = NavSatStatus.STATUS_GBAS_FIX

            # TODO this should probably also include covariance of base fix?
            out.position_covariance[0] = self.rtk_h_accuracy**2
            out.position_covariance[4] = self.rtk_h_accuracy**2
            out.position_covariance[8] = self.rtk_v_accuracy**2

            pub = self.pub_rtk_fix
            self.last_rtk_pos = msg

            # If we are getting this message, RTK is our best fix, so publish this as our best fix.
            self.pub_fix.publish(out)
        else:

            self.last_spp_time = time.time()

            out.status.status = NavSatStatus.STATUS_FIX

            # TODO hack, piksi should provide these numbers
            if self.last_dops:
                out.position_covariance[0] = (self.last_dops.hdop * 1E-2)**2
                out.position_covariance[4] = (self.last_dops.hdop * 1E-2)**2
                out.position_covariance[8] = (self.last_dops.vdop * 1E-2)**2
            else:
                out.position_covariance[0] = COV_NOT_MEASURED
                out.position_covariance[4] = COV_NOT_MEASURED
                out.position_covariance[8] = COV_NOT_MEASURED

            pub = self.pub_spp_fix
            self.last_pos = msg

            # Check if SPP is currently our best available fix
            if self.rtk_fix_mode <= 0:
                self.pub_fix.publish(out)

        pub.publish(out)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
	def recieve_gps(self, data ):
		msg = NavSatFix()
		msg.latitude = data.vector.x 
		msg.longitude = data.vector.y 
		msg.altitude = data.vector.z

		msg.header.stamp = data.header.stamp
		msg.header.frame_id = data.header.frame_id

		self.publisher['gps'].publish(msg)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
    def default(self, ci='unused'):
        """ Publish the data of the gps sensor as a custom ROS NavSatFix message """
        gps = NavSatFix()
        gps.header = self.get_ros_header()

        # TODO ros.org/doc/api/sensor_msgs/html/msg/NavSatFix.html
        gps.latitude = self.data['x']
        gps.longitude = self.data['y']
        gps.altitude = self.data['z']

        self.publish(pose)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
def ExtFix2Fix(data):
    fix = NavSatFix()
    fix.header = data.header
    fix.status.status = data.status.status
    fix.status.service = data.status.position_source
    fix.latitude = data.latitude
    fix.longitude = data.longitude
    fix.altitude = data.altitude
    fix.position_covariance = data.position_covariance
    fix.position_covariance_type = data.position_covariance_type
    return fix

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import altitude [as 别名]
	def callback(self):
		
		rate = rospy.Rate(10)
		ports = [5801,5802,5803]
		sockets = []
		pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub]
		
		for i in ports:
			
			sock = socket.socket()
			host = socket.gethostname()
			sock.connect((host, i))
			sockets.append(sock)
			
			e2 = 6.69437999014e-3
			a = 6378137.0
			
		while not rospy.is_shutdown():
			
			for i in range(len(sockets)):
				
				
				navsat = NavSatFix()
				ecef_xyz = Point()
				ecef_pose = Pose()
				#ecef_stampedPose = PoseStamped()
				
				navsat.header.stamp = rospy.Time.now()
					
				#Get the position message from the RTKRCV server
				msgStr = sockets[i].recv(1024)
			
				#Split the message
				msg = msgStr.split()
				
				navsat.latitude = float(msg[2])
				navsat.longitude = float(msg[3])
				navsat.altitude = float(msg[4])
				navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])]
				navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
				
				N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2))
				
				ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0)
				ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0)
				ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0)
				
				ecef_pose.position = ecef_xyz
				#ecef_stampedPose.pose = ecef_pose
				
				pubs[i].publish(navsat)
				
			rate.sleep()