nclt2ros/transformer/sensor_data.py
def ms25_to_imu(self, imu_list, i):
        """converts ms25 data to ROS imu messages

        :param imu_list: list containing the imu data from sensor ms25
        :param        i: list row counter

        :return: fill bag with imu, mag, timestamp, tf_static_msg
        """

        # load data from list
        utime    = imu_list[i, 0]
        mag_xs   = imu_list[i, 1]
        mag_ys   = imu_list[i, 2]
        mag_zs   = imu_list[i, 3]
        accel_xs = imu_list[i, 4]
        accel_ys = imu_list[i, 5]
        accel_zs = imu_list[i, 6]
        rot_rs   = imu_list[i, 7]
        rot_ps   = imu_list[i, 8]
        rot_hs   = imu_list[i, 9]

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

        # get imu and base link
        imu_link   = self.imu_frame
        base_link  = self.body_frame

        # create ros imu message
        imu = Imu()
        imu.header.stamp = timestamp
        imu.header.frame_id = imu_link

        # swap x,y due to the enu frame, negate z
        quaternion = tf.transformations.quaternion_from_euler(
            rot_ps, rot_rs, -rot_hs
        )

        # alternative
        #quaternion = tf.transformations.quaternion_from_euler(
        #    rot_rs, -rot_ps, -rot_hs
        #)

        imu.orientation.x = quaternion[0]
        imu.orientation.y = quaternion[1]
        imu.orientation.z = quaternion[2]
        imu.orientation.w = quaternion[3]

        # swap x,y due to the enu frame, negate z
        imu.angular_velocity.x = rot_ps  # rot_rs
        imu.angular_velocity.y = rot_rs  # -rot_ps
        imu.angular_velocity.z = -rot_hs
        imu.angular_velocity_covariance = IMU_VEL_COVAR

        # swap x,y due to the enu frame, negate z
        imu.linear_acceleration.x = accel_ys   # accel_xs
        imu.linear_acceleration.y = accel_xs   # -accel_ys
        imu.linear_acceleration.z = -accel_zs
        imu.linear_acceleration_covariance = IMU_ACCEL_COVAR

        # enu frame
        mag = MagneticField()
        mag.header.stamp     = timestamp
        mag.magnetic_field.x = mag_ys
        mag.magnetic_field.y = mag_xs
        mag.magnetic_field.z = -mag_zs

        # create base_link imu static transformer
        imu_static_transform_stamped = geometry_msgs.msg.TransformStamped()
        imu_static_transform_stamped.header.stamp = timestamp
        imu_static_transform_stamped.header.frame_id = base_link
        imu_static_transform_stamped.child_frame_id = imu_link

        imu_static_transform_stamped.transform.translation.x = -0.11
        imu_static_transform_stamped.transform.translation.y = 0.18
        imu_static_transform_stamped.transform.translation.z = 0.71

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

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

        return imu, mag, timestamp, tf_static_msg
converts ms25 data to ROS imu messages

        :param imu_list: list containing the imu data from sensor ms25
        :param        i: list row counter

        :return: fill bag with imu, mag, timestamp, tf_static_msg
        
converts ms25 data to ROS imu messages
converts ms25 data to ROS imu messages
def ms25_to_imu(self, imu_list, i):
        utime    = imu_list[i, 0]
        mag_xs   = imu_list[i, 1]
        mag_ys   = imu_list[i, 2]
        mag_zs   = imu_list[i, 3]
        accel_xs = imu_list[i, 4]
        accel_ys = imu_list[i, 5]
        accel_zs = imu_list[i, 6]
        rot_rs   = imu_list[i, 7]
        rot_ps   = imu_list[i, 8]
        rot_hs   = imu_list[i, 9]
        timestamp = rospy.Time.from_sec(utime / 1e6)
        imu_link   = self.imu_frame
        base_link  = self.body_frame
        imu = Imu()
        imu.header.stamp = timestamp
        imu.header.frame_id = imu_link
        quaternion = tf.transformations.quaternion_from_euler(
            rot_ps, rot_rs, -rot_hs
        )
        imu.orientation.x = quaternion[0]
        imu.orientation.y = quaternion[1]
        imu.orientation.z = quaternion[2]
        imu.orientation.w = quaternion[3]
        imu.angular_velocity.x = rot_ps  
        imu.angular_velocity.y = rot_rs  
        imu.angular_velocity.z = -rot_hs
        imu.angular_velocity_covariance = IMU_VEL_COVAR
        imu.linear_acceleration.x = accel_ys   
        imu.linear_acceleration.y = accel_xs   
        imu.linear_acceleration.z = -accel_zs
        imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
        mag = MagneticField()
        mag.header.stamp     = timestamp
        mag.magnetic_field.x = mag_ys
        mag.magnetic_field.y = mag_xs
        mag.magnetic_field.z = -mag_zs
        imu_static_transform_stamped = geometry_msgs.msg.TransformStamped()
        imu_static_transform_stamped.header.stamp = timestamp
        imu_static_transform_stamped.header.frame_id = base_link
        imu_static_transform_stamped.child_frame_id = imu_link
        imu_static_transform_stamped.transform.translation.x = -0.11
        imu_static_transform_stamped.transform.translation.y = 0.18
        imu_static_transform_stamped.transform.translation.z = 0.71
        quat = tf.transformations.quaternion_from_euler(0, 0, 0)
        imu_static_transform_stamped.transform.rotation.x = quat[0]
        imu_static_transform_stamped.transform.rotation.y = quat[1]
        imu_static_transform_stamped.transform.rotation.z = quat[2]
        imu_static_transform_stamped.transform.rotation.w = quat[3]
        tf_static_msg = tf2_msgs.msg.TFMessage([imu_static_transform_stamped])
        return imu, mag, timestamp, tf_static_msg
def ms25_to_imu ( self , imu_list , i ) : utime = imu_list [ i , 0 ] mag_xs = imu_list [ i , 1 ] mag_ys = imu_list [ i , 2 ] mag_zs = imu_list [ i , 3 ] accel_xs = imu_list [ i , 4 ] accel_ys = imu_list [ i , 5 ] accel_zs = imu_list [ i , 6 ] rot_rs = imu_list [ i , 7 ] rot_ps = imu_list [ i , 8 ] rot_hs = imu_list [ i , 9 ] timestamp = rospy . Time . from_sec ( utime / 1e6 ) imu_link = self . imu_frame base_link = self . body_frame imu = Imu ( ) imu . header . stamp = timestamp imu . header . frame_id = imu_link quaternion = tf . transformations . quaternion_from_euler ( rot_ps , rot_rs , - rot_hs ) imu . orientation . x = quaternion [ 0 ] imu . orientation . y = quaternion [ 1 ] imu . orientation . z = quaternion [ 2 ] imu . orientation . w = quaternion [ 3 ] imu . angular_velocity . x = rot_ps imu . angular_velocity . y = rot_rs imu . angular_velocity . z = - rot_hs imu . angular_velocity_covariance = IMU_VEL_COVAR imu . linear_acceleration . x = accel_ys imu . linear_acceleration . y = accel_xs imu . linear_acceleration . z = - accel_zs imu . linear_acceleration_covariance = IMU_ACCEL_COVAR mag = MagneticField ( ) mag . header . stamp = timestamp mag . magnetic_field . x = mag_ys mag . magnetic_field . y = mag_xs mag . magnetic_field . z = - mag_zs imu_static_transform_stamped = geometry_msgs . msg . TransformStamped ( ) imu_static_transform_stamped . header . stamp = timestamp imu_static_transform_stamped . header . frame_id = base_link imu_static_transform_stamped . child_frame_id = imu_link imu_static_transform_stamped . transform . translation . x = - 0.11 imu_static_transform_stamped . transform . translation . y = 0.18 imu_static_transform_stamped . transform . translation . z = 0.71 quat = tf . transformations . quaternion_from_euler ( 0 , 0 , 0 ) imu_static_transform_stamped . transform . rotation . x = quat [ 0 ] imu_static_transform_stamped . transform . rotation . y = quat [ 1 ] imu_static_transform_stamped . transform . rotation . z = quat [ 2 ] imu_static_transform_stamped . transform . rotation . w = quat [ 3 ] tf_static_msg = tf2_msgs . msg . TFMessage ( [ imu_static_transform_stamped ] ) return imu , mag , timestamp , tf_static_msg
converts ms25 data to ROS imu messages
converts ms25 data to ROS imu messages
"""converts ms25 data to ROS imu messages

        :param imu_list: list containing the imu data from sensor ms25
        :param        i: list row counter

        :return: fill bag with imu, mag, timestamp, tf_static_msg
        """ # load data from list # create ros timestamp # get imu and base link # create ros imu message # swap x,y due to the enu frame, negate z # alternative #quaternion = tf.transformations.quaternion_from_euler( #    rot_rs, -rot_ps, -rot_hs #) # swap x,y due to the enu frame, negate z # rot_rs # -rot_ps # swap x,y due to the enu frame, negate z # accel_xs # -accel_ys # enu frame # create base_link imu static transformer # publish static transform
fill bag with imu, mag, timestamp, tf_static_msg
fill bag with imu mag timestamp tf_static_msg
self
imu_list
list containing the imu data from sensor ms25
list containing the imu data from sensor ms25
i
list row counter
list row counter