nclt2ros/transformer/sensor_data.py
def gps_rtk_to_navsat(self, gps_rtk_list, i):
        """ converts gps_rtk data to ROS NavSatFix messages

        :param gps_rtk_list: list, containing gps_rtk data
        :param            i: row counter

        :return: fill bag with navsat, track, speed, timestamp, tf_static_msg
        """

        # load data from list
        utime     = gps_rtk_list[i, 0]
        mode      = gps_rtk_list[i, 1]
        num_satss = gps_rtk_list[i, 2]
        lat       = gps_rtk_list[i, 3]
        lng       = gps_rtk_list[i, 4]
        alt       = gps_rtk_list[i, 5]
        track_raw = gps_rtk_list[i, 6]
        speed_raw = gps_rtk_list[i, 7]

        # create ros timestamp
        timestamp = rospy.Time.from_sec(utime / 1e6)

        # get gps_rtk and base link
        gps_rtk_link = self.gps_rtk_frame
        base_link    = self.body_frame

        # fill NavSat message
        status = NavSatStatus()

        if mode == 0 or mode == 1:
            status.status = NavSatStatus.STATUS_NO_FIX
        else:
            status.status = NavSatStatus.STATUS_FIX

        status.service = NavSatStatus.SERVICE_GPS

        num_sats = UInt16()
        num_sats.data = num_satss

        navsat = NavSatFix()
        navsat.header.stamp = timestamp
        navsat.header.frame_id = gps_rtk_link
        navsat.status = status

        navsat.latitude = np.rad2deg(lat)
        navsat.longitude = np.rad2deg(lng)
        navsat.altitude = alt

        navsat.position_covariance = NAVSAT_COVAR
        navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED

        track = Float64()
        track.data = track_raw

        speed = Float64()
        speed.data = speed_raw

        # create base_link gps_rtk_link static transformer
        gps_rtk_static_transform_stamped = geometry_msgs.msg.TransformStamped()
        gps_rtk_static_transform_stamped.header.stamp = timestamp
        gps_rtk_static_transform_stamped.header.frame_id = base_link
        gps_rtk_static_transform_stamped.child_frame_id = gps_rtk_link

        gps_rtk_static_transform_stamped.transform.translation.x = -0.24
        gps_rtk_static_transform_stamped.transform.translation.y = 0
        gps_rtk_static_transform_stamped.transform.translation.z = 1.24

        quat = tf.transformations.quaternion_from_euler(0, 0, 0)
        gps_rtk_static_transform_stamped.transform.rotation.x = quat[0]
        gps_rtk_static_transform_stamped.transform.rotation.y = quat[1]
        gps_rtk_static_transform_stamped.transform.rotation.z = quat[2]
        gps_rtk_static_transform_stamped.transform.rotation.w = quat[3]

        # publish static transform
        tf_static_msg = tf2_msgs.msg.TFMessage([gps_rtk_static_transform_stamped])

        return navsat, track, speed, timestamp, tf_static_msg
 converts gps_rtk data to ROS NavSatFix messages

        :param gps_rtk_list: list, containing gps_rtk data
        :param            i: row counter

        :return: fill bag with navsat, track, speed, timestamp, tf_static_msg
        
converts gps_rtk data to ROS NavSatFix messages
converts gps_rtk data to ROS NavSatFix messages
def gps_rtk_to_navsat(self, gps_rtk_list, i):
        utime     = gps_rtk_list[i, 0]
        mode      = gps_rtk_list[i, 1]
        num_satss = gps_rtk_list[i, 2]
        lat       = gps_rtk_list[i, 3]
        lng       = gps_rtk_list[i, 4]
        alt       = gps_rtk_list[i, 5]
        track_raw = gps_rtk_list[i, 6]
        speed_raw = gps_rtk_list[i, 7]
        timestamp = rospy.Time.from_sec(utime / 1e6)
        gps_rtk_link = self.gps_rtk_frame
        base_link    = self.body_frame
        status = NavSatStatus()
        if mode == 0 or mode == 1:
            status.status = NavSatStatus.STATUS_NO_FIX
        else:
            status.status = NavSatStatus.STATUS_FIX
        status.service = NavSatStatus.SERVICE_GPS
        num_sats = UInt16()
        num_sats.data = num_satss
        navsat = NavSatFix()
        navsat.header.stamp = timestamp
        navsat.header.frame_id = gps_rtk_link
        navsat.status = status
        navsat.latitude = np.rad2deg(lat)
        navsat.longitude = np.rad2deg(lng)
        navsat.altitude = alt
        navsat.position_covariance = NAVSAT_COVAR
        navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
        track = Float64()
        track.data = track_raw
        speed = Float64()
        speed.data = speed_raw
        gps_rtk_static_transform_stamped = geometry_msgs.msg.TransformStamped()
        gps_rtk_static_transform_stamped.header.stamp = timestamp
        gps_rtk_static_transform_stamped.header.frame_id = base_link
        gps_rtk_static_transform_stamped.child_frame_id = gps_rtk_link
        gps_rtk_static_transform_stamped.transform.translation.x = -0.24
        gps_rtk_static_transform_stamped.transform.translation.y = 0
        gps_rtk_static_transform_stamped.transform.translation.z = 1.24
        quat = tf.transformations.quaternion_from_euler(0, 0, 0)
        gps_rtk_static_transform_stamped.transform.rotation.x = quat[0]
        gps_rtk_static_transform_stamped.transform.rotation.y = quat[1]
        gps_rtk_static_transform_stamped.transform.rotation.z = quat[2]
        gps_rtk_static_transform_stamped.transform.rotation.w = quat[3]
        tf_static_msg = tf2_msgs.msg.TFMessage([gps_rtk_static_transform_stamped])
        return navsat, track, speed, timestamp, tf_static_msg
def gps_rtk_to_navsat ( self , gps_rtk_list , i ) : utime = gps_rtk_list [ i , 0 ] mode = gps_rtk_list [ i , 1 ] num_satss = gps_rtk_list [ i , 2 ] lat = gps_rtk_list [ i , 3 ] lng = gps_rtk_list [ i , 4 ] alt = gps_rtk_list [ i , 5 ] track_raw = gps_rtk_list [ i , 6 ] speed_raw = gps_rtk_list [ i , 7 ] timestamp = rospy . Time . from_sec ( utime / 1e6 ) gps_rtk_link = self . gps_rtk_frame base_link = self . body_frame status = NavSatStatus ( ) if mode == 0 or mode == 1 : status . status = NavSatStatus . STATUS_NO_FIX else : status . status = NavSatStatus . STATUS_FIX status . service = NavSatStatus . SERVICE_GPS num_sats = UInt16 ( ) num_sats . data = num_satss navsat = NavSatFix ( ) navsat . header . stamp = timestamp navsat . header . frame_id = gps_rtk_link navsat . status = status navsat . latitude = np . rad2deg ( lat ) navsat . longitude = np . rad2deg ( lng ) navsat . altitude = alt navsat . position_covariance = NAVSAT_COVAR navsat . position_covariance_type = NavSatFix . COVARIANCE_TYPE_APPROXIMATED track = Float64 ( ) track . data = track_raw speed = Float64 ( ) speed . data = speed_raw gps_rtk_static_transform_stamped = geometry_msgs . msg . TransformStamped ( ) gps_rtk_static_transform_stamped . header . stamp = timestamp gps_rtk_static_transform_stamped . header . frame_id = base_link gps_rtk_static_transform_stamped . child_frame_id = gps_rtk_link gps_rtk_static_transform_stamped . transform . translation . x = - 0.24 gps_rtk_static_transform_stamped . transform . translation . y = 0 gps_rtk_static_transform_stamped . transform . translation . z = 1.24 quat = tf . transformations . quaternion_from_euler ( 0 , 0 , 0 ) gps_rtk_static_transform_stamped . transform . rotation . x = quat [ 0 ] gps_rtk_static_transform_stamped . transform . rotation . y = quat [ 1 ] gps_rtk_static_transform_stamped . transform . rotation . z = quat [ 2 ] gps_rtk_static_transform_stamped . transform . rotation . w = quat [ 3 ] tf_static_msg = tf2_msgs . msg . TFMessage ( [ gps_rtk_static_transform_stamped ] ) return navsat , track , speed , timestamp , tf_static_msg
converts gps_rtk data to ROS NavSatFix messages
converts gps_rtk data to ROS NavSatFix messages
""" converts gps_rtk data to ROS NavSatFix messages

        :param gps_rtk_list: list, containing gps_rtk data
        :param            i: row counter

        :return: fill bag with navsat, track, speed, timestamp, tf_static_msg
        """ # load data from list # create ros timestamp # get gps_rtk and base link # fill NavSat message # create base_link gps_rtk_link static transformer # publish static transform
fill bag with navsat, track, speed, timestamp, tf_static_msg
fill bag with navsat track speed timestamp tf_static_msg
self
gps_rtk_list
list, containing gps_rtk data
list containing gps_rtk data
i
None