diff --git a/beluga/test/beluga/motion/test_differential_drive_model.cpp b/beluga/test/beluga/motion/test_differential_drive_model.cpp index e35d60f26..c31fdc428 100644 --- a/beluga/test/beluga/motion/test_differential_drive_model.cpp +++ b/beluga/test/beluga/motion/test_differential_drive_model.cpp @@ -106,7 +106,7 @@ auto get_statistics(Range&& range) { } TEST(DifferentialDriveModelSamples, Translate) { - const double tolerance = 0.01; + const double tolerance = 0.015; const double alpha = 0.2; const double origin = 5.0; const double distance = 3.0; @@ -119,9 +119,9 @@ TEST(DifferentialDriveModelSamples, Translate) { const auto pose = SE2d{SO2d{0.0}, Vector2d{origin, 0.0}}; return state_sampling_function(pose, generator).translation().x(); }) | - ranges::views::take_exactly(1'000'000) | ranges::views::common; + ranges::views::take_exactly(100'000) | ranges::views::common; const auto [mean, stddev] = get_statistics(view); - ASSERT_NEAR(mean, origin + distance, 0.01); + ASSERT_NEAR(mean, origin + distance, tolerance); ASSERT_NEAR(stddev, std::sqrt(alpha * distance * distance), tolerance); } @@ -139,7 +139,7 @@ TEST(DifferentialDriveModelSamples, RotateFirstQuadrant) { const auto pose = SE2d{SO2d{initial_angle}, Vector2d{0.0, 0.0}}; return state_sampling_function(pose, generator).so2().log(); }) | - ranges::views::take_exactly(1'000'000) | ranges::views::common; + ranges::views::take_exactly(100'000) | ranges::views::common; const auto [mean, stddev] = get_statistics(view); ASSERT_NEAR(mean, initial_angle + motion_angle, tolerance); ASSERT_NEAR(stddev, std::sqrt(alpha * motion_angle * motion_angle), tolerance); @@ -159,7 +159,7 @@ TEST(DifferentialDriveModelSamples, RotateThirdQuadrant) { const auto pose = SE2d{SO2d{initial_angle}, Vector2d{0.0, 0.0}}; return state_sampling_function(pose, generator).so2().log(); }) | - ranges::views::take_exactly(1'000'000) | ranges::views::common; + ranges::views::take_exactly(100'000) | ranges::views::common; const auto [mean, stddev] = get_statistics(view); ASSERT_NEAR(mean, initial_angle + motion_angle, tolerance); @@ -180,7 +180,7 @@ TEST(DifferentialDriveModelSamples, RotateTranslateRotateFirstQuadrant) { const auto pose = SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}}; return state_sampling_function(pose, generator).translation().norm(); }) | - ranges::views::take_exactly(1'000'000) | ranges::views::common; + ranges::views::take_exactly(100'000) | ranges::views::common; const auto [mean, stddev] = get_statistics(view); ASSERT_NEAR(mean, 1.41, tolerance); @@ -206,7 +206,7 @@ TEST(DifferentialDriveModelSamples, RotateTranslateRotateThirdQuadrant) { const auto pose = SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}}; return state_sampling_function(pose, generator).translation().norm(); }) | - ranges::views::take_exactly(1'000'000) | ranges::views::common; + ranges::views::take_exactly(100'000) | ranges::views::common; const auto [mean, stddev] = get_statistics(view); ASSERT_NEAR(mean, 1.41, 0.01); diff --git a/beluga/test/beluga/motion/test_omnidirectional_drive_model.cpp b/beluga/test/beluga/motion/test_omnidirectional_drive_model.cpp index 28f2ecafb..db15ee43e 100644 --- a/beluga/test/beluga/motion/test_omnidirectional_drive_model.cpp +++ b/beluga/test/beluga/motion/test_omnidirectional_drive_model.cpp @@ -135,7 +135,7 @@ TEST(OmnidirectionalDriveModelSamples, RotateFirstQuadrant) { const auto pose = SE2d{SO2d{initial_angle}, Vector2d{0.0, 0.0}}; return state_sampling_function(pose, generator).so2().log(); }) | - ranges::views::take_exactly(1'000'000) | ranges::views::common; + ranges::views::take_exactly(100'000) | ranges::views::common; const auto [mean, stddev] = get_statistics(view); ASSERT_NEAR(mean, initial_angle + motion_angle, tolerance); ASSERT_NEAR(stddev, std::sqrt(alpha * motion_angle * motion_angle), tolerance); @@ -155,7 +155,7 @@ TEST(OmnidirectionalDriveModelSamples, RotateThirdQuadrant) { const auto pose = SE2d{SO2d{initial_angle}, Vector2d{0.0, 0.0}}; return state_sampling_function(pose, generator).so2().log(); }) | - ranges::views::take_exactly(1'000'000) | ranges::views::common; + ranges::views::take_exactly(100'000) | ranges::views::common; const auto [mean, stddev] = get_statistics(view); ASSERT_NEAR(mean, initial_angle + motion_angle, tolerance); diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index a5f41a5ef..75bc895a8 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -550,18 +550,6 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_ return; } - const auto& [base_pose_in_map, base_pose_covariance] = last_known_estimate_.value(); - - // New pose messages are only published on updates to the filter. - if (new_estimate.has_value()) { - auto message = geometry_msgs::msg::PoseWithCovarianceStamped{}; - message.header.stamp = laser_scan->header.stamp; - message.header.frame_id = get_parameter("global_frame_id").as_string(); - tf2::toMsg(base_pose_in_map, message.pose.pose); - tf2::covarianceEigenToRowMajor(base_pose_covariance, message.pose.covariance); - pose_pub_->publish(message); - } - // Transforms are always published to keep them current. if (enable_tf_broadcast_ && get_parameter("tf_broadcast").as_bool()) { auto message = geometry_msgs::msg::TransformStamped{}; @@ -574,6 +562,17 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_ message.transform = tf2::toMsg(*last_known_odom_transform_in_map_); tf_broadcaster_->sendTransform(message); } + + // New pose messages are only published on updates to the filter. + if (new_estimate.has_value()) { + auto message = geometry_msgs::msg::PoseWithCovarianceStamped{}; + message.header.stamp = laser_scan->header.stamp; + message.header.frame_id = get_parameter("global_frame_id").as_string(); + const auto& [base_pose_in_map, base_pose_covariance] = new_estimate.value(); + tf2::toMsg(base_pose_in_map, message.pose.pose); + tf2::covarianceEigenToRowMajor(base_pose_covariance, message.pose.covariance); + pose_pub_->publish(message); + } } void AmclNode::initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) { diff --git a/beluga_amcl/test/test_amcl_node.cpp b/beluga_amcl/test/test_amcl_node.cpp index 6f00ed8ac..fc4f86d20 100644 --- a/beluga_amcl/test/test_amcl_node.cpp +++ b/beluga_amcl/test/test_amcl_node.cpp @@ -65,6 +65,11 @@ class BaseNodeFixture : public T { return spin_until([this] { return tester_node_->latest_pose().has_value(); }, 1000ms, amcl_node_, tester_node_); } + bool wait_for_transform(const std::string& target, const std::string& source) { + return spin_until( + [&, this] { return tester_node_->can_transform(target, source); }, 1000ms, amcl_node_, tester_node_); + } + bool wait_for_particle_cloud() { tester_node_->create_particle_cloud_subscriber(); return spin_until( @@ -249,8 +254,7 @@ TEST_F(TestNode, BroadcastWhenInitialPoseSet) { ASSERT_TRUE(wait_for_initialization()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); tester_node_->publish_laser_scan(); - ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); } TEST_F(TestNode, NoBroadcastWhenNoInitialPose) { @@ -261,7 +265,6 @@ TEST_F(TestNode, NoBroadcastWhenNoInitialPose) { ASSERT_TRUE(wait_for_initialization()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); tester_node_->publish_laser_scan(); - ASSERT_TRUE(wait_for_pose_estimate()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); } @@ -274,8 +277,7 @@ TEST_F(TestNode, BroadcastWithGlobalLocalization) { ASSERT_FALSE(tester_node_->can_transform("map", "odom")); ASSERT_TRUE(request_global_localization()); tester_node_->publish_laser_scan(); - ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); } TEST_F(TestNode, IgnoreGlobalLocalizationBeforeInitialize) { @@ -508,6 +510,7 @@ TEST_F(TestNode, InitialPoseBeforeInitialize) { amcl_node_->configure(); amcl_node_->activate(); tester_node_->publish_default_initial_pose(); + spin_for(10ms, amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_FALSE(wait_for_pose_estimate()); } @@ -520,9 +523,10 @@ TEST_F(TestNode, InitialPoseAfterInitialize) { ASSERT_TRUE(wait_for_initialization()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); tester_node_->publish_default_initial_pose(); + spin_for(10ms, amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); } TEST_F(TestNode, InitialPoseWithWrongFrame) { @@ -533,6 +537,7 @@ TEST_F(TestNode, InitialPoseWithWrongFrame) { ASSERT_TRUE(wait_for_initialization()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); tester_node_->publish_initial_pose_with_wrong_frame(); + spin_for(10ms, amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); @@ -546,9 +551,10 @@ TEST_F(TestNode, CanUpdatePoseEstimate) { ASSERT_TRUE(wait_for_initialization()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); tester_node_->publish_default_initial_pose(); + spin_for(10ms, amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); { const auto [pose, _] = amcl_node_->estimate(); @@ -558,9 +564,10 @@ TEST_F(TestNode, CanUpdatePoseEstimate) { } tester_node_->publish_initial_pose(1., 1.); + spin_for(10ms, amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); { const auto [pose, _] = amcl_node_->estimate(); @@ -578,9 +585,10 @@ TEST_F(TestNode, CanForcePoseEstimate) { ASSERT_TRUE(wait_for_initialization()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); tester_node_->publish_default_initial_pose(); + spin_for(10ms, amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); { const auto [pose, _] = amcl_node_->estimate(); @@ -592,7 +600,7 @@ TEST_F(TestNode, CanForcePoseEstimate) { ASSERT_TRUE(request_nomotion_update()); tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); { const auto [pose, _] = amcl_node_->estimate(); @@ -663,6 +671,7 @@ TEST_F(TestNode, TransformValue) { ASSERT_NEAR(pose.translation().y(), 2.0, 0.01); ASSERT_NEAR(pose.so2().log(), Sophus::Constants::pi() / 3, 0.01); + ASSERT_TRUE(wait_for_transform("map", "odom")); const auto transform = tester_node_->lookup_transform("map", "odom"); EXPECT_NEAR(transform.translation().x(), -2.00, 0.01); EXPECT_NEAR(transform.translation().y(), -2.00, 0.01); diff --git a/beluga_amcl/test/test_ndt_amcl_node.cpp b/beluga_amcl/test/test_ndt_amcl_node.cpp index 83ca1b6ec..cce87d049 100644 --- a/beluga_amcl/test/test_ndt_amcl_node.cpp +++ b/beluga_amcl/test/test_ndt_amcl_node.cpp @@ -71,6 +71,11 @@ class BaseNodeFixture : public T { return spin_until([this] { return tester_node_->latest_pose().has_value(); }, 1000ms, ndt_amcl_node_, tester_node_); } + bool wait_for_transform(const std::string& target, const std::string& source) { + return spin_until( + [&, this] { return tester_node_->can_transform(target, source); }, 1000ms, ndt_amcl_node_, tester_node_); + } + bool wait_for_particle_cloud() { tester_node_->create_particle_cloud_subscriber(); return spin_until( @@ -233,7 +238,7 @@ TEST_F(TestNode, BroadcastWhenInitialPoseSet) { ASSERT_FALSE(tester_node_->can_transform("map", "odom")); tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); } TEST_F(TestNode, NoBroadcastWhenNoInitialPose) { @@ -385,6 +390,7 @@ TEST_F(TestNode, InitialPoseBeforeInitialize) { ndt_amcl_node_->activate(); ASSERT_FALSE(wait_for_pose_estimate()); tester_node_->publish_default_initial_pose(); + spin_for(10ms, ndt_amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); } @@ -396,9 +402,10 @@ TEST_F(TestNode, InitialPoseAfterInitialize) { ASSERT_TRUE(wait_for_initialization()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); tester_node_->publish_default_initial_pose(); + spin_for(10ms, ndt_amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); } TEST_F(TestNode, InitialPoseWithWrongFrame) { @@ -408,6 +415,7 @@ TEST_F(TestNode, InitialPoseWithWrongFrame) { ASSERT_TRUE(wait_for_initialization()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); tester_node_->publish_initial_pose_with_wrong_frame(); + spin_for(10ms, ndt_amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_FALSE(wait_for_pose_estimate()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); @@ -420,9 +428,10 @@ TEST_F(TestNode, CanUpdatePoseEstimate) { ASSERT_TRUE(wait_for_initialization()); ASSERT_FALSE(tester_node_->can_transform("map", "odom")); tester_node_->publish_default_initial_pose(); + spin_for(10ms, ndt_amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); { const auto [pose, _] = ndt_amcl_node_->estimate(); @@ -432,9 +441,10 @@ TEST_F(TestNode, CanUpdatePoseEstimate) { } tester_node_->publish_initial_pose(1., 1.); + spin_for(10ms, ndt_amcl_node_); // ensure orderly processing tester_node_->publish_laser_scan(); ASSERT_TRUE(wait_for_pose_estimate()); - ASSERT_TRUE(tester_node_->can_transform("map", "odom")); + ASSERT_TRUE(wait_for_transform("map", "odom")); { const auto [pose, _] = ndt_amcl_node_->estimate(); @@ -492,6 +502,7 @@ TEST_F(TestNode, TransformValue) { ASSERT_NEAR(pose.translation().y(), 2.0, 0.01); ASSERT_NEAR(pose.so2().log(), Sophus::Constants::pi() / 3, 0.01); + ASSERT_TRUE(wait_for_transform("map", "odom")); const auto transform = tester_node_->lookup_transform("map", "odom"); EXPECT_NEAR(transform.translation().x(), -2.00, 0.01); EXPECT_NEAR(transform.translation().y(), -2.00, 0.01);