Imu msg converter - ashBabu/Utilities GitHub Wiki

Overview

The xsens imu produces msgs in its custom msg format. To convert that to the ros format, the following code is used

import os
import rosbag
import rospy
from sensor_msgs.msg import Imu
import sensor_msgs.point_cloud2 as pc2
import numpy as np
import tf_conversions

def convert_xsens_to_ros_imu(imu_msg):
    imu = Imu()
    imu.header = imu_msg.header

    roll = imu_msg.roll
    pitch = imu_msg.pitch
    yaw = imu_msg.yaw

    q = tf_conversions.transformations.quaternion_from_euler(roll, pitch, yaw)
    imu.orientation.w = q[0]
    imu.orientation.x = q[1]
    imu.orientation.y = q[2]
    imu.orientation.z = q[3]

    imu.angular_velocity.x = 0
    imu.angular_velocity.y = 0
    imu.angular_velocity.z = 0
    imu.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]

    imu.linear_acceleration.x = 0
    imu.linear_acceleration.y = 0
    imu.linear_acceleration.z = 0
    imu.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]

    return imu

def process_bag(input_bag_path, output_bag_path):
    with rosbag.Bag(output_bag_path, 'a') as outbag:
        with rosbag.Bag(input_bag_path, 'r') as inbag:
            for topic, msg, t in inbag.read_messages():
                if topic == '/hai_xsens/filter/orientation':
                    ros_imu_msg = convert_xsens_to_ros_imu(msg)
                    outbag.write('/hai_xsens/filter/orientation_ros', ros_imu_msg, t)
                else:
                    outbag.write(topic, msg, t)

def process_folder(folder_path):
    for filename in os.listdir(folder_path):
        if filename.endswith('.bag'):
            input_bag_path = os.path.join(folder_path, filename)
            output_bag_path = os.path.join(folder_path, 'converted_' + filename)
            process_bag(input_bag_path, output_bag_path)

if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser(description="Convert ROS bag messages")
    parser.add_argument("folder_path", help="Path to the folder containing ROS bags")
    args = parser.parse_args()

    process_folder(args.folder_path)

• run python3 name_of_the_code.py /path/to/ros1bags_folder after making it executable