---

本文介绍了 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 resultdef calculate_speed(linear_speed):EV_Speed_H = int((linear_speed * 185)) // 256EV_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_Ldata_EV1['EV_Speed_H'] = EV_Speed_Hdef calculate_angle(linear_speed, angular_speed):# 转弯的角度 = arctan(( 角速度 / 线速度 ) * 车长 ) Steer_Angle = -math.atan((angular_speed/linear_speed)*1) * radian2angle * 4.5print('Steer_Angle:%f' % Steer_Angle)# 实际测试下来，方向盘转动的角度范围是-27.5~27.5，对应的取值范围是924~1124  27.5*3.6=99Steer_Angle_H = int((Steer_Angle * 3.6) + 1024) // 256Steer_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_Hdata_EPS2['Steer_Angle_L'] = Steer_Angle_Ldef calculate_angle(angular_rad):Steer_Angle = -angular_rad * radian2angleprint("Steer_Angle:", Steer_Angle)# 实际测试下来，方向盘转动的角度范围是-27.5~27.5，对应的取值范围是924~1124  27.5*3.6=99Steer_Angle_H = int((Steer_Angle * 3.6) + 1024) // 256Steer_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_Hdata_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']) / 185Steer_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 / 1if (message['Gear'] == 1):feedback_twist_linear_x = feedback_twist_linear_xelif (message['Gear'] == 2):feedback_twist_linear_x = -feedback_twist_linear_xelse: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_valueself.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 00udp_send_can(drive_by_wire_command)# 2. 接收autoware传来的线速度和角速度EV_Speed = msg.longitudinal.speed# angular_velocity = msg.lateral.steering_tire_rotation_rateangular_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'] = 1calculate_speed(EV_Speed)# calculate_angle(EV_Speed, angular_velocity)calculate_angle(angular_rad)elif (EV_Speed < 0):data_EV1['Gear'] = 2calculate_speed(-EV_Speed)# calculate_angle(-EV_Speed, angular_velocity)calculate_angle(angular_rad)else:data_EV1['Gear'] = 0calculate_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 = data1msg.angular.z = data2self.publisher_data.publish(msg)def pubilsh_control_mode(self, data):msg = ControlModeReport()msg.mode = dataself.publisher_control_mode.publish(msg)def pubilsh_gear(self, data):msg = GearReport()msg.report = dataself.publisher_gear.publish(msg)def pubilsh_steering(self, data):msg = SteeringReport()msg.steering_tire_angle = dataself.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 = data1msg.longitudinal_velocity = data2msg.lateral_velocity = data3msg.heading_rate = data4self.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 setuppackage_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='car@todo.todo',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_directorydef 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.0io_method: "mmap"# 坐标系frame_id: "camera"# 像素格式pixel_format: "mjpeg2rgb"  # see usb_cam/supported_formats for list of supported formats# 像素尺寸image_width: 1920image_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: truewhite_balance: 4000# 曝光autoexposure: trueexposure: 100# 聚焦autofocus: falsefocus: -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 osfrom ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Nodedef 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 <serial/serial.h>// $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); // 超时定义，单位：msser.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);           // 设置循环频率为100Hzser.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<<"串口缓冲区的数据有"<<numinbuf<<"个"<<std::endl;// 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]"<<PosDelimiter[0]<<std::endl;StateParser = 2;// std::cout<<"寻找帧头$GPGGA完成"<<std::endl;}else // 帧头错误{memset(OneFrame, 0, sizeof(OneFrame));StateParser = 0;// std::cout<<"寻找帧头$GPGGA失败"<<std::endl;}}break;// 接收各数据域case 2:// std::cout<<"开始接受各个数据域"<<std::endl;OneFrame[CntByte] = rbuf[i];// std::cout<<"接受字符"<<rbuf[i]<<std::endl;CntByte++; // 指向下一个空位if (rbuf[i] == ',' || rbuf[i] == '*'){CntDelimiter++; // 分隔符计数// std::cout<<"分隔符个数："<<CntDelimiter<<std::endl;PosDelimiter[CntDelimiter] = CntByte - 1; // 记下分隔符位置// std::cout<<"PosDelimiter["<<CntDelimiter<<"]"<<PosDelimiter[CntDelimiter]<<std::endl;field_len[CntDelimiter - 1] = PosDelimiter[CntDelimiter] - PosDelimiter[CntDelimiter - 1] - 1;// std::cout<<"第"<<CntDelimiter<<"段数据长"<<field_len[CntDelimiter]<<std::endl;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<<"第"<<j<<"段数据长"<<field_len[j]<<std::endl;}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<CR><LF>共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;}} // 校验和字节是hexelse{memset(OneFrame, 0, sizeof(OneFrame));StateParser = 0;}break;// 等待结束标志<CR>=0x0dcase 5:OneFrame[CntByte] = rbuf[i];CntByte++; // 指向下一个空位if (rbuf[i] == '\r'){StateParser = 6;}else{memset(OneFrame, 0, sizeof(OneFrame));StateParser = 0;}break;// 等待结束标志<LF>=0x0acase 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<<"发布成功"<<std::endl;memset(OneFrame, 0, sizeof(OneFrame));StateParser = 0;break;default:memset(OneFrame, 0, sizeof(OneFrame));StateParser = 0;break;} // switch(StateParser)}     // for(int i=0; i<numgetted; 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即可，这里取200char str[3];unsigned int tmpint;int cscomputed; // 计算得到的校验，除去$*hh<CR><LF>共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