From 6286274ff21c2f22e85c454ad13a7a9660ae21f4 Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Thu, 5 Mar 2026 02:24:16 +0800
Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E5=A4=8D=20TF=20=E6=97=B6?=
 =?UTF-8?q?=E9=97=B4=E6=88=B3=E9=97=AE=E9=A2=98=E5=92=8C=20NAV=20=E7=9B=AE?=
 =?UTF-8?q?=E6=A0=87=E9=87=8D=E5=A4=8D=E5=8F=91=E9=80=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

问题:
1. TF 查询使用 TimePointZero 导致时间戳错误
2. NAV 模式每次 timer 都发送目标，应该只发送一次

修复:
- TF 查询添加超时参数 (0.5秒)
- 添加 nav_goal_sent_ 标志，NAV 模式只发送一次目标
- 使用 WARN_THROTTLE 限制 TF 错误日志频率

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
---
 .../include/rm_simpal_move/simple_move.hpp        |  1 +
 src/rm_nav/rm_simple_move/src/simple_move.cpp     | 15 ++++++++++-----
 2 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp b/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp
index 9e67349..3d30a92 100644
--- a/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp
+++ b/src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp
@@ -94,6 +94,7 @@ private:
     std::atomic<bool> running_;
     std::atomic<bool> goal_reached_;
     std::atomic<bool> has_goal_;
+    std::atomic<bool> nav_goal_sent_;  // NAV 模式下目标是否已发送
 
     AHRS_Eulr_t current_eulr_;
     Goal_t goal_pose_;
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 47cec7b..4a5f062 100644
--- a/src/rm_nav/rm_simple_move/src/simple_move.cpp
+++ b/src/rm_nav/rm_simple_move/src/simple_move.cpp
@@ -62,6 +62,7 @@ namespace rm_simpal_move
           running_(true),
           goal_reached_(false),
           has_goal_(false),
+          nav_goal_sent_(false),
           pid_linear_x_(0.8f, 0.0f, 0.1f, 3.0f),
           pid_linear_y_(0.8f, 0.0f, 0.1f, 3.0f),
           pid_angular_(1.5f, 0.0f, 0.2f, 2.0f)
@@ -137,7 +138,7 @@ namespace rm_simpal_move
         try
         {
             // 获取当前位置（始终发布，不管有没有目标）
-            auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero);
+            auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero, tf2::durationFromSec(0.5));
             AHRS_GetEulr(&current_eulr_, trans.transform.rotation);
 
             // 发布当前位置
@@ -175,7 +176,7 @@ namespace rm_simpal_move
 
             // 计算目标点在 base_link 坐标系下的位置
             geometry_msgs::msg::TransformStamped goal_in_base_link;
-            goal_in_base_link = tf_buffer_.lookupTransform("base_link", "goal_pose", tf2::TimePointZero);
+            goal_in_base_link = tf_buffer_.lookupTransform("base_link", "goal_pose", tf2::TimePointZero, tf2::durationFromSec(0.5));
 
             float goal_x_in_base_link = goal_in_base_link.transform.translation.x;
             float goal_y_in_base_link = goal_in_base_link.transform.translation.y;
@@ -195,8 +196,11 @@ namespace rm_simpal_move
                 // PID 模式
                 publish_pid_control();
             } else {
-                // NAV 模式
-                publish_nav_goal();
+                // NAV 模式：只在第一次发送目标
+                if (!nav_goal_sent_) {
+                    publish_nav_goal();
+                    nav_goal_sent_ = true;
+                }
             }
 
             // 发布状态
@@ -204,7 +208,7 @@ namespace rm_simpal_move
         }
         catch (const tf2::TransformException &ex)
         {
-            RCLCPP_WARN(this->get_logger(), "TF 变换失败: %s", ex.what());
+            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "TF 变换失败: %s", ex.what());
         }
     }
 
@@ -384,6 +388,7 @@ namespace rm_simpal_move
 
         has_goal_ = true;
         goal_reached_ = false;
+        nav_goal_sent_ = false;  // 重置 NAV 目标发送标志
 
         // 重置 PID
         pid_linear_x_.reset();