【python/ros】python小程序实现rosbag数据包解析保存

序言
  • 模型训练要求感知数据集按照如下格式存储

    # txt: 时间戳 目标id 坐标x 坐标y 速度x 速度y
    timestamp:float agent_ID:int pos_x:float pos_y:float v_x:float v_y:float
    
  • python实现rosbag包解析,提取感知topic消息字段并存入文件

1. 读取bag包
  • ros提供了解析bag包的python api,按如下方式载入bag

    import rosbag
    bag_file = '/your/path/xxx.bag'
    bag_data = rosbag.Bag(bag_file, "r")
    
  • 可通过如下命令查看bag包含哪些topic、消息类型和帧率

    info = bag_data.get_type_and_topic_info()
    print(info)
    
  • 如图显示感知topic消息数350,帧率10Hz
    在这里插入图片描述

2. 读取特定topic的数据
  • 如果不给定topic,会遍历所有的topic

  • 查看topic的方式:方式1如上;方式2 rosbag info xxx.bag;方式3 通过read_messages()返回值

  • read_messages()每次迭代返回3个值:topic, msg, t。假如感知topic名字是/perception/obstacles,msg是具体的消息数据,t表示时间戳

    perception_data = bag_data.read_messages('/perception/obstacles')
    for topic, msg, t in perception_data:
        print(topic)
    
3. msg解析与存储
  • 同样,可通过多种方式查看msg有哪些字段:方式1 topic能看到消息,查看proto的消息字段;方式2 业务代码中消息解析字段;方式3 rostopic echo topic_name可打印看到消息内容

  • 我们字典来按照目标id存储消息解析的结果,如下

    # frame_id = 0
    object = {}		# {id : [[frame1 data],[frame2 data]]}
    for topic, msg, t in perception_data:
        if msg is not None:
            for obstacle in msg.obstacles:
    	        item = []
    	        # item.append(frame_id)
    	        item.append(obstacle.timestamp)
    	        item.append(obstacle.track_id)
    	        item.append(target.position.x)
    	        item.append(target.position.y)
    	        item.append(target.velocity.x)
    	        item.append(target.velocity.y)
    	        if obstacle.track_id in object.keys():
    	            object[obstacle.track_id].append(item)
    	        else:
    	            object[obstacle.track_id] = [item]
    
4. 数据按要求格式写入文件
  • 新建存储perception数据的文件

    # 执行后自动创建
    file = open('/disk_f/traing_data/' + 'perception_obstacles.txt', "w", encoding='utf-8')
    
  • 数据转换成string类型存储(结合实际需要)

    fields = ' '.join(map(lambda x: str(x), values))    # .join 将元素以指定分隔符连成新字符串
    file.write(fields + '\n')
    
  • 将数据按照id从大到小排列(时间戳是按时间排列的,不然也可以指定按时间戳排序)

    # object.items返回所有key:value数据对
    data = sorted(object.items(), key=lambda x: x[0])
    data = dict(data)
    
  • 数据写入文件:打开-写入-关闭

    file = open('/disk_f/traing_data/' + 'perception_obstacles.txt', "w", encoding='utf-8')
    if object is not None:
        data = sorted(object.items(), key=lambda x: x[0])
        data = dict(data)
        for target_id in data.keys():
            # break
            frame_id = 0
            for values in data[target_id]:
                fields = ' '.join(map(lambda x: str(x), values))
                file.write(str(frame_id) + '  ' + fields + '\n')
                frame_id += 12
    file.close()
    
  • 写入后效果

5. 完整程序
  • python实现rosbag数据包特定topic消息的解析保存

    import rosbag
    
    '''
    ====== 1. load xxx.bag data
    '''
    bag_file = '/your/path/xxx.bag'
    bag_data = rosbag.Bag(bag_file, "r")
    
    info = bag_data.get_type_and_topic_info()
    # print(info)
    
    '''
    ====== 2. save msg fields
    '''
    perception_data = bag_data.read_messages('/perception/obstacles')
    object = {}
    for topic, msg, t in perception_data:
        if msg is not None:
            for obstacle in msg.obstacles:
    	        item = []
    	        # item.append(frame_id)
    	        item.append(obstacle.timestamp)
    	        item.append(obstacle.track_id)
    	        item.append(obstacle.position.x)
    	        item.append(obstacle.position.y)
    	        item.append(obstacle.velocity.x)
    	        item.append(obstacle.velocity.y)
    	        if obstacle.track_id in object.keys():
    	            object[obstacle.track_id].append(item)  # or object[obstacle.track_id] += [item]
    	        else:
    	            object[obstacle.track_id] = [item]      # otherwise [1, 2, 3, [4, 5, 6]]
    
    '''
    ====== 3. data save as txt
    '''
    file = open('/disk_f/traing_data/' + 'perception_obstacles.txt', "w", encoding='utf-8')
    if object is not None:
        data = sorted(object.items(), key=lambda x: x[0])     # sorted by key i.e. track_id
        data = dict(data)
        for target_id in data.keys():
            frame_id = 0
            for values in data[target_id]:
                fields = ' '.join(map(lambda x: str(x), values))
                file.write(str(frame_id) + '  ' + fields + '\n')
                frame_id += 12
    file.close()
    

5.1 item[]放在for循环外导致的异常
  • 如上代码,我们需要将每次循环的item放入字典,但如果 item[]放在for循环外,如下

    item = []
    for topic, msg, t in perception_data:
    	if msg is not None:
    		for obstacle in msg.obstacles:
    			item.clear()
    			item.append(obstacle.timestamp)
    			if obstacle.track_id in object.keys():
    				object[obstacle.track_id].append(item)
    			else:
    				object[obstacle.track_id] = [item]
    
  • 会出现循环结束后字典中只存了最后一个目标的item。原因在于使用同一个item对象,它的值就是最后一次赋值的值;把item[]放在for循环内,每次新建一个列表对象就不会有这个问题。或拷贝时尝试 item.copy()

5.2 字典写入二维列表
  • 如上,如果dict[id].append(item_list)直接添加的话,得到的是 [1, 2, 3, [4, 5, 6]]不符合预期

  • 一维列表合并为二维列表的方式

    l1 = list([1,2,3,4])
    l2 = list([5,6,7,8])
    l3 = [l1] + [l2]
    print(l3)	# [[1, 2, 3, 4], [5, 6, 7, 8]]
    
5.3 字典排序
  • 使用python内置函数

    sorted(iterable, key=None, reverse=False)
    

    iterable:可迭代对象;key:用来比较的元素,取自迭代对象中;reverse:默认False升序, True降序

  • 返回值是列表,通过dict(list_)转为字典格式。其他字典排序的例子,参考 python字典排序方法

5.4 topic数据保存成csv/txt方法
  • 见文章:rosbag的topic数据保存成csv/txt文件

参考文章:
python解析rosbag的图像数据
python提取bag中的图像和点云数据
python同步解析bag包1
python解析bag包2
无法把列表正常加到字典中
一维列表合并为二维列表
python字典的使用
python用字典存储数据

created by shuaixio, 2022.06.04

你可能感兴趣的:(Python,ros系统,自动驾驶,python,自动驾驶,ros,rosbag,rostopic)