@@ -196,6 +196,7 @@ class Tester : public ::testing::Test
196
196
197
197
// CollisionMonitor node
198
198
std::shared_ptr<CollisionMonitorWrapper> cm_;
199
+ rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
199
200
200
201
// Footprint publisher
201
202
nav2::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
@@ -233,6 +234,8 @@ class Tester : public ::testing::Test
233
234
Tester::Tester ()
234
235
{
235
236
cm_ = std::make_shared<CollisionMonitorWrapper>();
237
+ executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
238
+ executor_->add_node (cm_->get_node_base_interface ());
236
239
cm_->declare_parameter (" enable_stamped_cmd_vel" , rclcpp::ParameterValue (false ));
237
240
238
241
footprint_pub_ = cm_->create_publisher <geometry_msgs::msg::PolygonStamped>(
@@ -737,7 +740,7 @@ bool Tester::waitData(
737
740
if (cm_->correctDataReceived (expected_dist, stamp)) {
738
741
return true ;
739
742
}
740
- rclcpp::spin_some (cm_-> get_node_base_interface () );
743
+ executor_-> spin_some ( );
741
744
std::this_thread::sleep_for (10ms);
742
745
}
743
746
return false ;
@@ -750,7 +753,7 @@ bool Tester::waitCmdVel(const std::chrono::nanoseconds & timeout)
750
753
if (cmd_vel_out_) {
751
754
return true ;
752
755
}
753
- rclcpp::spin_some (cm_-> get_node_base_interface () );
756
+ executor_-> spin_some ( );
754
757
std::this_thread::sleep_for (10ms);
755
758
}
756
759
return false ;
@@ -766,7 +769,7 @@ bool Tester::waitFuture(
766
769
if (status == std::future_status::ready) {
767
770
return true ;
768
771
}
769
- rclcpp::spin_some (cm_-> get_node_base_interface () );
772
+ executor_-> spin_some ( );
770
773
std::this_thread::sleep_for (10ms);
771
774
}
772
775
return false ;
@@ -782,7 +785,7 @@ bool Tester::waitToggle(
782
785
if (status == std::future_status::ready) {
783
786
return true ;
784
787
}
785
- rclcpp::spin_some (cm_-> get_node_base_interface () );
788
+ executor_-> spin_some ( );
786
789
std::this_thread::sleep_for (10ms);
787
790
}
788
791
return false ;
@@ -795,7 +798,7 @@ bool Tester::waitActionState(const std::chrono::nanoseconds & timeout)
795
798
if (action_state_) {
796
799
return true ;
797
800
}
798
- rclcpp::spin_some (cm_-> get_node_base_interface () );
801
+ executor_-> spin_some ( );
799
802
std::this_thread::sleep_for (10ms);
800
803
}
801
804
return false ;
@@ -808,7 +811,7 @@ bool Tester::waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout)
808
811
if (collision_points_marker_msg_) {
809
812
return true ;
810
813
}
811
- rclcpp::spin_some (cm_-> get_node_base_interface () );
814
+ executor_-> spin_some ( );
812
815
std::this_thread::sleep_for (10ms);
813
816
}
814
817
return false ;
0 commit comments