Python NavSatFix.position_covariance方法代码示例

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

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance [as 别名]
def gps2navsat(filename, bag):
	with open(filename, 'r') as file:
		try:
			while True:
				data = struct.unpack('qddd', file.read(8*4))
				time = data[0]
				local = data[1:]
				lat_lon_el_theta = struct.unpack('dddd', file.read(8*4))
				gps_cov = struct.unpack('dddddddddddddddd', file.read(8*16))

				if abs(lat_lon_el_theta[0]) < 1e-1:
					continue

				navsat = NavSatFix()
				navsat.header.frame_id = 'gps'
				navsat.header.stamp = rospy.Time.from_sec(time * 1e-6)
				navsat.status.status = NavSatStatus.STATUS_FIX
				navsat.status.service = NavSatStatus.SERVICE_GPS

				navsat.latitude = lat_lon_el_theta[0]
				navsat.longitude = lat_lon_el_theta[1]
				navsat.altitude = lat_lon_el_theta[2]

				navsat.position_covariance = numpy.array(gps_cov).reshape(4, 4)[:3, :3].flatten().tolist()
				navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN

				bag.write('/gps/fix', navsat, navsat.header.stamp)

				geopoint = GeoPointStamped()
				geopoint.header = navsat.header
				geopoint.position.latitude = lat_lon_el_theta[0]
				geopoint.position.longitude = lat_lon_el_theta[1]
				geopoint.position.altitude = lat_lon_el_theta[2]

				bag.write('/gps/geopoint', geopoint, geopoint.header.stamp)

		except:
			print 'done'

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance [as 别名]
def talker():
    # Subscribe to Vicon messages
    viconTopic = rospy.get_param('topic')
    rospy.Subscriber(viconTopic, TransformStamped, callback)

    # Start a publisher for the GPS messages
    pub = rospy.Publisher('GPS/position', NavSatFix) # FIXME

    # Start the node
    rospy.init_node('talker')

    
    # Populate the NavSatFix message from the parameter server
    statusMsg = NavSatStatus()
    statusMsg.status = rospy.get_param('status', -1)
    statusMsg.service = rospy.get_param('service', 1)

    fixMsg = NavSatFix()
    fixMsg.header.stamp = rospy.Time.now()
    fixMsg.header.frame_id = "/world"

    fixMsg.status = statusMsg
    fixMsg.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
    #position could be modified with some gaussian noise added to it and then calculate the covariance matrix.
    fixMsg.position_covariance_type = rospy.get_param('position_covariance_type', 0); 

    while not rospy.is_shutdown():
        
        [fixMsg.longitude, fixMsg.latitude, fixMsg.altitude] = c.xyz2gps([parentFrameLong, parentFrameLat, parentFrameAlt], latestViconMsg.transform.translation.x, latestViconMsg.transform.translation.y, latestViconMsg.transform.translation.z, parentFrameAngle)

        statusMsg.status = rospy.get_param('status', statusMsg.status)
        statusMsg.service = rospy.get_param('service', statusMsg.service)
        # put the sigma and calculate the cov matrix here

        #rospy.loginfo([fixMsg.longitude, fixMsg.latitude, fixMsg.altitude]) 
        pub.publish(fixMsg)
        rospy.sleep(0.1)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance [as 别名]
def talker():
	pubGPS = rospy.Publisher('poseGPS', NavSatFix, queue_size=1000)
	pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000)
	pubTwist = rospy.Publisher('twistGPS', TwistWithCovarianceStamped, queue_size=1000)
	rospy.init_node('imugpspublisher', anonymous=True)
	rate = rospy.Rate(100) # 100hz
	while not rospy.is_shutdown():
		data = ser.readline()
		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
					pubGPS.publish(poseGPS)
					twistGPS = TwistWithCovarianceStamped()
					twistGPS.header = poseGPS.header
					twistGPS.twist.twist.linear.x = float_list[3]*knots*math.cos(latest_yaw)
					twistGPS.twist.twist.linear.y = float_list[3]*knots*math.sin(latest_yaw)
					twistGPS.twist.twist.linear.z = 0.0
					##angular data not used here
					twistGPS.twist.covariance = [0.01, 0.01, 0, 0, 0, 0]
					pubTwist.publish(twistGPS)
					log = "GPS Data: %f %f %f %f Publish at Time: %s" % (float_list[0], float_list[1], float_list[2], float_list[3], rospy.get_time())
				except: log = "GPS Data Error! Data :  %s" % data
			else:
				log = "GPS Data Error! Data :  %s" % data
			rospy.loginfo(log)
		elif data[0] == 'Y':
			data = data.replace("Y","").replace("\n","").replace("\r","")
			data_list = data.split(',')
			if len(data_list) == 9:
				try:
					##data_list structure: accel, magni, gyro
					float_list = [float(i) for i in data_list]
					imuData = Imu()
					imuData.header.frame_id = "base_link"
					imuData.header.stamp = rospy.Time.now()
					latest_yaw = float_list[3]
					##data form in yaw, pitch, roll
					quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx')
					imuData.orientation.x = quat[0]
					imuData.orientation.y = quat[1]
					imuData.orientation.z = quat[2]
					imuData.orientation.w = quat[3]
					imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale)
					imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale)
					imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale)
					imuData.linear_acceleration.x = float_list[0]*accel_scale
					imuData.linear_acceleration.y = -float_list[1]*accel_scale
					imuData.linear_acceleration.z = -float_list[2]*accel_scale
					imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6]
					imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7]
					imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3]
					pubIMU.publish(imuData)
					log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \
					% (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z,
						float_list[3], float_list[4], float_list[5],
						imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time())
				except: log = "IMU Data Error! Data :  %s" % data
			else: log = "Data Error! Data :  %s" % data
			rospy.loginfo(log)
		else:
			log = "Data Error or Message: %s" % data
			rospy.loginfo(log)
		rate.sleep()

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance [as 别名]
    def navigation_handler(self, data):
        """ Rebroadcasts navigation data in the following formats:
        1) /odom => /base footprint transform (if enabled, as per REP 105)
        2) Odometry message, with parent/child IDs as in #1
        3) NavSatFix message, for systems which are knowledgeable about GPS stuff
        4) IMU messages
        """
        rospy.logdebug("Navigation received")
        # If we don't have a fix, don't publish anything...
        if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
            return
        
        orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(data.pitch), RAD(data.heading)).GetQuaternion()

        # UTM conversion
        (zone, easting, northing) = LLtoUTM(23, data.latitude, data.longitude)
        # Initialize starting point if we haven't yet
        # TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid?
        if not self.init and self.zero_start:
            self.origin.x = easting
            self.origin.y = northing
            self.init = True

        # Publish origin reference for others to know about
        p = Pose()
        p.position.x = self.origin.x
        p.position.y = self.origin.y
        self.pub_origin.publish(p)

        #
        # Odometry 
        # TODO: Work out these covariances properly from DOP
        #
        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = self.odom_frame
        odom.child_frame_id = self.base_frame
        odom.pose.pose.position.x = easting - self.origin.x
        odom.pose.pose.position.y = northing - self.origin.y
        odom.pose.pose.position.z = data.altitude
        odom.pose.pose.orientation = Quaternion(*orient)
        odom.pose.covariance = POSE_COVAR
        # Twist is relative to /vehicle frame
        odom.twist.twist.linear.x = data.speed
        odom.twist.twist.linear.y = 0
        odom.twist.twist.linear.z = -data.down_vel
        odom.twist.twist.angular.x = RAD(data.ang_rate_long)
        odom.twist.twist.angular.y = RAD(-data.ang_rate_trans)
        odom.twist.twist.angular.z = RAD(-data.ang_rate_down)
        odom.twist.covariance = TWIST_COVAR

        self.pub_odom.publish(odom)

        #
        # Odometry transform (if required)
        #
        if self.publish_tf:
            self.tf_broadcast.sendTransform(
                (odom.pose.pose.position.x, odom.pose.pose.position.y,
                 odom.pose.pose.position.z), Quaternion(*orient),
                 odom.header.stamp,odom.child_frame_id, odom.frame_id)

        # 
        # NavSatFix
        # TODO: Work out these covariances properly from DOP
        #
        navsat = NavSatFix()
        navsat.header.stamp = rospy.Time.now()
        navsat.header.frame_id = self.odom_frame
        navsat.status = self.nav_status

        navsat.latitude = data.latitude
        navsat.longitude = data.longitude
        navsat.altitude = data.altitude

        navsat.position_covariance = NAVSAT_COVAR
        navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
        
        self.pub_navsatfix.publish(navsat)
        
        #
        # IMU
        # TODO: Work out these covariances properly
        #
        imu = Imu()
        imu.header.stamp == rospy.Time.now()
        imu.header.frame_id = self.base_frame
      
        # Orientation
        imu.orientation = Quaternion(*orient)
        imu.orientation_covariance = IMU_ORIENT_COVAR
 
        # Angular rates
        imu.angular_velocity.x = RAD(data.ang_rate_long)
        imu.angular_velocity.y = RAD(-data.ang_rate_trans)
        imu.angular_velocity.y = RAD(-data.ang_rate_down)
        imu.angular_velocity_covariance = IMU_VEL_COVAR

        # Linear acceleration
        imu.linear_acceleration.x = data.long_accel
#.........这里部分代码省略.........

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance [as 别名]

#.........这里部分代码省略.........
            self.origin.y = northing
            self.init = True

        # Publish origin reference for others to know about
        p = Pose()
        p.position.x = self.origin.x
        p.position.y = self.origin.y
        self.pub_origin.publish(p)

        # IMU
        # TODO: Work out these covariances properly. Logs provide covariances in local frame, not body
        imu = Imu()
        imu.header.stamp == rospy.Time.now()
        imu.header.frame_id = self.base_frame

        # Orientation
        # orient=PyKDL.Rotation.RPY(RAD(data.roll),RAD(data.pitch),RAD(data.heading)).GetQuaternion()
        # imu.orientation = Quaternion(*orient)
        imu.orientation.x = data.inspvax.pitch
        imu.orientation.y = data.inspvax.roll
        imu.orientation.z = data.inspvax.azimuth
        imu.orientation.w = 0
        IMU_ORIENT_COVAR[0] = POW(2, data.inspvax.pitch_std)
        IMU_ORIENT_COVAR[4] = POW(2, data.inspvax.roll_std)
        IMU_ORIENT_COVAR[8] = POW(2, data.inspvax.azimuth_std)
        imu.orientation_covariance = IMU_ORIENT_COVAR

        # Angular rates (rad/s)
        # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz
        imu.angular_velocity.x = data.corrimudata.pitch_rate * self.imu_rate
        imu.angular_velocity.y = data.corrimudata.roll_rate * self.imu_rate
        imu.angular_velocity.z = data.corrimudata.yaw_rate * self.imu_rate
        imu.angular_velocity_covariance = IMU_VEL_COVAR

        # Linear acceleration (m/s^2)
        imu.linear_acceleration.x = data.corrimudata.x_accel * self.imu_rate
        imu.linear_acceleration.y = data.corrimudata.y_accel * self.imu_rate
        imu.linear_acceleration.z = data.corrimudata.z_accel * self.imu_rate
        imu.linear_acceleration_covariance = IMU_ACCEL_COVAR

        self.pub_imu.publish(imu)

        # Odometry
        # TODO: Work out these covariances properly
        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = self.odom_frame
        odom.child_frame_id  = self.base_frame
        odom.pose.pose.position.x = easting  - self.origin.x
        odom.pose.pose.position.y = northing - self.origin.y
        odom.pose.pose.position.z = data.inspvax.altitude
        #odom.pose.pose.orientation = Quaternion(*orient)
        odom.pose.pose.orientation = imu.orientation
        POSE_COVAR[21] = IMU_ORIENT_COVAR[0]
        POSE_COVAR[28] = IMU_ORIENT_COVAR[4]
        POSE_COVAR[35] = IMU_ORIENT_COVAR[8]
        odom.pose.covariance = POSE_COVAR

        # Twist is relative to vehicle frame
        odom.twist.twist.linear.x = data.bestxyz.velx
        odom.twist.twist.linear.y = data.bestxyz.vely
        odom.twist.twist.linear.z = data.bestxyz.velz
        TWIST_COVAR[0]  = pow(2,data.bestxyz.velx_std)
        TWIST_COVAR[7]  = pow(2,data.bestxyz.vely_std)
        TWIST_COVAR[14] = pow(2,data.bestxyz.velz_std)
        odom.twist.twist.angular = imu.angular_velocity
        odom.twist.covariance = TWIST_COVAR

        self.pub_odom.publish(odom)

        #
        # Odometry transform (if required)
        #
        if self.publish_tf:
            self.tf_broadcast.sendTransform(
                (odom.pose.pose.position.x, odom.pose.pose.position.y,
                 odom.pose.pose.position.z), odom.pose.pose.orientation, #Quaternion(*orient),
                 odom.header.stamp,odom.child_frame_id, odom.frame_id)

        #
        # NavSatFix
        # TODO: Work out these covariances properly from DOP
        #
        navsat = NavSatFix()
        navsat.header.stamp = rospy.Time.now()
        navsat.header.frame_id = self.odom_frame
        navsat.status = self.nav_status

        # position, in degrees
        navsat.latitude  = data.bestpos.latitude
        navsat.longitude = data.bestpos.longitude
        navsat.altitude  = data.bestpos.altitude
        NAVSAT_COVAR[0] = pow(2, data.bestpos.lat_std) # in meters
        NAVSAT_COVAR[4] = pow(2, data.bestpos.lon_std)
        NAVSAT_COVAR[8] = pow(2, data.bestpos.hgt_std)

        navsat.position_covariance = NAVSAT_COVAR
        navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN

        self.pub_navsatfix.publish(navsat)