From f82cbf86b2b1333bcfcf7ddc44a1f240ddc566ec Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Thu, 5 Mar 2026 02:30:48 +0800
Subject: [PATCH] =?UTF-8?q?fix:=20NAV=20=E6=A8=A1=E5=BC=8F=E5=9C=A8?=
 =?UTF-8?q?=E6=94=B6=E5=88=B0=E7=9B=AE=E6=A0=87=E6=97=B6=E7=AB=8B=E5=8D=B3?=
 =?UTF-8?q?=E5=8F=91=E9=80=81=E5=AF=BC=E8=88=AA=E8=AF=B7=E6=B1=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

问题: TF 查询失败导致 timer_callback 提前退出，publish_nav_goal() 未执行

修复: 将 NAV 模式的目标发送移到 nav_goal_callback 中
- 收到目标后立即发送 Action 请求，不依赖 TF
- 避免 TF 时间戳问题影响导航功能
- 测试成功：导航到 (4, -4) 完成

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
---
 src/rm_nav/rm_simple_move/src/simple_move.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/rm_nav/rm_simple_move/src/simple_move.cpp b/src/rm_nav/rm_simple_move/src/simple_move.cpp
index 4a5f062..b82ba3a 100644
--- a/src/rm_nav/rm_simple_move/src/simple_move.cpp
+++ b/src/rm_nav/rm_simple_move/src/simple_move.cpp
@@ -398,6 +398,12 @@ namespace rm_simpal_move
         RCLCPP_INFO(this->get_logger(), "收到新目标: mode=%s, x=%.2f, y=%.2f, angle=%.2f",
                     control_mode_ == 0 ? "PID" : "NAV",
                     msg->target_x, msg->target_y, msg->target_angle);
+
+        // 如果是 NAV 模式，立即发送导航目标
+        if (control_mode_ == 1) {
+            publish_nav_goal();
+            nav_goal_sent_ = true;
+        }
     }
 
     /**