Skip to content

Commit

Permalink
Fix tests failing for Jazzy
Browse files Browse the repository at this point in the history 
Signed-off-by: Michel Hidalgo <[email protected]>
  • Loading branch information
@hidmic
hidmic committed May 20, 2024
1 parent 413a7c3 commit b516300
Show file tree
Hide file tree
Showing 5 changed files with 54 additions and 35 deletions.
14 changes: 7 additions & 7 deletions beluga/test/beluga/motion/test_differential_drive_model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
}

Expand All @@ -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);
Expand All @@ -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);

Expand All @@ -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);

Expand All @@ -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);

Expand Down
4 changes: 2 additions & 2 deletions beluga/test/beluga/motion/test_omnidirectional_drive_model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);

Expand Down
23 changes: 11 additions & 12 deletions beluga_amcl/src/amcl_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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{};
Expand All @@ -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) {
Expand Down
29 changes: 19 additions & 10 deletions beluga_amcl/test/test_amcl_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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) {
Expand All @@ -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"));
}

Expand All @@ -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) {
Expand Down Expand Up @@ -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());
}
Expand All @@ -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) {
Expand All @@ -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"));
Expand All @@ -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();
Expand All @@ -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();
Expand All @@ -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();
Expand All @@ -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();
Expand Down Expand Up @@ -663,6 +671,7 @@ TEST_F(TestNode, TransformValue) {
ASSERT_NEAR(pose.translation().y(), 2.0, 0.01);
ASSERT_NEAR(pose.so2().log(), Sophus::Constants<double>::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);
Expand Down
19 changes: 15 additions & 4 deletions beluga_amcl/test/test_ndt_amcl_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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());
}
Expand All @@ -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) {
Expand All @@ -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"));
Expand All @@ -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();
Expand All @@ -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();
Expand Down Expand Up @@ -492,6 +502,7 @@ TEST_F(TestNode, TransformValue) {
ASSERT_NEAR(pose.translation().y(), 2.0, 0.01);
ASSERT_NEAR(pose.so2().log(), Sophus::Constants<double>::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);
Expand Down

0 comments on commit b516300

Please sign in to comment.