fix: 修复 /current_pose 没有数据的问题

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

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

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
Robofish 2026-03-04 22:19:41 +08:00
parent 58c4fc993e
commit ebdafcca7c

View File

@ -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();