如何将sensor_msgs/FluidPressure重发布为geometry_msgs/PointStamped消息

核心转换逻辑 #

流体压力换算为深度的公式如下，可根据实际仿真参数调整常量取值：
高度值 = (标准大气压 - 实测流体压力) / (海水密度 * 重力加速度)

常用默认参数：标准大气压101325Pa，海水密度1025kg/m³，重力加速度9.81m/s²
ENU坐标系下水下深度对应的z值为负数，需根据实际使用的坐标系调整输出符号

方案1：自定义Python转换节点 #

直接编写ROS节点完成消息订阅、换算、重发布操作，代码示例如下：

#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import FluidPressure
from geometry_msgs.msg import PointStamped

# 配置参数
P0 = 101325.0  # 标准大气压，单位Pa，若传感器输出为表压则设为0
RHO = 1025.0   # 海水密度，单位kg/m³
G = 9.81       # 重力加速度，单位m/s²
TARGET_FRAME = "rexrov2/base_link"  # 可替换为实际压力传感器frame_id

pub = None

def pressure_callback(msg):
    global pub
    # 压力转高度
    height = (P0 - msg.fluid_pressure) / (RHO * G)
    # 构造PointStamped消息
    out_msg = PointStamped()
    out_msg.header = msg.header
    out_msg.header.frame_id = TARGET_FRAME  # 也可直接复用原msg的frame_id
    out_msg.point.x = 0.0
    out_msg.point.y = 0.0
    out_msg.point.z = height  # ENU坐标系下水下深度为负，无需额外调整符号
    pub.publish(out_msg)

if __name__ == "__main__":
    rospy.init_node("pressure_to_pointstamped")
    pub = rospy.Publisher("/rexrov2/pressure", PointStamped, queue_size=10)
    rospy.Subscriber("/rexrov2/pressure_raw", FluidPressure, pressure_callback, queue_size=10)
    rospy.spin()

使用步骤：

• 将代码保存为ROS包下的scripts目录中，添加可执行权限
• 修改代码中订阅/发布的话题名、参数匹配你的仿真配置
• 运行节点即可完成消息实时转换

方案2：无需编码快速实现 #

使用ROS官方提供的topic_tools/transform节点直接完成转换，无需编写自定义代码，可直接在命令行运行或写入launch文件：

命令行运行示例 #

rosrun topic_tools transform /rexrov2/pressure_raw /rexrov2/pressure geometry_msgs/PointStamped \
'geometry_msgs/PointStamped(header=m.header, point=geometry_msgs/Point(0,0, (101325.0 - m.fluid_pressure)/(1025.0*9.81)))'

launch文件写入示例 #

<node pkg="topic_tools" type="transform" name="pressure_convert"
      args="/rexrov2/pressure_raw /rexrov2/pressure geometry_msgs/PointStamped 'geometry_msgs/PointStamped(header=m.header, point=geometry_msgs/Point(0,0, (101325.0 - m.fluid_pressure)/(1025.0*9.81)))'" />

验证方法 #

运行转换节点后执行以下命令验证输出是否符合要求：

• 查看话题类型是否匹配：rostopic info /rexrov2/pressure
• 查看输出的高度值是否符合实际仿真深度：rostopic echo /rexrov2/pressure

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