个人网站建设计划报告,谷歌seo外包,益阳网站建设公司有哪些,广告传媒公司经营范围有哪些文章目录 一、激光雷达驱动二、IMU驱动2.1 上位机配置4.2 IMU校准4.3 安装ROS驱动 三、CAN驱动四、相机驱动4.1 安装驱动4.2 修改相机参数 五、GNSS驱动 本文介绍了 Autoware.universe 各个传感器ROS2驱动#xff0c;本系列其他文章#xff1a; Autoware.universe部署01… 文章目录 一、激光雷达驱动二、IMU驱动2.1 上位机配置4.2 IMU校准4.3 安装ROS驱动 三、CAN驱动四、相机驱动4.1 安装驱动4.2 修改相机参数 五、GNSS驱动 本文介绍了 Autoware.universe 各个传感器ROS2驱动本系列其他文章 Autoware.universe部署01Ubuntu20.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速腾聚创雷达ROS1ROS2代码都是同一个链接所以将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 build5设备连接与网络配置
准备一套速腾聚创16线激光雷达包括激光雷达、电源盒子、航插头以及网线本文使用的PC系统是Ubuntu 18.04系统也可使用Ubuntu 16.04或Ubuntu 20.04准备AC 220V电源或DC 12V电源
如下图所示将雷达一端的航插头接口与雷达电源盒子的航插头接口两个对准红点接好 电源盒子接上电源接上网线网线一端接入到PC一端接入到电源盒子如下图 雷达点云盒子连接雷达、点云以及网线网线另一端连接计算机或者通过交换机转接设置网络为静态IPIP地址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°
2IMU模块上电后打开上位机显示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 pyserial2在配套资料中找到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) ICIMU模块的为
10c4:ea60 Silicon Labs CP210x UART Bridge然后创建.rules文件或者直接在上面IMU的文件中修改填写以下内容
KERNELttyUSB*, ATTRS{idVendor}10c4, ATTRS{idProduct}ea60, MODE:0777, SYMLINKimu_usb
KERNELttyUSB*, ATTRS{idVendor}0403, ATTRS{idProduct}6001, MODE:0777, SYMLINKgnss然后
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, baudrate460800, timeout0.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.699Steer_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.699Steer_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(targetprocess_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(targetreceive_data, args(node,)).start()# 保持节点运行检测是否收到退出指令CtrlCrclpy.spin(node)# 清理并关闭ros2节点node.destroy_node()rclpy.shutdown()if __name__ __main__:main()编写setup.py和launch文件
from setuptools import setuppackage_name can_ros2_bridgesetup(namepackage_name,version0.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_safeTrue,maintainercar,maintainer_emailcartodo.todo,descriptionTODO: Package description,licenseTODO: 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(packagecan_ros2_bridge,executablecmd_vel_to_can,namecan,parameters[{dbc: config},],outputboth)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(packageusb_cam,executableusb_cam_node_exe,nameusb_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 installCmake配置
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 DataGGAGPS定位信息
// 字段1UTC 时间hhmmss.sss时分秒格式
// 字段2纬度ddmm.mmmm度分格式前导位数不足则补0
// 字段3纬度N北纬或S南纬
// 字段4经度dddmm.mmmm度分格式前导位数不足则补0
// 字段5经度E东经或W西经
// 字段6GPS状态0未定位1单点定位2伪距/SBAS3无效PPS4RTK固定5RTK浮动
// 字段7正在使用的卫星数量
// 字段8HDOP水平精度因子0.5 - 99.9
// 字段9海拔高度-9999.9 — 99999.9
// 字段10M为单位米
// 字段11地球椭球面相对大地水准面的高度
// 字段12M为单位米
// 字段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_publishersensor_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_uniquesensor_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::coutPosDelimiter[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分隔符个数CntDelimiterstd::endl;PosDelimiter[CntDelimiter] CntByte - 1; // 记下分隔符位置// std::coutPosDelimiter[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-1322个字段{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); // 计算得到的校验除去$*hhCRLF共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;// 等待结束标志CR0x0dcase 5:OneFrame[CntByte] rbuf[i];CntByte; // 指向下一个空位if (rbuf[i] \r){StateParser 6;}else{memset(OneFrame, 0, sizeof(OneFrame));StateParser 0;}break;// 等待结束标志LF0x0acase 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 i0; inumgetted; 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; // 计算得到的校验除去$*hhCRLF共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::Publishersensor_msgs::msg::NavSatFix::SharedPtr gnss_pub_;
};int main(int argc, char *argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_sharedGNSSPublisher(gnss_driver_exe));rclcpp::shutdown();return 0;
}启动再订阅GNSS数据可以看到GNSS数据
source install/setup.bash
ros2 launch m620_driver m620.launch.py
