jetson orin nano+ubuntu22.04+ros2+宇树L2激光雷达+fastlio2实现px4定点模式_px4与jetson连接
最近做课题要实现使用激光雷达作为px4外部定点数据，ros1的配置很多而ros2少的感人。踩了很多坑后终于搞定了，这篇文章用来记录（自用），可供参考。
使用jetson orin nano作为实现的平台，ubuntu版本为22.04 humble。以下内容默认ubuntu22.04安装完成。
1.ROS2安装
直接使用鱼香ros一键安装即可
wget http://fishros.com/install -O fishros && . fishros根据对话框自己选择安装，这一块不多赘述 ，网上教程很多。
2.构建Micro XRCE-DDS
由于ros2和px4连接采用Micro XRCE-DDS，所以需要下载这玩意（通过串口连接jetson和px4之后用来创建通讯的中间交互部分）。
详细的下载步骤见：ROS 2 用户指南 | PX4 Guide (main)
核心程序：
git clone https://github.com/eProsima/Micro-XRCE-DDS-Agent.git #不用官方的代码了，那个容易出错cd Micro-XRCE-DDS-Agentmkdir buildcd buildcmake ..makesudo make installsudo ldconfig /usr/local/lib/如果之前在ros2中做过px4飞行仿真，这个应该是配置过的，使用这个代码查看是否配置成功：
MicroXRCEAgent udp4 -p 8888照着px4的官网配置即可，有空的话可以照着px4的官网做一遍仿真。
3.jetson和px4的串口通讯
可以参考Jetson Nano利用ROS2通过MicroDDS与PX4通讯_jetson nano ros2-CSDN博客，写的比较详细。
配置好Micro XRCE-DDS之后，就可以开始尝试通过串口连接jetson和px4了。使用usb转ttl即可，接口我这边选择的是TELEM2，自己焊接一下线缆连接即可（接口的引脚自己网上找找）连接图：
有一个关键点，为了使用Micro XRCE-DDS，需要将px4通过usb连接到电脑，通过qgc地面站配置参数。需要将DDS配置为你对应连接的接口，以及配置波特率。
如果你的jetson安装过对应的串口驱动，就可以检测到(串口一般都显示为ttyUSB)
之后可以使用这个代码连接（使用qgc中配置的波特率）：
MicroXRCEAgent serial -D /dev/ttyUSB0 -b 921600
之后另开一个终端，查看ros2话题(第一次是每使用dds，第二次是使用dds)：
发现topic list中多了许多px4的话题，这就证明通过dds连接jetson和px4成功了，之后就可以通过这些topic进行数据的交互 。
当然这里存在一些问题，有的时候你的系统中没有安装对应的串口驱动将找不到串口。如果出现这个问题，只需要安装对于的串口驱动即可。可以参考:
Linux-ubuntu22.04串口驱动安装(CH34X)_ubuntu安装ch340驱动-CSDN博客ubuntu安装串口驱动(找不到ttyUSB*)_ubuntu usb转串口找不到-CSDN博客
如果报错
[ 7033.078452] usb 1-13: usbfs: interface 0 claimed by ch341 while \'brltty\' sets config #1可以参考 :
Ubuntu22.04 CH340系列串口驱动（没有ttyUSB）问题解决方案_usbfs: interface 0 claimed by ch341 while \'brltty\'-CSDN博客
一定要安装上对应的驱动才行。
4.宇树L2激光雷达配置与fastlio2配置
由于px4定点信息需要通过fastlio2输出的里程计转换获得，由于我采用宇树L2激光雷达，所以二者都需要进行配置。
首先激光雷达的配置看L2雷达使用教学_哔哩哔哩_bilibili，里面说的很详细，照着弄就行，需要完成视频说的对应有线ip配置。
至于fastlio2，可以通过这里下载https://github.com/newkillerwhale/unilidar_fastlio_ros2/tree/ros2
这个项目里面配置好了宇树L2对应的文件，可以直接使用，配置流程照着项目里面写的就行，如果不太清楚照着这个顺序：
1.首先建一个fastlio2使用的工作空间，我的名称为fast_lio2，在里面建一个src文件夹（这是建ros工作空间的基础部分），然后把上面在github中下的功能包放在src中。
2.在编译fast_lio2工作空间之前，需要先完成Livox ROS Driver 2和Livox SDK2的编译（这两个功能包是驱动fastlio2工作的必要组成部分），下面是这个两个包的地址，流程可以根据项目一步步做：
https://github.com/Livox-SDK/livox_ros_driver2https://github.com/Livox-SDK/Livox-SDK2/blob/master/README.md总结一下，具体的配置代码：
#首先新建一个工作空间，下载两个功能包到src中git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2cd ~/wslivox/srcgit clone https://github.com/Livox-SDK/Livox-SDK2.git #下载完成后，先进行sdk2的编译cd ./Livox-SDK2/mkdir buildcd buildcmake .. && make -j2 #这里github中写是cmake .. && make -j，建议改为j2不容易报错sudo make install#完成之后，进行livox_ros_driver2的编译cd ~/ws_livox/src/livox_ros_driver2source /opt/ros/humble/setup.sh #由于我的ros2为humble所以这么用，版本不一样的去上面的网站里面查./build.sh humble最后一步可能会报错，根据报错内容修改再编译即可。比如我编译的时候报错如下（图找不到了）：
kkkitten@ubuntu:~/ws_livox/src/livox_ros_driver2$ ./build.sh humbleWorking Path: /home/kkkitten/ws_livox/src/livox_ros_driver2ROS version is: ROS2Starting >>> livox_ros_driver2Starting >>> livox_sdk2--- stderr: livox_ros_driver2 Traceback (most recent call last): File \"/home/kkkitten/ws_livox/build/livox_ros_driver2/ament_cmake_python/livox_ros_driver2/setup.py\", line 4, in setup( File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/__init__.py\", line 115, in setup return distutils.core.setup(**attrs) File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/_distutils/core.py\", line 186, in setup return run_commands(dist) File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/_distutils/core.py\", line 202, in run_commands dist.run_commands() File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/_distutils/dist.py\", line 1002, in run_commands self.run_command(cmd) File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/dist.py\", line 1102, in run_command super().run_command(command) File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/_distutils/dist.py\", line 1021, in run_command cmd_obj.run() File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/command/egg_info.py\", line 312, in run self.find_sources() File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/command/egg_info.py\", line 320, in find_sources mm.run() File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/command/egg_info.py\", line 548, in run self.prune_file_list() File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/command/sdist.py\", line 162, in prune_file_list super().prune_file_list() File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/_distutils/command/sdist.py\", line 386, in prune_file_list base_dir = self.distribution.get_fullname() File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/_core_metadata.py\", line 275, in get_fullname return _distribution_fullname(self.get_name(), self.get_version()) File \"/home/kkkitten/.local/lib/python3.10/site-packages/setuptools/_core_metadata.py\", line 293, in _distribution_fullname canonicalize_version(version, strip_trailing_zero=False),TypeError: canonicalize_version() got an unexpected keyword argument \'strip_trailing_zero\'gmake[2]: *** [CMakeFiles/ament_cmake_python_build_livox_ros_driver2_egg.dir/build.make:70：CMakeFiles/ament_cmake_python_build_livox_ros_driver2_egg] 错误 1gmake[1]: *** [CMakeFiles/Makefile2:451：CMakeFiles/ament_cmake_python_build_livox_ros_driver2_egg.dir/all] 错误 2gmake[1]: *** 正在等待未完成的任务....gmake: *** [Makefile:146：all] 错误 2---Failed <<< livox_ros_driver2 [25.0s, exited with code 2]Aborted <<< livox_sdk2 [59.8s] Summary: 0 packages finished [1min 0s] 1 package failed: livox_ros_driver2 1 package aborted: livox_sdk2 2 packages had stderr output: livox_ros_driver2 livox_sdk2kkkitten@ubuntu:~/ws_livox/src/livox_ros_driver2$这是因为Python setuptools版本不兼容，把它降级即可：
pip install setuptools==58.2.0 # 降级到兼容版本然后再编译就通过了。编译通过之后需要将 livox_ros_driver2 的环境变量添加到 ~/.bashrc 中，以便在启动fastlio2时能自动加载第二步安装的Livox 驱动，避免频繁source。以下是具体步骤：
ls ~/ws_livox/install # 确认有 livox_ros_driver2 文件夹echo \"source ~/ws_livox/install/setup.bash\" >> ~/.bashrcsource ~/.bashrc # 使更改生效ros2 pkg list | grep livox # 应输出 livox_ros_driver23.完成上面的配置之后，回到fast_lio2工作空间中，进行编译
rosdep install --from-paths src --ignore-src -ycolcon build --symlink-install. ./install/setup.bash # use setup.zsh if use zsh有的时候rosdep不好用，所以可以使用rosdepc，可以通过我这篇文章最开始的鱼香ros安装rosdepc，之后使用rosdepc安装依赖：
rosdepc install --from-paths src --ignore-src -y编译通过之后，通过下面这两个命令依次启动L2激光雷达以及fastlio2的建图（要在对应的工作空间中），不同的激光雷达去找对应的yaml文件：
ros2 launch unitree_lidar_ros2 launch.pyros2 launch fast_lio mapping.launch.py config_file:=unilidar_l2.yamlfastlio2即可正常进行建图：
这是对应的话题：
其中/Odometry是后续给px4提供定点数据的原始数据来源。
成功之后，如果你的px4版本比较新（我的固件版本是v1.15），需要在fastlio2功能包中找到laserMapping.cpp添加一个函数（大约在626行）：
templatevoid set_posestamp(T & out){ out.pose.position.x = state_point.pos(0); out.pose.position.y = state_point.pos(1); out.pose.position.z = state_point.pos(2); out.pose.orientation.x = geoQuat.x; out.pose.orientation.y = geoQuat.y; out.pose.orientation.z = geoQuat.z; out.pose.orientation.w = geoQuat.w; }//以下是新添加的部分templatevoid set_twistamp(T & out){out.twist.linear.x = state_point.vel(0);out.twist.linear.y = state_point.vel(1);out.twist.linear.z = state_point.vel(2);// angular xyzout.twist.angular.x = Measures.imu.back()->angular_velocity.x;out.twist.angular.y = Measures.imu.back()->angular_velocity.y;out.twist.angular.z = Measures.imu.back()->angular_velocity.z;}//以上是新添加的部分void publish_odometry(const rclcpp::Publisher这个新的添加部分是使 /Odometry输出中增加速度的输出，这是因为新版本px4的视觉融合相较于旧版本增加了速度，所以需要通过这个函数获得速度的输出。方法参考于：https://zhuanlan.zhihu.com/p/492496857
5.px4定点数据发送
在前面通过DDS连接之后，可以观察到其中一个px4话题为/fmu/in/vehicle_visual_odometry，这个话题就是我们需要发布从fastlio2中获取位置数据的地方。为了使用这个话题对应的数据格式，需要去px4官网下载px4_msgs功能包。注意，vehicle_visual_odometry使用vehicleodometry数据格式！！！同时也需要在qgc地面站中进行配置，把EKF2_EV_CTRL配置为11，EKF2_HGT_MODE设置为视觉模式。
所以实现px4定点模式，只需要将前面获得的fastlio2的/Odometry话题数据进行一定处理，之后发布到/fmu/in/vehicle_visual_odometry中即可实现，以下是通过ai写的程序，可以参考一下（工作空间怎么搭建自行百度，我把px4_msgs包也放进同一个工作空间中了，作为依赖）：
注意，这个程序只是用来测试px4能不能进入定点模式，实机使用的话是需要按照自己的需求更改的！！！，没弄清楚原理之前不要随便上实机！！！
fastlio2_to_px4.py
#!/usr/bin/env python3import rclpyfrom rclpy.node import Nodefrom nav_msgs.msg import Odometryfrom px4_msgs.msg import VehicleOdometry, TimesyncStatus # 使用正确的VehicleOdometry类型import numpy as npfrom collections import dequedef quaternion_matrix(q): x, y, z, w = q return np.array([ [1-2*y*y-2*z*z, 2*x*y-2*z*w, 2*x*z+2*y*w], [2*x*y+2*z*w, 1-2*x*x-2*z*z, 2*y*z-2*x*w], [2*x*z-2*y*w, 2*y*z+2*x*w, 1-2*x*x-2*y*y] ])def euler_from_quaternion(q): x, y, z, w = q t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + y * y) roll = np.arctan2(t0, t1) t2 = +2.0 * (w * y - z * x) t2 = np.clip(t2, -1.0, 1.0) pitch = np.arcsin(t2) t3 = +2.0 * (w * z + x * y) t4 = +1.0 - 2.0 * (y * y + z * z) yaw = np.arctan2(t3, t4) return roll, pitch, yawimport threadingimport timeimport logging# 配置日志记录logging.basicConfig(level=logging.DEBUG)logger = logging.getLogger(\'FastLIOToPX4\')class FastLIOToPX4(Node): def __init__(self): super().__init__(\'fastlio_to_px4\') logger.info(\"Initializing FastLIO to PX4 converter...\") # 声明参数 self.declare_parameters( namespace=\'\', parameters=[ (\'fastlio_topic\', \'/Odometry\'), (\'visual_odom_topic\', \'/fmu/in/vehicle_visual_odometry\'), (\'publish_rate\', 30.0), (\'position_variance\', [0.01, 0.01, 0.01]), (\'orientation_variance\', [0.0001, 0.0001, 0.0001]), (\'velocity_variance\', [0.01, 0.01, 0.01]), (\'quality_value\', 80) # int8类型，0-127 ] ) # 获取参数值 self.fastlio_topic = self.get_parameter(\'fastlio_topic\').value self.visual_odom_topic = self.get_parameter(\'visual_odom_topic\').value self.publish_rate = self.get_parameter(\'publish_rate\').value self.quality_value = int(self.get_parameter(\'quality_value\').value) # 确保质量值在有效范围内 self.quality_value = max(0, min(127, self.quality_value)) logger.info(f\"Using quality value: {self.quality_value}\") # 初始化变量 self.prev_position = np.zeros(3) self.prev_time = None self.current_position = np.zeros(3) self.estimated_velocity = np.zeros(3) self.q_mav = np.array([0.0, 0.0, 0.0, 1.0]) # 时间同步变量 self.TimesyncStatus_offset = 0 self.last_TimesyncStatus_received = self.get_clock().now() # 线程安全锁 self.lock = threading.Lock() # 创建订阅者 self.fastlio_sub = self.create_subscription( Odometry, self.fastlio_topic, self.fastlio_callback, 10 ) # 创建时间同步订阅者 self.TimesyncStatus_sub = self.create_subscription( TimesyncStatus, \'/fmu/out/TimesyncStatus\', self.TimesyncStatus_callback, 10 ) # 创建发布者 - 使用VehicleOdometry消息类型 self.visual_odom_pub = self.create_publisher( VehicleOdometry, self.visual_odom_topic, 10 ) logger.info(f\"Publisher created for topic: {self.visual_odom_topic}\") # 创建定时器 self.timer = self.create_timer(1.0/self.publish_rate, self.publish_visual_odometry) self.start_time = time.time() logger.info(\"FastLIO to PX4 converter initialized successfully\") def TimesyncStatus_callback(self, msg): \"\"\"处理时间同步消息\"\"\" ros_time = self.get_clock().now() px4_time_us = msg.timestamp ros_time_ns = ros_time.nanoseconds self.TimesyncStatus_offset = (px4_time_us * 1000) - ros_time_ns self.last_TimesyncStatus_received = ros_time def get_px4_timestamp(self): \"\"\"获取PX4兼容的系统时间戳（微秒）\"\"\" if self.TimesyncStatus_offset == 0: return int(self.get_clock().now().nanoseconds / 1000) ros_time = self.get_clock().now() px4_time_us = (ros_time.nanoseconds + self.TimesyncStatus_offset) // 1000 return int(px4_time_us) def fastlio_callback(self, msg): with self.lock: current_time = self.get_clock().now() self.current_position = np.array([ msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z ]) # 估算速度 if self.prev_time is not None: dt = (current_time - self.prev_time).nanoseconds * 1e-9 if dt > 0: displacement = self.current_position - self.prev_position self.estimated_velocity = displacement / dt self.prev_position = self.current_position.copy() self.prev_time = current_time self.q_mav = np.array([ msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ]) def publish_visual_odometry(self): with self.lock:try: # 创建VehicleOdometry消息对象 visual_odom = VehicleOdometry() px4_time = self.get_px4_timestamp() visual_odom.timestamp = px4_time visual_odom.timestamp_sample = px4_time # 坐标系设置 - 根据您提供的定义 visual_odom.pose_frame = VehicleOdometry.POSE_FRAME_FRD # FRD world-fixed frame = 2 visual_odom.velocity_frame = VehicleOdometry.VELOCITY_FRAME_BODY_FRD # BODY_FRD = 3 # 位置转换 (ENU to FRD) # FRD: X=Forward, Y=Right, Z=Down # 所以我们需要将ENU转换为FRD visual_odom.position[0] = np.float32(self.current_position[1]) # Y (East) -> X (Forward) visual_odom.position[1] = np.float32(self.current_position[0]) # X (North) -> Y (Right) visual_odom.position[2] = np.float32(-self.current_position[2]) # Z (Up) -> -Z (Down) # 速度设置 - 在机体坐标系中 visual_odom.velocity[0] = np.float32(self.estimated_velocity[0] + 0.001) # 添加微小扰动 visual_odom.velocity[1] = np.float32(self.estimated_velocity[1] + 0.001) visual_odom.velocity[2] = np.float32(self.estimated_velocity[2] + 0.001) # 方向 - 保持原样 visual_odom.q[0] = np.float32(self.q_mav[0]) visual_odom.q[1] = np.float32(self.q_mav[1]) visual_odom.q[2] = np.float32(self.q_mav[2]) visual_odom.q[3] = np.float32(self.q_mav[3]) # 角速度 visual_odom.angular_velocity[0] = np.float32(0.001) visual_odom.angular_velocity[1] = np.float32(0.001) visual_odom.angular_velocity[2] = np.float32(0.001) # 方差设置 visual_odom.position_variance[0] = np.float32(0.1) # X visual_odom.position_variance[1] = np.float32(0.1) # Y visual_odom.position_variance[2] = np.float32(0.2) # Z visual_odom.orientation_variance[0] = np.float32(0.05) # Roll visual_odom.orientation_variance[1] = np.float32(0.05) # Pitch visual_odom.orientation_variance[2] = np.float32(0.1) # Yaw visual_odom.velocity_variance[0] = np.float32(0.5) # Vx visual_odom.velocity_variance[1] = np.float32(0.5) # Vy visual_odom.velocity_variance[2] = np.float32(0.5) # Vz # 设置质量值 (int8) visual_odom.quality = int(self.quality_value) # 重置计数器 visual_odom.reset_counter = 0 # 发布消息 self.visual_odom_pub.publish(visual_odom) # 定期日志 now = time.time() if now - self.start_time > 1.0: self.start_time = now logger.info( f\"Published VO: quality={visual_odom.quality}, \" f\"position=[{visual_odom.position[0]:.2f}, {visual_odom.position[1]:.2f}, {visual_odom.position[2]:.2f}]\" ) except Exception as e: logger.error(f\"Publish error: {str(e)}\", exc_info=True)def main(args=None): rclpy.init(args=args) node = FastLIOToPX4() try: rclpy.spin(node) except KeyboardInterrupt: logger.info(\"Node shutdown by user\") except Exception as e: logger.error(f\"Node crashed: {str(e)}\", exc_info=True) finally: node.destroy_node() rclpy.shutdown()if __name__ == \'__main__\': main()注意，这个程序只是用来测试px4能不能进入定点模式，实机使用的话是需要按照自己的需求更改的！！！，没弄清楚原理之前不要随便上实机！！！
如果这个程序能正常运行，就全部关闭另开四个终端依次运行以下程序（我把这些程序运行的文件夹地址也贴出来了，仅供参考）：
kkkitten@kkkitten-desktop:~$ MicroXRCEAgent serial -D /dev/ttyUSB0 -b 921600kkkitten@kkkitten-desktop:~/unilidar_sdk2-2.0.4/unitree_lidar_ros2$ ros2 launch unitree_lidar_ros2 launch.pykkkitten@kkkitten-desktop:~/fast_lio2$ ros2 launch fast_lio mapping.launch.py config_file:=unilidar_l2.yamlkkkitten@kkkitten-desktop:~/new_ros2_ws$ ros2 run fastlio2_px4_bridge fastlio2_to_px4后续如果嫌需要开四个窗口太麻烦，可以自己写一个launch文件一次性启动全部节点，这里就不说具体怎么实现了。
如果运行无报错，可以使用这个代码查看实际的发布信息：
ros2 topic echo /fmu/in/vehicle_visual_odometry
有数据就成功了，这个时候我们再将px4通过usb连接到电脑上，打开qgc地面站，使用一下代码查看数据是否通过DDS发送过来了：
listener vehicle_visual_odometry
此时可以看到local_position_ned显示了对应的数据。
尝试使用遥控器切换到定点模式，成功！也能正常解锁。（由于硬件还没安装到无人机上所以没有尝试飞行）
-------------------------------------------------------------------------------------------------------------------------------
这段时间在实机上试了试，效果还可以（我实机的程序不是上面那个，实机不要用上面那个，请自己更改！！！）
px4定点
