实验笔记之——CARMEN （.log .clf）文件转换为rosbag

Posted 2022-06-20 gwpscut

直接给出源代码如下，更多介绍可以参考博文：

学习笔记之——数据集的生成_gwpscut的博客-CSDN博客

#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
import rospy
import rosbag
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from math import pi
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
import tf
 
def make_tf_msg(x, y, z,theta, t,parentid,childid):
    trans = TransformStamped()
    trans.header.stamp = t
    trans.header.frame_id = parentid
    trans.child_frame_id = childid
    trans.transform.translation.x = x
    trans.transform.translation.y = y
    trans.transform.translation.z = z
    q = tf.transformations.quaternion_from_euler(0, 0, theta)
    trans.transform.rotation.x = q[0]
    trans.transform.rotation.y = q[1]
    trans.transform.rotation.z = q[2]
    trans.transform.rotation.w = q[3]
 
    msg = TFMessage()
    msg.transforms.append(trans)
    return msg
 
 #    0   |      1       | 2 - 1+num_readings | 2+num_readings | 3+num_readings |
# xLASER | num_readings |  [range_readings]  |       x        |        y       |
#-------------------------------------------------------------------------------
# 4+num_readings | 5+num_readings | 6+num_readings | 7+num_readings |
#      theta     |     odom_x     |     odom_y     |   odom_theta   |
#-------------------------------------------------------------------------------
# 8+num_readings | 9+num_readings | 10+num_readings
# ipc_timestamp | ipc_hostname  |logger_timestamp
 
with open('mit-killian.clf') as dataset:
    with rosbag.Bag('MIT.bag', 'w') as bag:
        for line in dataset.readlines():
            line = line.strip()
            tokens = line.split(' ')
            if len(tokens) <= 2:
                continue
            if tokens[0] == 'FLASER':  #FLASER这是原始文件中原来标注激光数据的，一般不需要更改
                msg = LaserScan()
                num_scans = int(tokens[1])
 
                if num_scans != 180 or len(tokens) < num_scans + 9:
                    rospy.logwarn("unsupported scan format")
                    continue
 
                msg.header.frame_id = 'base_laser'  #这是激光坐标系id，根据需要更改，在ROS中读取数据时会使用到，一般也不需要更改。
                t = rospy.Time(float(tokens[(num_scans + 8)])) #获取时间
                msg.header.stamp = t
                msg.angle_min = -90.0 / 180.0 * pi
                msg.angle_max = 90.0 / 180.0 * pi
                msg.angle_increment = pi / num_scans
                msg.time_increment = 0.2 / 360.0
                msg.scan_time = 0.2
                msg.range_min = 0.001
                msg.range_max = 50.0
                msg.ranges = [float(r) for r in tokens[2:(num_scans + 2)]]
 
                bag.write('scan', msg, t)  #这是激光数据发布的话题，根据节点程序订阅的具体话题更改。
 
                tf_msg = make_tf_msg(0.1, 0, 0.2, 0, t,'base_link','base_laser')
                bag.write('tf', tf_msg, t)
 
                # odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 2):(num_scans + 5)]] 
                # odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 5):(num_scans + 8)]]
                # msg1=Odometry()
                # msg1.header.stamp=t
                # msg1.header.frame_id='odom'
                # msg1.child_frame_id='base_link'
                # msg1.pose.pose.position.x=odom_x
                # msg1.pose.pose.position.y=odom_y
                # msg1.pose.pose.position.z=0
                # q = tf.transformations.quaternion_from_euler(0, 0, odom_theta)
                # msg1.pose.pose.orientation.x = q[0]
                # msg1.pose.pose.orientation.y = q[1]
                # msg1.pose.pose.orientation.z = q[2]
                # msg1.pose.pose.orientation.w = q[3]
                # bag.write('odom', msg1, t) 
 
 
#    0 | 1 | 2 |  3  | 4 | 5 |  6  |      7      |      8     |        9
#  ODOM| x | y |theta| tv| rv|accel|ipc_timestamp|ipc_hostname|logger_timestamp 
            elif tokens[0] == 'ODOM':  #标志里程计数据
                odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
                t = rospy.Time(float(tokens[7]))
                msg=Odometry()
                msg.header.stamp=t
                msg.header.frame_id='odom'
                msg.child_frame_id='base_link'
                msg.pose.pose.position.x=odom_x
                msg.pose.pose.position.y=odom_y
                msg.pose.pose.position.z=0
                q = tf.transformations.quaternion_from_euler(0, 0, odom_theta)
                msg.pose.pose.orientation.x = q[0]
                msg.pose.pose.orientation.y = q[1]
                msg.pose.pose.orientation.z = q[2]
                msg.pose.pose.orientation.w = q[3]
                bag.write('odom', msg, t) 
 
                # tf_msg = make_tf_msg(odom_x, odom_y,0 , odom_theta, t,'odom','base_link')
                # bag.write('tf', tf_msg, t)