共享内存与ros通信--同时也聊聊slam-gps中的轨迹融合

原创 于 2025-11-27 22:06:54 发布 · 880 阅读 · 20 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#机器人 #slam #共享内存 #ros1 #ros2 #humble #python

这个项目的初衷是想要在只有ros2的数据环境下运行ros1的一些算法，在想要怎么把这两者合理的通信起来。

对于大型的数据来说（相机，雷达数据），肯定是没办法直接通过话题进行传递的，所以我们需要一个ros1和ros2的沟通的方式，通过查找资料发现，确实是有做这个事的相关的功能包，但是它的局限在于只能满足一些标准的ros消息。

目录

ros1_bridge功能包

比如使用 ros1_bridge，

  1. 前置条件
    你的机器上必须同时安装了 ROS 1（如 Noetic）和 ROS 2（如 Foxy, Humble 等）。
    两个版本的 ROS 必须包含相同的消息定义（例如 std_msgs, sensor_msgs 等）。
    这个bridge是运行在ros2上的：
sudo apt install ros-<ros2-distro>-ros1-bridge

顺序是：

# 1. 先加载 ROS 1 环境
source /opt/ros/noetic/setup.bash

# 2. 再加载 ROS 2 环境
source /opt/ros/<ros2-distro>/setup.bash

# 3. 检查环境变量（可选，确保 ROS_MASTER_URI 正确）
echo $ROS_MASTER_URI

# 4. 运行动态桥接（Dynamic Bridge）
ros2 run ros1_bridge dynamic_bridge

这样就能把所有的标准消息都桥接起来了。总结来说，就是运行在ros2上：
它是双向的（Bidirectional）
它不仅仅把 ROS 2 转给 ROS 1，它能同时处理两个方向的数据流：
ROS 1 -> ROS 2：如果有 ROS 1 的节点在发数据，ROS 2 的节点在听，Bridge 会把 ROS 1 的消息“搬运”过去。

ROS 2 -> ROS 1：如果有 ROS 2 的节点在发数据，ROS 1 的节点在听，Bridge 也会把 ROS 2 的消息“搬运”回来。

注意：
这里的重点误区（坑点）
误区： “我只要运行了 bridge，所有 ROS 1 的话题就会出现在 ROS 2 里。” 真相： 不是的。 默认情况下（使用 dynamic_bridge），它是**懒惰（Lazy）**的。

只有当 ROS 2 这一侧真的有一个节点在订阅（监听） /camera 话题时，Bridge 才会去 ROS 1 那边把 /camera 搬运过来。

如果 ROS 2 那边没人听，Bridge 就不会通过网络搬运数据，以节省带宽。
身份： 它是一个特殊的 ROS 2 节点，但体内植入了 ROS 1 的基因。
方向： 双向互通。
前提： 必须在同一个 Shell 里让它同时看见两个版本的 ROS 环境变量。

你别说，这个节点还是挺有意思的。

共享内存空间

第二种方法，就是共享内存的形式了，共享内存又分为文件形式和虚拟形式。这里需要注意，真正的共享内存不是文件形式，而是虚拟形式的。
我这里直接先说我的案例：

我需要的是把ros1的slam算法，用ros2的数据跑起来，所以需要将ros2的topic写道共享内存中，然后在ros1中读取共享内存的数据，发布ros1的topic，最后再把这个数据接入到lio_sam的算法中实现闭环。

这里把相应的功能封装到了ros2_to_ros1_ws中：
在这里插入图片描述
包含ros1和ros2的两个环境，各自的ros环境进行编译，以下是采集数据过程的一些说明：
通信之间通过共享内存实现消息的共享，从而实现了：
ros1的算法，ros2的数据。

ros2的数据采集

首先我们通过ros2的节点将需要的topic转为共享内存空间.
包括:
1.激光雷达
2.imu
3.gps
4.tf
至于共享内存的两种模式,还没有特别注意…现在也还是基本可以使用.

ros1读取共享内存并发布话题

接着在ros1中接入了ros2的相关话题,得到最终的ros1的话题消息:
1.激光雷达
2.imu
3.gps
4.tf

启动顺序:
首先启动ros2的共享内存节点

ros2 launch data_bridge_ros2 data_bridge.launch.py

接着可以启动ros1读取共享内存的节点

roslaunch data_bridge_ros1 data_bridge.launch

然后运行ros1的算法

roslaunch lio_sam run.launch

此时可以记录一些数据，比如

rosbag record /lio_sam/mapping/odometry /gps_imu_data

然后再播放bag：

ros2 bag play rosbag2_2025_10_11-09_57_36_0.db3

slam建图的轨迹比较

由于项目本身是为了slam而做的，这里遇到的一个非常重要的问题就在于定位建图需要比较slam的轨迹和gps的轨迹的error值。

在这里插入图片描述

比如我这里是需要记录gps_imu_data数据以及/lio_sam/mapping/odometry,实时记录这两条轨迹,至于怎么比较呢?

这个我通过一个py来读取bag数据来重新进行融合.
evaluate_trajectory.py：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rosbag
import numpy as np
import math
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D  # 引入3D绘图工具
from scipy.spatial.transform import Rotation as R

def read_bag_data(bag_path, gps_topic, slam_topic):
    """
    读取bag文件，提取GPS和SLAM的 (time, x, y, z) 数据
    """
    gps_data = []
    slam_data = []
    
    print(f"开始读取 Bag 文件: {bag_path} ...")
    
    try:
        bag = rosbag.Bag(bag_path)
    except Exception as e:
        print(f"无法打开bag文件: {e}")
        return None, None

    for topic, msg, t in bag.read_messages(topics=[gps_topic, slam_topic]):
        if topic == gps_topic:
            # 提取自定义 GPS 消息
            timestamp = msg.header.stamp.to_sec()
            
            # 使用用户指定的 x, y, z 字段
            x = msg.x
            y = msg.y
            z = msg.z
            gps_data.append([timestamp, x, y, z])
            
        elif topic == slam_topic:
            # 提取 nav_msgs/Odometry
            timestamp = msg.header.stamp.to_sec()
            pos = msg.pose.pose.position
            slam_data.append([timestamp, pos.x, pos.y, pos.z])

    bag.close()
    
    # 转换为 Numpy 数组并按时间排序
    gps_data = np.array(gps_data)
    slam_data = np.array(slam_data)
    
    # 按时间戳排序
    if len(gps_data) > 0:
        gps_data = gps_data[gps_data[:, 0].argsort()]
    if len(slam_data) > 0:
        slam_data = slam_data[slam_data[:, 0].argsort()]
        
    print(f"读取完成。GPS点数: {len(gps_data)}, SLAM点数: {len(slam_data)}")
    return gps_data, slam_data

def synchronize_trajectories(ref_data, target_data):
    """
    时间同步
    """
    start_time = max(ref_data[0, 0], target_data[0, 0])
    end_time = min(ref_data[-1, 0], target_data[-1, 0])
    
    ref_mask = (ref_data[:, 0] >= start_time) & (ref_data[:, 0] <= end_time)
    ref_sync = ref_data[ref_mask]
    
    if len(ref_sync) == 0:
        print("错误：两个轨迹没有时间重叠！")
        return None, None

    query_times = ref_sync[:, 0]
    
    target_interp_x = np.interp(query_times, target_data[:, 0], target_data[:, 1])
    target_interp_y = np.interp(query_times, target_data[:, 0], target_data[:, 2])
    target_interp_z = np.interp(query_times, target_data[:, 0], target_data[:, 3])
    
    target_sync = np.column_stack((query_times, target_interp_x, target_interp_y, target_interp_z))
    
    return ref_sync, target_sync

def compute_rigid_transform(source_points, target_points):
    """
    SVD 计算 rigid transform
    """
    centroid_source = np.mean(source_points, axis=0)
    centroid_target = np.mean(target_points, axis=0)
    
    source_centered = source_points - centroid_source
    target_centered = target_points - centroid_target
    
    H = np.dot(source_centered.T, target_centered)
    
    U, S, Vt = np.linalg.svd(H)
    
    R_matrix = np.dot(Vt.T, U.T)
    
    if np.linalg.det(R_matrix) < 0:
        Vt[-1, :] *= -1
        R_matrix = np.dot(Vt.T, U.T)
    
    t_vec = centroid_target.T - np.dot(R_matrix, centroid_source.T)
    
    return R_matrix, t_vec

def calculate_error(slam_xyz, gps_aligned_xyz):
    errors = np.linalg.norm(slam_xyz - gps_aligned_xyz, axis=1)
    rmse = np.sqrt(np.mean(errors**2))
    mean_error = np.mean(errors)
    max_error = np.max(errors)
    return errors, rmse, mean_error, max_error

def main():
    # ================= 配置区域 =================
    bag_file = '2025-11-27-16-44-35.bag'  # 请修改为你的bag文件名
    gps_topic = '/gps_imu_data'
    slam_topic = '/lio_sam/mapping/odometry'
    # ===========================================

    # 1. 读取数据
    gps_raw, slam_raw = read_bag_data(bag_file, gps_topic, slam_topic)
    
    if gps_raw is None or len(gps_raw) < 10 or len(slam_raw) < 10:
        print("数据不足，无法计算。")
        return

    # 2. 同步数据
    slam_sync, gps_sync = synchronize_trajectories(slam_raw, gps_raw)
    
    if slam_sync is None:
        return

    slam_xyz = slam_sync[:, 1:4]
    gps_xyz = gps_sync[:, 1:4]

    # 3. 计算变换矩阵 (GPS -> SLAM)
    # T_gps_to_slam: 把世界坐标(UTM)转到局部坐标(SLAM)
    R_mat, t_vec = compute_rigid_transform(gps_xyz, slam_xyz)

    # 组装 4x4 变换矩阵 T
    T = np.eye(4)
    T[:3, :3] = R_mat
    T[:3, 3] = t_vec

    print("\n" + "="*30)
    print("矩阵 A: GPS (UTM) -> SLAM 的变换矩阵:")
    print("(用于将 GPS 坐标拉回局部坐标系进行误差对比)")
    print("="*30)
    np.set_printoptions(suppress=True, precision=6)
    print(T)
    # 也可以打印一下旋转的欧拉角，方便直观理解
    r = R.from_matrix(R_mat)
    print(f"\n旋转 (欧拉角 xyz, degrees): {r.as_euler('xyz', degrees=True)}")
    print(f"平移 (x, y, z): {t_vec}")

    
    # 4. 应用变换计算误差
    gps_aligned = np.dot(gps_xyz, R_mat.T) + t_vec
    errors, rmse, mean_err, max_err = calculate_error(slam_xyz, gps_aligned)

    print("\n" + "="*30)
    print("定位偏差评估结果 (米):")
    print("="*30)
    print(f"RMSE (均方根误差): {rmse:.4f} m")
    print(f"Mean Error (平均误差): {mean_err:.4f} m")
    print(f"Max Error (最大误差): {max_err:.4f} m")

    # =========================================================
    # 新增部分：计算逆矩阵 (SLAM -> GPS) 并输出
    # =========================================================
    print("\n" + "="*30)
    print("矩阵 B (逆矩阵): SLAM -> GPS (UTM) 的变换参数:")
    print("(即 SLAM 原点在地图上的实际坐标和朝向)")
    print("==============================")
    
    # 计算逆矩阵
    T_inv = np.linalg.inv(T)
    
    # 提取平移
    x_final = T_inv[0, 3]
    y_final = T_inv[1, 3]
    z_final = T_inv[2, 3]
    
    # 提取旋转并转为欧拉角
    R_inv = T_inv[:3, :3]
    r_inv_obj = R.from_matrix(R_inv)
    # 使用 'xyz' 顺序输出 roll, pitch, yaw
    euler_inv = r_inv_obj.as_euler('xyz', degrees=True) 
    
    print(f"x: {x_final:.10f}")
    print(f"y: {y_final:.10f}")
    print(f"z: {z_final:.10f}")
    print(f"roll: {euler_inv[0]:.10f}")
    print(f"pitch: {euler_inv[1]:.10f}")
    print(f"yaw: {euler_inv[2]:.10f}")
    print("==============================\n")

    # 6. 绘图结果
    fig = plt.figure(figsize=(14, 7))

    # 轨迹对比图 (3D)
    ax1 = fig.add_subplot(121, projection='3d')
    ax1.plot(slam_xyz[:, 0], slam_xyz[:, 1], slam_xyz[:, 2], 
             'b-', label='Lidar SLAM', alpha=0.7, linewidth=1)
    ax1.plot(gps_aligned[:, 0], gps_aligned[:, 1], gps_aligned[:, 2], 
             'r--', label='Aligned GPS', alpha=0.7, linewidth=1)
    
    ax1.set_title('Trajectory Comparison (3D View)')
    ax1.set_xlabel('X (m)')
    ax1.set_ylabel('Y (m)')
    ax1.set_zlabel('Z (m)')
    ax1.legend()

    # 误差随时间变化图
    ax2 = fig.add_subplot(122)
    time_axis = slam_sync[:, 0] - slam_sync[0, 0]
    ax2.plot(time_axis, errors, 'g-', label='3D Error', linewidth=1)
    ax2.set_title(f'Localization Error over Time (RMSE={rmse:.3f}m)')
    ax2.set_xlabel('Time (s)')
    ax2.set_ylabel('Error (m)')
    ax2.grid(True)

    plt.tight_layout()
    plt.show()

if __name__ == "__main__":
    main()

这个bag包的两个话题数据对应的消息内容为：

cyun@cyun:~$ rostopic info /gps_imu_data 
Type: car_interfaces/GpsImuInterface

Publishers: 
 * /play_1764251971042717169 (http://cyun:36305/)

Subscribers: None

---
nav_flag: "2"
x_acc: 0.10093999654054642
y_acc: -0.06272000074386597
x_gyro: 0.009999999776482582
y_gyro: -0.10000000149011612
z_acc: 9.97052001953125
z_gyro: 0.2800000011920929
acc: 9.971228216286516
gps_week: 2387.0
gps_ms: 525521959.6250057
nav_flag_g: "2"
header: 
  seq: 0
  stamp: 
    secs: 1760147921
    nsecs: 959624960
  frame_id: "gps_link"
gps_time: 1760147921959624960
PosLon: 117.29809985
PosLat: 38.99651669
PosAlt: -3.32
lat_g: 38.99651669
lon_g: 117.29809985
east_speed_g: 0.39500001072883606
north_speed_g: -2.260999917984009
up_speed_g: -0.0020000000949949026
alt_g: -3.32
heading_g: -80.44999694824219
VelE: 0.39500001072883606
VelN: -2.260999917984009
VelU: -0.0020000000949949026
Vel: 2.2952450060069522
AngleHeading: -80.44999694824219
AnglePitch: 0.019999999552965164
AngleRoll: 0.07000000029802322
system_state: 2
GpsNumSatsUsed: 12
satellite_status: 0
GpsAge: 0.1
state: True
posX: 525814.2787988522
posY: 4316432.294969826
posZ: -3.32
process_time: 3.8623809814453125e-05
Gps_Cfault: False
x: 525814.2787988522
y: 4316432.294969826
z: -3.32
pitch: 0.019999999552965164
roll: 0.07000000029802322
yaw: -80.44999694824219
vel: 2.2952450060069522
lat: 38.99651669
lon: 117.29809985
state_ndt: True
---

cyun@cyun:~/Documents/backup_9.22/9.22/ros2_to_ros1_ws/ros1$ rostopic info /lio_sam/mapping/odometry 
Type: nav_msgs/Odometry

Publishers: 
 * /play_1764251971042717169 (http://cyun:36305/)

Subscribers: None

---
header: 
  seq: 436
  stamp: 
    secs: 1760147957
    nsecs: 679423332
  frame_id: "odom"
child_frame_id: "odom_mapping"
pose: 
  pose: 
    position: 
      x: 141.18804931640625
      y: 40.92380905151367
      z: 11.70459270477295
    orientation: 
      x: -0.018105755638900916
      y: -0.019853375077504946
      z: 0.24379445744093267
      w: 0.9694546341277954
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

为了比较轨迹，是需要用到时间戳的，在同一时间下来比较两条轨迹之间的不同。

运行py之后即可看到两条轨迹之间的偏差：
在这里插入图片描述
同时得到了两个矩阵：

==============================
矩阵 A: GPS (UTM) -> SLAM 的变换矩阵:
(用于将 GPS 坐标拉回局部坐标系进行误差对比)
==============================
[[     -0.105942      -0.992226      -0.065304 4338655.191419]
 [      0.993562      -0.102975      -0.047236  -77929.614784]
 [      0.040145      -0.069887       0.996747  280564.759149]
 [      0.             0.             0.             1.      ]]

旋转 (欧拉角 xyz, degrees): [-4.010763 -2.30073  96.08635 ]
平移 (x, y, z): [4338655.191419  -77929.614784  280564.759149]

==============================
定位偏差评估结果 (米):
==============================
RMSE (均方根误差): 2.8448 m
Mean Error (平均误差): 2.7005 m
Max Error (最大误差): 5.5193 m

==============================
矩阵 B (逆矩阵): SLAM -> GPS (UTM) 的变换参数:
(即 SLAM 原点在地图上的实际坐标和朝向)
==============================
x: 525809.2958430700
y: 4316508.2026283247
z: -3.2533143504
roll: -2.7132511843
pitch: 3.7442862653
yaw: -96.0944834560
==============================

两种矩阵的含义：
这两种矩阵分别是解决什么问题，请清楚的说明一下。比如已知gps坐标，转到slam系下之类的具体的例子

这两种矩阵（我们暂时称为 矩阵 A 和 矩阵 B）互为逆矩阵。清楚地理解它们对于自动驾驶和机器人开发至关重要。

以下是具体的场景说明：

  1. 矩阵 A：GPS (UTM) → SLAM
    （世界坐标系 → 局部坐标系）

这是代码中 compute_rigid_transform 直接计算出来的矩阵。

输入：一个巨大的 GPS/UTM 坐标（例如：x=525844.5, y=4316346.1）。

输出：一个很小的 SLAM 局部坐标（例如：x=5.0, y=2.0）。

数学特征：平移向量 t 通常非常巨大且为负值（或者经过旋转后变大），目的是把几百万的坐标值“减去”，拉回到 0 附近。

“我知道地球上的经纬度，我想知道它在我当前雷达地图里的哪个位置。”

  1. 矩阵 B：SLAM → GPS (UTM)
    （局部坐标系 → 世界坐标系）

这是矩阵 A 的逆矩阵（np.linalg.inv(T)）。

输入：一个 SLAM 局部坐标（例如：x=0, y=0，即建图原点）。

输出：一个 GPS/UTM 坐标（例如：x=525844.6, y=4316346.1）。

数学特征：平移向量 t 就是 SLAM 原点在地球上的实际坐标。旋转部分就是 车辆起始的朝向（方位角）。

注意这两个矩阵是完全不同的，所以我们平时会看到两种不同的变换，不要感到奇怪。

评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
表情包 代码片
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

白云千载尽

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值