robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: 修复 TF 时间戳问题和 NAV 目标重复发送

Browse Source 
问题:
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>
This commit is contained in:
Robofish 2026-03-05 02:24:16 +08:00
parent ecadb68f55
commit 6286274ff2
2 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/rm_nav/rm_simple_move/include/rm_simpal_move/simple_move.hpp
View File

@ -94,6 +94,7 @@ private:
std::atomic<bool> running_; std::atomic<bool> running_;
std::atomic<bool> goal_reached_; std::atomic<bool> goal_reached_;
std::atomic<bool> has_goal_; std::atomic<bool> has_goal_;
std::atomic<bool> nav_goal_sent_; // NAV 模式下目标是否已发送
AHRS_Eulr_t current_eulr_; AHRS_Eulr_t current_eulr_;
Goal_t goal_pose_; Goal_t goal_pose_;

13
src/rm_nav/rm_simple_move/src/simple_move.cpp
View File

@ -62,6 +62,7 @@ namespace rm_simpal_move
running_(true), running_(true),
goal_reached_(false), goal_reached_(false),
has_goal_(false), has_goal_(false),
nav_goal_sent_(false),
pid_linear_x_(0.8f, 0.0f, 0.1f, 3.0f), pid_linear_x_(0.8f, 0.0f, 0.1f, 3.0f),
pid_linear_y_(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) pid_angular_(1.5f, 0.0f, 0.2f, 2.0f)
@ -137,7 +138,7 @@ namespace rm_simpal_move
try 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); AHRS_GetEulr(&current_eulr_, trans.transform.rotation);
// 发布当前位置 // 发布当前位置
@ -175,7 +176,7 @@ namespace rm_simpal_move
// 计算目标点在 base_link 坐标系下的位置 // 计算目标点在 base_link 坐标系下的位置
geometry_msgs::msg::TransformStamped goal_in_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_x_in_base_link = goal_in_base_link.transform.translation.x;
float goal_y_in_base_link = goal_in_base_link.transform.translation.y; float goal_y_in_base_link = goal_in_base_link.transform.translation.y;
@ -195,8 +196,11 @@ namespace rm_simpal_move
// PID 模式 // PID 模式
publish_pid_control(); publish_pid_control();
} else { } else {
// NAV 模式 // NAV 模式：只在第一次发送目标
if (!nav_goal_sent_) {
publish_nav_goal(); publish_nav_goal();
nav_goal_sent_ = true;
}
} }
// 发布状态 // 发布状态
@ -204,7 +208,7 @@ namespace rm_simpal_move
} }
catch (const tf2::TransformException &ex) 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; has_goal_ = true;
goal_reached_ = false; goal_reached_ = false;
nav_goal_sent_ = false; // 重置 NAV 目标发送标志
// 重置 PID // 重置 PID
pid_linear_x_.reset(); pid_linear_x_.reset();