Autoware.universe部署04：universe传感器ROS2驱动

---

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

一、激光雷达驱动

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

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

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

（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

（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

在打开的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

（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

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

ll /dev/imu_usb

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

其中GNSS的为：

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

IMU模块的为：

10c4:ea60 Silicon Labs CP210x UART Bridge

然后创建.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"

然后：

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

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

ll /dev/imu_usb
ll /dev/gnss

记得把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)

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

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

报下面的错误，是因为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()

编写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',
        ],
    },
)

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,
        ]
    )

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

四、相机驱动

4.1 安装驱动

本文使用的相机没有官方驱动，采用的相机驱动源码地址：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 #但是这个运行可能有一些问题，我们在下一节重新写一个

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

ros2 run usb_cam show_image.py

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

ls /dev/

可以通过插拔相机判断挂载地址，我们是/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）将相机分辨率修改为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]
      ),
   ])

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

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

五、GNSS驱动

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

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

Cmake配置：

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

接下来编写串口通信，读取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\n",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] = '\0';                  // 字符串结束标志
                                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] == '\r')
                            {
                                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;
}

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

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