ROS Noetic节点无报错但无法保存.h5/.npy格式里程计数据求助

DevOps 小满

2026-8-26

问题根因

文件存到了非预期路径：代码里用的是不带路径的文件名，属于相对路径，ROS节点运行时的工作目录是你启动节点时终端所在的目录（如果用roslaunch启动默认是~/.ros），不是脚本存放的目录，去脚本文件夹找自然找不到文件。
关闭时的保存逻辑位置完全错误：你把if rospy.is_shutdown()的判断放在了rospy.spin()前面，节点刚启动时还没收到关闭信号，这个判断永远为假；等你按Ctrl+C停节点的时候，rospy.spin()才会退出，前面的if判断早就执行完了，这段保存h5的代码从头到尾不会运行。
HDF5文件操作逻辑有缺陷：节点一启动就用w模式打开h5文件，但直到退出才写数据，也没做可靠的关闭逻辑，数据一直卡在内存缓存里没落盘，就算生成了文件也是空的损坏文件。
回调里高频写盘效率极低：每收到一帧消息就全量重写整个npy文件，数据量上来之后IO会被占满，很容易丢数据甚至卡节点。

修复方案

所有保存路径统一用绝对路径，要么手动写死固定路径，要么用rospkg自动获取功能包路径拼接，禁止使用裸相对路径。
用rospy.on_shutdown()注册节点关闭回调，把最终存文件的逻辑放在这个回调里，只要节点正常退出（包括Ctrl+C触发的退出）就一定会执行。
h5文件不要全程保持打开状态，存数据的时候再打开，用with上下文管理器写完自动关闭，避免缓存不落盘、文件损坏的问题。
回调里只更新内存里的数组，不要每次收到消息都写盘，减少IO开销，需要中途备份的话可以加个计数，每存够100帧写一次即可。

修正后的可运行代码如下：

import rospy
import os
import rospkg
from nav_msgs.msg import Path
import numpy as np
import h5py

# 全局数组初始化，第一行预留给占位的全0数据，后续会剔除
odomdata = np.zeros((1,8))

# 拼接绝对保存路径，避免找不到文件
rospack = rospkg.RosPack()
# 把下面引号里的内容替换成你存这个脚本的ROS功能包名称
pkg_path = rospack.get_path("替换成你的功能包名")
save_dir = os.path.join(pkg_path, "saved_odom")
os.makedirs(save_dir, exist_ok=True)
npy_path = os.path.join(save_dir, "odomData.npy")
h5_path = os.path.join(save_dir, "odomData.h5")

def shutdown_hook():
    """节点关闭时自动执行的保存逻辑"""
    # 去掉初始化时的第一行全0占位数据
    final_data = odomdata[1:, :]
    # 保存npy文件
    np.save(npy_path, final_data)
    print(f"NPY文件已保存至: {npy_path}")
    # 保存h5文件，with上下文会自动关闭文件，保证数据落盘
    with h5py.File(h5_path, 'w') as hf:
        hf.create_dataset('odom_dataset', data=final_data)
    print(f"H5文件已保存至: {h5_path}")

def callback(data):
    global odomdata
    # 空Path消息直接跳过，避免取[-1]时报错
    if len(data.poses) == 0:
        return
    # 解析最新位姿数据
    last_pose = data.poses[-1]
    x_pos = last_pose.pose.position.x
    y_pos = last_pose.pose.position.y
    z_pos = last_pose.pose.position.z
    x_orient = last_pose.pose.orientation.x
    y_orient = last_pose.pose.orientation.y
    z_orient = last_pose.pose.orientation.z
    w_orient = last_pose.pose.orientation.w
    sec = last_pose.header.stamp.secs
    nsec = last_pose.header.stamp.nsecs * (10**-9)
    float_time = sec + nsec
    # 拼接新数据到内存数组
    odomarray = np.array([[float_time, x_pos, y_pos, z_pos, x_orient, y_orient, z_orient, w_orient]])
    odomdata = np.append(odomdata, odomarray, axis=0)

def listener():
    rospy.init_node('odomlistener')
    # 注册关闭钩子，节点退出时自动执行保存
    rospy.on_shutdown(shutdown_hook)
    rospy.Subscriber('/ssl_slam/trajectory', Path, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

验证步骤

把代码里的功能包名替换成你自己的，或者直接把npy_path、h5_path改成自定义的固定绝对路径（比如/home/你的用户名/odom_data/odomData.npy），提前建好对应文件夹。
给脚本加可执行权限：chmod +x 你的脚本文件名.py
运行节点，接收几帧数据之后按Ctrl+C退出，看终端打印的保存路径，直接去对应目录找文件即可。

内容的提问来源于stack exchange，提问作者hunterlineage