ROS中Publisher发消息但Subscriber无法正确接收的问题排查

问题现象 #

自行实现的Publisher向/move_base_simple/goal话题发送目标点，前两个目标能被Subscriber正常接收，但第三个目标(1.8,-1.6,1.6)未被捕获，Subscriber反而收到了frame_id为map的大坐标值。通过rostopic echo确认存在其他节点也在向该话题发布消息。

Publisher代码 #

target_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
...
void fsm::pubNewTarget(double next_target_[3])
{
    geometry_msgs::PoseStamped goal;
    goal.header.stamp = ros::Time::now();
    goal.header.frame_id = "world";

    goal.pose.position.x = next_target_[0];
    goal.pose.position.y = next_target_[1];
    goal.pose.position.z = next_target_[2];
    goal.pose.orientation.x = 0;
    goal.pose.orientation.y = 0;
    goal.pose.orientation.z = 0;
    goal.pose.orientation.w = 1;

    target_pub_.publish(goal);
    ROS_INFO("Published next target to /move_base_simple/goal: %f,%f,%f",goal.pose.position.x,goal.pose.position.y,goal.pose.position.z);

    cur_target_ = Eigen::Vector3d(next_target_[0], next_target_[1], next_target_[2]);
}

Subscriber代码 #

waypoint_sub_ = nh.subscribe("/move_base_simple/goal", 1, &EGOReplanFSM::waypointCallback, this);//(in a class)
...
void EGOReplanFSM::waypointCallback(const geometry_msgs::PoseStampedPtr &msg)
{
    if (msg->pose.position.z < -0.1)
        return;

    cout << "Triggered!" << endl;
    // trigger_ = true;
    init_pt_ = odom_pos_;

    Eigen::Vector3d end_wp(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    ROS_INFO("target: %f,%f,%f",end_wp(0), end_wp(1), end_wp(2));

    planNextWaypoint(end_wp);
}

运行日志 #

[ INFO] [1718241065.242124381, 4061.818000000]: Published next target to /move_base_simple/goal: 1.800000,0.000000,1.600000
[ INFO] [1718241081.398708229, 4077.926000000]: Published next target to /move_base_simple/goal: 1.800000,1.600000,1.600000
[ INFO] [1718241147.840353409, 4138.438000000]: Published next target to /move_base_simple/goal: 1.800000,-1.600000,1.600000
[ INFO] [1718241065.242303178, 4061.818000000]: target: 1.800000,0.000000,1.600000
[ INFO] [1718241081.398879554, 4077.926000000]: target: 1.800000,1.600000,1.600000
[ INFO] [1718241091.883711821, 4088.330000000]: target: 642719.066674,4671808.218637,0.000000

rostopic echo结果 #

WARNING: no messages received and simulated time is active.
Is /clock being published?
header: 
  seq: 1
  stamp: 
    secs: 3359
    nsecs: 182000000
  frame_id: "world"
pose: 
  position: 
    x: 1.8
    y: 1.6
    z: 1.6
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
---
header: 
  seq: 150
  stamp: 
    secs: 3368
    nsecs: 330504441
  frame_id: "map"
pose: 
  position: 
    x: 642719.0517464812
    y: 4671808.218661448
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
---
header: 
  seq: 151
  stamp: 
    secs: 3369
    nsecs: 478522309
  frame_id: "map"
pose: 
  position: 
    x: 642719.0680617385
    y: 4671808.256290132
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
---
header: 
  seq: 152
  stamp: 
    secs: 3370
    nsecs: 522523667
  frame_id: "map"
pose: 
  position: 
    x: 642719.0755269051
    y: 4671808.256290132
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

异常原因 #

1. 多节点发布冲突：/move_base_simple/goal是ROS中用于导航目标点的通用话题，你的节点之外存在其他节点（比如GPS相关节点、其他导航工具）也在向该话题发送消息，这些消息会被你的Subscriber一并接收。
2. 订阅队列长度限制：你的Subscriber设置的队列长度为1，当多个消息同时到达时，新消息会直接覆盖旧消息。如果第三个目标发布时，其他节点的消息刚好先到达或同时到达，你的目标消息会被挤出队列，导致Subscriber无法捕获。

解决方案 #

• 改用自定义话题：放弃使用通用的/move_base_simple/goal，创建专属话题（例如/my_robot/navigation_goal），彻底避免和其他节点的消息冲突。
• 增加订阅队列长度：将订阅时的队列参数从1调整为更大的值（比如10），降低消息被覆盖的概率，但无法从根源解决多节点发布问题。
• 添加消息过滤逻辑：在回调函数中增加判断，只处理符合预期的消息，比如只接收frame_id为world的消息：

  void EGOReplanFSM::waypointCallback(const geometry_msgs::PoseStampedPtr &msg)
  {
      // 过滤非world坐标系的消息
      if (msg->header.frame_id != "world")
          return;
      if (msg->pose.position.z < -0.1)
          return;
      // 后续处理逻辑...
  }
  

• 定位并停止无关发布节点：执行rostopic info /move_base_simple/goal查看所有发布该话题的节点，找到无关节点后停止其运行，消除消息来源冲突。

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