Exemplo n.º 1
0
    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_pose_utm = GpsLocal()
        current_pose_utm.header.stamp = current_time
        current_pose_utm.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:
            self.seq = self.seq + 1
            current_fix.position_covariance_type = \
                NavSatFix.COVARIANCE_TYPE_APPROXIMATED
            data = parsed_sentence['GGA']
            gps_qual = data['fix_type']

            try:
                # Unpack the fix params for this quality value
                current_fix.status.status, self.default_epe, self.fix_type = self.fix_mappings[
                    gps_qual]
            except KeyError:
                current_fix.status.status = NavSatStatus.STATUS_NO_FIX
                self.fix_type = 'Unknown'

            if gps_qual == 0:
                current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
                self.using_receiver_epe = False
                self.invalid_cnt += 1

            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

            # 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 = self.default_epe
            if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
                self.lat_std_dev = self.default_epe
            if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
                self.alt_std_dev = self.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] = (hdop *
                                                  self.alt_std_dev)**2  # FIXME

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

            self.fix_pub.publish(current_fix)

            if not math.isnan(latitude) and not math.isnan(longitude) and (
                    gps_qual == 5 or gps_qual == 4):

                UTMNorthing, UTMEasting = LLtoUTM(latitude, longitude)[0:2]

                current_pose_utm.position.x = UTMEasting
                current_pose_utm.position.y = UTMNorthing
                current_pose_utm.position.z = altitude

                # Pose x/y/z covariance is whatever we decided h & v covariance is.
                # Here is it the same as for ECEF coordinates
                current_pose_utm.covariance[0] = (hdop * self.lon_std_dev)**2
                current_pose_utm.covariance[4] = (hdop * self.lat_std_dev)**2
                current_pose_utm.covariance[8] = (hdop * self.alt_std_dev)**2

                current_pose_utm.rtk_fix = True if gps_qual == 4 else False

                self.local_pub.publish(current_pose_utm)

            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)

            if (self.diag_pub_time < rospy.get_time()):
                self.diag_pub_time += 1
                diag_arr = DiagnosticArray()
                diag_arr.header.stamp = rospy.get_rostime()
                diag_arr.header.frame_id = frame_id
                diag_msg = DiagnosticStatus()
                diag_msg.name = 'GPS_status'
                diag_msg.hardware_id = 'GPS'
                diag_msg.level = DiagnosticStatus.OK
                diag_msg.message = 'Received GGA Fix'
                diag_msg.values.append(
                    KeyValue('Sequence number', str(self.seq)))
                diag_msg.values.append(
                    KeyValue('Invalid fix count', str(self.invalid_cnt)))
                diag_msg.values.append(KeyValue('Latitude', str(latitude)))
                diag_msg.values.append(KeyValue('Longitude', str(longitude)))
                diag_msg.values.append(KeyValue('Altitude', str(altitude)))
                diag_msg.values.append(KeyValue('GPS quality', str(gps_qual)))
                diag_msg.values.append(KeyValue('Fix type', self.fix_type))
                diag_msg.values.append(
                    KeyValue('Number of satellites',
                             str(data['num_satellites'])))
                diag_msg.values.append(
                    KeyValue('Receiver providing accuracy',
                             str(self.using_receiver_epe)))
                diag_msg.values.append(KeyValue('Hdop', str(hdop)))
                diag_msg.values.append(
                    KeyValue('Latitude std dev', str(hdop * self.lat_std_dev)))
                diag_msg.values.append(
                    KeyValue('Longitude std dev',
                             str(hdop * self.lon_std_dev)))
                diag_msg.values.append(
                    KeyValue('Altitude std dev', str(hdop * self.alt_std_dev)))
                diag_arr.status.append(diag_msg)
                self.diag_pub.publish(diag_arr)

        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)

                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)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                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)

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

            # Use receiver-provided error estimate if available
            self.using_receiver_epe = True
            self.lon_std_dev = data['lon_std_dev']
            self.lat_std_dev = data['lat_std_dev']
            self.alt_std_dev = data['alt_std_dev']

        else:
            return False
Exemplo n.º 2
0
    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. Sentece was: %s" %
                           nmea_string)
            return False

        if timestamp:
            current_time = timestamp
        else:
            current_time = rospy.get_rostime()
        current_fix = GPS()
        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 'RMC' in parsed_sentence:
            data = parsed_sentence['RMC']
            if data['fix_valid']:
                self.speed = data['speed']
                if not math.isnan(data['true_course']):
                    self.ground_course = data['true_course']

        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.fix = True
                current_fix.NumSat = data['num_satellites']
            # 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.covariance = 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']
            current_fix.altitude = altitude

            current_fix.speed = self.speed
            current_fix.ground_course = self.ground_course

            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:
                current_fix.fix = True
                current_fix.NumSat = data['num_satellites']

                # 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
                if data['fix_valid']:
                    current_fix.speed = data['speed']
                    if not math.isnan(data['true_course']):
                        current_fix.ground_course = data['true_course']
                    else:
                        current_fix.ground_course = self.ground_course

                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)

        else:
            return False
Exemplo n.º 3
0
    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. Sentece was: %s" % nmea_string)
            return False

        if timestamp:
            current_time = timestamp
        else:
            current_time = rospy.get_rostime()
        current_fix = GPS()
        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 "RMC" in parsed_sentence:
            data = parsed_sentence["RMC"]
            if data["fix_valid"]:
                self.speed = data["speed"]
                if not math.isnan(data["true_course"]):
                    self.ground_course = data["true_course"]

        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.fix = True
                current_fix.NumSat = 4
            # 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.covariance = 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"]
            current_fix.altitude = altitude

            current_fix.speed = self.speed
            current_fix.ground_course = self.ground_course

            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:
                current_fix.fix = True
                current_fix.NumSat = 4

                # 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
                if data["fix_valid"]:
                    current_fix.speed = data["speed"]
                    if not math.isnan(data["true_course"]):
                        current_fix.ground_course = data["true_course"]
                    else:
                        current_fix.ground_course = self.ground_course

                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)

        else:
            return False
Exemplo n.º 4
0
    def add_sentence(self, nmea_string, frame_id, timestamp=None):
        """Public method to provide a new NMEA sentence to the driver.

        Args:
            nmea_string (str): NMEA sentence in string form.
            frame_id (str): TF frame ID of the GPS receiver.
            timestamp(rospy.Time, optional): Time the sentence was received.
                If timestamp is not specified, the current time is used.

        Returns:
            bool: True if the NMEA string is successfully processed, False if there is an error.
        """
        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
        if not self.use_GNSS_time:
            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']

            if self.use_GNSS_time:
                if math.isnan(data['utc_time'][0]):
                    rospy.logwarn("Time in the NMEA sentence is NOT valid")
                    return False
                current_fix.header.stamp = rospy.Time(data['utc_time'][0],
                                                      data['utc_time'][1])

            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]

            self.valid_fix = (fix_type > 0)

            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'][0]) or self.use_GNSS_time):
                current_time_ref.time_ref = rospy.Time(data['utc_time'][0],
                                                       data['utc_time'][1])
                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)

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

            if self.use_GNSS_time:
                if math.isnan(data['utc_time'][0]):
                    rospy.logwarn("Time in the NMEA sentence is NOT valid")
                    return False
                current_fix.header.stamp = rospy.Time(data['utc_time'][0],
                                                      data['utc_time'][1])

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

                if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
                    current_time_ref.time_ref = rospy.Time(
                        data['utc_time'][0], data['utc_time'][1])
                    self.time_ref_pub.publish(current_time_ref)

            # Publish velocity from RMC regardless, since GGA doesn't provide
            # it.
            if data['fix_valid']:
                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)
        elif 'GST' in parsed_sentence:
            data = parsed_sentence['GST']

            # Use receiver-provided error estimate if available
            self.using_receiver_epe = True
            self.lon_std_dev = data['lon_std_dev']
            self.lat_std_dev = data['lat_std_dev']
            self.alt_std_dev = data['alt_std_dev']
        elif 'HDT' in parsed_sentence:
            data = parsed_sentence['HDT']
            if data['heading']:
                current_heading = QuaternionStamped()
                current_heading.header.stamp = current_time
                current_heading.header.frame_id = frame_id
                q = quaternion_from_euler(0, 0, math.radians(data['heading']))
                current_heading.quaternion.x = q[0]
                current_heading.quaternion.y = q[1]
                current_heading.quaternion.z = q[2]
                current_heading.quaternion.w = q[3]
                self.heading_pub.publish(current_heading)
        else:
            return False
Exemplo n.º 5
0
    def add_sentence(self, nmea_string, frame_id, timestamp=None):
        """Public method to provide a new NMEA sentence to the driver.

        Args:
            nmea_string (str): NMEA sentence in string form.
            frame_id (str): TF frame ID of the GPS receiver.
            timestamp(rospy.Time, optional): Time the sentence was received.
                If timestamp is not specified, the current time is used.

        Returns:
            bool: True if the NMEA string is successfully processed, False if there is an error.
        """
        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
        if not self.use_GNSS_time:
            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']

            if self.use_GNSS_time:
                if math.isnan(data['utc_time'][0]):
                    rospy.logwarn("Time in the NMEA sentence is NOT valid")
                    return False
                current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1])

            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
            elif gps_qual == 9:
                # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9
                # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
                current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
            else:
                current_fix.status.status = NavSatStatus.STATUS_NO_FIX

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

            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']
            current_fix.altitude = altitude

            self.fix_pub.publish(current_fix)

            if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
                current_time_ref.time_ref = rospy.Time(
                    data['utc_time'][0], data['utc_time'][1])
                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)

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

            if self.use_GNSS_time:
                if math.isnan(data['utc_time'][0]):
                    #rospy.logwarn("Time in the NMEA sentence is NOT valid")
                    return False
                current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1])

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

                if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
                    current_time_ref.time_ref = rospy.Time(
                        data['utc_time'][0], data['utc_time'][1])
                    self.time_ref_pub.publish(current_time_ref)

            # Publish velocity from RMC regardless, since GGA doesn't provide
            # it.
            if data['fix_valid']:
                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)
        else:
            return False
Exemplo n.º 6
0
    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

        self.current_nmea.data = nmea_string

        if timestamp:
            current_time = timestamp
        else:
            current_time = rospy.get_rostime()

        self.current_fix.header.stamp = current_time
        self.current_fix.header.frame_id = frame_id
        self.current_fix.status.header.stamp = current_time
        self.current_fix.status.header.frame_id = frame_id

        if 'GGA' in parsed_sentence:
            self.current_fix.position_covariance_type = \
                GPSFix.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]
            self.current_fix.status.status = gps_qual[1]
            self.current_fix.position_covariance_type = gps_qual[2]

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

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

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

            # Altitude is above ellipsoid, so adjust for mean-sea-level
            altitude = data['altitude'] + data['mean_sea_level']
            self.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

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

            if not math.isnan(data['utc_time']):
                dt_time = datetime.fromtimestamp(data['utc_time'])
                self.current_fix.time = dt_time.hour * 10000 + dt_time.minute * 100 + dt_time.second
                self.last_valid_fix_time = rospy.Time.from_sec(
                    data['utc_time'])

        if '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:
                self.current_fix.track = data['true_course']
                self.current_fix.speed = data['speed']

        if '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']:
                    self.current_fix.status.status = GPSStatus.STATUS_FIX
                else:
                    self.current_fix.status.status = GPSStatus.STATUS_NO_FIX

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

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

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

                if not math.isnan(data['utc_time']):
                    self.current_fix.time = data['utc_time']

            # Publish velocity from RMC regardless, since GGA doesn't provide
            # it.
            if data['fix_valid']:
                self.current_fix.track = data['true_course']
                self.current_fix.speed = data['speed']

        if 'GST' in parsed_sentence:
            data = parsed_sentence['GST']

            # Use receiver-provided error estimate if available
            self.using_receiver_epe = True
            self.lon_std_dev = data['lon_std_dev']
            self.lat_std_dev = data['lat_std_dev']
            self.alt_std_dev = data['alt_std_dev']
        if 'HDT' in parsed_sentence:
            data = parsed_sentence['HDT']
            if data['heading']:
                self.current_fix.dip = math.radians(data['heading'])
#                current_heading = QuaternionStamped()
#                current_heading.header.stamp = current_time
#                current_heading.header.frame_id = frame_id
#                q = quaternion_from_euler(0, 0, math.radians(data['heading']))
#                current_heading.quaternion.x = q[0]
#                current_heading.quaternion.y = q[1]
#                current_heading.quaternion.z = q[2]
#                current_heading.quaternion.w = q[3]
#                self.heading_pub.publish(current_heading)
#        else:
#            return False
        self.fix_pub.publish(self.current_fix)
        self.nmea_pub.publish(self.current_nmea)
Exemplo n.º 7
0
    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. 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)

                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)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data["fix_valid"]:
                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"])
                current_vel.twist.angular.z = data["true_course"]
                self.vel_pub.publish(current_vel)
        else:
            return False
Exemplo n.º 8
0
    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]
            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.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)
#                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)

# Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                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)
        elif 'GST' in parsed_sentence:
            data = parsed_sentence['GST']

            # Use receiver-provided error estimate if available
            self.using_receiver_epe = True
            self.lon_std_dev = data['lon_std_dev']
            self.lat_std_dev = data['lat_std_dev']
            self.alt_std_dev = data['alt_std_dev']
        elif 'HDT' in parsed_sentence:
            data = parsed_sentence['HDT']
            if data['heading']:
                self.heading = math.radians(data['heading'])
                current_heading = QuaternionStamped()
                current_heading.header.stamp = current_time
                current_heading.header.frame_id = frame_id
                q = quaternion_from_euler(0, 0, math.radians(data['heading']))
                current_heading.quaternion.x = q[0]
                current_heading.quaternion.y = q[1]
                current_heading.quaternion.z = q[2]
                current_heading.quaternion.w = q[3]
                self.heading_pub.publish(current_heading)
        elif 'HDG' in parsed_sentence:
            data = parsed_sentence['HDG']
            if data['heading']:
                magnetic_current_heading = QuaternionStamped()
                magnetic_current_heading.header.stamp = current_time
                magnetic_current_heading.header.frame_id = frame_id
                q = quaternion_from_euler(0, 0, math.radians(data['heading']))
                magnetic_current_heading.quaternion.x = q[0]
                magnetic_current_heading.quaternion.y = q[1]
                magnetic_current_heading.quaternion.z = q[2]
                magnetic_current_heading.quaternion.w = q[3]
                self.magnetic_heading_pub.publish(magnetic_current_heading)
        elif 'VTG' in parsed_sentence:
            data = parsed_sentence['VTG']
            if data['speed']:
                current_vel = TwistStamped()
                current_vel.header.stamp = current_time
                current_vel.header.frame_id = frame_id
                current_vel.twist.linear.x = data['speed']
                self.vel_pub.publish(current_vel)

                self.vel_pub.publish(current_vel)
        else:
            return False
Exemplo n.º 9
0
    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. 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:
            #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
            
            # covariances diagonals as rospy arguments
            current_fix.position_covariance[0] = self.gps_covariance_pos[0]
            current_fix.position_covariance[4] = self.gps_covariance_pos[1]
            current_fix.position_covariance[8] = self.gps_covariance_pos[2]
            current_fix.position_covariance_type =\
                NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN

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

            if not math.isnan(data['utc_time']):
                current_time_ref.time_ref = rospy.Time.from_sec(int(int(timestamp.to_sec())/(60*60))*(60*60)+data['utc_time'])
                self.time_ref_pub.publish(current_time_ref)
		if self.use_GPS_time:
                    self.gps_time = current_time_ref.time_ref
                    current_fix.header.stamp = current_time_ref.time_ref
                current_fix.header.stamp = current_fix.header.stamp+rospy.Duration(self.time_delay)
            #print(timestamp.to_sec())
            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']
            #rospy.loginfo('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)

                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)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                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)
        elif 'VTG' in parsed_sentence:
            data = parsed_sentence['VTG']
            track_made_good_degrees_true = data['track_made_good_degrees_true']
            track_made_good_degrees_magnetic = data['track_made_good_degrees_magnetic']
            speed = data['speed']
            SPEED_OVER_GROUND = data['speed_over_ground']
            if not math.isnan(track_made_good_degrees_true):
                DIRECTION_REFERENCE  = "True"
                COURSE_OVER_GROUND = track_made_good_degrees_true
            elif not math.isnan(track_made_good_degrees_magnetic):
                DIRECTION_REFERENCE  = "Magnetic"
                COURSE_OVER_GROUND = track_made_good_degrees_magnetic
            else:
                DIRECTION_REFERENCE  = "Null"
		COURSE_OVER_GROUND = float(0)
		SPEED_OVER_GROUND = float('NaN')
            current_vtg = Course_Speed()
            current_vtg.header.stamp = current_time
            current_vtg.header.frame_id = frame_id
            current_vtg.DIRECTION_REFERENCE = DIRECTION_REFERENCE
            current_vtg.COURSE_OVER_GROUND  = COURSE_OVER_GROUND 
            current_vtg.SPEED_OVER_GROUND = SPEED_OVER_GROUND
            self.vtg_pub.publish(current_vtg)
            #rospy.loginfo(track_made_good_degrees_magnetic)
        elif 'HDT' in parsed_sentence:
            #rospy.loginfo('HDT')     
            data = parsed_sentence['HDT']
            heading_degrees = data['heading_degrees']
            current_hdt = GPHDT()
            current_hdt.header.stamp = current_time
            current_hdt.header.frame_id = frame_id
            current_hdt.HEADING_DEGREES = heading_degrees
            if self.use_GPS_time and not self.gps_time==None:
                    current_hdt.header.stamp = self.gps_time-rospy.Duration(0,1000000)
            elif self.use_GPS_time:
                    current_hdt.header.stamp.secs = 0
            current_hdt.header.stamp = current_hdt.header.stamp+rospy.Duration(self.time_delay)+rospy.Duration(self.time_delay_heading)
            #print(current_hdt.header.stamp.to_sec())
            self.hdt_pub.publish(current_hdt)
        else:
            rospy.loginfo('Not valid')
            return False
Exemplo n.º 10
0
    def add_sentence(self, nmea_string, frame_id, timestamp=None):
        global lat_error, lon_error,started, trustedCount, badFlag,fixCount, gst_rms
        global rmc_lon,rmc_lat, hdop_GPGSA, pdop_GPGSA, vdop_GPGSA, GPGSA_fix
        global hdop_GAGSA, pdop_GAGSA, vdop_GAGSA, hdop_GLGSA, pdop_GLGSA, vdop_GLGSA
        global SNR1,SNR2,SNR3, SNR4, hdop_GGA
        global rmc_longitude, rmc_latitude, rmc_altitude, gpsQuality
        global gga_longitude, gga_latitude , gga_altitude
        global kph_speed, age_of_diff
        global out_cov, utc_time, satNo, gps_qual
        global gps_line, gps_line_first_half, gps_line_second_half
        global received_time_is_nan, received_utc_time, previous_received_utc_time
        global VTG_received, GGA_received, use_gps_date_once, file_name, publish_gps

        #if rospy.has_param('publish_gps'):
        #    publish_gps = rospy.get_param('publish_gps')

        trustedFix = NavSatFix()
        unTrustedFix = NavSatFix()
        trustedFloat = NavSatFix()
        semiTrustedFloat = NavSatFix()
        unTrustedFloat = NavSatFix()
        
        allMsg = Odometry()
        trustedFix.header.frame_id = frame_id
        unTrustedFix.header.frame_id = frame_id
        trustedFloat.header.frame_id = frame_id
        semiTrustedFloat.header.frame_id = frame_id
        unTrustedFloat.header.frame_id = frame_id

        allMsg.header.frame_id = frame_id

        if not check_nmea_checksum(nmea_string):
            self.get_logger().warn("Received a sentence with an invalid checksum. " +
                          "Sentence was: %s" % repr(nmea_string))
            return False


        '''
        Publish gps lines to Ros string Msg
        '''
        if "$GN" in nmea_string:
            gps_line_first_half += nmea_string + "," + str(time.time()) + ","
        else:
            gps_line_second_half += nmea_string + "," + str(time.time()) + ","            
                
        if 'GNVTG' in nmea_string:
            gps_line += gps_line_first_half + gps_line_second_half + str(self.get_clock().now().to_msg()) + "\n"
            with open(file_name, 'a') as csvfile:
                csvfile.write(gps_line)

            gps_string_msg = String()
            gps_string_msg.data = gps_line
            self.string_pub.publish(gps_string_msg)
            gps_line = ''
            gps_line_first_half = ''
            gps_line_second_half = ''

        if ('GPGSV' in nmea_string and len(nmea_string) < 100): # This message is received too short when it's empty causing runtime error in parsing (This error appeared in ROS2)
            return
        parsed_sentence = parser.parse_nmea_sentence(nmea_string)
        if not parsed_sentence:
            self.get_logger().debug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string)
            return False


        if 'GAGSA' in parsed_sentence:
            gsa_data=parsed_sentence['GSA']
            
            if math.isnan(gsa_data['PDop']):
                pdop_GAGSA = 100.0
            else:
                pdop_GAGSA=gsa_data['PDop']

            if math.isnan(gsa_data['HDop']) :
                hdop_GAGSA = 100.0
            else:
                hdop_GAGSA=gsa_data['HDop']

            if math.isnan(gsa_data['VDop']) :
                vdop_GAGSA = 100.0
            else:
                vdop_GAGSA=gsa_data['VDop']

        if 'GLGSA' in parsed_sentence:
            gsa_data=parsed_sentence['GSA']
            
            if math.isnan(gsa_data['PDop']):
                pdop_GLGSA = 100.0
            else:
                pdop_GLGSA=gsa_data['PDop']

            if math.isnan(gsa_data['HDop']) :
                hdop_GLGSA = 100.0
            else:
                hdop_GLGSA=gsa_data['HDop']

            if math.isnan(gsa_data['VDop']) :
                vdop_GLGSA = 100.0
            else:
                vdop_GLGSA=gsa_data['VDop']

        if 'GPGSA' in parsed_sentence:
            gsa_data=parsed_sentence['GSA']
            
            if math.isnan(gsa_data['PDop']):
                pdop_GPGSA = 100.0
            else:
                pdop_GPGSA=gsa_data['PDop']

            if math.isnan(gsa_data['HDop']) :
                hdop_GPGSA = 100.0
            else:
                hdop_GPGSA=gsa_data['HDop']

            if math.isnan(gsa_data['VDop']) :
                vdop_GPGSA = 100.0
            else:
                vdop_GPGSA=gsa_data['VDop']

            if math.isnan(gsa_data['fix']):
                GPGSA_fix = 100
            else:
                GPGSA_fix = gsa_data['fix']

        if 'GPGSV' in parsed_sentence:
            gsv_data=parsed_sentence['GSV']
            msgNo=gsv_data['MSGNo']
            if math.isnan(gsv_data['SNR1']) :
                SNR1[msgNo-1] = 100.0
            else: 
                SNR1[msgNo-1]=gsv_data['SNR1']

            if math.isnan(gsv_data['SNR2']) :
                SNR2[msgNo-1] = 100.0
            else:
                SNR2[msgNo-1]=gsv_data['SNR2']

            if math.isnan(gsv_data['SNR3']) :
                SNR3[msgNo-1] = 100.0
            else:
                SNR3[msgNo-1]=gsv_data['SNR3']

            if math.isnan(gsv_data['SNR4']) :
                SNR4[msgNo-1] = 100.0
            else:
                SNR4[msgNo-1]=gsv_data['SNR4']


        if 'GST' in parsed_sentence:
            gst_data=parsed_sentence['GST']

            if math.isnan(gst_data['ranges_std_dev']):
                gst_rms = 100.0
            else:
                gst_rms = gst_data['ranges_std_dev']

            if math.isnan(gst_data['lat_std_dev']) or math.isnan(gst_data['lon_std_dev']):
                lat_error = 100.0
                lon_error = 100.0
            else:
                lat_error=gst_data['lat_std_dev']
                lon_error=gst_data['lon_std_dev']

        if 'RMC' in parsed_sentence: #20200914 was elif
            data = parsed_sentence['RMC']
            date_line = data['date'] + str("_") + str(int(data['utc_time']))
            #gps_string_date_msg = String()
            #gps_string_date_msg.data = date_line
            #self.string_date_pub.publish(gps_string_date_msg)

            if (use_gps_date_once):
                use_gps_date_once = 0
                file_name = "/home/jetson/csv/GPS_LINES_" + data['date'] + str(int(data['utc_time'])) + ".csv"
                
            rmc_longitude = data['longitude']
            rmc_latitude = data['latitude']
            #rmc_altitude = data['altitude']

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                current_vel = TwistStamped()
                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)

        if 'VTG' in parsed_sentence:
            VTG_received = 1
            vtg_data = parsed_sentence['VTG']
            if math.isnan(vtg_data['kph_speed']):
                kph_speed = 100.0
            else:
                kph_speed = vtg_data['kph_speed']


        if 'GGA' in parsed_sentence:
            GGA_received = 1
            data = parsed_sentence['GGA']
            gps_qual = data['fix_type']
            age_of_diff = data['age_of_diff']

            gga_latitude = data['latitude']
            if data['latitude_direction'] == 'S' :
                gga_latitude = -gga_latitude
            gga_longitude = data['longitude']
            if data['longitude_direction'] == 'W':
                gga_longitude = -gga_longitude
            gga_altitude = data['altitude']
            hdop_GGA = data['hdop']
            satNo = data['num_satellites']
            if not math.isnan(data['utc_time']):
                received_utc_time = data['utc_time']
                received_time_is_nan = 0
            else:
                received_time_is_nan = 1



        if VTG_received and GGA_received:
            VTG_received = 0
            GGA_received = 0
            previous_received_utc_time = received_utc_time
            if gps_qual == 0:
                trustedFix.status.status = NavSatStatus.STATUS_NO_FIX
            elif gps_qual == 1:
                trustedFix.status.status = NavSatStatus.STATUS_FIX
            elif gps_qual == 2:
                trustedFix.status.status = NavSatStatus.STATUS_SBAS_FIX
            elif gps_qual in (4, 5):
                trustedFix.status.status = NavSatStatus.STATUS_GBAS_FIX
            elif gps_qual == 9:
                # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9
                # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
                trustedFix.status.status = NavSatStatus.STATUS_SBAS_FIX
            else:
                trustedFix.status.status = NavSatStatus.STATUS_NO_FIX

            trustedFix.status.service = NavSatStatus.SERVICE_GPS

            lat_error2 = lat_error*lat_error
            lon_error2 = lon_error*lon_error
            underroot = 0.5*(lat_error2 + lon_error2)

            #print "ALL :::::: 1sigma" , math.sqrt(underroot), ",2sigma" , 2*math.sqrt(underroot), ",3sigma" , 3*math.sqrt(underroot)
            #print "LAT :::::: 1sigma" , math.sqrt(underroot), ",2sigma" , 2*math.sqrt(underroot), ",3sigma" , 3*math.sqrt(underroot)
            #print "LON :::::: 1sigma" , math.sqrt(underroot), ",2sigma" , 2*math.sqrt(underroot), ",3sigma" , 3*math.sqrt(underroot)

            latitude = gga_latitude                                        
            trustedFix.latitude = latitude 
            unTrustedFix.latitude = latitude 
            trustedFloat.latitude = latitude 
            semiTrustedFloat.latitude = latitude 
            unTrustedFloat.latitude = latitude 

            allMsg.pose.pose.position.x = latitude 


            longitude = gga_longitude
            trustedFix.longitude = longitude 
            unTrustedFix.longitude = longitude
            trustedFloat.longitude = longitude 
            semiTrustedFloat.longitude = longitude 
            unTrustedFloat.longitude = longitude

            allMsg.pose.pose.position.y = longitude 


            #31Dec2019
            altitude = gga_altitude
            trustedFix.altitude = altitude 
            unTrustedFix.altitude = altitude
            trustedFloat.altitude = altitude 
            semiTrustedFloat.altitude = altitude 
            unTrustedFloat.altitude = altitude

            allMsg.pose.pose.position.z = altitude 

            newTime = self.get_clock().now().to_msg()

            trustedFix.header.stamp =newTime
            unTrustedFix.header.stamp = newTime
            trustedFloat.header.stamp = newTime
            semiTrustedFloat.header.stamp = newTime
            unTrustedFloat.header.stamp = newTime

            allMsg.header.stamp = newTime

            
            #if (badFlag != 0): 
            #    print (" setting badFlag",badFlag)
            #rospy.set_param('badGpsFlag', badFlag)
            #print (" sdone etting badFlag")
            if gps_qual == 4 and lat_error <= 0.05 and lon_error <= 0.05: #TODO tuning this number 14 Oct 2019
                gpsQuality = 4
                out_cov = 0.00001
                trustedFix.position_covariance[0] = out_cov
                trustedFix.position_covariance[4] = out_cov
                trustedFix.position_covariance[8] = 0
                trustedCount += 2
                if fixCount < 3: # To solve float to fix change in orientation
                    out_cov = 3*math.sqrt(underroot)
                    trustedFix.position_covariance[0] = out_cov
                    trustedFix.position_covariance[4] = out_cov
                    trustedFix.position_covariance[8] = 0
                fixCount += 1
                
                if started:
                    print ("Staaaaaarting... fix")
                    started=0
                if publish_gps:
                    print("trustedFix_pub") 
                    self.trustedFix_pub.publish(trustedFix)

            elif gps_qual == 4 :
                gpsQuality = 3
                badFlag += 5                
                out_cov = 3*math.sqrt(underroot)
                unTrustedFix.position_covariance[0] = out_cov 
                unTrustedFix.position_covariance[4] = out_cov
                unTrustedFix.position_covariance[8] = 0
                trustedCount = 0
                fixCount = 0
                if started:
                    print ("Staaaaaarting...fix")
                    started=0
                if publish_gps:
                    print("unTrustedFix_pub") 
                    self.unTrustedFix_pub.publish(unTrustedFix)

            elif gps_qual == 5 and lat_error < 0.1 and lon_error < 0.1:   
                gpsQuality = 5
                fixCount = 0           
                if started:
                    print ("Staaaaaarting... float")
                    started=0

                out_cov = 3*math.sqrt(underroot)
                trustedFloat.position_covariance[0] = out_cov
                trustedFloat.position_covariance[4] = out_cov
                trustedFloat.position_covariance[8] = 0
                trustedCount += 1
                if publish_gps:
                    self.trustedFloat_pub.publish(trustedFloat)
                    print("trustedFloat_pub") 
                    #trustedFloat.position_covariance[0] = 1.0
                    #trustedFloat.position_covariance[4] = 1.0
                    #trustedFloat.position_covariance[8] = 0 
                    #self.trustedFloat2_pub.publish(trustedFloat)
                                                            
            elif gps_qual == 5 and lat_error < 0.3 and lon_error < 0.3:  
                gpsQuality = 6
                fixCount = 0            
                if started:
                    print ("Staaaaaarting... float")
                    started=0
                out_cov = 3*math.sqrt(underroot)
                semiTrustedFloat.position_covariance[0] = out_cov
                semiTrustedFloat.position_covariance[4] = out_cov
                semiTrustedFloat.position_covariance[8] = 0
                trustedCount = 0
                if publish_gps:
                    print("semiTrustedFloat_pub") 
                    self.semiTrustedFloat_pub.publish(semiTrustedFloat)

            elif gps_qual == 5 :   
                gpsQuality = 7
                fixCount = 0
                badFlag += 5                            
                if started:
                    print ("Staaaaaarting... float")
                    started=0
                out_cov = 3*math.sqrt(underroot)
                unTrustedFloat.position_covariance[0] = out_cov
                unTrustedFloat.position_covariance[4] = out_cov
                unTrustedFloat.position_covariance[8] = 0
                trustedCount = 0
                if publish_gps:
                    print("unTrustedFloat_pub") 
                    self.unTrustedFloat_pub.publish(unTrustedFloat)


            else: 
                gpsQuality = 2
                fixCount = 0
                badFlag += 5                            
                if started:
                    print ("Staaaaaarting... float")
                    started=0
                out_cov = 3*math.sqrt(underroot)
                unTrustedFloat.position_covariance[0] = out_cov
                unTrustedFloat.position_covariance[4] = out_cov
                unTrustedFloat.position_covariance[8] = 0
                trustedCount = 0
                if publish_gps:
                    print("unTrustedFloat_pub") 
                    self.unTrustedFloat_pub.publish(unTrustedFloat)

            if satNo < 5 or hdop_GPGSA  > 4 or pdop_GPGSA > 8 :
                print (" **********************************************************************")
                print ("satNo , hdop_GPGSA , pdop_GPGSA ",satNo , hdop_GPGSA , pdop_GPGSA)
                print (" Very BAD GPS *********************************************************")
                badFlag += 5
                #os.system('spd-say "Warning, GPS is very bad" -t male1 -i -40 -r -10')
            elif satNo < 5 or hdop_GPGSA  > 1.8 or pdop_GPGSA > 5 :
                print( " :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::")
                print ("satNo , hdop_GPGSA , pdop_GPGSA ",satNo , hdop_GPGSA , pdop_GPGSA)
                badFlag += 1
                print (" BAD GPS :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::")
                #os.system('spd-say "Warning, GPS is bad" -t male1 -i -40 -r -10')
            elif satNo > 6 and hdop_GPGSA  < 1.7 and pdop_GPGSA < 4.3 and lat_error < 0.09 and lon_error < 0.09 and badFlag>0 :
                badFlag -= 1
           
            if trustedCount > 40 and badFlag > 0:
                badFlag = 0
                print (" resetting badFlag")

            '''
Exemplo n.º 11
0
    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)

        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)

                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)

            # Publish velocity from RMC regardless, since GGA doesn't provide
            # it.
            if data['fix_valid']:
                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)
        elif 'GST' in parsed_sentence:
            data = parsed_sentence['GST']

            # Use receiver-provided error estimate if available
            self.using_receiver_epe = True
            self.lon_std_dev = data['lon_std_dev']
            self.lat_std_dev = data['lat_std_dev']
            self.alt_std_dev = data['alt_std_dev']
        elif 'HDT' in parsed_sentence:
            data = parsed_sentence['HDT']
            if data['heading']:
                current_heading = QuaternionStamped()
                current_heading.header.stamp = current_time
                current_heading.header.frame_id = frame_id
                q = quaternion_from_euler(0, 0, math.radians(data['heading']))
                current_heading.quaternion.x = q[0]
                current_heading.quaternion.y = q[1]
                current_heading.quaternion.z = q[2]
                current_heading.quaternion.w = q[3]
                self.heading_pub.publish(current_heading)
        else:
            return False
Exemplo n.º 12
0
    def add_sentence_original(self, nmea_string, frame_id, timestamp=None):

        if not check_nmea_checksum(nmea_string):
            self.get_logger().warn("Received a sentence with an invalid checksum. " +
                                   "Sentence was: %s" % nmea_string)
            return False

        parsed_sentence = parser.parse_nmea_sentence(nmea_string)
        if not parsed_sentence:
            self.get_logger().debug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string)
            return False

        if timestamp:
            current_time = timestamp
        else:
            current_time = self.get_clock().now().to_msg()

        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 current_fix.status.status > 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
            print("add sentence gga")
            self.trustedFix_pub.publish(current_fix)

            if not math.isnan(data['utc_time']):
                current_time_ref.time_ref = rclpy.time.Time(seconds=data['utc_time']).to_msg()
                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)

        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.trustedFix_pub.publish(current_fix)

                if not math.isnan(data['utc_time']):
                    current_time_ref.time_ref = rclpy.time.Time(seconds=data['utc_time']).to_msg()
                    self.time_ref_pub.publish(current_time_ref)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                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)
        elif 'GST' in parsed_sentence:
            data = parsed_sentence['GST']

            # Use receiver-provided error estimate if available
            self.using_receiver_epe = True
            self.lon_std_dev = data['lon_std_dev']
            self.lat_std_dev = data['lat_std_dev']
            self.alt_std_dev = data['alt_std_dev']
        elif 'HDT' in parsed_sentence:
            data = parsed_sentence['HDT']
            if data['heading']:
                current_heading = QuaternionStamped()
                current_heading.header.stamp = current_time
                current_heading.header.frame_id = frame_id
                q = quaternion_from_euler(0, 0, math.radians(data['heading']))
                current_heading.quaternion.x = q[0]
                current_heading.quaternion.y = q[1]
                current_heading.quaternion.z = q[2]
                current_heading.quaternion.w = q[3]
                self.heading_pub.publish(current_heading)
        else:
            return False
Exemplo n.º 13
0
    def add_sentence(self, nmea_string, frame_id, timestamp=None):
        if not check_nmea_checksum(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. 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
            elif gps_qual == 9:
                # Support specifically for NOVATEL OEM4 recievers
                # which report WAAS fix as 9
                # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
                current_fix.status.status = NavSatStatus.STATUS_SBAS_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']
            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)

                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)

            # Publish velocity from RMC regardless,
            # since GGA doesn't provide it.
            if data['fix_valid']:
                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)
        else:
            return False
Exemplo n.º 14
0
    def add_sentence(self, nmea_string, timestamp):

        time = 0.0
        fix = False
        NumSat = 0
        latitude = 0.0
        longitude = 0.0
        altitude = 0.0
        speed = 0.0
        ground_course = 0.0
        covariance = 0.0

        if not check_nmea_checksum(nmea_string):
            logger.debug("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:
            logger.debug("Failed to parse NMEA sentence. Sentece was: %s" %
                         nmea_string)
            return False

        if 'RMC' in parsed_sentence:
            data = parsed_sentence['RMC']
            if data['fix_valid']:
                self.speed = data['speed']
                if not math.isnan(data['true_course']):
                    self.ground_course = data['true_course']

        if not self.use_RMC and 'GGA' in parsed_sentence:
            data = parsed_sentence['GGA']
            gps_qual = data['fix_type']
            if gps_qual > 0:
                fix = True
                NumSat = data['num_satellites']

            time = timestamp

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

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

            hdop = data['hdop']
            covariance = hdop**2
            # position_covariance[4] = hdop ** 2
            # position_covariance[8] = (2 * hdop) ** 2  # FIXME
            # position_covariance_type = \

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

            speed = self.speed
            ground_course = self.ground_course

            return (time, fix, NumSat, latitude, longitude, altitude, speed,
                    ground_course, covariance)

        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:
                time = timestamp
                fix = True
                NumSat = data['num_satellites']

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

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

                altitude = float('NaN')

                # position_covariance_type = \
                if data['fix_valid']:
                    speed = data['speed']
                    if not math.isnan(data['true_course']):
                        ground_course = data['true_course']
                    else:
                        ground_course = self.ground_course

                return (time, fix, NumSat, latitude, longitude, altitude,
                        speed, ground_course, covariance)
            return False

        else:
            return False
Exemplo n.º 15
0
    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. 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

        gpsfix = self.gpsfix
        gpsfix.header.stamp = current_time
        gpsfix.header.frame_id = 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
            elif gps_qual == 9:
                # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9
                # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
                current_fix.status.status = NavSatStatus.STATUS_SBAS_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']
            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)

            gpsfix.hdop = data['hdop']
            gpsfix.time = data['utc_time']

        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)

                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)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                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)
                gpsfix.speed = data['speed']
                gpsfix.track = math.degrees(data['true_course'])
        elif 'GSV' in parsed_sentence:
            data = parsed_sentence['GSV']
            msg = Gpgsv()
            msg.header.stamp = rospy.Time.now()
            msg.n_msgs = data['n_msgs']
            msg.msg_number = data['msg_number']
            msg.n_satellites = data['n_satellites']

            gpsfix.status.satellites_visible = msg.n_satellites

            if data['msg_number'] == 1:
                gpsfix.status.satellite_visible_prn = []
                gpsfix.status.satellite_visible_z = []
                gpsfix.status.satellite_visible_azimuth = []
                gpsfix.status.satellite_visible_snr = []

            for i in range(0, 4):
                try:
                    sat = GpgsvSatellite(data['prn%d' % i],
                                         data['elevation%d' % i],
                                         data['azimuth%d' % i],
                                         data['snr%d' % i])
                    msg.satellites.append(sat)
                    gpsfix.status.satellite_visible_prn.append(data['prn%d' %
                                                                    i])
                    gpsfix.status.satellite_visible_z.append(
                        data['elevation%d' % i])
                    gpsfix.status.satellite_visible_azimuth.append(
                        data['azimuth%d' % i])
                    gpsfix.status.satellite_visible_snr.append(data['snr%d' %
                                                                    i])
                except:
                    pass

            self.sat_pub.publish(msg)

        elif 'GSA' in parsed_sentence:
            data = parsed_sentence['GSA']
            gpsfix.pdop = data['pdop']
            gpsfix.hdop = data['hdop']
            gpsfix.vdop = data['vdop']
            gpsfix.status.satellites_used = len(data['prns'])
            gpsfix.status.satellite_used_prn = data['prns']

        else:
            return False

        if ('RMC' in parsed_sentence
                and self.use_RMC) or ('GGA' in parsed_sentence
                                      and not self.use_RMC):
            gpsfix.latitude = current_fix.latitude
            gpsfix.longitude = current_fix.longitude
            gpsfix.altitude = current_fix.altitude
            gpsfix.status.status = current_fix.status.status
            gpsfix.status.position_source = gpsfix.status.orientation_source = gpsfix.status.motion_source = current_fix.status.service
            self.gpsfix_pub.publish(gpsfix)
Exemplo n.º 16
0
    def add_sentence(self,
                     message,
                     frame_id,
                     timestamp=None,
                     sentence_type="NMEA",
                     ubx_message_definitions=None):
        """Public method to provide a new sentence to the driver.

        Args:
            message (str): NMEA or UBX sentence in string form.
            frame_id (str): TF frame ID of the GPS receiver.
            timestamp (rospy.Time, optional): Time the sentence was received.
                If timestamp is not specified, the current time is used.
            sentence_type (str, optional): NMEA or UBX
            ubx_message_definitions (str, optional): Message definitions for UBX sentences

        Return:
            bool: True if the NMEA string is successfully processed, False if there is an error.
        """
        if not "UBX" == sentence_type:
            if not check_nmea_checksum(message):
                rospy.logwarn(
                    "Received a sentence with an invalid checksum. " +
                    "Sentence was: %s" % repr(message))
                return False

            parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
                message)
            if not parsed_sentence:
                rospy.logdebug(
                    "Failed to parse NMEA sentence. Sentence was: %s" %
                    message)
                return False
        elif "UBX" == sentence_type:
            parsed_sentence = libnmea_navsat_driver.parser_ubx.parse_ubx_sentence(
                message, ubx_message_definitions)
            if not parsed_sentence:
                rospy.logdebug(
                    "Failed to parse UBX sentence. Sentence was: %s" % message)
                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
        if not self.use_GNSS_time:
            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']

            if self.use_GNSS_time:
                if math.isnan(data['utc_time'][0]):
                    rospy.logwarn("Time in the NMEA sentence is NOT valid")
                    return False
                current_fix.header.stamp = rospy.Time(data['utc_time'][0],
                                                      data['utc_time'][1])

            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]

            self.valid_fix = (fix_type > 0)

            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'][0]) or self.use_GNSS_time):
                current_time_ref.time_ref = rospy.Time(data['utc_time'][0],
                                                       data['utc_time'][1])
                self.last_valid_fix_time = current_time_ref
                self.time_ref_pub.publish(current_time_ref)

            if self.use_extended_nmea_messages:
                info = NavSatInfo()
                info.header.stamp = current_time
                info.header.frame_id = frame_id
                info.num_satellites = data['num_satellites']
                info.last_dgps_update = data['age_dgps']
                self.age_dgps = data['age_dgps']
                info.hdop = data['hdop']
                self.info_pub.publish(info)

        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)

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

            if self.use_GNSS_time:
                if math.isnan(data['utc_time'][0]):
                    rospy.logwarn("Time in the NMEA sentence is NOT valid")
                    return False
                current_fix.header.stamp = rospy.Time(data['utc_time'][0],
                                                      data['utc_time'][1])

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

                if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
                    current_time_ref.time_ref = rospy.Time(
                        data['utc_time'][0], data['utc_time'][1])
                    self.time_ref_pub.publish(current_time_ref)

            # Publish velocity from RMC regardless, since GGA doesn't provide
            # it.
            if data['fix_valid']:
                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)

        elif 'GRS' in parsed_sentence and self.use_extended_nmea_messages:
            data = parsed_sentence['GRS']
            msg = NavSatGRS()
            msg.header.stamp = current_time
            msg.header.frame_id = frame_id

            msg.utc_time = data['utc_time']
            msg.mode = data['mode']
            msg.residual_01 = data['residual_01']
            msg.residual_02 = data['residual_02']
            msg.residual_03 = data['residual_03']
            msg.residual_04 = data['residual_04']
            msg.residual_05 = data['residual_05']
            msg.residual_06 = data['residual_06']
            msg.residual_07 = data['residual_07']
            msg.residual_08 = data['residual_08']
            msg.residual_09 = data['residual_09']
            msg.residual_10 = data['residual_10']
            msg.residual_11 = data['residual_11']
            # The following two attributes have been introduced with NMEA 4.11
            msg.system_id = data['system_id']
            msg.signal_id = data['signal_id']
            self.grs_pub.publish(msg)

        elif 'GST' in parsed_sentence and self.use_extended_nmea_messages:
            data = parsed_sentence['GST']

            # Use receiver-provided error estimate if available
            self.using_receiver_epe = True
            self.lon_std_dev = data['lon_std_dev']
            self.lat_std_dev = data['lat_std_dev']
            self.alt_std_dev = data['alt_std_dev']

            msg = NavSatGST()
            msg.header.stamp = current_time
            msg.header.frame_id = frame_id
            msg.utc_time = data['utc_time']
            msg.ranges_std_dev = data['ranges_std_dev']
            msg.semi_major_ellipse_std_dev = data['semi_major_ellipse_std_dev']
            msg.semi_minor_ellipse_std_dev = data['semi_minor_ellipse_std_dev']
            msg.semi_major_orientation = data['semi_major_orientation']
            msg.lat_std_dev = data['lat_std_dev']
            msg.lon_std_dev = data['lon_std_dev']
            msg.alt_std_dev = data['alt_std_dev']
            self.gst_pub.publish(msg)

        elif 'HDT' in parsed_sentence:
            data = parsed_sentence['HDT']
            if data['heading']:
                current_heading = QuaternionStamped()
                current_heading.header.stamp = current_time
                current_heading.header.frame_id = frame_id
                q = quaternion_from_euler(0, 0, math.radians(data['heading']))
                current_heading.quaternion.x = q[0]
                current_heading.quaternion.y = q[1]
                current_heading.quaternion.z = q[2]
                current_heading.quaternion.w = q[3]
                self.heading_pub.publish(current_heading)

        elif self.use_trimble_messages and 'PTNL,VHD' in parsed_sentence:
            data = parsed_sentence['PTNL,VHD']
            msg = NavSatTrimbleHeading()
            msg.header.stamp = current_time
            msg.header.frame_id = frame_id

            msg.azimuth = data['azimuth']
            msg.azimuth_rate = data['azimuth_rate']
            msg.vertical_angle = data['vertical_angle']
            msg.vertical_angle_rate = data['vertical_angle_rate']
            msg.range = data['range']
            msg.range_rate = data['range_rate']
            # explanation of the status codes in message definition
            msg.status = data['fix_type']
            msg.num_satellites = data['num_satellites']
            msg.pdop = data['pdop']

            self.trimble_heading_pub.publish(msg)

        elif self.use_trimble_messages and 'PTNL,AVR' in parsed_sentence:
            data = parsed_sentence['PTNL,AVR']
            msg = NavSatTrimbleMovingBase()
            msg.header.stamp = current_time
            msg.header.frame_id = frame_id

            msg.yaw = data['yaw']
            if self.ccw_heading:
                msg.yaw = -msg.yaw
            msg.yaw += self.heading_offset
            # wrap yaw angle to [-pi, pi)
            msg.yaw = (msg.yaw + math.pi) % (2 * math.pi) - math.pi
            msg.tilt = data['tilt']
            msg.roll = data['roll']
            msg.range = data['range']
            # explanation of the status codes in message definition
            msg.status = data['fix_type']
            msg.pdop = data['pdop']
            msg.num_satellites = data['num_satellites']

            self.trimble_moving_base_pub.publish(msg)

            # An dual antenna system can only measure tow out of the
            # three angles tilt, roll, yaw. Yaw is always measured.
            # Assuming all not measured angles are zero.
            if math.isnan(msg.tilt):
                msg.tilt = 0.
            if math.isnan(msg.roll):
                msg.roll = 0.
            msg2 = Imu()
            msg2.header.stamp = current_time
            msg2.header.frame_id = frame_id
            q = quaternion_from_euler(msg.tilt, msg.roll, msg.yaw)
            msg2.orientation.x = q[0]
            msg2.orientation.y = q[1]
            msg2.orientation.z = q[2]
            msg2.orientation.w = q[3]

            # Calculate the orientation error estimate based on the position
            # error estimates and the baseline. Very conservative by taking the
            # largest possible error at both ends in both directions. Set high
            # error if there is no proper vector fix (msg.status is not 3).
            if msg.status == 3:
                total_position_error = math.sqrt(self.lat_std_dev**2 +
                                                 self.lon_std_dev**2)
                angular_error_estimate = math.atan2(total_position_error * 2,
                                                    msg.range)
                angular_error_cov = angular_error_estimate**2
            else:
                angular_error_cov = 1

            msg2.orientation_covariance = [
                angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov, 0.0, 0.0,
                0.0, angular_error_cov
            ]
            msg2.angular_velocity_covariance = [
                -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
            ]
            msg2.linear_acceleration_covariance = [
                -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
            ]
            self.trimble_moving_base_imu_pub.publish(msg2)

        # Documentation source:
        # https://www.u-blox.com/sites/default/files/u-blox_ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf
        # Some comments have been directly copied from the documentation
        elif self.use_ublox_messages and 'PUBX,00' in parsed_sentence:
            data = parsed_sentence['PUBX,00']
            msg = NavSatUbloxPubxPosition()
            msg.header.stamp = current_time
            msg.header.frame_id = frame_id

            msg.utc_time = rospy.Time(data['utc_time'][0], data['utc_time'][1])
            msg.latitude = data['latitude']
            if data['latitude_direction'] == 'S':
                msg.latitude = -msg.latitude
            msg.longitude = data['longitude']
            if data['longitude_direction'] == 'W':
                msg.longitude = -msg.longitude
            msg.altitude = data['altitude']
            msg.nav_stat = data['nav_stat']
            msg.h_acc = data['h_acc']
            msg.v_acc = data['v_acc']
            msg.sog = data['sog'] / 3.6
            msg.cog = data['cog'] / 180.0 * math.pi
            msg.v_vel = data['v_vel']
            msg.diff_age = data['diff_age']
            msg.hdop = data['hdop']
            msg.vdop = data['vdop']
            msg.tdop = data['tdop']
            msg.num_svs = data['num_svs']
            msg.dr = data['dr']

            self.ublox_pubx_position_pub.publish(msg)

        # Documentation source:
        # https://www.u-blox.com/sites/default/files/u-blox_ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf
        # Some comments have been directly copied from the documentation
        elif self.use_ublox_messages and 'UBX-NAV-RELPOSNED' in parsed_sentence:
            data = parsed_sentence['UBX-NAV-RELPOSNED']
            msg = NavSatUbloxRelPos()
            msg.header.stamp = current_time
            msg.header.frame_id = frame_id

            msg.tow = data['iTOW'] * 0.001
            msg.n = data['relPosN'] * 0.01 + data['relPosHPN'] * 0.0001
            msg.e = data['relPosE'] * 0.01 + data['relPosHPE'] * 0.0001
            msg.d = data['relPosD'] * 0.01 + data['relPosHPD'] * 0.0001
            msg.length = data['relPosLength'] * 0.01 + data[
                'relPosHPLength'] * 0.0001
            msg.heading = data['relPosHeading'] * 0.00001 / 180.0 * math.pi
            if self.ccw_heading:
                msg.heading = -msg.heading
            # only add offset if heading is valid - invalid heading should always be 0.0
            data['flags'] = format(data['flags'], '032b')
            if 1 == int(data['flags'][23], 2):
                msg.heading += self.heading_offset
            # wrap yaw angle to [-pi, pi)
            msg.heading = (msg.heading + math.pi) % (2 * math.pi) - math.pi
            msg.acc_n = data['accN'] * 0.0001
            msg.acc_e = data['accE'] * 0.0001
            msg.acc_d = data['accD'] * 0.0001
            msg.acc_length = data['accLength'] * 0.0001
            msg.acc_heading = data['accHeading'] * 0.00001 / 180.0 * math.pi

            msg.rel_pos_heading_valid = int(data['flags'][23], 2)
            msg.ref_obs_miss = int(data['flags'][24], 2)
            msg.ref_pos_miss = int(data['flags'][25], 2)
            msg.is_moving = int(data['flags'][26], 2)
            msg.carr_soln = int(data['flags'][27:29], 2)
            msg.rel_pos_valid = int(data['flags'][29], 2)
            msg.diff_soln = int(data['flags'][30], 2)
            msg.gnss_fix_ok = int(data['flags'][31], 2)

            # accuracy estimates in non RTK fixed modes shouldn't be relied on
            if msg.carr_soln == 2 and msg.rel_pos_valid == 1:
                self.lat_std_dev = msg.acc_n
                self.lon_std_dev = msg.acc_e
                self.alt_std_dev = msg.acc_d
            elif msg.carr_soln == 1 and msg.rel_pos_valid == 1:
                self.lat_std_dev = self.default_epe_quality5
                self.lon_std_dev = self.default_epe_quality5
                self.alt_std_dev = self.default_epe_quality5
            else:
                self.lat_std_dev = self.default_epe_quality1
                self.lon_std_dev = self.default_epe_quality1
                self.alt_std_dev = self.default_epe_quality1

            self.ublox_relpos_pub.publish(msg)

            # report erroneous messages
            if msg.carr_soln != 0:
                if msg.ref_obs_miss == 1 \
                  or msg.ref_pos_miss == 1 \
                  or msg.rel_pos_valid == 0 \
                  or msg.gnss_fix_ok == 0:
                    rospy.logerr(
                        "GNSS receiver failed to calculate GNSS fix despite available correction data. "
                        "Consider lowering the frequency, receiver load might be too high."
                    )
                    return False

            # publish odometry and IMU messages for fusion in case message is fine
            if msg.carr_soln == 0:
                if msg.diff_soln == 1:
                    rospy.logwarn_throttle(
                        10, "Unable to calculate RTK solution.")

                return False

            msg2 = Odometry()
            msg2.header.stamp = current_time
            msg2.header.frame_id = frame_id

            # follow enu conventions
            msg2.pose.pose.position.x = msg.e
            msg2.pose.pose.position.y = msg.n
            msg2.pose.pose.position.z = -msg.d
            msg2.pose.covariance[0] = self.lon_std_dev**2
            msg2.pose.covariance[7] = self.lat_std_dev**2
            msg2.pose.covariance[14] = self.alt_std_dev**2

            # output heading once in moving base mode
            if msg.is_moving == 1:
                # take the already inverted heading from above and not the raw
                # value
                q = quaternion_from_euler(0., 0., msg.heading)
                msg2.pose.pose.orientation.x == q[0]
                msg2.pose.pose.orientation.y == q[1]
                msg2.pose.pose.orientation.z == q[2]
                msg2.pose.pose.orientation.w == q[3]

                # degrade estimated when not in RTK fixed mode
                msg2.pose.covariance[21] = msg.acc_heading**2 + (2 -
                                                                 msg.carr_soln)
                msg2.pose.covariance[28] = msg.acc_heading**2 + (2 -
                                                                 msg.carr_soln)
                msg2.pose.covariance[35] = msg.acc_heading**2 + (2 -
                                                                 msg.carr_soln)
            else:
                msg2.pose.covariance[21] = -1.0
                msg2.pose.covariance[28] = -1.0
                msg2.pose.covariance[35] = -1.0

            self.ublox_relpos_odom_pub.publish(msg2)

            # Publish IMU message if in moving base mode
            if msg.is_moving == 1:
                msg3 = Imu()
                msg3.header.stamp = current_time
                msg3.header.frame_id = frame_id
                xyzw = quaternion_from_euler(0.0, 0.0, msg.heading)
                msg3.orientation.x = xyzw[0]
                msg3.orientation.y = xyzw[1]
                msg3.orientation.z = xyzw[2]
                msg3.orientation.w = xyzw[3]

                if msg.carr_soln == 2:
                    angular_error_cov = msg.acc_heading**2
                else:
                    angular_error_cov = msg.acc_heading**2 + (2 -
                                                              msg.carr_soln)

                msg3.orientation_covariance = [
                    angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov, 0.0,
                    0.0, 0.0, angular_error_cov
                ]
                msg3.angular_velocity_covariance = [
                    -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
                ]
                msg3.linear_acceleration_covariance = [
                    -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
                ]
                self.ublox_relpos_imu_pub.publish(msg3)

        elif self.use_ublox_messages and 'UBX-NAV-GEOFENCE' in parsed_sentence:
            data = parsed_sentence['UBX-NAV-GEOFENCE']
            msg = NavSatUbloxGeoFence()
            msg.header.stamp = current_time
            msg.header.frame_id = frame_id

            msg.tow = data['iTOW'] * 0.001
            msg.status = data['status']
            msg.num_fences = data['numFences']
            msg.comb_state = data['combState']
            msg.status = data['status']
            # The original message is variable length, so all fences are
            # initialized with "deactivated" state.
            msg.fence1 = NavSatUbloxGeoFence.DEACTIVATED
            msg.fence2 = NavSatUbloxGeoFence.DEACTIVATED
            msg.fence3 = NavSatUbloxGeoFence.DEACTIVATED
            msg.fence4 = NavSatUbloxGeoFence.DEACTIVATED
            try:
                msg.fence1 = data['state'][0]
                msg.fence2 = data['state'][1]
                msg.fence3 = data['state'][2]
                msg.fence4 = data['state'][3]
            except KeyError:
                pass

            self.ublox_geofence_pub.publish(msg)

        elif self.use_ublox_messages and 'UBX-NAV-PVT' in parsed_sentence:
            data = parsed_sentence['UBX-NAV-PVT']
            msg = NavSatUbloxPositionVelocityTime()
            msg.header.stamp = current_time
            msg.header.frame_id = frame_id

            msg.tow = data['iTOW'] * 0.001
            msg.year = data['year']
            msg.month = data['month']
            msg.day = data['day']
            msg.hour = data['hour']
            msg.min = data['min']
            msg.sec = data['sec']

            bitfield_valid = format(data['valid'], '08b')
            msg.valid_date = int(bitfield_valid[7], 2)
            msg.valid_time = int(bitfield_valid[6], 2)
            msg.fully_resolved = int(bitfield_valid[5], 2)
            msg.valid_mag = int(bitfield_valid[4], 2)

            msg.t_acc = data['tAcc'] / 1.e9
            msg.nano = data['nano']
            msg.fix_type = data['fixType']

            bitfield_flags = format(data['flags'], '08b')
            msg.gnss_fix_ok = int(bitfield_flags[7], 2)
            msg.diff_soln = int(bitfield_flags[6], 2)
            msg.psm_state = int(bitfield_flags[3:6], 2)
            msg.head_veh_valid = int(bitfield_flags[2], 2)
            msg.carr_soln = int(bitfield_flags[0:2], 2)

            bitfield_flags2 = format(data['flags2'], '08b')
            msg.confirmed_avai = int(bitfield_flags2[2], 2)
            msg.confirmed_date = int(bitfield_flags2[1], 2)
            msg.confirmed_time = int(bitfield_flags2[0], 2)

            msg.num_sv = data['numSV']
            msg.lon = data['lon'] / 1.e7
            msg.lat = data['lat'] / 1.e7
            msg.height = data['height'] / 1.e3
            msg.h_msl = data['hMSL'] / 1.e3
            msg.h_acc = data['hAcc'] / 1.e3
            msg.v_acc = data['vAcc'] / 1.e3
            msg.vel_n = data['velN'] / 1.e3
            msg.vel_e = data['velE'] / 1.e3
            msg.vel_d = data['velD'] / 1.e3
            msg.g_speed = data['gSpeed'] / 1.e3
            msg.head_mot = data['headMot'] / 1.e5 / 180.0 * math.pi
            msg.s_acc = data['sAcc'] / 1.e3
            msg.head_acc = data['headAcc'] / 1.e5 / 180.0 * math.pi
            msg.p_dop = data['pDOP'] / 1.e2
            msg.head_veh = data['headVeh'] / 1.e5 / 180.0 * math.pi
            msg.mag_dec = data['magDec'] / 1.e2 / 180.0 * math.pi
            msg.mag_acc = data['magAcc'] / 1.e2 / 180.0 * math.pi

            self.ublox_position_velocity_time_pub.publish(msg)

        elif self.use_ublox_messages and 'UBX-RAW' in parsed_sentence:
            data = parsed_sentence['UBX-RAW']
            msg = String()
            # transmitting binary data (bytes) directly is not possible anymore
            # with ROS and Python3, therefore base64 encoding is applied
            msg.data = base64.b64encode(data['raw']).decode('ascii')
            self.ublox_raw.publish(msg)

        else:
            return False
Exemplo n.º 17
0
    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.seq = self.seq
        self.seq += 1
        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];

            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)

            # Publish GPGGA msgs
            gpgga_msg = Gpgga()
            gpgga_msg.header.stamp = current_time
            gpgga_msg.header.frame_id = frame_id

            gpgga_msg.message_id = data['sent_id']
            gpgga_msg.utc_seconds = data['utc_time']
            gpgga_msg.lat = latitude
            gpgga_msg.lon = longitude
            gpgga_msg.lat_dir = data['latitude_direction']
            gpgga_msg.lon_dir = data['longitude_direction']
            gpgga_msg.gps_qual = data['fix_type']
            gpgga_msg.num_sats = data['num_satellites']
            gpgga_msg.hdop = hdop
            gpgga_msg.alt = data['altitude']
            gpgga_msg.altitude_units = data['altitude_units']
            gpgga_msg.undulation = data['mean_sea_level']
            gpgga_msg.undulation_units = data['mean_sea_level_units']
            gpgga_msg.diff_age = data['diff_age']*10
            gpgga_msg.station_id = data['station_id']

            self.gpgga_pub.publish(gpgga_msg)

            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)

                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)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                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)
        elif 'GST' in parsed_sentence:
            data = parsed_sentence['GST']
            # Use receiver-provided error estimate if available
            self.using_receiver_epe = True
            self.lon_std_dev = data['lon_std_dev']
            self.lat_std_dev = data['lat_std_dev']
            self.alt_std_dev = data['alt_std_dev']
        else:
            return False
Exemplo n.º 18
0
    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
            altitude = data['altitude'] + data['mean_sea_level']
            current_fix.altitude = altitude

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

            self.fix_pub.publish(current_fix)
            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

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

                self.fix_pub.publish(current_fix)
                self.time_ref_pub.publish(current_time_ref)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                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)
        else:
            return False
Exemplo n.º 19
0
    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. 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
            elif gps_qual == 9:
                # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9
                # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
                current_fix.status.status = NavSatStatus.STATUS_SBAS_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']
            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)

                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)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                current_vel_global = Vector3Stamped()
                current_vel_global.header.stamp = current_time
                current_vel_global.header.frame_id = frame_id
                current_vel_global.vector.x = data['speed'] * \
                    math.sin(data['true_course'])
                current_vel_global.vector.y = data['speed'] * \
                    math.cos(data['true_course'])
                self.vel_pub.publish(current_vel_global)
                current_vel_local = TwistStamped()
                current_vel_local.header.stamp = current_time
                current_vel_local.header.frame_id = frame_id
                current_vel_local.twist.linear.x = data['speed']
                self.vel_pub2.publish(current_vel_local)
        elif 'HDT' in parsed_sentence:
            data = parsed_sentence['HDT']
            if data['heading_north']:
                self.hdt_pub.publish(data['heading_north'])
        elif 'ROT' in parsed_sentence:
            data = parsed_sentence['ROT']
            current_rot = TwistStamped()
            current_rot.header.stamp = current_time
            current_rot.header.frame_id = frame_id
            current_rot.twist.angular.z = data['rate'] * 3.14 / 180 * 60
            self.rot_pub.publish(current_rot)
        elif 'VTG' in parsed_sentence:
            data = parsed_sentence['VTG']
            current_speed = TwistStamped()
            current_speed.header.stamp = current_time
            current_speed.header.frame_id = frame_id
            current_speed.twist.linear.x = data['speed_kph'] / 360 * 1000
            self.speed_pub.publish(current_speed)
        elif 'AVR' in parsed_sentence:
            data = parsed_sentence['AVR']
            current_pyr = TwistStamped()
            current_pyr.header.stamp = current_timecur
            current_pyr.header.frame_id = frame_id
            current_pyr.twist.angular.z = data['yaw']
            current_pyr.twist.angular.y = data['pitch']
            current_pyr.twist.angular.x = data['roll']
            if data['fix_type'] > 0:
                self.pyr_pub.publish(current_pyr)
        else:
            return False
Exemplo n.º 20
0
    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
            altitude = data['altitude'] + data['mean_sea_level']
            current_fix.altitude = altitude

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

            self.fix_pub.publish(current_fix)
            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

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

                self.fix_pub.publish(current_fix)
                self.time_ref_pub.publish(current_time_ref)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                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)
        else:
            return False
Exemplo n.º 21
0
    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. 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']
            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)

                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)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if data['fix_valid']:
                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)
        else:
            return False
Exemplo n.º 22
0
    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. 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 'B' in parsed_sentence:
            data = parsed_sentence['B']

            # Only publish a fix from RMC if the use_RMC flag is set.
            if True:

                current_fix.status.status = NavSatStatus.STATUS_FIX
                current_fix.status.service = NavSatStatus.SERVICE_GPS

                current_fix.latitude = data['latitude']

                current_fix.longitude = data['longitude']

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

                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)

            # Publish velocity from RMC regardless, since GGA doesn't provide it.
            if True:
                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)
        else:
            return False
Exemplo n.º 23
0
    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

        rospy.logdebug(parsed_sentence)

        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

        self.extend_fix.header.stamp = current_time
        self.extend_fix.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)

            # set extend fix
            self.extend_fix.status.header.stamp = current_time
            self.extend_fix.status.header.frame_id = frame_id
            self.extend_fix.status.status = gps_qual[1]
            self.extend_fix.status.satellites_used = data['num_satellites']
            self.extend_fix.status.motion_source = GPSStatus.SOURCE_GPS
            self.extend_fix.status.orientation_source = GPSStatus.SOURCE_GPS
            self.extend_fix.status.position_source = GPSStatus.SOURCE_GPS
            self.extend_fix.latitude = current_fix.latitude
            self.extend_fix.longitude = current_fix.longitude
            self.extend_fix.altitude = current_fix.altitude
            self.extend_fix.position_covariance = current_fix.position_covariance
            self.position_covariance_type = current_fix.position_covariance_type

            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)
                self.extend_fix.time = current_time_ref.time_ref.to_sec()

        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)
                self.extend_fix.track = data['true_course']
                self.extend_fix.speed = data['speed']
            self.extend_fix_pub.publish(self.extend_fix)

        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)

                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)

            # Publish velocity from RMC regardless, since GGA doesn't provide
            # it.
            if data['fix_valid']:
                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)
                self.extend_fix.track = data['true_course']
        elif 'GST' in parsed_sentence:
            data = parsed_sentence['GST']

            # Use receiver-provided error estimate if available
            self.using_receiver_epe = True
            self.lon_std_dev = data['lon_std_dev']
            self.lat_std_dev = data['lat_std_dev']
            self.alt_std_dev = data['alt_std_dev']
        elif 'HDT' in parsed_sentence:
            data = parsed_sentence['HDT']
            if data['heading']:
                current_heading = QuaternionStamped()
                current_heading.header.stamp = current_time
                current_heading.header.frame_id = frame_id
                q = quaternion_from_euler(0, 0, math.radians(data['heading']))
                current_heading.quaternion.x = q[0]
                current_heading.quaternion.y = q[1]
                current_heading.quaternion.z = q[2]
                current_heading.quaternion.w = q[3]
                self.heading_pub.publish(current_heading)
        elif 'GSA' in parsed_sentence:
            data = parsed_sentence['GSA']
            self.star_use_gps = [
                data['sate_id1'], data['sate_id2'], data['sate_id3'],
                data['sate_id4'], data['sate_id5'], data['sate_id6'],
                data['sate_id7'], data['sate_id8'], data['sate_id9'],
                data['sate_id10'], data['sate_id11'], data['sate_id12']
            ]
            self.star_use_gps = filter(lambda star: star != 0,
                                       self.star_use_gps)
            self.extend_fix.pdop = data['pdop']
            self.extend_fix.hdop = data['hdop']
            self.extend_fix.vdop = data['vdop']
        elif 'BDGSA' in parsed_sentence:
            data = parsed_sentence['BDGSA']
            self.star_use_bd = [
                data['sate_id1'], data['sate_id2'], data['sate_id3'],
                data['sate_id4'], data['sate_id5'], data['sate_id6'],
                data['sate_id7'], data['sate_id8'], data['sate_id9'],
                data['sate_id10'], data['sate_id11'], data['sate_id12']
            ]
            self.star_use_bd = filter(lambda star: star != 0, self.star_use_bd)
            self.extend_fix.pdop = data['pdop']
            self.extend_fix.hdop = data['hdop']
            self.extend_fix.vdop = data['vdop']
            self.extend_fix.status.satellite_used_prn = self.star_use_gps + self.star_use_bd

        elif 'GSV' in parsed_sentence:
            data = parsed_sentence['GSV']
            if data['index'] == 1:
                self.star_map_gps = []
            self.star_map_gps.append({
                'id': data['id_satellites1'],
                'elevation': data['elevation_satellites1'],
                'azimuth': data['azimuth_satellites1'],
                'snr': data['snr1']
            })
            self.star_map_gps.append({
                'id': data['id_satellites2'],
                'elevation': data['elevation_satellites2'],
                'azimuth': data['azimuth_satellites2'],
                'snr': data['snr2']
            })
            self.star_map_gps.append({
                'id': data['id_satellites3'],
                'elevation': data['elevation_satellites3'],
                'azimuth': data['azimuth_satellites3'],
                'snr': data['snr3']
            })
            self.star_map_gps.append({
                'id': data['id_satellites4'],
                'elevation': data['elevation_satellites4'],
                'azimuth': data['azimuth_satellites4'],
                'snr': data['snr4']
            })
            self.star_map_gps = filter(lambda star: star['id'] != 0,
                                       self.star_map_gps)
        elif 'BDGSV' in parsed_sentence:
            data = parsed_sentence['BDGSV']
            if data['index'] == 1:
                self.star_map_bd = []
            self.star_map_bd.append({
                'id': data['id_satellites1'],
                'elevation': data['elevation_satellites1'],
                'azimuth': data['azimuth_satellites1'],
                'snr': data['snr1']
            })
            self.star_map_bd.append({
                'id': data['id_satellites2'],
                'elevation': data['elevation_satellites2'],
                'azimuth': data['azimuth_satellites2'],
                'snr': data['snr2']
            })
            self.star_map_bd.append({
                'id': data['id_satellites3'],
                'elevation': data['elevation_satellites3'],
                'azimuth': data['azimuth_satellites3'],
                'snr': data['snr3']
            })
            self.star_map_bd.append({
                'id': data['id_satellites4'],
                'elevation': data['elevation_satellites4'],
                'azimuth': data['azimuth_satellites4'],
                'snr': data['snr4']
            })
            self.star_map_bd = filter(lambda star: star['id'] != 0,
                                      self.star_map_bd)
            self.star_map = self.star_map_gps + self.star_map_bd
            if data['length'] == data['index']:
                self.extend_fix.status.satellites_visible = len(self.star_map)
                self.extend_fix.status.satellite_visible_prn = [
                    star['id'] for star in self.star_map
                ]
                self.extend_fix.status.satellite_visible_snr = [
                    star['snr'] for star in self.star_map
                ]
                self.extend_fix.status.satellite_visible_azimuth = [
                    star['azimuth'] for star in self.star_map
                ]
                self.extend_fix.status.satellite_visible_z = [
                    star['elevation'] for star in self.star_map
                ]
        else:
            return False
Exemplo n.º 24
0
    #useRMC == False -> generate info from GGA
    navData = NavSatFix()
    gpsVel = TwistStamped()
    gpstime = TimeReference()
    gpstime.source = time_ref_source
    navData.header.frame_id = frame_id
    gpsVel.header.frame_id = frame_id
    GPSLock = False
    try:
        GPS = serial.Serial(port=GPSport, baudrate=GPSrate, timeout=2)
        #Read in GPS
        while not rospy.is_shutdown():
            #read GPS line
            data = GPS.readline().strip()

            if not check_nmea_checksum(data):
                rospy.logwarn("Received a sentence with an invalid checksum. Sentence was: %s" % data)
                continue

            timeNow = rospy.get_rostime()
            fields = data.split(',')
            for i in fields:
                i = i.strip(',')
            try:
                if useRMC:
                    #Check for satellite lock
                    if 'GSA' in fields[0]:
                        lockState = int(fields[2])
                        #print 'lockState=',lockState
                        if lockState == 3:
                            GPSLock = True