根据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文章来源:https://www.toymoban.com/news/detail-856928.html
sudo wireshark
可以看到底盘反馈的数据:6666->8882端口,192.168.4.101->192.168.4.101(图不是我的因此IP不一样,关键是数据传输路线)
数据通了之后修改代码IP和端口,即可实现CAN通信文章来源地址https://www.toymoban.com/news/detail-856928.html
到了这里,关于Autoware.universe部署06:使用DBC文件进行UDP的CAN通信代码编写的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!