目录标题
- 一、安装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网络调试工具：
sudo apt-get install wireshark
然后打开wireshark
sudo wireshark
可以看到底盘反馈的数据：6666->8882端口，192.168.4.101->192.168.4.101（图不是我的因此IP不一样，关键是数据传输路线）
数据通了之后修改代码IP和端口，即可实现CAN通信
