Achieve 95% coverage on all distributions

Signed-off-by: Michel Hidalgo <[email protected]>
Ekumen-OS · Oct 1, 2024 · 4dfc931 · 4dfc931
1 parent 47c8071
commit 4dfc931
Show file tree

Hide file tree

Showing 2 changed files with 51 additions and 3 deletions.
diff --git a/beluga_amcl/test/test_amcl_nodelet.cpp b/beluga_amcl/test/test_amcl_nodelet.cpp
@@ -111,6 +111,8 @@ class Tester {
     laser_scan_publisher_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 1);
 
     global_localization_client_ = nh_.serviceClient<std_srvs::Empty>("global_localization");
+
+    nomotion_update_client_ = nh_.serviceClient<std_srvs::Empty>("request_nomotion_update");
   }
 
   void create_pose_subscriber() {
@@ -120,7 +122,7 @@ class Tester {
 
   void pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& message) { latest_pose_ = *message; }
 
-  const auto& latest_pose() const { return latest_pose_; }
+  auto& latest_pose() { return latest_pose_; }
 
   void create_particle_cloud_subscriber() {
     particle_cloud_subscriber_ = nh_.subscribe<geometry_msgs::PoseArray>(
@@ -241,6 +243,16 @@ class Tester {
     return global_localization_client_.call(srv);
   }
 
+  template <class Rep, class Period>
+  bool wait_for_nomotion_update_service(const std::chrono::duration<Rep, Period>& timeout) {
+    return nomotion_update_client_.waitForExistence(ros::Duration(std::chrono::duration<double>(timeout).count()));
+  }
+
+  bool request_nomotion_update() {
+    std_srvs::Empty srv;
+    return nomotion_update_client_.call(srv);
+  }
+
  private:
   static bool static_map_callback(nav_msgs::GetMap::Request&, nav_msgs::GetMap::Response& response) {
     response.map = make_dummy_map();
@@ -267,6 +279,7 @@ class Tester {
   tf2_ros::TransformListener tf_listener_;
 
   ros::ServiceClient global_localization_client_;
+  ros::ServiceClient nomotion_update_client_;
 };
 
 /// Base node fixture class with common utilities.
@@ -279,13 +292,12 @@ class BaseTestFixture : public T {
     tester_ = std::make_shared<Tester>();
   }
 
-  void TearDown() override {}
-
   bool wait_for_initialization() {
     return spin_until([this] { return amcl_nodelet_->is_initialized(); }, 1000ms);
   }
 
   bool wait_for_pose_estimate() {
+    tester_->latest_pose().reset();
     return spin_until([this] { return tester_->latest_pose().has_value(); }, 1000ms);
   }
 
@@ -301,6 +313,13 @@ class BaseTestFixture : public T {
     return tester_->request_global_localization();
   }
 
+  bool request_nomotion_update() {
+    if (!tester_->wait_for_nomotion_update_service(500ms)) {
+      return false;
+    }
+    return tester_->request_nomotion_update();
+  }
+
  protected:
   std::shared_ptr<AmclNodeletUnderTest> amcl_nodelet_;
   std::shared_ptr<Tester> tester_;
@@ -551,6 +570,24 @@ TEST_F(TestFixture, FirstMapOnly) {
   }
 }
 
+TEST_F(TestFixture, CanForcePoseEstimate) {
+  beluga_amcl::AmclConfig config;
+  EXPECT_TRUE(amcl_nodelet_->default_config(config));
+  config.set_initial_pose = false;
+  EXPECT_TRUE(amcl_nodelet_->set(config));
+  tester_->publish_map();
+  EXPECT_TRUE(request_global_localization());
+  EXPECT_TRUE(wait_for_initialization());
+  tester_->create_pose_subscriber();
+  tester_->publish_laser_scan();
+  EXPECT_TRUE(wait_for_pose_estimate());
+  EXPECT_TRUE(tester_->can_transform("map", "odom"));
+  EXPECT_TRUE(request_nomotion_update());
+  tester_->publish_laser_scan();
+  EXPECT_TRUE(wait_for_pose_estimate());
+  EXPECT_TRUE(tester_->can_transform("map", "odom"));
+}
+
 TEST_F(TestFixture, KeepCurrentEstimate) {
   beluga_amcl::AmclConfig config;
   ASSERT_TRUE(amcl_nodelet_->default_config(config));

diff --git a/beluga_ros/test/test_amcl.cpp b/beluga_ros/test/test_amcl.cpp
@@ -115,6 +115,17 @@ TEST(TestAmcl, UpdateWithParticles) {
   ASSERT_TRUE(estimate.has_value());
 }
 
+TEST(TestAmcl, UpdateWithParticlesWithMotion) {
+  auto amcl = make_amcl();
+  ASSERT_EQ(amcl.particles().size(), 0);
+  amcl.initialize_from_map();
+  ASSERT_EQ(amcl.particles().size(), 50UL);
+  auto estimate = amcl.update(Sophus::SE2d{}, make_dummy_laser_scan());
+  ASSERT_TRUE(estimate.has_value());
+  estimate = amcl.update(Sophus::SE2d{0.0, {1.0, 0.0}}, make_dummy_laser_scan());
+  ASSERT_TRUE(estimate.has_value());
+}
+
 TEST(TestAmcl, UpdateWithParticlesNoMotion) {
   auto amcl = make_amcl();
   ASSERT_EQ(amcl.particles().size(), 0);