最近做课题要实现使用激光雷达作为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-Agent
mkdir build
cd build
cmake ..
make
sudo make install
sudo 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_driver2
https://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_driver2
cd ~/wslivox/src
git clone https://github.com/Livox-SDK/Livox-SDK2.git
#下载完成后，先进行sdk2的编译
cd ./Livox-SDK2/
mkdir build
cd build
cmake .. && make -j2 #这里github中写是cmake .. && make -j，建议改为j2不容易报错
sudo make install
#完成之后，进行livox_ros_driver2的编译
cd ~/ws_livox/src/livox_ros_driver2
source /opt/ros/humble/setup.sh #由于我的ros2为humble所以这么用，版本不一样的去上面的网站里面查
./build.sh humble
最后一步可能会报错，根据报错内容修改再编译即可。比如我编译的时候报错如下（图找不到了）：
kkkitten@ubuntu:~/ws_livox/src/livox_ros_driver2$ ./build.sh humble
Working Path: /home/kkkitten/ws_livox/src/livox_ros_driver2
ROS version is: ROS2
Starting >>> livox_ros_driver2
Starting >>> 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 <module>
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] 错误 1
gmake[1]: *** [CMakeFiles/Makefile2:451：CMakeFiles/ament_cmake_python_build_livox_ros_driver2_egg.dir/all] 错误 2
gmake[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_sdk2
kkkitten@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" >> ~/.bashrc
source ~/.bashrc # 使更改生效
ros2 pkg list | grep livox # 应输出 livox_ros_driver2
3.完成上面的配置之后，回到fast_lio2工作空间中，进行编译
rosdep install –from-paths src –ignore-src -y
colcon 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.py
ros2 launch fast_lio mapping.launch.py config_file:=unilidar_l2.yaml
fastlio2即可正常进行建图：
这是对应的话题：
其中/Odometry是后续给px4提供定点数据的原始数据来源。
成功之后，如果你的px4版本比较新（我的固件版本是v1.15），需要在fastlio2功能包中找到laserMapping.cpp添加一个函数（大约在626行）：
template<typename T>
void 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;
}
//以下是新添加的部分
template<typename T>
void 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 xyz
out.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<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped, std::unique_ptr<tf2_ros::TransformBroadcaster> & tf_br)
{
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "odom";
odomAftMapped.header.stamp = get_ros_time(lidar_end_time);
set_posestamp(odomAftMapped.pose);
//以下是新添加的部分
set_twistamp(odomAftMapped.twist);
//以上是新添加的部分
pubOdomAftMapped->publish(odomAftMapped);
auto P = kf.get_P();
这个新的添加部分是使 /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 python3
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from px4_msgs.msg import VehicleOdometry, TimesyncStatus # 使用正确的VehicleOdometry类型
import numpy as np
from collections import deque
def 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, yaw
import threading
import time
import 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 921600
kkkitten@kkkitten-desktop:~/unilidar_sdk2-2.0.4/unitree_lidar_ros2$ ros2 launch unitree_lidar_ros2 launch.py
kkkitten@kkkitten-desktop:~/fast_lio2$ ros2 launch fast_lio mapping.launch.py config_file:=unilidar_l2.yaml
kkkitten@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定点
