From ebdafcca7c5d02d11437116481d653beaf175307 Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Wed, 4 Mar 2026 22:19:41 +0800
Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E5=A4=8D=20/current=5Fpose=20?=
 =?UTF-8?q?=E6=B2=A1=E6=9C=89=E6=95=B0=E6=8D=AE=E7=9A=84=E9=97=AE=E9=A2=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

问题: 当没有收到目标点时，timer_callback 直接返回，不发布当前位置

修复:
- 始终发布当前位置到 /current_pose
- 只有在有目标点时才执行控制逻辑
- 改善了节点的可观测性

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

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 c4f9993..d7e7bcc 100644
--- a/src/rm_nav/rm_simple_move/src/simple_move.cpp
+++ b/src/rm_nav/rm_simple_move/src/simple_move.cpp
@@ -132,13 +132,9 @@ namespace rm_simpal_move
      */
     void RMSimpleMove::timer_callback()
     {
-        if (!has_goal_) {
-            return;
-        }
-
         try
         {
-            // 获取当前位置
+            // 获取当前位置（始终发布，不管有没有目标）
             auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero);
             AHRS_GetEulr(&current_eulr_, trans.transform.rotation);
 
@@ -152,6 +148,11 @@ namespace rm_simpal_move
             current_pose_msg.pose.orientation = trans.transform.rotation;
             current_pose_pub_->publish(current_pose_msg);
 
+            // 如果没有目标，只发布当前位置，不执行控制
+            if (!has_goal_) {
+                return;
+            }
+
             // 发布目标点 TF
             geometry_msgs::msg::TransformStamped goal_transform;
             goal_transform.header.stamp = this->get_clock()->now();