nclt2ros/transformer/sensor_data.py
def gt_to_odometry(self, gt_list, gt_cov_list, i):
        """converts ground_truth odometry to ROS odometry messages

        :param gt_list: list containing the ground truth data
        :param       i: row counter

        :return: fill bag with gt, timestamp, tf_static_msg
        """

        # load data from list
        utime     = gt_list[i, 0]
        x         = gt_list[i, 1]
        y         = gt_list[i, 2]
        z         = gt_list[i, 3]
        roll_rad  = gt_list[i, 4]
        pitch_rad = gt_list[i, 5]
        yaw_rad   = gt_list[i, 6]

        # get upper diagonal of the covariance matrix
        xx = gt_cov_list[self.i_gt, 1];  xy = gt_cov_list[self.i_gt, 2];  xz = gt_cov_list[self.i_gt, 3];  xr = gt_cov_list[self.i_gt, 4]
        xp = gt_cov_list[self.i_gt, 5];  xh = gt_cov_list[self.i_gt, 6];  yy = gt_cov_list[self.i_gt, 7];  yz = gt_cov_list[self.i_gt, 8]
        yr = gt_cov_list[self.i_gt, 9];  yp = gt_cov_list[self.i_gt, 10]; yh = gt_cov_list[self.i_gt, 11]; zz = gt_cov_list[self.i_gt, 12]
        zr = gt_cov_list[self.i_gt, 13]; zp = gt_cov_list[self.i_gt, 14]; zh = gt_cov_list[self.i_gt, 15]; rr = gt_cov_list[self.i_gt, 16]
        rp = gt_cov_list[self.i_gt, 17]; rh = gt_cov_list[self.i_gt, 18]; pp = gt_cov_list[self.i_gt, 19]; ph = gt_cov_list[self.i_gt, 20]
        hh = gt_cov_list[self.i_gt, 21]

        # load utime from gt_cov list
        utime_gt_cov = gt_cov_list[self.i_gt, 0]

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

        # get ground truth link
        gt_link = str(self.ground_truth_frame)

        # create odometry message for ground truth
        gt = Odometry()
        gt.header.stamp = timestamp
        gt.header.frame_id = gt_link
        # odom.child_frame_id = 'base_link'

        # change due to the ENU frame
        gt.pose.pose.position.x = y - 107.724666286       # x # TODO not hardcoding
        gt.pose.pose.position.y = x - 75.829339527800     # y
        gt.pose.pose.position.z = -z + 3.272894625813646  # z

        # create quaternion from euler angles
        quaternion = tf.transformations.quaternion_from_euler(roll_rad, pitch_rad, yaw_rad)
        gt.pose.pose.orientation.x = quaternion[0]
        gt.pose.pose.orientation.y = quaternion[1]
        gt.pose.pose.orientation.z = quaternion[2]
        gt.pose.pose.orientation.w = quaternion[3]

        # fill covariance matrices
        if utime > utime_gt_cov:
            gt.pose.covariance[0] = xx;  gt.pose.covariance[1] = xy;  gt.pose.covariance[2] = xz;  gt.pose.covariance[3] = xr
            gt.pose.covariance[4] = xp;  gt.pose.covariance[5] = xh

            gt.pose.covariance[7] = yy;  gt.pose.covariance[8] = yz;  gt.pose.covariance[9] = yr;  gt.pose.covariance[10] = yp
            gt.pose.covariance[11] = yh

            gt.pose.covariance[14] = zz; gt.pose.covariance[15] = zr; gt.pose.covariance[16] = zp; gt.pose.covariance[17] = zh

            gt.pose.covariance[21] = rr; gt.pose.covariance[22] = rp; gt.pose.covariance[23] = rh

            gt.pose.covariance[28] = pp; gt.pose.covariance[29] = ph

            gt.pose.covariance[35] = hh

            self.last_gt_cov = gt.pose.covariance
            self.i_gt += 1

        elif self.init_gt_cov:
            gt.pose.covariance = self.last_gt_cov

        else:
            gt.pose.covariance = POSE_COVAR

            self.init_gt_cov = True
            self.last_gt_cov = gt.pose.covariance

        # TODO fill TWIST
        gt.twist.twist.linear.x = 0.0
        gt.twist.twist.linear.y = 0.0
        gt.twist.twist.linear.z = 0.0
        gt.twist.twist.angular.x = 0.0
        gt.twist.twist.angular.y = 0.0
        gt.twist.twist.angular.z = 0.0

        # create world ground truth static transformer
        gt_static_transform_stamped = geometry_msgs.msg.TransformStamped()
        gt_static_transform_stamped.header.stamp = timestamp
        gt_static_transform_stamped.header.frame_id = "world"
        gt_static_transform_stamped.child_frame_id = gt_link

        gt_static_transform_stamped.transform.translation.x = 0
        gt_static_transform_stamped.transform.translation.y = 0
        gt_static_transform_stamped.transform.translation.z = 0

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

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

        return gt, timestamp, tf_static_msg
converts ground_truth odometry to ROS odometry messages

        :param gt_list: list containing the ground truth data
        :param       i: row counter

        :return: fill bag with gt, timestamp, tf_static_msg
        
converts ground_truth odometry to ROS odometry messages
converts ground_truth odometry to ROS odometry messages
def gt_to_odometry(self, gt_list, gt_cov_list, i):
        utime     = gt_list[i, 0]
        x         = gt_list[i, 1]
        y         = gt_list[i, 2]
        z         = gt_list[i, 3]
        roll_rad  = gt_list[i, 4]
        pitch_rad = gt_list[i, 5]
        yaw_rad   = gt_list[i, 6]
        xx = gt_cov_list[self.i_gt, 1];  xy = gt_cov_list[self.i_gt, 2];  xz = gt_cov_list[self.i_gt, 3];  xr = gt_cov_list[self.i_gt, 4]
        xp = gt_cov_list[self.i_gt, 5];  xh = gt_cov_list[self.i_gt, 6];  yy = gt_cov_list[self.i_gt, 7];  yz = gt_cov_list[self.i_gt, 8]
        yr = gt_cov_list[self.i_gt, 9];  yp = gt_cov_list[self.i_gt, 10]; yh = gt_cov_list[self.i_gt, 11]; zz = gt_cov_list[self.i_gt, 12]
        zr = gt_cov_list[self.i_gt, 13]; zp = gt_cov_list[self.i_gt, 14]; zh = gt_cov_list[self.i_gt, 15]; rr = gt_cov_list[self.i_gt, 16]
        rp = gt_cov_list[self.i_gt, 17]; rh = gt_cov_list[self.i_gt, 18]; pp = gt_cov_list[self.i_gt, 19]; ph = gt_cov_list[self.i_gt, 20]
        hh = gt_cov_list[self.i_gt, 21]
        utime_gt_cov = gt_cov_list[self.i_gt, 0]
        timestamp = rospy.Time.from_sec(utime / 1e6)
        gt_link = str(self.ground_truth_frame)
        gt = Odometry()
        gt.header.stamp = timestamp
        gt.header.frame_id = gt_link
        gt.pose.pose.position.x = y - 107.724666286       
        gt.pose.pose.position.y = x - 75.829339527800     
        gt.pose.pose.position.z = -z + 3.272894625813646  
        quaternion = tf.transformations.quaternion_from_euler(roll_rad, pitch_rad, yaw_rad)
        gt.pose.pose.orientation.x = quaternion[0]
        gt.pose.pose.orientation.y = quaternion[1]
        gt.pose.pose.orientation.z = quaternion[2]
        gt.pose.pose.orientation.w = quaternion[3]
        if utime > utime_gt_cov:
            gt.pose.covariance[0] = xx;  gt.pose.covariance[1] = xy;  gt.pose.covariance[2] = xz;  gt.pose.covariance[3] = xr
            gt.pose.covariance[4] = xp;  gt.pose.covariance[5] = xh
            gt.pose.covariance[7] = yy;  gt.pose.covariance[8] = yz;  gt.pose.covariance[9] = yr;  gt.pose.covariance[10] = yp
            gt.pose.covariance[11] = yh
            gt.pose.covariance[14] = zz; gt.pose.covariance[15] = zr; gt.pose.covariance[16] = zp; gt.pose.covariance[17] = zh
            gt.pose.covariance[21] = rr; gt.pose.covariance[22] = rp; gt.pose.covariance[23] = rh
            gt.pose.covariance[28] = pp; gt.pose.covariance[29] = ph
            gt.pose.covariance[35] = hh
            self.last_gt_cov = gt.pose.covariance
            self.i_gt += 1
        elif self.init_gt_cov:
            gt.pose.covariance = self.last_gt_cov
        else:
            gt.pose.covariance = POSE_COVAR
            self.init_gt_cov = True
            self.last_gt_cov = gt.pose.covariance
        gt.twist.twist.linear.x = 0.0
        gt.twist.twist.linear.y = 0.0
        gt.twist.twist.linear.z = 0.0
        gt.twist.twist.angular.x = 0.0
        gt.twist.twist.angular.y = 0.0
        gt.twist.twist.angular.z = 0.0
        gt_static_transform_stamped = geometry_msgs.msg.TransformStamped()
        gt_static_transform_stamped.header.stamp = timestamp
        gt_static_transform_stamped.header.frame_id = "world"
        gt_static_transform_stamped.child_frame_id = gt_link
        gt_static_transform_stamped.transform.translation.x = 0
        gt_static_transform_stamped.transform.translation.y = 0
        gt_static_transform_stamped.transform.translation.z = 0
        quat = tf.transformations.quaternion_from_euler(0, 0, 0)
        gt_static_transform_stamped.transform.rotation.x = quat[0]
        gt_static_transform_stamped.transform.rotation.y = quat[1]
        gt_static_transform_stamped.transform.rotation.z = quat[2]
        gt_static_transform_stamped.transform.rotation.w = quat[3]
        tf_static_msg = tf2_msgs.msg.TFMessage([gt_static_transform_stamped])
        return gt, timestamp, tf_static_msg
def gt_to_odometry ( self , gt_list , gt_cov_list , i ) : utime = gt_list [ i , 0 ] x = gt_list [ i , 1 ] y = gt_list [ i , 2 ] z = gt_list [ i , 3 ] roll_rad = gt_list [ i , 4 ] pitch_rad = gt_list [ i , 5 ] yaw_rad = gt_list [ i , 6 ] xx = gt_cov_list [ self . i_gt , 1 ] ; xy = gt_cov_list [ self . i_gt , 2 ] ; xz = gt_cov_list [ self . i_gt , 3 ] ; xr = gt_cov_list [ self . i_gt , 4 ] xp = gt_cov_list [ self . i_gt , 5 ] ; xh = gt_cov_list [ self . i_gt , 6 ] ; yy = gt_cov_list [ self . i_gt , 7 ] ; yz = gt_cov_list [ self . i_gt , 8 ] yr = gt_cov_list [ self . i_gt , 9 ] ; yp = gt_cov_list [ self . i_gt , 10 ] ; yh = gt_cov_list [ self . i_gt , 11 ] ; zz = gt_cov_list [ self . i_gt , 12 ] zr = gt_cov_list [ self . i_gt , 13 ] ; zp = gt_cov_list [ self . i_gt , 14 ] ; zh = gt_cov_list [ self . i_gt , 15 ] ; rr = gt_cov_list [ self . i_gt , 16 ] rp = gt_cov_list [ self . i_gt , 17 ] ; rh = gt_cov_list [ self . i_gt , 18 ] ; pp = gt_cov_list [ self . i_gt , 19 ] ; ph = gt_cov_list [ self . i_gt , 20 ] hh = gt_cov_list [ self . i_gt , 21 ] utime_gt_cov = gt_cov_list [ self . i_gt , 0 ] timestamp = rospy . Time . from_sec ( utime / 1e6 ) gt_link = str ( self . ground_truth_frame ) gt = Odometry ( ) gt . header . stamp = timestamp gt . header . frame_id = gt_link gt . pose . pose . position . x = y - 107.724666286 gt . pose . pose . position . y = x - 75.829339527800 gt . pose . pose . position . z = - z + 3.272894625813646 quaternion = tf . transformations . quaternion_from_euler ( roll_rad , pitch_rad , yaw_rad ) gt . pose . pose . orientation . x = quaternion [ 0 ] gt . pose . pose . orientation . y = quaternion [ 1 ] gt . pose . pose . orientation . z = quaternion [ 2 ] gt . pose . pose . orientation . w = quaternion [ 3 ] if utime > utime_gt_cov : gt . pose . covariance [ 0 ] = xx ; gt . pose . covariance [ 1 ] = xy ; gt . pose . covariance [ 2 ] = xz ; gt . pose . covariance [ 3 ] = xr gt . pose . covariance [ 4 ] = xp ; gt . pose . covariance [ 5 ] = xh gt . pose . covariance [ 7 ] = yy ; gt . pose . covariance [ 8 ] = yz ; gt . pose . covariance [ 9 ] = yr ; gt . pose . covariance [ 10 ] = yp gt . pose . covariance [ 11 ] = yh gt . pose . covariance [ 14 ] = zz ; gt . pose . covariance [ 15 ] = zr ; gt . pose . covariance [ 16 ] = zp ; gt . pose . covariance [ 17 ] = zh gt . pose . covariance [ 21 ] = rr ; gt . pose . covariance [ 22 ] = rp ; gt . pose . covariance [ 23 ] = rh gt . pose . covariance [ 28 ] = pp ; gt . pose . covariance [ 29 ] = ph gt . pose . covariance [ 35 ] = hh self . last_gt_cov = gt . pose . covariance self . i_gt += 1 elif self . init_gt_cov : gt . pose . covariance = self . last_gt_cov else : gt . pose . covariance = POSE_COVAR self . init_gt_cov = True self . last_gt_cov = gt . pose . covariance gt . twist . twist . linear . x = 0.0 gt . twist . twist . linear . y = 0.0 gt . twist . twist . linear . z = 0.0 gt . twist . twist . angular . x = 0.0 gt . twist . twist . angular . y = 0.0 gt . twist . twist . angular . z = 0.0 gt_static_transform_stamped = geometry_msgs . msg . TransformStamped ( ) gt_static_transform_stamped . header . stamp = timestamp gt_static_transform_stamped . header . frame_id = "world" gt_static_transform_stamped . child_frame_id = gt_link gt_static_transform_stamped . transform . translation . x = 0 gt_static_transform_stamped . transform . translation . y = 0 gt_static_transform_stamped . transform . translation . z = 0 quat = tf . transformations . quaternion_from_euler ( 0 , 0 , 0 ) gt_static_transform_stamped . transform . rotation . x = quat [ 0 ] gt_static_transform_stamped . transform . rotation . y = quat [ 1 ] gt_static_transform_stamped . transform . rotation . z = quat [ 2 ] gt_static_transform_stamped . transform . rotation . w = quat [ 3 ] tf_static_msg = tf2_msgs . msg . TFMessage ( [ gt_static_transform_stamped ] ) return gt , timestamp , tf_static_msg
converts ground_truth odometry to ROS odometry messages
converts ground_truth odometry to ROS odometry messages
"""converts ground_truth odometry to ROS odometry messages

        :param gt_list: list containing the ground truth data
        :param       i: row counter

        :return: fill bag with gt, timestamp, tf_static_msg
        """ # load data from list # get upper diagonal of the covariance matrix # load utime from gt_cov list # create ros timestamp # get ground truth link # create odometry message for ground truth # odom.child_frame_id = 'base_link' # change due to the ENU frame # x # TODO not hardcoding # y # z # create quaternion from euler angles # fill covariance matrices # TODO fill TWIST # create world ground truth static transformer # publish static transform
fill bag with gt, timestamp, tf_static_msg
fill bag with gt timestamp tf_static_msg
self
gt_list
list containing the ground truth data
list containing the ground truth data
gt_cov_list
i
None