Autoware.universe部署06：使用DBC文件进行UDP的CAN通信代码编写

目录标题

一、安装DBC文件编辑工具VectorCANdb++
二、编写DBC文件
- 2.1 CAN通信协议
- 2.2 编写DBC文件
- - 2.2.1 根据CAN协议设置signals
  - 2.2.2 设置报文
  - 2.2.3 建立节点
三、根据DBC文件编写ROS2驱动程序
四、实际通信调试

根据CAN协议编写DBC文件，通过DBC文件编写ROS2包进行UDP通信，获取底盘速度转发至Autoware.Universe以及订阅Autoware.Universe控制命令，下发至CAN控制底盘运动（本文适用于CAN盒通过网线连接进行UDP通信），本系列其他文章：
Autoware.universe部署01：Ubuntu20.04安装Autoware.universe并与Awsim联调
Autoware.universe部署02：高精Lanelet2地图的绘制
Autoware.universe部署03：与Carla（二进制版）联调
Autoware.universe部署04：universe传感器ROS2驱动
Autoware.universe部署05：实车调试

一、安装DBC文件编辑工具VectorCANdb++

在windows系统下安装VectorCANdb++，下载链接：https://www.vector.com/cn/zh/download/candb-31-sp3/
在这里插入图片描述
下载完成后常规安装，选择安装路径，其他过程一路Next：

安装完成后在左下角程序列表中可以找到

二、编写DBC文件

2.1 CAN通信协议

下面是松灵的HUNTER SE通信协议，是一款阿克曼模型可编程UGV（ UNMANNED GROUND VEHICLE ），它是一款采用阿克曼转向设计的底盘。下面只列举了三帧数据，每一帧数据包含一系列字节（例如：0x01, 0x11, 0x00,0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00）的数据，我们需要注意的是每一帧数据的发送与接收节点，帧ID（例如：0x01, 0x11），数据长度，以及功能数据（例如：0x00,0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00）：
在这里插入图片描述

2.2 编写DBC文件

有了通讯协议之后，打开VectorCANdb++，点击File->Create Database->CANTemplate.dbc后点击OK，创建文件名：
在这里插入图片描述

2.2.1 根据CAN协议设置signals

点击signals->new
在这里插入图片描述
以运动回馈帧的数据为例，根据上面的通讯协议数据帧包含8个字节byte，即8个信号signals，每个字节signals包含8个比特bit，协议中第3、4、5、6个字节没有定义数据，就默认为0，以速度高位为例：

Value Type高位选择有符号数据signed，低位选择无符号选择Unsigned
Length（信号bit长度）8
Factor（数据精度）一般为1
Minimum与Maximum根据表格来填，表格是-4800～4800包含的是两位数据，这里单个信号是不一样的。协议中给出的是十六进制数，而这边的最大值最小值范围是十进制数，转化一下：如果是有符号的字节符号占一位，那么转化成十进制就是-127～127，如果是无符号的就是0～255。如果要遵守协议即-4800～4800，那么高位最好写成-18～18『（4800-256）/256=17.5』，写大一点没问题

按照协议写好所有的信号如下：

2.2.2 设置报文

在Messages下新建报文：注意帧ID以及数据字节数
在这里插入图片描述
之后建立报文与信号的关系，鼠标左键按住设置好的signals，拖动到新建的Messages上面，注意顺序要与协议文件中顺序一致

如果顺序错了，双击signals设置开始bit数可以更改

全都拖好了，双击新建的Messages，然后点击Layout，如下图所示，可以检查一下报文设置是否正确（图片上的字节顺序，从右至左，从上到下，依次增大）
在这里插入图片描述

2.2.3 建立节点

（1）建立发送和接受节点
右键点击Network nodes -> New，只需输入创建的网络节点名字进行新建操作
（2）需要发送的报文直接拖到目标节点下的Tx Messages下面即可
（3）需要接收的报文添加：双击打开Receive，选择Mapped Rx Sig.，然后选择Add:all from one message添加我们建立的报文，会将信号添加如下：
在这里插入图片描述
检查节点间的收发关系：在”View”中选择”Communication Matrix…”，可以看到各个节点的收发信号：

在”File”中选择”Consistency Check”,此时会在一致性检查窗口中输出检查结果。会有状态信息及对应的说明，以供我们检查出错/警告报警的原因。下面是我们建立的没有问题的dbc
在这里插入图片描述
编写完成后即可保存，其内容如下：

VERSION ""


NS_ : 
	NS_DESC_
	CM_
	BA_DEF_
	BA_
	VAL_
	CAT_DEF_
	CAT_
	FILTER
	BA_DEF_DEF_
	EV_DATA_
	ENVVAR_DATA_
	SGTYPE_
	SGTYPE_VAL_
	BA_DEF_SGTYPE_
	BA_SGTYPE_
	SIG_TYPE_REF_
	VAL_TABLE_
	SIG_GROUP_
	SIG_VALTYPE_
	SIGTYPE_VALTYPE_
	BO_TX_BU_
	BA_DEF_REL_
	BA_REL_
	BA_DEF_DEF_REL_
	BU_SG_REL_
	BU_EV_REL_
	BU_BO_REL_
	SG_MUL_VAL_

BS_:

BU_: Base_CAN Base Control


BO_ 1057 Control_mode: 1 Control
 SG_ Control_mode : 0|1@1+ (1,0) [0|1] ""  Base

BO_ 273 Motion_Control: 8 Control
 SG_ Steer_Angle_control_L : 56|8@1+ (1,0) [0|255] ""  Base
 SG_ Steer_Angle_control_H : 48|8@1- (1,0) [-127|127] ""  Base
 SG_ Stay_7 : 40|8@1- (1,0) [0|0] ""  Base
 SG_ Stay_6 : 32|8@1- (1,0) [0|0] ""  Base
 SG_ Stay_5 : 24|8@1- (1,0) [0|0] ""  Base
 SG_ Stay_4 : 16|8@1- (1,0) [0|0] ""  Base
 SG_ Velocity_control_Low : 8|8@1+ (1,0) [0|255] ""  Base
 SG_ Velocity_control_High : 0|8@1- (1,0) [-127|127] ""  Base

BO_ 545 Motion_Feedback: 8 Base_CAN
 SG_ Steer_Angle_L : 56|8@1+ (1,0) [0|255] ""  Control
 SG_ Steer_Angle_H : 48|8@1- (1,0) [-127|127] ""  Control
 SG_ Stay_3 : 40|8@1- (1,0) [0|0] ""  Control
 SG_ Stay_2 : 32|8@1- (1,0) [0|0] ""  Control
 SG_ Stay_1 : 24|8@1- (1,0) [0|0] ""  Control
 SG_ Stay_0 : 16|8@1- (1,0) [0|0] ""  Control
 SG_ Velocity_Low : 8|8@1+ (1,0) [0|255] ""  Control
 SG_ Velocity_High : 0|8@1- (1,0) [-127|127] ""  Control



BA_DEF_  "MultiplexExtEnabled" ENUM  "No","Yes";
BA_DEF_  "BusType" STRING ;
BA_DEF_DEF_  "MultiplexExtEnabled" "No";
BA_DEF_DEF_  "BusType" "CAN";

重点关注报文以及信号：

报文格式：
BO_ MessageId MessageName: MessageSize Transmitter
			
		MessageId 			为10进制表示的报文ID，类型为longlogn型，即CAN ID
		MessageName 		报文的名字，与C语言命令规范相同
		MessageSize 		报文数据段字节数
		Transmitter 		该报文的网络节点，如果该报文没有指定发送节点，则该值需设置为”Vector__XXX”

举例：
以下是DBC中代表一条消息的描述信息
BO_ 545 Motion_Feedback: 8 Base_CAN


解释
BO_								代表一条消息的起始标识
545  							消息ID的十进制形式，=0x3f7
Motion_Feedback					消息名
8								消息报文长度，帧字节数
Base_CAN						发出该消息的网络节点，标识为Vector__XXX时未指明具体节点

信号格式
SG_ SignalName (SigTypeDefinition) : StartBit|SignalSize@ByteOrder ValueType (Factor,Offset) [Min|Max] Unit Receiver

		SignalName (SigTypeDefinition) 	表示该信号的名字 和 多路选择信号的定义
		SigTypeDefinition	是可选项，有3种格式：
				a> 空
				b> M 表示多路选择器信号
				c> m50 表示被多路选择器选择的信号，50表示当‘M’定义的信号的值等于50的时候，该报文使用此通路
		StartBit|SignalSize 表示该信号的起始位及信号长度
		ByteOrder 	表示信号的字节顺序：0代表Motorola格式，1代表Inter格式
		ValueType 	表示该信号的数值类型：+表示无符号数，-表示有符号数
		Factor,Offset 	表示因子，偏移量；这两个值用于信号的原始值与物理值之间的转换。 转换公式：物理值=原始值*因子+偏移量
		Min|Max 	表示该信号的最小值和最大值，即指定了该信号值的范围；这两个值为double类型
		Unit 	表示该信号的物理单位，为字符串类型
		Receiver 表示该信号的接收节点(可以是多个节点)；若该信号没有指定的接收节点，则必须设置为” Vector__XXX”

举例：
每条报文消息里面有多个报文信号，报文信号的信息的起始标识为"SG_", 
它以一个"BO_"开始至下一"BO_"之间的内容止，详细报文消息以缩进1或2个空格符形式类似树图子节点的方式呈现。
一条消息下的一个信号的信息，此处缩进一个空格
 SG_ Steer_Angle_L : 56|8@1+ (1,0) [0|255] ""  Control

解释：
SG_							代表一个信号信息的起始标识
Steer_Angle_L	     	 	信号名，分长名与短名，此处是短名。长名非必须存在，可以不定义
56							信号起始bit
|							分割符号
8							信号总bit长度
@1+							@0表示是Motorola格式（Intel格式是1），+表示是无符号数据
(1,0)						(精度值，偏移值）
[0|255]						[最小值|最大值]， 物理意义的最小与最大，现实世界的有物理意义的值，比如此处仪表续航里程最大999KM
""						"单位"
Control						接收处理此信号的节点，同样可以不指明，写为Vector__XXX

三、根据DBC文件编写ROS2驱动程序

在得到了dbc文件之后，即可调用cantools库进行UDP通信的程序编写

pip3 install cantools==39.0.0

参考栏目其他文章，Autoware.universe需要接收底盘反馈的速度/vehicle/status/velocity_status等消息，同时发送自定义的速度控制消息twist_cmd_feedback到底盘以控制小车运动，代码编写如下：

# -*- coding: utf-8 -*-
import math
import time
import socket
import cantools
import threading
import rclpy
from rclpy.node import Node
from builtin_interfaces.msg import Time
from binascii import hexlify
from geometry_msgs.msg import TwistStamped, Twist
from autoware_auto_control_msgs.msg import AckermannControlCommand
from autoware_auto_vehicle_msgs.msg import ControlModeReport, GearReport, SteeringReport, VelocityReport

# 创建UDP socket套接字
# AF_INET表示使用ipv4,默认不变，SOCK_DGRAM表示使用UDP通信协议
udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

# 绑定UDP的本机端口port
local_addr = ("192.168.1.102", 8882)  # 本地ip，端口号
udp_socket.bind(local_addr)  # 绑定端口

# 指定要接收的前五个字节的CAN协议数据
EXPECTED_DATA = bytes([0x08, 0x00, 0x00, 0x02, 0x11])

# 控制小车以0.15m/S的速度前进(0x96->150mm/s,150/100=0.15m/s)
test1 = bytes([0x08, 0x00, 0x00, 0x01, 0x11, 0x00,
              0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])
# 控制小车转向0.2rad(0xC8->200x0.001rad->0.2rad)
test2 = bytes([0x08, 0x00, 0x00, 0x01, 0x11, 0x00,
              0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC8])

# 运动控制消息
data_Control_mode = {'Velocity_control_High': 0, 'Velocity_control_Low': 0,
                     'Stay_4': 0, 'Stay_5': 0, 'Stay_6': 0, 'Stay_7': 0,
                     'Steer_Angle_control_H': 0, 'Steer_Angle_control_L': 0}

# 运动反馈消息
data_Motion_Feedback = {'Velocity_High': 32, 'Velocity_Low': 0,
                        'Stay_0': 0, 'Stay_1': 0, 'Stay_2': 0, 'Stay_3': 0,
                        'Steer_Angle_H': 0, 'Steer_Angle_L': 0, }

# Control_mode的帧信息与帧ID
message_Control_mode_front = bytes([0x08, 0x00, 0x00, 0x01, 0x11])

# ---------------------------------------------------------接收底盘数据
def calculate_speed(linear_speed):
    # 根据通讯协议将线速度m/s转化为int16的高低位
    # 车体行进速度，单位mm/s(有效值+ -4800)
    if linear_speed >= 0:
        if linear_speed > 4.8:
            linear_speed = 4.8
        Speed_H, Speed_L = divmod(int(linear_speed * 1000), 256)
        data_Control_mode['Velocity_control_Low'] = Speed_L
        data_Control_mode['Velocity_control_High'] = Speed_H
    else:
        if linear_speed < -4.8:
            linear_speed = -4.8
        Speed_H, Speed_L = divmod(int(-linear_speed * 1000), 256)
        data_Control_mode['Velocity_control_Low'] = Speed_L
        # 设置最高位为1表示负数
        data_Control_mode['Velocity_control_High'] = 0x80 | Speed_H

    # print('EV_Speed_H:%f' % EV_Speed_H)
    # print('EV_Speed_L:%f' % EV_Speed_L)


def calculate_angle(linear_speed, angular_speed):
    # 转向内转角角度单位：0.001rad (有效值+-400)
    # 转弯的角度 = arctan(( 角速度 / 线速度 ) * 车长 )
    Steer_Angle = math.atan((angular_speed/linear_speed) * 1)
    # print('Steer_Angle:%f' % Steer_Angle)

    if Steer_Angle >= 0:
        if Steer_Angle > 0.4:
            Steer_Angle = 0.4
        Steer_Angle_H, Steer_Angle_L = divmod(int(Steer_Angle * 1000), 256)
        data_Control_mode['Steer_Angle_control_L'] = Steer_Angle_L
        data_Control_mode['Steer_Angle_control_H'] = Steer_Angle_H
    else:
        if linear_speed < -0.4:
            linear_speed = -0.4
        Steer_Angle_H, Steer_Angle_L = divmod(int(-Steer_Angle * 1000), 256)
        data_Control_mode['Steer_Angle_control_L'] = Steer_Angle_L
        # 设置最高位为1表示负数
        data_Control_mode['Steer_Angle_control_H'] = 0x80 | Steer_Angle_H


def calculate_angle(Steer_Angle):
    # print("Steer_Angle:", Steer_Angle)
    if Steer_Angle >= 0:
        if Steer_Angle > 0.4:
            Steer_Angle = 0.4
        Steer_Angle_H, Steer_Angle_L = divmod(int(Steer_Angle * 1000), 256)
        data_Control_mode['Steer_Angle_control_L'] = Steer_Angle_L
        data_Control_mode['Steer_Angle_control_H'] = Steer_Angle_H
    else:
        if linear_speed < -0.4:
            linear_speed = -0.4
        Steer_Angle_H, Steer_Angle_L = divmod(int(-Steer_Angle * 1000), 256)
        data_Control_mode['Steer_Angle_control_L'] = Steer_Angle_L
        # 设置最高位为1表示负数
        data_Control_mode['Steer_Angle_control_H'] = 0x80 | Steer_Angle_H


# udp向底盘发送can协议
def udp_send_can(message_name):
    # 底盘ip和端口号
    udp_socket.sendto(message_name, ("192.168.1.10", 6666))

# 处理接收到的CAN消息的函数
def process_can_message(data, node):
    # 解码CAN消息
    can_data = list(data[5:])  # 从第6个字节开始是CAN数据
    message = node.db.decode_message("Motion_Feedback", can_data)

    # 打印解码结果
    # print(message)
    # print('Steer_Angle_L:', message['Steer_Angle_L'])
    # print('Steer_Angle_H:', message['Steer_Angle_H'])
    # print('DM_Speed_L:', message['Velocity_Low'])
    # print('DM_Speed_H:', message['Velocity_High'])

    # 处理CAN中接收到的数据，转化成线速度和角速度
    feedback_twist_linear_x = (
        message['Velocity_High'] * 256 + message['Velocity_Low']) / 1000
    Steer_Angle = (message['Steer_Angle_H'] * 256 +
                   message['Steer_Angle_L'] ) / 1000
    
    # 角速度 = tan(转向角度) * 线速度 / 前后轮轴距
    feedback_twist_angular_z = math.tan(Steer_Angle) * feedback_twist_linear_x / 1

    # 发布处理后的Twist消息到另一个话题
    node.publish_data(feedback_twist_linear_x, feedback_twist_angular_z)
    node.pubilsh_control_mode(1)
    node.pubilsh_gear(2)
    node.pubilsh_steering(Steer_Angle)
    node.pubilsh_velocity("base_link", feedback_twist_linear_x, 0.0, 0.0)


# 接收数据的线程函数
def receive_data(node):
    while rclpy.ok():
        # 一帧指令有多少字节
        data, addr = udp_socket.recvfrom(13)
        # print(hexlify(data).decode('ascii'))

        # 确保接收到的数据满足预期的CAN数据
        if data[:5] == EXPECTED_DATA:
            # 在新的线程中处理CAN消息，以保证实时性
            threading.Thread(target=process_can_message,
                             args=(data, node)).start()


# -----------------------------------------------------------控制底盘
class TopicSubscriberPublisher(Node):
    def __init__(self):
        super().__init__('cmd_vel_to_can_hunter')
        # 加载dbc文件
        self.declare_parameter("dbc_hunter")
        dbcfile = self.get_parameter(
            "dbc_hunter").get_parameter_value().string_value
        self.db = cantools.database.load_file(dbcfile)

        # CAN指令模式：08 00 00 04 21 01
        self.can_command = bytes([0x08, 0x00, 0x00, 0x04, 0x21, 0x01])

        self.subscriber = self.create_subscription(
            AckermannControlCommand, 'control/command/control_cmd', self.sub_callback, 10)
        
        self.publisher_data = self.create_publisher(
            Twist, 'twist_cmd_feedback', 10)
        self.publisher_control_mode = self.create_publisher(
            ControlModeReport, '/vehicle/status/control_mode', 10)
        self.publisher_gear = self.create_publisher(
            GearReport, '/vehicle/status/gear_status', 10)
        self.publisher_steering = self.create_publisher(
            SteeringReport, '/vehicle/status/steering_status', 10)
        self.publisher_velocity = self.create_publisher(
            VelocityReport, '/vehicle/status/velocity_status', 10)
        # self.get_logger().info('TopicSubscriberPublisher node initialized')

    def sub_callback(self, msg):
        # 1. 发送CAN指令模式：08 00 00 04 21 01
        udp_send_can(self.can_command)

        # 2. 接收autoware传来的线速度和角速度
        Speed = msg.longitudinal.speed
        # angular_velocity = msg.lateral.steering_tire_rotation_rate
        angular_rad = msg.lateral.steering_tire_angle
        # print('angular_velocity:%f' % angular_velocity)
        # print('angular_rad:%f' % angular_rad)

        # 3. 计算速度、角度
        calculate_speed(Speed)
        # calculate_angle(1, angular_velocity)
        calculate_angle(Speed, angular_rad)

        # 4. 发送can消息
        message_Motion_Control = self.db.encode_message(
            "Motion_Control", data_Control_mode)
        message_linear_velocity = message_Control_mode_front + message_Motion_Control
        # print(hexlify(message_linear_velocity).decode('ascii'))
        udp_send_can(message_linear_velocity)

    def publish_data(self, data1, data2):
        msg = Twist()
        msg.linear.x = data1
        msg.angular.z = data2
        self.publisher_data.publish(msg)

    def pubilsh_control_mode(self, data):
        msg = ControlModeReport()
        msg.mode = data
        self.publisher_control_mode.publish(msg)

    def pubilsh_gear(self, data):
        msg = GearReport()
        msg.report = data
        self.publisher_gear.publish(msg)

    def pubilsh_steering(self, data):
        msg = SteeringReport()
        msg.steering_tire_angle = data
        self.publisher_steering.publish(msg)

    def pubilsh_velocity(self, data1, data2, data3, data4):
        msg = VelocityReport()
        # 获取当前时间
        # 秒
        sec_ = int(time.time())
        # 纳秒
        nanosec_ = int((time.time()-sec_)*1e9)
        msg.header.stamp = Time(sec=sec_, nanosec=nanosec_)
        msg.header.frame_id = data1
        msg.longitudinal_velocity = data2
        msg.lateral_velocity = data3
        msg.heading_rate = data4
        self.publisher_velocity.publish(msg)


def main():
    # 初始化
    rclpy.init()

    # 新建一个节点
    node = TopicSubscriberPublisher()

    # 启动接收CAN数据的线程
    threading.Thread(target=receive_data, args=(node,)).start()

    # 在CAN指令模式下，需要保证0X111指令帧以小于500MS的周期（建议周期20MS）发送，否则HUNTER
    # SE会判定为控制信号心跳丢失而进入报错（0X211反馈上层通讯失联），系统报错后会进入待机模式
    # 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.spin(node)

    # 清理并关闭ros2节点
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

编写setup.py和launch文件

from setuptools import setup

package_name = 'can_ros2_bridge'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    # 安装文件至install
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        ('share/' +package_name, ['launch/can_hunter.launch.py']),
        ('share/' +package_name, ['config/Hunter.dbc']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='car',
    maintainer_email='car@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    # 可执行文件
    entry_points={
        'console_scripts': [
            'cmd_vel_to_can_hunter = can_ros2_bridge.send_message_hunter:main',
        ],
    },
)

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 去install找配置文件
    config_hunter = os.path.join(
      get_package_share_directory('can_ros2_bridge'),
      'Hunter.dbc'
      )

    can_ros2_bridge = Node(
        package='can_ros2_bridge',
        executable='cmd_vel_to_can_hunter',
        name='can',
        parameters=[{'dbc_hunter': config_hunter}],
        output="both"
    )

    return LaunchDescription(
        [
            can_ros2_bridge,
        ]
    )

四、实际通信调试

上述代码中，有两个需要注意的，分别是主机和CAN盒的IP以及端口，CAN盒的IP以及端口是固定的（可能需要在上位机中修改），可以在盒子上看到，需要设置本机IP与CAN盒在同一局域网下，例如CAN盒的是"192.168.1.10", 6666，那么设置主机静态IP，最后一位不同即可：
在这里插入图片描述
接着安装wireshark网络调试工具：