如何在不同坐标系下重发布Odometry（里程计）数据？

1 报错核心原因 #

你收到的Call to publish() on an invalid Publisher报错和静态坐标系配置无关，属于代码基础逻辑错误：你声明了全局的ros::Publisher odom_pub变量，但全程没有对它进行初始化，没有调用advertise()方法创建实际的发布实例，所以回调函数调用publish()时ROS检测到发布器无效，直接触发FATAL级错误。

2 基础报错修复 #

只需要在主函数中补充发布器初始化逻辑即可，修正后的完整主函数代码如下：

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;
  // 新增：初始化里程计发布器，话题名可根据需求自行修改，队列长度设为10
  odom_pub = node.advertise<nav_msgs::Odometry>("/rexrov2/odom_in_base", 10);
  ros::Subscriber sub = node.subscribe("/rexrov2/pose_gt", 10, &poseCallback);
  ros::spin();
  return 0;
};

修改完成后即可解决报错问题，你测试的x轴线速度可以正常发布。

3 完整坐标系转换实现 #

你当前的代码仅复制了原里程计的x轴线速度，没有完成world系到机器人本体坐标系的转换逻辑，要实现符合要求的转换，需要用原里程计的姿态四元数对world系下的速度做旋转变换，回调函数的完整实现如下：

#include <tf/transform_datatypes.h> // 需要新增该头文件支持坐标转换操作

ros::Publisher odom_pub;

void poseCallback(const nav_msgs::OdometryConstPtr& msg){
    // 直接复用原消息的所有字段，后续只修改速度部分
    nav_msgs::Odometry output_odom = *msg;
    // 提取原消息中world系到base_link系的姿态四元数
    tf::Quaternion world_q;
    tf::quaternionMsgToTF(msg->pose.pose.orientation, world_q);
    tf::Transform world_to_base(world_q);
    // 把world系下的线速度转换到base_link系
    tf::Vector3 linear_world(
        msg->twist.twist.linear.x,
        msg->twist.twist.linear.y,
        msg->twist.twist.linear.z
    );
    tf::Vector3 linear_base = world_to_base.inverse() * linear_world;
    // 把world系下的角速度转换到base_link系
    tf::Vector3 angular_world(
        msg->twist.twist.angular.x,
        msg->twist.twist.angular.y,
        msg->twist.twist.angular.z
    );
    tf::Vector3 angular_base = world_to_base.inverse() * angular_world;
    // 赋值转换后的速度
    output_odom.twist.twist.linear.x = linear_base.x();
    output_odom.twist.twist.linear.y = linear_base.y();
    output_odom.twist.twist.linear.z = linear_base.z();
    output_odom.twist.twist.angular.x = angular_base.x();
    output_odom.twist.twist.angular.y = angular_base.y();
    output_odom.twist.twist.angular.z = angular_base.z();
    // 发布转换后的里程计
    odom_pub.publish(output_odom);
}

4 注意事项 #

• 如果你仅需要本体坐标系下的速度数据，不需要位姿数据，可以直接删除位姿相关的字段赋值
• 确认原里程计的child_frame_id与你的机器人本体坐标系名一致，若不一致可手动修改output_odom.child_frame_id字段为对应名称

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