Spin the node from the blackboard before calling getCurrentPose (#808)

ros-navigation · Jun 6, 2019 · c6490ea · c6490ea
1 parent 916a6ee
commit c6490ea
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 0 deletions.
diff --git 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<geometry_msgs::msg::PoseWithCovarianceStamped>();
 
+    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
@@ -69,6 +69,7 @@ class IsLocalizedCondition : public BT::ConditionNode
   {
     auto current_pose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
 
+    rclcpp::spin_some(node_);
     if (!robot_->getCurrentPose(current_pose)) {
       RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
       return false;