fix: 修复 /current_pose 没有数据的问题
问题: 当没有收到目标点时,timer_callback 直接返回,不发布当前位置 修复: - 始终发布当前位置到 /current_pose - 只有在有目标点时才执行控制逻辑 - 改善了节点的可观测性 Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
parent
58c4fc993e
commit
ebdafcca7c
@ -132,13 +132,9 @@ namespace rm_simpal_move
|
|||||||
*/
|
*/
|
||||||
void RMSimpleMove::timer_callback()
|
void RMSimpleMove::timer_callback()
|
||||||
{
|
{
|
||||||
if (!has_goal_) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
// 获取当前位置
|
// 获取当前位置(始终发布,不管有没有目标)
|
||||||
auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero);
|
auto trans = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero);
|
||||||
AHRS_GetEulr(¤t_eulr_, trans.transform.rotation);
|
AHRS_GetEulr(¤t_eulr_, trans.transform.rotation);
|
||||||
|
|
||||||
@ -152,6 +148,11 @@ namespace rm_simpal_move
|
|||||||
current_pose_msg.pose.orientation = trans.transform.rotation;
|
current_pose_msg.pose.orientation = trans.transform.rotation;
|
||||||
current_pose_pub_->publish(current_pose_msg);
|
current_pose_pub_->publish(current_pose_msg);
|
||||||
|
|
||||||
|
// 如果没有目标,只发布当前位置,不执行控制
|
||||||
|
if (!has_goal_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// 发布目标点 TF
|
// 发布目标点 TF
|
||||||
geometry_msgs::msg::TransformStamped goal_transform;
|
geometry_msgs::msg::TransformStamped goal_transform;
|
||||||
goal_transform.header.stamp = this->get_clock()->now();
|
goal_transform.header.stamp = this->get_clock()->now();
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user