Replace rclcpp::spin_some and rclcpp::spin_all (#5521)

* Get rid of rclcpp spin all and spin some

Signed-off-by: mini-1235 <[email protected]>

* Linting

Signed-off-by: mini-1235 <[email protected]>

* Vector object server

Signed-off-by: mini-1235 <[email protected]>

* Linting

Signed-off-by: mini-1235 <[email protected]>

* Update nav2_rviz_plugins/src/utils.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Error log

Signed-off-by: Maurice <[email protected]>

---------

Signed-off-by: mini-1235 <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Maurice <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

Author: mini-1235

nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp

@@ -202,15 +202,19 @@ class BTActionNodeTestFixture : public ::testing::Test
     action_server_ = std::make_shared<FibonacciActionServer>();
     server_thread_ = std::make_shared<std::thread>(
       []() {
+        rclcpp::executors::SingleThreadedExecutor executor;
+        executor.add_node(action_server_);
         while (rclcpp::ok() && BTActionNodeTestFixture::action_server_ != nullptr) {
-          rclcpp::spin_some(BTActionNodeTestFixture::action_server_);
+          executor.spin_some();
           std::this_thread::sleep_for(100ns);
         }
       });
   }
 
   void TearDown() override
   {
+    // Sleep for some time to avoid race condition
+    std::this_thread::sleep_for(std::chrono::milliseconds(80));
     action_server_.reset();
     tree_.reset();
     server_thread_->join();

nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp

@@ -31,6 +31,8 @@ class ControllerSelectorTestFixture : public ::testing::Test
   static void SetUpTestCase()
   {
     node_ = std::make_shared<nav2::LifecycleNode>("controller_selector_test_fixture");
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    executor_->add_node(node_->get_node_base_interface());
 
     // Configure and activate the lifecycle node
     node_->configure();
@@ -67,6 +69,7 @@ class ControllerSelectorTestFixture : public ::testing::Test
     config_ = nullptr;
     node_.reset();
     factory_.reset();
+    executor_.reset();
   }
 
   void TearDown() override
@@ -76,12 +79,15 @@ class ControllerSelectorTestFixture : public ::testing::Test
 
 protected:
   static nav2::LifecycleNode::SharedPtr node_;
+  static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
   static BT::NodeConfiguration * config_;
   static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
   static std::shared_ptr<BT::Tree> tree_;
 };
 
 nav2::LifecycleNode::SharedPtr ControllerSelectorTestFixture::node_ = nullptr;
+rclcpp::executors::SingleThreadedExecutor::SharedPtr ControllerSelectorTestFixture::executor_ =
+  nullptr;
 
 BT::NodeConfiguration * ControllerSelectorTestFixture::config_ = nullptr;
 std::shared_ptr<BT::BehaviorTreeFactory> ControllerSelectorTestFixture::factory_ = nullptr;
@@ -126,7 +132,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic)
     tree_->rootNode()->executeTick();
     controller_selector_pub->publish(selected_controller_cmd);
 
-    rclcpp::spin_some(node_->get_node_base_interface());
+    executor_->spin_some();
   }
 
   // check controller updated
@@ -173,7 +179,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic)
     tree_->rootNode()->executeTick();
     controller_selector_pub->publish(selected_controller_cmd);
 
-    rclcpp::spin_some(node_->get_node_base_interface());
+    executor_->spin_some();
   }
 
   // check controller updated

nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp

@@ -31,6 +31,8 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test
   static void SetUpTestCase()
   {
     node_ = std::make_shared<nav2::LifecycleNode>("goal_checker_selector_test_fixture");
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    executor_->add_node(node_->get_node_base_interface());
 
     // Configure and activate the lifecycle node
     node_->configure();
@@ -67,6 +69,7 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test
     config_ = nullptr;
     node_.reset();
     factory_.reset();
+    executor_.reset();
   }
 
   void TearDown() override
@@ -76,12 +79,15 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test
 
 protected:
   static nav2::LifecycleNode::SharedPtr node_;
+  static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
   static BT::NodeConfiguration * config_;
   static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
   static std::shared_ptr<BT::Tree> tree_;
 };
 
 nav2::LifecycleNode::SharedPtr GoalCheckerSelectorTestFixture::node_ = nullptr;
+rclcpp::executors::SingleThreadedExecutor::SharedPtr GoalCheckerSelectorTestFixture::executor_ =
+  nullptr;
 
 BT::NodeConfiguration * GoalCheckerSelectorTestFixture::config_ = nullptr;
 std::shared_ptr<BT::BehaviorTreeFactory> GoalCheckerSelectorTestFixture::factory_ = nullptr;
@@ -127,7 +133,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic)
     tree_->rootNode()->executeTick();
     goal_checker_selector_pub->publish(selected_goal_checker_cmd);
 
-    rclcpp::spin_some(node_->get_node_base_interface());
+    executor_->spin_some();
   }
 
   // check goal_checker updated
@@ -175,7 +181,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic)
     tree_->rootNode()->executeTick();
     goal_checker_selector_pub->publish(selected_goal_checker_cmd);
 
-    rclcpp::spin_some(node_->get_node_base_interface());
+    executor_->spin_some();
   }
 
   // check goal_checker updated

nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp

@@ -31,6 +31,8 @@ class PlannerSelectorTestFixture : public ::testing::Test
   static void SetUpTestCase()
   {
     node_ = std::make_shared<nav2::LifecycleNode>("planner_selector_test_fixture");
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    executor_->add_node(node_->get_node_base_interface());
 
     // Configure and activate the lifecycle node
     node_->configure();
@@ -65,6 +67,7 @@ class PlannerSelectorTestFixture : public ::testing::Test
     config_ = nullptr;
     node_.reset();
     factory_.reset();
+    executor_.reset();
   }
 
   void TearDown() override
@@ -74,12 +77,15 @@ class PlannerSelectorTestFixture : public ::testing::Test
 
 protected:
   static nav2::LifecycleNode::SharedPtr node_;
+  static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
   static BT::NodeConfiguration * config_;
   static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
   static std::shared_ptr<BT::Tree> tree_;
 };
 
 nav2::LifecycleNode::SharedPtr PlannerSelectorTestFixture::node_ = nullptr;
+rclcpp::executors::SingleThreadedExecutor::SharedPtr PlannerSelectorTestFixture::executor_ =
+  nullptr;
 
 BT::NodeConfiguration * PlannerSelectorTestFixture::config_ = nullptr;
 std::shared_ptr<BT::BehaviorTreeFactory> PlannerSelectorTestFixture::factory_ = nullptr;
@@ -125,7 +131,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic)
     tree_->rootNode()->executeTick();
     planner_selector_pub->publish(selected_planner_cmd);
 
-    rclcpp::spin_some(node_->get_node_base_interface());
+    executor_->spin_some();
   }
 
   // check planner updated
@@ -173,7 +179,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic)
     tree_->rootNode()->executeTick();
     planner_selector_pub->publish(selected_planner_cmd);
 
-    rclcpp::spin_some(node_->get_node_base_interface());
+    executor_->spin_some();
   }
 
   // check planner updated

nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp

@@ -30,6 +30,8 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test
   static void SetUpTestCase()
   {
     node_ = std::make_shared<nav2::LifecycleNode>("progress_checker_selector_test_fixture");
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    executor_->add_node(node_->get_node_base_interface());
 
     // Configure and activate the lifecycle node
     node_->configure();
@@ -66,6 +68,7 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test
     config_ = nullptr;
     node_.reset();
     factory_.reset();
+    executor_.reset();
   }
 
   void TearDown() override
@@ -75,12 +78,15 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test
 
 protected:
   static nav2::LifecycleNode::SharedPtr node_;
+  static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
   static BT::NodeConfiguration * config_;
   static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
   static std::shared_ptr<BT::Tree> tree_;
 };
 
 nav2::LifecycleNode::SharedPtr ProgressCheckerSelectorTestFixture::node_ = nullptr;
+rclcpp::executors::SingleThreadedExecutor::SharedPtr ProgressCheckerSelectorTestFixture::
+executor_ = nullptr;
 
 BT::NodeConfiguration * ProgressCheckerSelectorTestFixture::config_ = nullptr;
 std::shared_ptr<BT::BehaviorTreeFactory> ProgressCheckerSelectorTestFixture::factory_ = nullptr;
@@ -129,7 +135,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic)
     tree_->rootNode()->executeTick();
     progress_checker_selector_pub->publish(selected_progress_checker_cmd);
 
-    rclcpp::spin_some(node_->get_node_base_interface());
+    executor_->spin_some();
   }
 
   // check progress_checker updated
@@ -179,7 +185,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic)
     tree_->rootNode()->executeTick();
     progress_checker_selector_pub->publish(selected_progress_checker_cmd);
 
-    rclcpp::spin_some(node_->get_node_base_interface());
+    executor_->spin_some();
   }
 
   // check goal_checker updated

nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp

@@ -31,6 +31,8 @@ class SmootherSelectorTestFixture : public ::testing::Test
   static void SetUpTestCase()
   {
     node_ = std::make_shared<nav2::LifecycleNode>("smoother_selector_test_fixture");
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    executor_->add_node(node_->get_node_base_interface());
 
     // Configure and activate the lifecycle node
     node_->configure();
@@ -67,6 +69,7 @@ class SmootherSelectorTestFixture : public ::testing::Test
     config_ = nullptr;
     node_.reset();
     factory_.reset();
+    executor_.reset();
   }
 
   void TearDown() override
@@ -76,12 +79,15 @@ class SmootherSelectorTestFixture : public ::testing::Test
 
 protected:
   static nav2::LifecycleNode::SharedPtr node_;
+  static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
   static BT::NodeConfiguration * config_;
   static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
   static std::shared_ptr<BT::Tree> tree_;
 };
 
 nav2::LifecycleNode::SharedPtr SmootherSelectorTestFixture::node_ = nullptr;
+rclcpp::executors::SingleThreadedExecutor::SharedPtr SmootherSelectorTestFixture::executor_ =
+  nullptr;
 
 BT::NodeConfiguration * SmootherSelectorTestFixture::config_ = nullptr;
 std::shared_ptr<BT::BehaviorTreeFactory> SmootherSelectorTestFixture::factory_ = nullptr;
@@ -127,7 +133,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic)
     tree_->rootNode()->executeTick();
     smoother_selector_pub->publish(selected_smoother_cmd);
 
-    rclcpp::spin_some(node_->get_node_base_interface());
+    executor_->spin_some();
   }
 
   // check smoother updated
@@ -175,7 +181,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic)
     tree_->rootNode()->executeTick();
     smoother_selector_pub->publish(selected_smoother_cmd);
 
-    rclcpp::spin_some(node_->get_node_base_interface());
+    executor_->spin_some();
   }
 
   // check smoother updated

nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp

@@ -29,6 +29,8 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test
   static void SetUpTestCase()
   {
     node_ = std::make_shared<nav2::LifecycleNode>("test_is_battery_charging");
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    executor_->add_node(node_->get_node_base_interface());
     factory_ = std::make_shared<BT::BehaviorTreeFactory>();
 
     config_ = new BT::NodeConfiguration();
@@ -58,17 +60,21 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test
     battery_pub_.reset();
     node_.reset();
     factory_.reset();
+    executor_.reset();
   }
 
 protected:
   static nav2::LifecycleNode::SharedPtr node_;
+  static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
   static BT::NodeConfiguration * config_;
   static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
   static nav2::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr
     battery_pub_;
 };
 
 nav2::LifecycleNode::SharedPtr IsBatteryChargingConditionTestFixture::node_ = nullptr;
+rclcpp::executors::SingleThreadedExecutor::SharedPtr IsBatteryChargingConditionTestFixture::
+executor_ = nullptr;
 BT::NodeConfiguration * IsBatteryChargingConditionTestFixture::config_ = nullptr;
 std::shared_ptr<BT::BehaviorTreeFactory> IsBatteryChargingConditionTestFixture::factory_ = nullptr;
 nav2::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr
@@ -90,32 +96,32 @@ TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status)
   battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
   battery_pub_->publish(battery_msg);
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  rclcpp::spin_some(node_->get_node_base_interface());
+  executor_->spin_some();
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
 
   battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING;
   battery_pub_->publish(battery_msg);
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  rclcpp::spin_some(node_->get_node_base_interface());
+  executor_->spin_some();
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
 
   battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
   battery_pub_->publish(battery_msg);
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  rclcpp::spin_some(node_->get_node_base_interface());
+  executor_->spin_some();
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
 
   battery_msg.power_supply_status =
     sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING;
   battery_pub_->publish(battery_msg);
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  rclcpp::spin_some(node_->get_node_base_interface());
+  executor_->spin_some();
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
 
   battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL;
   battery_pub_->publish(battery_msg);
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  rclcpp::spin_some(node_->get_node_base_interface());
+  executor_->spin_some();
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
 }