Python NavSatFix.header方法代码示例

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

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import header [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 header [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 header [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 header [as 别名]
def subCB(msg_in):  
  global pub
                                                 
  msg_out = NavSatFix()
  msg_out.header = msg_in.header
  
  msg_out.status.status  = NavSatStatus.STATUS_FIX # TODO - fix this
  msg_out.status.service = NavSatStatus.SERVICE_GPS

  msg_out.latitude   = msg_in.LLA.x
  msg_out.longitude  = msg_in.LLA.y
  msg_out.altitude   = msg_in.LLA.z
  
  msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
  msg_out.position_covariance[1] = msg_in.PosUncerainty
  
  pub.publish(msg_out)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import header [as 别名]
def sub_insCB(msg_in): 
  global pub_imu
  global pub_gps
  
  global msg_imu
  
  msg_imu.header.stamp          = msg_in.header.stamp
  msg_imu.header.frame_id       = msg_in.header.frame_id
  
  # Convert the RPY data from the Vectornav into radians!
  roll  = (math.pi * msg_in.RPY.x) / 180.0
  pitch = (math.pi * msg_in.RPY.y) / 180.0
  yaw   = (math.pi * msg_in.RPY.z) / 180.0
  q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
  msg_imu.orientation.x = q[0]
  msg_imu.orientation.y = q[1]
  msg_imu.orientation.z = q[2]
  msg_imu.orientation.w = q[3]
         
  pub_imu.publish(msg_imu)
  
                                                 
  msg_gps                 = NavSatFix()
  msg_gps.header          = msg_in.header
  msg_gps.status.status   = NavSatStatus.STATUS_FIX # TODO - fix this
  msg_gps.status.service  = NavSatStatus.SERVICE_GPS
  msg_gps.latitude        = msg_in.LLA.x
  msg_gps.longitude       = msg_in.LLA.y
  msg_gps.altitude        = msg_in.LLA.z
  msg_gps.position_covariance_type  = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
  msg_gps.position_covariance[0]    = msg_in.PosUncerainty
  pub_gps.publish(msg_gps)
  
  
  msg_time = TimeReference()
  msg_time.header.stamp     = msg_in.header.stamp
  msg_time.header.frame_id  = msg_in.header.frame_id
  unix_time = 315964800 + (msg_in.Week * 7 * 24 * 3600) + msg_in.Time
  msg_time.time_ref = rospy.Time.from_sec(unix_time)
  pub_time.publish(msg_time)