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 running_; std::atomic goal_reached_; std::atomic has_goal_; + std::atomic 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(¤t_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();