本文介绍了 Autoware.universe 各个传感器ROS2驱动，本系列其他文章：
Autoware.universe部署01：Ubuntu20.04安装Autoware.universe并与Awsim联调
Autoware.universe部署02：高精Lanelet2地图的绘制
Autoware.universe部署03：与Carla（二进制版）联调

一、激光雷达驱动

以速腾32线激光雷达为例：
(1) 建立工作空间，下载两个功能包：

• 官方驱动下载地址：http://iyenn.com/index/link?url=https://github.com/RoboSense-LiDAR/rslidar_sdk
• 官方雷达ROS2消息类型下载地址：http://iyenn.com/index/link?url=https://github.com/RoboSense-LiDAR/rslidar_msg

mkdir -p ~/rs_driver/src
cd ~/rs_driver/
1
2

（2）速腾聚创雷达ROS1，ROS2代码都是同一个链接，所以将ununtu18.04里面用的驱动拿了过来，然后打开senser_driver/src/rslidar_sdk/CmakeLists.txt，选择CLOCON编译方式

（3）修改参数配置
主要是修改为你的lidar类型，坐标系以及点云话题：

（4）将下载的两个功能包一起放到src下，将原来的package.xml文件重命名为package.xml.bak备份，把package_ros2.xml文件重命名为package.xml，然后编译:

#编译
cd ~/rs_driver/
colcon build
1
2
3

（5）设备连接与网络配置

• 准备一套速腾聚创16线激光雷达（包括激光雷达、电源盒子、航插头以及网线）；
• 本文使用的PC系统是Ubuntu 18.04系统，也可使用Ubuntu 16.04或Ubuntu 20.04；
• 准备AC 220V电源或DC 12V电源；

如下图所示，将雷达一端的航插头接口与雷达电源盒子的航插头接口两个，对准红点接好；

电源盒子接上电源，接上网线（网线一端接入到PC，一端接入到电源盒子）如下图：

雷达点云盒子连接雷达、点云以及网线，网线另一端连接计算机（或者通过交换机转接），设置网络为静态IP，IP地址：192.168.1.102，子网掩码：255.255.255.0

（6）驱动雷达

source install/setup.bash
ros2 launch rslidar_sdk start.py
1
2

在打开的rviz2中，Frame坐标系改成velodyne，点云话题选择/points_raw，可以成功显示雷达点云

二、IMU驱动

本文设备为亚伯智能十轴IMU惯导模块

2.1 上位机配置

（1） 安装串口驱动
打开WIndows，在配套的资料中找到CP2102_USB驱动.zip文件，下载到本地电脑并解压，双击CP210xVCPInstaller_x64.exe文件打开安装程序，然后跟着提示一直点击下一步，直到完成即可。
（2）连接上位机
解压资料中的IMU标准上位机(V6.2.60).zip压缩包，进入上位机软件找到MiniIMU.exe。双击打开上位机软件，可以看到提示未能搜索到设备，关闭提示框。如果已经连接IMU模块会自动连接上模块。

将IMU模块通过USB-TypeC数据线连接到电脑上。然后点击然后点击菜单栏的‘自动监测设备’。连接成功，可以看到角度X、角度Y、角度Z有数据变化。如果连接不成功，请确认是否已经安装好串口驱动，或者换个USB口试试。

IMU模块出厂已经烧录好固件，连接上位机后可以用上位机查看IMU模块的当前姿态。
点击’三维‘，可以看到弹出一个窗口，默认会展示一台汽车模型，当我们改变IMU模块的姿态时，模型的姿态会跟着IMU模块的变化。Y轴为车头，IMU模块也应Y轴向前放置。

（3）参数配置
点击菜单栏上的‘配置’，会弹出一个窗口，查看右下角的状态，一定要是‘在线’才是正确的，如果出现‘离 线’则说明没连接上IMU模块。
通讯速率：串口通讯速率，默认9600，可选择其他波特率（4800~921600）。
回传速率：串口回传数据的速率，默认为10Hz，可修改为0.2Hz~200Hz。10HZ指的是1S回传10个数据包，按默认回传1个数据包是11个字节。
注：如果需要200HZ的回传速率，则只能勾选三个量，比如“加速度”，“角速度”，“角度”。
注：如果回传内容较多，同时通信的波特率又较低的情况下，可能没法传输这么多数据，此时模块会自动降频，并以允许的最大输出速率进行输出。简单点说 就是回传速率高的话，波特率也要设置高一点，一般用115200。这里我们波特率改为480600，回传速率改为100Hz

设置串口输出的内容，串口输出内容可查看协议文件解析数据。
注意：勾选上“GPS原始”之后模块只输出GPS原始的信息了，其它数据都不会输出。

4.2 IMU校准

（1）加速度计校准
将IMU模块平放在桌子或者其他设备上，如果发现‘角度X‘和’角度Y‘大于1°，那么需要进行加速度计校准。点击菜单栏中的’配置‘打开配置界面，保证IMU模块平放的情况下，点击’加速度‘，然后再点击’设置角度参考‘。

校准之后可以看到‘角度X‘和’角度Y‘接近于0°

（2）IMU模块上电后，打开上位机显示3D模型，转动模块Z轴航向角，3D模型抖动，或者反应迟钝，请在上
位机进行磁力校准设备。
点击配置界面中的‘磁场’，会弹出校准磁场的界面。磁场校准有多种校准方式，比较常规的校准方式为球形拟合法。

分别校准X/Y/Z轴，看chartYZ/chartXZ/chartXY变化。将IMU模块水平放置。然后缓慢绕X/Y/Z轴旋转360°以
上，chart界面蓝色数据分布在绿线旁为正常。为了数据更加准确，可多转几圈。

XYZ三轴都校准完成后点击‘结束校准’。注意，在校准Y轴时，只看chartXZ的数据就好，其他两个视图也有数据，不需要关心。同理其他两个轴也是一样。
（3）陀螺仪校准
陀螺仪默认开启自动校准功能，不需要额外设置。保持开启陀螺仪自动校准功能即可。

4.3 安装ROS驱动

（1）安装依赖：

pip3 install pyserial
1

（2）在配套资料中找到ROS2驱动包wit_ros2_imu，放入工作空间编译，遇到以下警告：

根据提示将setup.cfg的横线改为下划线：

（3）绑定端口
为了防止多个usb设备同时插入的时候，系统识别错误，我们给该模块的串口名字绑定成/dev/imu_usb，终端输入

cd src/wit_ros_imu
sudo chmod 777 bind_usb.sh
sudo sh bind_usb.sh
1
2
3

重新插拔连接IMU模块的USB数据线。以生效绑定的端口，输入以下指令检测绑定端口是否成功，

ll /dev/imu_usb
1

不一定是ttyUSB0，只要显示是USB设备就行了。我们下面安装的GNSS模块也有一个串口，这里可以将其也给绑定。首先通过插拔两个端口，我们可以lsusb查看端口信息：

其中GNSS的为：

0403:6001 Future Technology Devices International, Ltd FT232 Serial (UART) IC
1

IMU模块的为：

10c4:ea60 Silicon Labs CP210x UART Bridge
1

然后创建.rules文件（或者直接在上面IMU的文件中修改），填写以下内容

KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="imu_usb"
KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", MODE:="0777", SYMLINK+="gnss"
1
2

然后：

sudo cp imu_usb.rules /etc/udev/rules.d
service udev reload
service udev restart
1
2
3

输入以下指令检测绑定端口是否成功，

ll /dev/imu_usb
ll /dev/gnss
1
2

记得把GNSS端口改成我们绑定的
（4）修改波特率
程序默认是使用9600的波特率，我们在上位机修改了波特率460800，那么则需要修改源码中的波特率，源码修改波特率的位置是，wit_ros2_imu/wit_ros2_imu/wit_ros2_imu.py

#149行
def driver_loop(self, port_name):
# 打开串口
    try:
        wt_imu = serial.Serial(port="/dev/imu_usb", baudrate=460800, timeout=0.5)
1
2
3
4
5

把9600改成上位机上修改的波特率，然后保存后退出，最后回到工作空间目录下进行编译即可。
（5）运行测试

source install/setup.bash
ros2 launch wit_ros2_imu rviz_and_imu.launch.py
1
2

报下面的错误，是因为launch语法的问题，可能是官方使用的ROS版本比较老

修改launch，并重新编译：

再次运行

查看IMU话题：
ros2 topic echo /imu/data_raw

三、CAN驱动

接收autoware.universe的控制话题，并下发到底盘控制实车运动，同时接收底盘反馈的车的速度信息，发送给Autoware进行位姿初始化，编写了ROS2版本的控制底盘程序（CAN协议不同不能通用，只能作为参考）：

# -*- 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

# 弧度转角度系数
radian2angle = 57.29577951308232

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

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

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

# 档位+车速0指令：08 00 00 00 E2 01 00 00 00 00 00 00 00
drive_by_wire_command = bytes([0x08, 0x00, 0x00, 0x00, 0xE2, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])
# 控制方向盘转到100度，转速100
test1 = bytes([0x08, 0x00, 0x00, 0x04, 0x69, 0x20, 0x00, 0x00, 0x04, 0x64, 0x00, 0x64, 0x24])
# 控制方向盘转到0度，转速50
test2 = bytes([0x08, 0x00, 0x00, 0x04, 0x69, 0x20, 0x00, 0x00, 0x04, 0x00, 0x00, 0x32, 0x16])

data_EV1 = {'Gear': 0, 'Invalid': 0, 'EV_Speed_L': 0, 'EV_Speed_H' : 0, 
            'Stay0': 0, 'Stay1': 0, 'Stay2': 0, 'Stay3': 0, 'Stay4': 0}

data_EPS2 = {'Work_mode': 32, 'Stay0': 0, 'Stay1': 0, 'Steer_Angle_H':0, 
             'Steer_Angle_L':0, 'Angle_alignment': 0, 'Angular_velocity': 100}

message_EV1_front = bytes([0x08, 0x00, 0x00, 0x00, 0xE2]) # EV1的帧信息与帧ID
message_EPS2_front = bytes([0x08, 0x00, 0x00, 0x04, 0x69]) # EPS2的帧信息与帧ID

# 计算异或校验值
def Calculate_XOR_value(dict_data):
    # 提取所有值
    values = list(dict_data.values())

    # 计算异或校验码
    result = values[0]
    for value in values[1:]:
        result ^= value

    # 返回
    return result

def calculate_speed(linear_speed):
    EV_Speed_H = int((linear_speed * 185)) // 256
    EV_Speed_L = int((linear_speed * 185)) % 256
    # print('EV_Speed_H:%f' % EV_Speed_H)
    # print('EV_Speed_L:%f' % EV_Speed_L)
    data_EV1['EV_Speed_L'] = EV_Speed_L
    data_EV1['EV_Speed_H'] = EV_Speed_H

def calculate_angle(linear_speed, angular_speed):
    # 转弯的角度 = arctan(( 角速度 / 线速度 ) * 车长 ) 
    Steer_Angle = -math.atan((angular_speed/linear_speed)*1) * radian2angle * 4.5
    print('Steer_Angle:%f' % Steer_Angle)

    # 实际测试下来，方向盘转动的角度范围是-27.5~27.5，对应的取值范围是924~1124  27.5*3.6=99
    Steer_Angle_H = int((Steer_Angle * 3.6) + 1024) // 256
    Steer_Angle_L = int((Steer_Angle * 3.6) + 1024) % 256
    # print('Steer_Angle_H:%f' % Steer_Angle_H)
    # print('Steer_Angle_L:%f' % Steer_Angle_L)
    data_EPS2['Steer_Angle_H'] = Steer_Angle_H
    data_EPS2['Steer_Angle_L'] = Steer_Angle_L

def calculate_angle(angular_rad):
    Steer_Angle = -angular_rad * radian2angle
    print("Steer_Angle:", Steer_Angle)
    # 实际测试下来，方向盘转动的角度范围是-27.5~27.5，对应的取值范围是924~1124  27.5*3.6=99
    Steer_Angle_H = int((Steer_Angle * 3.6) + 1024) // 256
    Steer_Angle_L = int((Steer_Angle * 3.6) + 1024) % 256
    # print('Steer_Angle_H:%f' % Steer_Angle_H)
    # print('Steer_Angle_L:%f' % Steer_Angle_L)
    data_EPS2['Steer_Angle_H'] = Steer_Angle_H
    data_EPS2['Steer_Angle_L'] = Steer_Angle_L

# udp向底盘发送can协议
def udp_send_can(message_name):
    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("VCU", can_data)

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

    # 处理CAN中接收到的数据，转化成线速度和角速度
    feedback_twist_linear_x = (message['DM_Speed_H'] * 256 + message['DM_Speed_L']) / 185

    Steer_Angle = (message['Steer_Angle_H'] * 256 + message['Steer_Angle_L'] - 1024) / 3.6
    # 角速度 = tan(转向角度) * 线速度 / 前后轮轴距
    feedback_twist_angular_z = math.tan(Steer_Angle / radian2angle) * feedback_twist_linear_x / 1

    if (message['Gear'] == 1):
        feedback_twist_linear_x = feedback_twist_linear_x
    elif (message['Gear'] == 2):
        feedback_twist_linear_x = -feedback_twist_linear_x
    else:
        feedback_twist_linear_x = 0.0

    # 发布处理后的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 / radian2angle)
    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')
        # 加载dbc文件
        self.declare_parameter("dbc")
        dbcfile = self.get_parameter("dbc").get_parameter_value().string_value
        self.db = cantools.database.load_file(dbcfile)

        self.subscriber = self.create_subscription(AckermannControlCommand, 'control/command/control_cmd', self.sub_callback, 10)
        # self.publisher = self.create_publisher(Twist, 'twist_cmd_feedback', self.pub_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. 发送档位+车速0指令：08 00 00 00 E2 01 00 00 00 00 00 00 00
        udp_send_can(drive_by_wire_command)

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

        # 3. 计算档位、速度、角度
        if (EV_Speed > 0):
            data_EV1['Gear'] = 1
            calculate_speed(EV_Speed)
            # calculate_angle(EV_Speed, angular_velocity)
            calculate_angle(angular_rad)

        elif (EV_Speed < 0):
            data_EV1['Gear'] = 2
            calculate_speed(-EV_Speed)
            # calculate_angle(-EV_Speed, angular_velocity)
            calculate_angle(angular_rad)

        else:
            data_EV1['Gear'] = 0
            calculate_speed(0)
            # calculate_angle(1, angular_velocity)
            calculate_angle(angular_rad)

        # 4. 发送can消息
        message_EV1 = self.db.encode_message("EV1", data_EV1)
        message_linear_velocity = message_EV1_front + message_EV1
        # print(hexlify(message_linear_velocity).decode('ascii'))
        udp_send_can(message_linear_velocity)

        # 计算异或校验码
        Check = Calculate_XOR_value(data_EPS2)
        data_EPS2.update({'Check' : Check})
        message_EPS2 = self.db.encode_message("EPS2", data_EPS2)
        message_angle = message_EPS2_front + message_EPS2
        # print(hexlify(message_angle).decode('ascii'))
        udp_send_can(message_angle)

    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()

    # 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.spin(node)

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

if __name__ == '__main__':
    main()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257

编写setup.py和launch文件

from setuptools import setup

package_name = 'can_ros2_bridge'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    # 安装文件
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        ('share/' +package_name, ['launch/can.launch.py']),
        ('share/' +package_name, ['config/CarCAN.dbc']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='car',
    maintainer_email='[email protected]',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    # 可执行文件
    entry_points={
        'console_scripts': [
            'cmd_vel_to_can = can_ros2_bridge.send_message:main',
        ],
    },
)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

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():
    config = os.path.join(
      get_package_share_directory('can_ros2_bridge'),
      'CarCAN.dbc'
      )
    can_ros2_bridge = Node(
        package='can_ros2_bridge',
        executable='cmd_vel_to_can',
        name='can',
        parameters=[{'dbc': config},],
        output="both"
    )

    return LaunchDescription(
        [
            can_ros2_bridge,
        ]
    )
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

修改静态IP（注意关掉WIFI），启动CAN，能成功控制底盘

四、相机驱动

4.1 安装驱动

本文使用的相机没有官方驱动，采用的相机驱动源码地址：http://iyenn.com/index/link?url=https://github.com/ros-drivers/usb_cam/tree/ros2，将代码下载下来放到工作空间src编译运行：

colcon build
source install/setup.bash
ros2 run usb_cam usb_cam_node_exe --ros-args --params-file src/usb_cam-ros2/config/params_1.yaml
# 或者
ros2 launch usb_cam camera.launch.py #但是这个运行可能有一些问题，我们在下一节重新写一个
1
2
3
4
5

再打开一个节点，显示图像：

ros2 run usb_cam show_image.py
1

如果是外置的相机，此时大概率无法驱动，因为相机默认挂载点是/dev/video0（但它一般是电脑自带的摄像头），查看相机挂载地址：

ls /dev/
1

可以通过插拔相机判断挂载地址，我们是/dev/video2，在参数文件中修改video_device为/dev/video2，再次驱动即可看到外置相机的图像

然后再次运行，可以驱动相机了，相机话题为/image_raw

4.2 修改相机参数

上面简单的运行实际上可能无法适配你的相机（可以驱动但是效果很差），我们需要修改参数，新建一个参数文件（例如config/JR_HF868.yaml），内容如下：

/**:
    ros__parameters:
      # 设备挂载点
      video_device: "/dev/video2"
      # 帧率
      framerate: 30.0
      io_method: "mmap"
      # 坐标系
      frame_id: "camera"
      # 像素格式
      pixel_format: "mjpeg2rgb"  # see usb_cam/supported_formats for list of supported formats
      # 像素尺寸
      image_width: 1920
      image_height: 1080
      # 相机名称
      camera_name: "JR_HF868"
      # 标定参数
      camera_info_url: "package://usb_cam/config/camera_info.yaml"
      # 亮度
      brightness: 50
      # 对比度
      contrast: 50
      # 饱和度
      saturation: 50
      # 清晰度
      sharpness: 80
      # 增益
      gain: -1
      # 白平衡
      auto_white_balance: true
      white_balance: 4000
      # 曝光
      autoexposure: true
      exposure: 100
      # 聚焦
      autofocus: false
      focus: -1
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

其中有几点需要注意的：
（1）将相机分辨率修改为1920*1080（或者你的相机支持的）；
（2）将设备挂载点改成/dev/video2（或者自己查到的）；
（3）默认的pixel_format，当相机运动过快，图片的运动畸变比较大，发现在运行相机节点的时候，会打印出相机支持的一些参数：

我们的相机在YUYV 4:2:2: 1920 x 1080这个参数下只支持6 Hz的帧率，相机在Motion-JPEG: 1920 x 1080这个参数下支持30 Hz的帧率，查找senser_driver/src/usb_cam-ros2/include/usb_cam/usb_cam.hpp文件，可以找到驱动支持的像素格式，有如下几种

修改pixel_format参数，改成mjpeg2rgb
（4）修改亮度，对比度，饱和度等参数
新写一个启动文件（launch/JR_HF868.launch.py）：

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
 
def generate_launch_description():
   config = os.path.join(
      get_package_share_directory('usb_cam'),
      'config',
      'JR_HF868.yaml'
      )
   return LaunchDescription([
      Node(
         package='usb_cam',
         executable='usb_cam_node_exe',
         name='usb_cam_node_exe',
         parameters=[config]
      ),
   ])
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

然后再重新编译，运行节点，现在相机的图像像素比较高，而且快速运动的时候畸变也小：

source install/setup.bash
ros2 launch usb_cam JR_HF868.launch.py
1
2

五、GNSS驱动

GNSS是可选的，这里使用的是华测M620RTK模块驱动。
由于ROS2没有再封装串口库serial，因此需要手动安装serial：

git clone https://github.com/ZhaoXiangBox/serial
cd serial && mkdir build
cmake .. && make
sudo make install
1
2
3
4

Cmake配置：

set(CMAKE_INSTALL_RPATH /usr/local/lib)
find_package(serial REQUIRED)
ament_target_dependencies(exe "serial")
1
2
3

接下来编写串口通信，读取GNSS数据（根据CHCCGI610的ROS1代码修改而来）

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"
#include 

// $GPGGA
// 例：$GPGGA,073217.00,3109.93434012,N,12117.26502692,E,4,48,0.59,25.441,M,11.090,M,2.8,0000*4F
// 字段0：$GPGGA，语句ID，表明该语句为Global Positioning System Fix Data（GGA）GPS定位信息
// 字段1：UTC 时间，hhmmss.sss，时分秒格式
// 字段2：纬度ddmm.mmmm，度分格式（前导位数不足则补0）
// 字段3：纬度N（北纬）或S（南纬）
// 字段4：经度dddmm.mmmm，度分格式（前导位数不足则补0）
// 字段5：经度E（东经）或W（西经）
// 字段6：GPS状态，0=未定位，1=单点定位，2=伪距/SBAS，3=无效PPS，4=RTK固定，5=RTK浮动
// 字段7：正在使用的卫星数量
// 字段8：HDOP水平精度因子（0.5 - 99.9）
// 字段9：海拔高度（-9999.9 — +99999.9）
// 字段10：M为单位米
// 字段11：地球椭球面相对大地水准面的高度
// 字段12：M为单位米
// 字段13：差分时间（从最近一次接收到差分信号开始的秒数，如果不是差分定位将为空）
// 字段14：差分站ID号0000 - 1023（前导位数不足则补0，如果不是差分定位将为空）*校验值

class GNSSPublisher : public rclcpp::Node
{
public:
    GNSSPublisher(const char *nodeName) : Node(nodeName),
                                          StateParser(0), CntByte(0), CntDelimiter(0), tmpint(0)
    {
        port_name = this->declare_parameter("~port_name", "/dev/ttyUSB0");
        PosDelimiter[15] = {0};
        temp_field[30] = {0};
        gnss_pub_ = this->create_publisher<sensor_msgs::msg::NavSatFix>("/sensing/gnss/ublox/nav_sat_fix", 10);

        try
        {
            // 设置串口属性，并打开串口
            ser.setPort(port_name);
            ser.setBaudrate(460800);
            serial::Timeout to = serial::Timeout::simpleTimeout(1000); // 超时定义，单位：ms
            ser.setTimeout(to);
            ser.open();
        }
        catch (serial::IOException &e)
        {
            std::cout << port_name + " open failed, please check the permission of port ,run command "sudo chmod 777 " + port_name + "" and try again！" << std::endl;
            getchar();
            rclcpp::shutdown();
        }

        // 检测串口是否已经打开，并给出提示信息
        if (ser.isOpen())
        {
            std::cout << port_name + " open successed!" << std::endl;
        }
        else
        {
            std::cout << port_name + " open failed!" << std::endl;
            getchar();
            rclcpp::shutdown();
        }

        rclcpp::Rate loop_rate(100);           // 设置循环频率为100Hz
        ser.flushInput();                      // 在开始正式接收数据前先清除串口的接收缓冲区
        memset(OneFrame, 0, sizeof(OneFrame)); // 清空gps字符串
        int framecnt = 0;
        CntByte = 0; // 指向OneFrame的第一个位置

        while (rclcpp::ok())
        {
            int i, j;
            int start; // 当前位置
            int pos;   // 下一个分隔符的位置
            int numinbuf;
            int numgetted;
            auto gnss_msg = std::make_unique<sensor_msgs::msg::NavSatFix>();

            try
            {
                numinbuf = ser.available(); // available()返回从串口缓冲区读回的字符数
                                            // std::cout<<"串口缓冲区的数据有"<
                                            // initrd.img.oldCLEAR();
                                            // printf("bytes in buf = %d
",numinbuf);
            }
            catch (serial::IOException &e)
            {
                std::cout << "Port crashed! Please check cable!" << std::endl;
                getchar();
                rclcpp::shutdown();
            }

            if (numinbuf > 0) // 串口缓冲区有数据
            {
                numgetted = ser.read(rbuf, numinbuf); // 串口缓冲区数据读到rbuf中
                if (numgetted == numinbuf)            // 取回的数据个数与缓冲区中有的数据个数相同，说明读串口成功
                {
                    for (int i = 0; i < numgetted; i++) // 对收到的字符逐个处理
                    {
                        // 在一帧数据的接收过程中，只要遇到非$GPCHC帧头就重新开始
                        // 此处理具有最高优先级，会重置状态机
                        if (rbuf[i] == '$' && rbuf[i + 3] != 'G' && rbuf[i + 4] != 'G' && rbuf[i + 5] != 'A')
                        {
                            memset(OneFrame, 0, sizeof(OneFrame));
                            StateParser = 0;
                            break; // 中断循环
                        }
                        // 正常处理过程
                        switch (StateParser)
                        {
                        // 等待语句开始标志'$'
                        case 0:
                            if (rbuf[i] == '$' && rbuf[i + 3] == 'G' && rbuf[i + 4] == 'G' && rbuf[i + 5] == 'A') // 收到语句开始标志
                            {
                                memset(OneFrame, 0, sizeof(OneFrame));
                                OneFrame[0] = '$';
                                CntByte = 1; // 开始对帧长度的计数
                                StateParser = 1;
                            }
                            break;

                        // 寻找帧头"$GPGGA,"
                        case 1:
                            OneFrame[CntByte] = rbuf[i];
                            CntByte++; // 指向下一个空位

                            if (rbuf[i] == ',')
                            {
                                if (strncmp(OneFrame, "$GPGGA,", 7) == 0)
                                {
                                    CntDelimiter = 0;              // 分隔符计数从0开始
                                    PosDelimiter[0] = CntByte - 1; // 记录分隔符在OneFrame中的位置
                                    // std::cout<<"PosDelimiter[0]"<
                                    StateParser = 2;
                                    // std::cout<<"寻找帧头$GPGGA完成"<
                                }
                                else // 帧头错误
                                {
                                    memset(OneFrame, 0, sizeof(OneFrame));
                                    StateParser = 0;
                                    // std::cout<<"寻找帧头$GPGGA失败"<
                                }
                            }
                            break;

                        // 接收各数据域
                        case 2:
                            // std::cout<<"开始接受各个数据域"<
                            OneFrame[CntByte] = rbuf[i];
                            // std::cout<<"接受字符"<
                            CntByte++; // 指向下一个空位

                            if (rbuf[i] == ',' || rbuf[i] == '*')
                            {
                                CntDelimiter++; // 分隔符计数
                                // std::cout<<"分隔符个数："<
                                PosDelimiter[CntDelimiter] = CntByte - 1; // 记下分隔符位置
                                // std::cout<<"PosDelimiter["<
                                field_len[CntDelimiter - 1] = PosDelimiter[CntDelimiter] - PosDelimiter[CntDelimiter - 1] - 1;
                                // std::cout<<"第"<
                                if (CntDelimiter == 14) // 0-14，共15个分隔符，开始数据解析
                                {
                                    // 计算出每个字段的长度
                                    for (int j = 0; j <= 13; j++) // 0-13，22个字段
                                    {
                                        field_len[j] = PosDelimiter[j + 1] - PosDelimiter[j] - 1;
                                        // std::cout<<"第"<
                                    }

                                    if (field_len[1] > 0)
                                    {
                                        memset(temp_field, 0, sizeof(temp_field));
                                        strncpy(temp_field, &OneFrame[PosDelimiter[1] + 1], field_len[1]);
                                        int temp = (int)(atof(temp_field) / 100);
                                        gnss_msg->latitude = temp + (atof(temp_field) - temp * 100) / 60;
                                    }

                                    if (field_len[3] > 0)
                                    {
                                        memset(temp_field, 0, sizeof(temp_field));
                                        strncpy(temp_field, &OneFrame[PosDelimiter[3] + 1], field_len[3]);
                                        int temp = (int)(atof(temp_field) / 100);
                                        gnss_msg->longitude = temp + (atof(temp_field) - temp * 100) / 60;
                                    }

                                    if (field_len[5] > 0)
                                    {
                                        memset(temp_field, 0, sizeof(temp_field));
                                        strncpy(temp_field, &OneFrame[PosDelimiter[5] + 1], field_len[5]);
                                        gnss_msg->status.status = atof(temp_field);
                                    }

                                    if (field_len[6] > 0)
                                    {
                                        memset(temp_field, 0, sizeof(temp_field));
                                        strncpy(temp_field, &OneFrame[PosDelimiter[6] + 1], field_len[6]);
                                        gnss_msg->status.service = atof(temp_field);
                                    }

                                    if (field_len[7] > 0)
                                    {
                                        memset(temp_field, 0, sizeof(temp_field));
                                        strncpy(temp_field, &OneFrame[PosDelimiter[7] + 1], field_len[7]);
                                        gnss_msg->position_covariance[0] = pow(atof(temp_field), 2);
                                        gnss_msg->position_covariance[4] = pow(atof(temp_field), 2);
                                        gnss_msg->position_covariance[8] = pow(atof(temp_field), 2);
                                    }

                                    if (field_len[8] > 0)
                                    {
                                        memset(temp_field, 0, sizeof(temp_field));
                                        strncpy(temp_field, &OneFrame[PosDelimiter[8] + 1], field_len[8]);
                                        gnss_msg->altitude = atof(temp_field);
                                    }

                                    StateParser = 3;
                                }
                            }
                            break;

                            // 校验和第一个字符
                        case 3:
                            OneFrame[CntByte] = rbuf[i];
                            CntByte++;                                                                                            // 指向下一个空位
                            if (rbuf[i - 1] == '*' && ((rbuf[i] >= '0' && rbuf[i] <= '9') || (rbuf[i] >= 'A' && rbuf[i] <= 'F'))) // 校验和字节应是一个十六进制数
                            {
                                StateParser = 4;
                            }
                            else
                            {
                                memset(OneFrame, 0, sizeof(OneFrame));
                                StateParser = 0;
                            }
                            break;

                            // 校验和第二个字符
                        case 4:
                            OneFrame[CntByte] = rbuf[i];
                            CntByte++; // 指向下一个空位

                            if ((rbuf[i] >= '0' && rbuf[i] <= '9') || (rbuf[i] >= 'A' && rbuf[i] <= 'F')) // 校验和字节应是一个十六进制数
                            {
                                // 检查校验
                                cscomputed = GetXorChecksum((char *)(OneFrame + 1), CntByte - 4); // 计算得到的校验，除去$*hh共6个字符
                                csreceived = 0;                                                   // 接收到的校验
                                strtemp[0] = OneFrame[CntByte - 2];
                                strtemp[1] = OneFrame[CntByte - 1];
                                strtemp[2] = '';                  // 字符串结束标志
                                sscanf(strtemp, "%x", &csreceived); // 字符串strtemp转换为16进制数

                                // 检查校验是否正确
                                if (cscomputed != csreceived) // 校验和不匹配
                                {
                                    memset(OneFrame, 0, sizeof(OneFrame));
                                    StateParser = 0;
                                }
                                else // 校验和匹配
                                {
                                    StateParser = 5;
                                }
                            } // 校验和字节是hex
                            else
                            {
                                memset(OneFrame, 0, sizeof(OneFrame));
                                StateParser = 0;
                            }

                            break;

                            // 等待结束标志=0x0d
                        case 5:
                            OneFrame[CntByte] = rbuf[i];
                            CntByte++; // 指向下一个空位
                            if (rbuf[i] == '
')
                            {
                                StateParser = 6;
                            }
                            else
                            {
                                memset(OneFrame, 0, sizeof(OneFrame));
                                StateParser = 0;
                            }
                            break;

                        // 等待结束标志=0x0a
                        case 6:
                            OneFrame[CntByte] = rbuf[i];
                            gnss_msg->header.stamp = this->get_clock()->now(); // ros时刻
                            gnss_msg->header.frame_id = "gnss_link";
                            gnss_pub_->publish(std::move(gnss_msg)); // 发布nav消息
                            // std::cout<<"发布成功"<

                            memset(OneFrame, 0, sizeof(OneFrame));
                            StateParser = 0;

                            break;

                        default:
                            memset(OneFrame, 0, sizeof(OneFrame));
                            StateParser = 0;
                            break;
                        } // switch(StateParser)
                    }     // for(int i=0; i
                }         // if(numgetted == numinbuf)
            }
            loop_rate.sleep();
        }
    }

private:
    // 全局变量
    serial::Serial ser;      // 声明串口对象
    int StateParser;         // 解析处理状态机状态
    int CntByte;             // 用于记录OneFrame中的实际数据长度
    int PosDelimiter[15];    // 用于记录分隔符位置
    int field_len[14];       // 字符串长度
    int CntDelimiter;        // 分隔符计数
    unsigned char rbuf[500]; // 接收缓冲区，要足够大，需要通过测试得出
    char OneFrame[250];      // 存放一帧数据，长度大于115即可，这里取200
    char str[3];
    unsigned int tmpint;
    int cscomputed; // 计算得到的校验，除去$*hh共6个字符
    int csreceived; // 接收到的校验
    char strtemp[3];
    char temp_field[30];
    std::string port_name;

    /*****************************
    功能：计算校验，字符串中所有字符的异或
    返回：返回一个无符号整数
    输入参数：<1>字符串指针，<2>字符串长度（指有效长度，不包括字符串结束标志）
    输出参数：校验结果
    ******************************/
    unsigned int GetXorChecksum(const char *pch, int len)
    {
        unsigned int cs = 0;
        int i;

        for (i = 0; i < len; i++)
            cs ^= pch[i];

        return cs;
    }
    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gnss_pub_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<GNSSPublisher>("gnss_driver_exe"));
    rclcpp::shutdown();
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352

启动，再订阅GNSS数据可以看到GNSS数据

source install/setup.bash
ros2 launch m620_driver m620.launch.py
1
2