本文介绍了 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 <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); // 超时定义，单位：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<<"串口缓冲区的数据有"<<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;
}
} // 校验和字节是hex
else
{
memset(OneFrame, 0, sizeof(OneFrame));
StateParser = 0;
}
break;
// 等待结束标志<CR>=0x0d
case 5:
OneFrame[CntByte] = rbuf[i];
CntByte++; // 指向下一个空位
if (rbuf[i] == '\r')
{
StateParser = 6;
}
else
{
memset(OneFrame, 0, sizeof(OneFrame));
StateParser = 0;
}
break;
// 等待结束标志<LF>=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<<"发布成功"<<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即可，这里取200
char 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