Python msg.NavSatFix类代码示例

def talker():
    UDP_IP = "192.168.0.107"
    UDP_PORT = 9002

    sock = socket.socket(socket.AF_INET, # Internet
                         socket.SOCK_DGRAM) # UDP
    sock.bind((UDP_IP, UDP_PORT))
    pub = rospy.Publisher('gps/fix', NavSatFix, queue_size=10)
    rospy.init_node('test_relay', anonymous=True)
    rate = rospy.Rate(100) # 10hz
    while not rospy.is_shutdown():
       data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
       print "received message:", data
       data = re.sub("[A-Za-z]","",str(data))
       result = str(data).split(":")
       print result
       rospy.loginfo(result)
       
       
       msg = NavSatFix()
       msg.header = Header()
       msg.latitude = float(result[1])
       msg.longitude = float(result[2])
       #rospy.loginfo(msg)
       pub.publish(msg)

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",
    )

def joy_callback(msg):
    if msg.buttons[12] == 1:
        gps_goal = NavSatFix()
        gps_goal.latitude = 33.726526
        gps_goal.longitude = -83.299984
        gps_put.publish(gps_goal)
    if msg.buttons[14] == 1:
        client.cancel_all_goals()

 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)

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()

    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)

	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)

    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)

def address2geo(address):
    gurl = 'https://maps.googleapis.com/maps/api/geocode/xml?address='+address
    res = urllib.urlopen(gurl).read()
    ans = NavSatFix()
    i1 = res.find('lat')
    i2 = res.find('/lat',i1)
    ans.latitude = float(res[i1+4:i2-1])
    i1 = res.find('lng')
    i2 = res.find('/lng',i1)
    ans.longitude = float(res[i1+4:i2-1])
    return ans

def gps_fix_sim():
    pub=rospy.Publisher('sensor_msgs/NavSatFix', NavSatFix, queue_size=10)
    rospy.init_node('gps_sim', anonymous=True)
    rate = rospy.Rate(2)
    
    while not rospy.is_shutdown():
       gps_fix = NavSatFix()
       gps_fix.latitude = 36.6
       gps_fix.longitude = -121.1
       gps_fix.altitude = 1.0
       pub.publish(gps_fix)
       rate.sleep()

def pub_gps(msg_type, msg, bridge):
    pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1)
    fix = NavSatFix()
    fix.header.stamp = bridge.project_ap_time(msg)
    fix.header.frame_id = 'base_footprint'
    fix.latitude = msg.lat/1e07
    fix.longitude = msg.lon/1e07
    # NOTE: absolute (MSL) altitude
    fix.altitude = msg.alt/1e03
    fix.status.status = NavSatStatus.STATUS_FIX
    fix.status.service = NavSatStatus.SERVICE_GPS
    fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
    pub.publish(fix)

 def convert_gps_to_pose(self, coords):
     # create NavSatFix msg
     nav_msg = NavSatFix()
     nav_msg.header.stamp = rospy.get_rostime()
     nav_msg.header.frame_id = 'odom'
     # x is longitude, y is latitude
     nav_msg.longitude = coords['x']
     nav_msg.latitude = coords['y']
     # convert using /latlong_goal_node/AddLatLongGoal service
     # returns PoseStamped
     response = self.latlong_service(nav_msg)
     rospy.loginfo("Converted NavSatFix to PoseStamped -> (%f, %f)" % (response.goal_xy.pose.position.x,
                   response.goal_xy.pose.position.y))
     return response.goal_xy

    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)

 def testAutoOriginFromNavSatFix(self):
     rospy.init_node('test_initialize_origin')
     nsf_pub = rospy.Publisher('fix', NavSatFix, queue_size=2)
     origin_sub = self.subscribeToOrigin()
     self.test_stamp = True
     nsf_msg = NavSatFix()
     nsf_msg.latitude = swri['latitude']
     nsf_msg.longitude = swri['longitude']
     nsf_msg.altitude = swri['altitude']
     nsf_msg.header.frame_id = "/far_field"
     nsf_msg.header.stamp = msg_stamp
     r = rospy.Rate(10.0)
     while not rospy.is_shutdown():
         nsf_pub.publish(nsf_msg)
         r.sleep()

def calc_lat_long(init_pos,distance,brng):
  '''
    * calculate the latitude and longitude given distance and bearing
    * distnace input in [m] brng in [rad]
  '''
  R = 6371.0 * 1000
  p2 = NavSatFix()
  lat1 = init_pos.latitude*PI/180.0
  lon1 = init_pos.longitude*PI/180.0
	  
  p2.latitude = (asin( sin(lat1)*cos(distance/R) + cos(lat1)*sin(distance/R)*cos(brng)))
  p2.longitude = (lon1 + atan2(sin(brng)*sin(distance/R)*cos(lat1),cos(distance/R)-sin(lat1)*sin(p2.latitude)))*180 / PI
  p2.latitude = p2.latitude *180 / PI

  return p2