Python NavSatFix.position_covariance[0]方法代码示例

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance[0] [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 position_covariance[0] [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 position_covariance[0] [as 别名]
 def publish(self, data):
     msg = NavSatFix()
     msg.header.frame_id = self._frameId
     msg.header.stamp = rospy.get_rostime()
     msg.latitude = data.getLat()
     msg.longitude = data.getLng()
     msg.altitude = data.getMeters()
     if data.getFix() == 1:
         msg.status.status = 0
     else:
         msg.status.status = -1
     msg.position_covariance_type = 1
     msg.position_covariance[0] = data.getHDOP() * data.getHDOP()
     msg.position_covariance[4] = data.getHDOP() * data.getHDOP()
     msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP()
     msg.status.service = 1
     self._pub.publish(msg)

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

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance[0] [as 别名]
    def publish(self, data):
        now = int(round(time.time() * 1000))
        msg = NavSatFix()
        msg.header.frame_id = self._frameId
        msg.header.stamp = rospy.get_rostime()
        msg.latitude = data.getLat()
        msg.longitude = data.getLng()
        msg.altitude = data.getMeters()
        cur_fix =  data.getFix()
        #print cur_fix
        if  (cur_fix != self._old_fix):
            msg.status.status = 0
            self._old_fix = cur_fix
            self._wd = now
	elif (now - self._wd) < GPS_WD_TIMEOUT:
            msg.status.status = 0
        else:
            msg.status.status = -1
        msg.position_covariance_type = 1
        msg.position_covariance[0] = data.getHDOP() * data.getHDOP()
        msg.position_covariance[4] = data.getHDOP() * data.getHDOP()
        msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP()
        msg.status.service = 1
        self._pub.publish(msg)

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance[0] [as 别名]
    def add_sentence(self, nmea_string, frame_id, timestamp=None):
        if not check_nmea_checksum(nmea_string):
            rospy.logwarn("Received a sentence with an invalid checksum. " +
                          "Sentence was: %s" % repr(nmea_string))
            return False

        parsed_sentence = libjavad_navsat_driver.parser.parse_nmea_sentence(nmea_string)
        if not parsed_sentence:
            rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string)
            return False

        if timestamp:
            current_time = timestamp
        else:
            current_time = rospy.get_rostime()
        current_fix = NavSatFix()
        current_fix.header.stamp = current_time
        current_fix.header.frame_id = frame_id
        current_time_ref = TimeReference()
        current_time_ref.header.stamp = current_time
        current_time_ref.header.frame_id = frame_id
        if self.time_ref_source:
            current_time_ref.source = self.time_ref_source
        else:
            current_time_ref.source = frame_id

        if not self.use_RMC and 'GGA' in parsed_sentence:
            data = parsed_sentence['GGA']
            gps_qual = data['fix_type']
            if gps_qual == 0:
                current_fix.status.status = NavSatStatus.STATUS_NO_FIX
            elif gps_qual == 1:
                current_fix.status.status = NavSatStatus.STATUS_FIX
            elif gps_qual == 2:
                current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
            elif gps_qual in (4, 5):
                current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX
            else:
                current_fix.status.status = NavSatStatus.STATUS_NO_FIX

            current_fix.status.service = NavSatStatus.SERVICE_GPS

            current_fix.header.stamp = current_time

            latitude = data['latitude']
            if data['latitude_direction'] == 'S':
                latitude = -latitude
            current_fix.latitude = latitude

            longitude = data['longitude']
            if data['longitude_direction'] == 'W':
                longitude = -longitude
            current_fix.longitude = longitude

            hdop = data['hdop']
            current_fix.position_covariance[0] = hdop ** 2
            current_fix.position_covariance[4] = hdop ** 2
            current_fix.position_covariance[8] = (2 * hdop) ** 2  # FIXME
            current_fix.position_covariance_type = \
                NavSatFix.COVARIANCE_TYPE_APPROXIMATED

            # Altitude is above ellipsoid, so adjust for mean-sea-level
            #altitude = data['altitude'] + data['mean_sea_level']
            altitude = data['altitude']
            current_fix.altitude = altitude

            self.fix_pub.publish(current_fix)

            if not math.isnan(data['utc_time']):
                current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
                self.time_ref_pub.publish(current_time_ref)

        elif 'RMC' in parsed_sentence:
            data = parsed_sentence['RMC']

            # Only publish a fix from RMC if the use_RMC flag is set.
            if self.use_RMC:
                if data['fix_valid']:
                    current_fix.status.status = NavSatStatus.STATUS_FIX
                else:
                    current_fix.status.status = NavSatStatus.STATUS_NO_FIX

                current_fix.status.service = NavSatStatus.SERVICE_GPS

                latitude = data['latitude']
                if data['latitude_direction'] == 'S':
                    latitude = -latitude
                current_fix.latitude = latitude

                longitude = data['longitude']
                if data['longitude_direction'] == 'W':
                    longitude = -longitude
                current_fix.longitude = longitude

                current_fix.altitude = float('NaN')
                current_fix.position_covariance_type = \
                    NavSatFix.COVARIANCE_TYPE_UNKNOWN

                self.fix_pub.publish(current_fix)

#.........这里部分代码省略.........

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance[0] [as 别名]
                        latitude = float(fields[2][0:2]) + float(fields[2][2:])/60
                        if fields[3] == 'S':
                            latitude = -latitude
                        navData.latitude = latitude
                        print("Lat : %f" %latitude);
                        
                        longitude = float(fields[4][0:3]) + float(fields[4][3:])/60
                        if fields[5] == 'W':
                            longitude = -longitude
                        navData.longitude = longitude
                        print("Long : %f" %longitude);
                        
                        hdop = float(fields[8])
                        print("Horizontal Dilution of Position : %f" %hdop);
                        navData.position_covariance[0] = hdop**2
                        navData.position_covariance[4] = hdop**2
                        navData.position_covariance[8] = (2*hdop)**2 #FIX ME
                        navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED

                        #Altitude is above ellipsoid, so adjust for mean-sea-level
                        altitude = float(fields[9]) # Removed causing errors + float(fields[11])
                        navData.altitude = altitude
                        print("Altitude : %f" %altitude);

                        gpstime.header.stamp = timeNow
                        gpstime.time_ref = convertNMEATimeToROS(fields[1])

                        gpspub.publish(navData)
                        gpstimePub.publish(gpstime)
            except ValueError as e:

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance[0] [as 别名]
    def add_sentence(self, nmea_string, frame_id, timestamp=None):
        if not check_nmea_checksum(nmea_string):
            rospy.logwarn("Received a sentence with an invalid checksum. \
                Sentence was: %s" % nmea_string)
            return False

        parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string)
        if not parsed_sentence:
            rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" %
                nmea_string)
            return False

        if timestamp:
            current_time = timestamp
        else:
            current_time = rospy.get_rostime()
        current_fix = NavSatFix()
        current_fix.header.stamp = current_time
        current_fix.header.frame_id = frame_id

        current_heading = Imu()
	current_heading.header.stamp = current_time
	current_heading.header.frame_id = 'base_footprint'	

        current_direction = String() # For testing

        current_time_ref = TimeReference()
        current_time_ref.header.stamp = current_time
        current_time_ref.header.frame_id = frame_id
        if self.time_ref_source:
            current_time_ref.source = self.time_ref_source
        else:
            current_time_ref.source = frame_id

        # Add capture/publishing heading info
        if not self.use_RMC and 'HDT' in parsed_sentence:
            #rospy.loginfo("HDT!")
            data = parsed_sentence['HDT']
	    tempHeading = data['true_heading']
	    ccHeading = (2 * math.pi) - tempHeading

	    q = tf.transformations.quaternion_from_euler(0,0,ccHeading)
	    current_heading.orientation.x = q[0]
	    current_heading.orientation.y = q[1]
	    current_heading.orientation.z = q[2]
	    current_heading.orientation.w = q[3]

            #current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])

            #if   (current_heading.data < .3927):     current_direction.data = "N"
            #elif (current_heading.data < 1.178):     current_direction.data = "NE"
            #elif (current_heading.data < 1.9635):    current_direction.data = "E"
            #elif (current_heading.data < 2.74889):   current_direction.data = "SE"
            #elif (current_heading.data < 3.53429):   current_direction.data = "S"
            #elif (current_heading.data < 4.31969):   current_direction.data = "SW"
            #elif (current_heading.data < 5.10509):   current_direction.data = "W"
            #elif (current_heading.data < 5.89048):   current_direction.data = "NW"
            #else:                                    current_direction.data = "N"

            self.heading_pub.publish(current_heading)
            #self.direction_pub.publish(current_direction)
            #self.time_ref_pub.publish(current_time_ref)

        elif 'GGA' in parsed_sentence:
            #rospy.loginfo("GGA!")
            data = parsed_sentence['GGA']
            gps_qual = data['fix_type']
            if gps_qual == 0:
                current_fix.status.status = NavSatStatus.STATUS_NO_FIX
            elif gps_qual == 1:
                current_fix.status.status = NavSatStatus.STATUS_FIX
            elif gps_qual == 2:
                current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
            elif gps_qual in (4, 5):
                current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX
            else:
                current_fix.status.status = NavSatStatus.STATUS_NO_FIX

            current_fix.status.service = NavSatStatus.SERVICE_GPS

            current_fix.header.stamp = current_time

            latitude = data['latitude']
            if data['latitude_direction'] == 'S':
                latitude = -latitude
            current_fix.latitude = latitude

            longitude = data['longitude']
            if data['longitude_direction'] == 'W':
                longitude = -longitude
            current_fix.longitude = longitude

            hdop = data['hdop']
            current_fix.position_covariance[0] = hdop**2
            current_fix.position_covariance[4] = hdop**2
            current_fix.position_covariance[8] = (2*hdop)**2 # FIXME
            current_fix.position_covariance_type = \
                NavSatFix.COVARIANCE_TYPE_APPROXIMATED

            # Altitude is above ellipsoid, so adjust for mean-sea-level
#.........这里部分代码省略.........

# 需要导入模块: from sensor_msgs.msg import NavSatFix [as 别名]
# 或者: from sensor_msgs.msg.NavSatFix import position_covariance[0] [as 别名]
    def add_sentence(self, nmea_string, frame_id, timestamp=None):
        if not check_nmea_checksum(nmea_string):
            rospy.logwarn("Received a sentence with an invalid checksum. " +
                          "Sentence was: %s" % repr(nmea_string))
            return False

        parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
            nmea_string)
        if not parsed_sentence:
            rospy.logdebug(
                "Failed to parse NMEA sentence. Sentence was: %s" %
                nmea_string)
            return False

        if timestamp:
            current_time = timestamp
        else:
            current_time = rospy.get_rostime()
        current_fix = NavSatFix()
        current_fix.header.stamp = current_time
        current_fix.header.frame_id = frame_id
        current_time_ref = TimeReference()
        current_time_ref.header.stamp = current_time
        current_time_ref.header.frame_id = frame_id
        if self.time_ref_source:
            current_time_ref.source = self.time_ref_source
        else:
            current_time_ref.source = frame_id

        if not self.use_RMC and 'GGA' in parsed_sentence:
            current_fix.position_covariance_type = \
                NavSatFix.COVARIANCE_TYPE_APPROXIMATED

            data = parsed_sentence['GGA']
            fix_type = data['fix_type']
            if not (fix_type in self.gps_qualities):
                fix_type = -1
            gps_qual = self.gps_qualities[fix_type]
            default_epe = gps_qual[0]
            current_fix.status.status = gps_qual[1]
            current_fix.position_covariance_type = gps_qual[2]

            if gps_qual > 0:
                self.valid_fix = True
            else:
                self.valid_fix = False

            current_fix.status.service = NavSatStatus.SERVICE_GPS

            latitude = data['latitude']
            if data['latitude_direction'] == 'S':
                latitude = -latitude
            current_fix.latitude = latitude

            longitude = data['longitude']
            if data['longitude_direction'] == 'W':
                longitude = -longitude
            current_fix.longitude = longitude

            # Altitude is above ellipsoid, so adjust for mean-sea-level
            altitude = data['altitude'] + data['mean_sea_level']
            current_fix.altitude = altitude

            # use default epe std_dev unless we've received a GST sentence with
            # epes
            if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
                self.lon_std_dev = default_epe
            if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
                self.lat_std_dev = default_epe
            if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
                self.alt_std_dev = default_epe * 2

            hdop = data['hdop']
            current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2
            current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2
            current_fix.position_covariance[8] = (
                2 * hdop * self.alt_std_dev) ** 2  # FIXME

            self.fix_pub.publish(current_fix)

            if not math.isnan(data['utc_time']):
                current_time_ref.time_ref = rospy.Time.from_sec(
                    data['utc_time'])
                self.last_valid_fix_time = current_time_ref
                self.time_ref_pub.publish(current_time_ref)

        elif not self.use_RMC and 'VTG' in parsed_sentence:
            data = parsed_sentence['VTG']

            # Only report VTG data when you've received a valid GGA fix as
            # well.
            if self.valid_fix:
                current_vel = TwistStamped()
                current_vel.header.stamp = current_time
                current_vel.header.frame_id = frame_id
                current_vel.twist.linear.x = data['speed'] * \
                    math.sin(data['true_course'])
                current_vel.twist.linear.y = data['speed'] * \
                    math.cos(data['true_course'])
                self.vel_pub.publish(current_vel)
#.........这里部分代码省略.........