diff --git a/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp b/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp index f8751f24fa..ccbf7c87c3 100644 --- a/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp +++ b/nav2_tasks/include/nav2_tasks/goal_reached_condition.hpp @@ -70,6 +70,7 @@ class GoalReachedCondition : public BT::ConditionNode { auto current_pose = std::make_shared(); + rclcpp::spin_some(node_); if (!robot_->getCurrentPose(current_pose)) { RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); return false; diff --git a/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp b/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp index 6615bfd6b8..1444ecf594 100644 --- a/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp +++ b/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp @@ -69,6 +69,7 @@ class IsLocalizedCondition : public BT::ConditionNode { auto current_pose = std::make_shared(); + rclcpp::spin_some(node_); if (!robot_->getCurrentPose(current_pose)) { RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); return false;