【Python】rosbag文件can消息提取生成asc文件

已于 2025-02-20 08:51:59 修改
阅读量485 收藏 9
点赞数 4
文章标签： python 经验分享
于 2025-02-14 17:24:30 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/qq_43265842/article/details/145637660
版权

目的将录制的rosbag文件内的can消息提取出来并转换为常规的can日志文件格式保存(如: asc文件)，方便使用can分析仪分析数据

1. rosbag文件解析

由于可能需要在window系统下使用，此处选择rosbags库实现，无需安装ros系统。

pip install rosbags

参考rosbags官方文档示例，修改过滤条件为" x.msgtype == ‘can_msgs/msg/Frame’ "即可遍历出目标rosbag文件内的所有can消息话题

目标rosbag文件录制的can消息类型为can_msgs::Frame

from pathlib import Path

from rosbags.highlevel import AnyReader
from rosbags.typesys import Stores, get_typestore

bagpath = Path('/home/ros/rosbag_2020_03_24')

# Create a type store to use if the bag has no message definitions.
typestore = get_typestore(Stores.ROS2_FOXY)

# Create reader instance and open for reading.
with AnyReader([bagpath], default_typestore=typestore) as reader:
    connections = [x for x in reader.connections if x.msgtype == 'can_msgs/msg/Frame']
    for connection, timestamp, rawdata in reader.messages(connections=connections):
         msg = reader.deserialize(rawdata, connection.msgtype)
         print(hex(msg.id).upper() + "\t\t" + " ".join(msg.data))

2. CAN日志文件生成

参考vector官方文档 CAN_LOG_TRIGGER_ASC_Format.pdf format v7.2生成实现

2.1 文件头生成

	def GenerateHeaderInfo(self)->str:
        header_info_template = '''// created by jluoyy-tool-can_format
date {WeekDay} {Month} {Day} {FullTime} {Year}
base {data_format} timestamps {time_format}
internal events logged
// version 7.2.0

Begin Triggerblock {WeekDay} {Month} {Day} {FullTime} {Year}
'''
        current_local_time = time.localtime(time.time())

        WeekDay  = time.strftime("%a", current_local_time)
        Month    = time.strftime("%b", current_local_time)
        Day      = time.strftime("%d", current_local_time)
        FullTime = time.strftime("%H:%M:%S.000 %p", current_local_time)
        Year     = time.strftime("%Y", current_local_time)

        header_info = header_info_template.format(
            WeekDay = WeekDay, Month = Month, Day = Day, FullTime = FullTime, Year = Year,
            data_format = self._data_format_, time_format = self._time_format_
        )

        self._frame_cnt_ = 0

        return header_info

2.2 CANMessage

    def GenerateCANMessage(self, timestamp_seconds: float, channel: int, msg_dir: str, can_id: int, byte_array: List[int])->str:

        self.SetBeginTimeStamp(timestamp_seconds)

        can_message_line_template = "{Time}\t{Channel}\t{ID}\t\t\t{Dir}   d {DLC} {Bytes} {MessageFlags}"

        ID           = "%X"%(can_id)
        Channel      = str(channel)
        Bytes        = " ".join(["%02X"%(x) for x in byte_array])
        DLC          = len(byte_array)
        MessageFlags = ""
        Time         = "%.8f"%(timestamp_seconds - self._begin_timestamp_)
        if msg_dir.lower().startswith("tx"):
            Dir = "Tx"
        else:
            Dir = "Rx"

        can_message_data = can_message_line_template.format(
            Time = Time, Channel = Channel, ID = ID, Dir = Dir, DLC = DLC, Bytes = Bytes, MessageFlags = MessageFlags
        )

        return can_message_data

2.3 CANExtendedMessage

    def GenerateCANExtendedMessage(self, timestamp_seconds: float, channel: int, msg_dir: str, can_id: int, byte_array: List[int])->str:

        self.SetBeginTimeStamp(timestamp_seconds)

        can_message_line_template = "{Time}\t{Channel}\t{ID}{Symbol_01}{Dir}   d {DLC} {Bytes} {MessageFlags}"
        
        ID           = "%Xx"%(can_id)
        Channel      = str(channel)
        Bytes        = " ".join(["%02X"%(x) for x in byte_array])
        DLC          = len(byte_array)
        MessageFlags = ""
        Time         = "%.8f"%(timestamp_seconds - self._begin_timestamp_)
        Symbol_01    = "\t" * (3 - int(len(ID)/4))
        if msg_dir.lower().startswith("tx"):
            Dir = "Tx"
        else:
            Dir = "Rx"

        can_message_data = can_message_line_template.format(
            Time = Time, Channel = Channel, ID = ID, Dir = Dir, DLC = DLC, Bytes = Bytes, MessageFlags = MessageFlags, 
            Symbol_01 = Symbol_01
        )

        return can_message_data

2.4 CANRemoteFrame

    def GenerateCANRemoteFrame(self, timestamp_seconds: float, channel: int, msg_dir: str, can_id: int)->str:

        self.SetBeginTimeStamp(timestamp_seconds)

        can_message_line_template = "{Time}\t{Channel}\t{ID}\t\t\t{Dir}\tr"
        
        ID           = "%X"%(can_id)
        Channel      = str(channel)
        Time         = "%.8f"%(timestamp_seconds - self._begin_timestamp_)
        if msg_dir.lower().startswith("tx"):
            Dir = "Tx"
        else:
            Dir = "Rx"

        can_message_data = can_message_line_template.format(
            Time = Time, Channel = Channel, ID = ID, Dir = Dir
        )

        return can_message_data

2.5 CANErrorFrame

    def GenerateCANErrorFrame(self, timestamp_seconds: float, channel: int, msg_dir: str)->str:

        self.SetBeginTimeStamp(timestamp_seconds)

        can_message_line_template = "{Time}\t{Channel}  ErrorFrame"

        Channel      = str(channel)
        Time         = "%.8f"%(timestamp_seconds - self._begin_timestamp_)

        can_message_data = can_message_line_template.format(
            Time = Time, Channel = Channel
        )

        return can_message_data

3. 完整代码

该代码仅为简单示例实现，channel、dir等参数暂未携带在包内，代码内直接设置为默认参数，能满足个人当前需求。若需要按实际通道或收发方向分类，需要按各自需求自行调整。

# -*- coding: utf-8 -*-

# ********************************************************** #
# Cause I know you are my polaris.                           #
# ********************************************************** #

import os,sys,time
import tkinter as tk
from typing import List
from pathlib import Path
from tkinter import filedialog

try:
    import rosbags, rospkg
    from rosbags.highlevel import AnyReader
    from rosbags.typesys import Stores, get_typestore
except:
    os.system("pip install rosbags rospkg")
    os.system("python -m pip install rosbags rospkg")
    import rosbags, rospkg
    from rosbags.highlevel import AnyReader
    from rosbags.typesys import Stores, get_typestore


class CANASC:
    def __init__(self, file_path:str="") -> None:
        self._data_format_ = "hex"
        self._time_format_ = "absolute"
        self._begin_timestamp_ = 0.0
        self._frame_cnt_ = 0

        if False is file_path.endswith(".asc"):
            self._file_path_  = ""
            self._file_stream_ = None
            raise Exception("文件类型错误, 应当为asc文件类型")
        else:
            self._file_path_  = file_path
            self._file_stream_ = open(file_path, 'w', encoding="utf-8")
        pass
    
    def __del__(self):
        self._file_stream_.write("End Triggerblock")
        self._file_stream_.close()
        pass

    def GenerateHeaderInfo(self)->str:
        header_info_template = '''// created by jluoyy-tool-can_format
date {WeekDay} {Month} {Day} {FullTime} {Year}
base {data_format} timestamps {time_format}
internal events logged
// version 7.2.0

Begin Triggerblock {WeekDay} {Month} {Day} {FullTime} {Year}
'''
        current_local_time = time.localtime(time.time())

        WeekDay  = time.strftime("%a", current_local_time)
        Month    = time.strftime("%b", current_local_time)
        Day      = time.strftime("%d", current_local_time)
        FullTime = time.strftime("%H:%M:%S.000 %p", current_local_time)
        Year     = time.strftime("%Y", current_local_time)

        header_info = header_info_template.format(
            WeekDay = WeekDay, Month = Month, Day = Day, FullTime = FullTime, Year = Year,
            data_format = self._data_format_, time_format = self._time_format_
        )

        self._frame_cnt_ = 0
        self._file_stream_.write(header_info)

        return header_info
    
    def SetBeginTimeStamp(self, timestamp_seconds: float):
        if 0 == self._frame_cnt_:
            self._begin_timestamp_ = timestamp_seconds
            self._frame_cnt_ += 1
            self._file_stream_.write("\t0.00000000 Start of measurement\n")
        else:
            self._frame_cnt_ += 1
    
    def GenerateCANMessage(self, timestamp_seconds: float, channel: int, msg_dir: str, can_id: int, byte_array: List[int])->str:

        self.SetBeginTimeStamp(timestamp_seconds)

        can_message_line_template = "{Time}\t{Channel}\t{ID}\t\t\t{Dir}   d {DLC} {Bytes} {MessageFlags}"

        ID           = "%X"%(can_id)
        Channel      = str(channel)
        Bytes        = " ".join(["%02X"%(x) for x in byte_array])
        DLC          = len(byte_array)
        MessageFlags = ""
        Time         = "%.8f"%(timestamp_seconds - self._begin_timestamp_)
        if msg_dir.lower().startswith("tx"):
            Dir = "Tx"
        else:
            Dir = "Rx"

        can_message_data = can_message_line_template.format(
            Time = Time, Channel = Channel, ID = ID, Dir = Dir, DLC = DLC, Bytes = Bytes, MessageFlags = MessageFlags
        )
        
        self._file_stream_.write("\t" + can_message_data + "\n")

        return can_message_data

    def GenerateCANExtendedMessage(self, timestamp_seconds: float, channel: int, msg_dir: str, can_id: int, byte_array: List[int])->str:

        self.SetBeginTimeStamp(timestamp_seconds)

        can_message_line_template = "{Time}\t{Channel}\t{ID}{Symbol_01}{Dir}   d {DLC} {Bytes} {MessageFlags}"
        
        ID           = "%Xx"%(can_id)
        Channel      = str(channel)
        Bytes        = " ".join(["%02X"%(x) for x in byte_array])
        DLC          = len(byte_array)
        MessageFlags = ""
        Time         = "%.8f"%(timestamp_seconds - self._begin_timestamp_)
        Symbol_01    = "\t" * (3 - int(len(ID)/4))
        if msg_dir.lower().startswith("tx"):
            Dir = "Tx"
        else:
            Dir = "Rx"

        can_message_data = can_message_line_template.format(
            Time = Time, Channel = Channel, ID = ID, Dir = Dir, DLC = DLC, Bytes = Bytes, MessageFlags = MessageFlags, 
            Symbol_01 = Symbol_01
        )
        
        self._file_stream_.write("\t" + can_message_data + "\n")

        return can_message_data

    def GenerateCANRemoteFrame(self, timestamp_seconds: float, channel: int, msg_dir: str, can_id: int)->str:

        self.SetBeginTimeStamp(timestamp_seconds)

        can_message_line_template = "{Time}\t{Channel}\t{ID}\t\t\t{Dir}\tr"
        
        ID           = "%X"%(can_id)
        Channel      = str(channel)
        Time         = "%.8f"%(timestamp_seconds - self._begin_timestamp_)
        if msg_dir.lower().startswith("tx"):
            Dir = "Tx"
        else:
            Dir = "Rx"

        can_message_data = can_message_line_template.format(
            Time = Time, Channel = Channel, ID = ID, Dir = Dir
        )

        self._file_stream_.write("\t" + can_message_data + "\n")

        return can_message_data

    def GenerateCANErrorFrame(self, timestamp_seconds: float, channel: int, msg_dir: str)->str:

        self.SetBeginTimeStamp(timestamp_seconds)

        can_message_line_template = "{Time}\t{Channel}  ErrorFrame"

        Channel      = str(channel)
        Time         = "%.8f"%(timestamp_seconds - self._begin_timestamp_)

        can_message_data = can_message_line_template.format(
            Time = Time, Channel = Channel
        )

        self._file_stream_.write("\t" + can_message_data + "\n")

        return can_message_data
# class CANASC:

# ----------------------------------------------- #

def main():
    
    input_path = ""
    root = tk.Tk()
    
    while "" == input_path:
        input_path = filedialog.askopenfilename(title="选择rosbag文件", filetypes={("roabag文件", "*.bag"), ("ALL", "*.*")}, defaultextension=".bag")

        if 0 == len(input_path):
            input_path = ""

        if "" == input_path or not os.path.exists(input_path):
            print("选择的文件路径错误: " + input_path)
            sel = input("是否重新选择(Y/N)：")
            if 'Y' == sel.upper():
                input_path = ""
            else:
                return
                
    root.destroy()
    root.mainloop()

    output_path = input_path.replace(".bag", "_bag") + ".asc"

    can_msg = CANASC(output_path)
    can_msg.GenerateHeaderInfo()

    rosbag_path = Path(input_path)
    typestore = get_typestore(Stores.ROS2_FOXY)
    
    with AnyReader([rosbag_path], default_typestore=typestore) as reader:
        connections = [x for x in reader.connections if x.msgtype == 'can_msgs/msg/Frame']
        is_stored_begintime = False
        for connection, timestamp, rawdata in reader.messages(connections=connections):
            if False is is_stored_begintime:
                can_msg.SetBeginTimeStamp(timestamp/1e9)
                is_stored_begintime = True
            msg = reader.deserialize(rawdata, connection.msgtype)

            line_text = ""
            if True is msg.is_error:
                line_text = can_msg.GenerateCANErrorFrame(timestamp/1e9, 1, "RX")
            elif True is msg.is_rtr:
                line_text = can_msg.GenerateCANRemoteFrame(timestamp/1e9, 1, "RX", msg.id)
            elif True is msg.is_extended:
               line_text = can_msg.GenerateCANExtendedMessage(timestamp/1e9, 1, "RX", msg.id, msg.data)
            else:
               line_text = can_msg.GenerateCANMessage(timestamp/1e9, 1, "RX", msg.id, msg.data)
        
            print(line_text.replace("\t", "  ") + "\t/* from " + connection.topic + " */", flush=True)
        pass
    pass

    del can_msg

    return

# ************************************* #

if __name__ == "__main__":
    main()
Jluoyy
关注
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值