diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp index 1c7f4d35..d85e945c 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp @@ -506,7 +506,8 @@ class UkfWrapper std::vector & updateVector, Eigen::VectorXd & measurement, Eigen::MatrixXd & measurementCovariance); - std::shared_ptr parent_node_{nullptr}; + // Not owned: the node owns the localizer (a shared_ptr here would leak the node). + easynav::LocalizerNode * parent_node_{nullptr}; //! @brief Whether or not we print diagnostic messages to the /diagnostics //! topic diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp index 39dfc836..e77c663f 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -83,6 +83,33 @@ namespace robot_localization { using namespace std::chrono_literals; +namespace +{ + +// The UKF averages the sigma points' angles circularly: past a variance of 2 rad^2 the mean flips +// by pi and the filter diverges to NaN. Unobserved angles (a velocities-only filter) are capped. +constexpr double kMaxAngleVariance = 1.0; + +void cap_angle_variances(Ukf & filter) +{ + Eigen::MatrixXd cov = filter.getEstimateErrorCovariance(); + bool capped = false; + for (const int i : {StateMemberRoll, StateMemberPitch, StateMemberYaw}) { + if (cov(i, i) > kMaxAngleVariance) { + // Scaling row and column keeps the covariance positive semidefinite. + const double s = std::sqrt(kMaxAngleVariance / cov(i, i)); + cov.row(i) *= s; + cov.col(i) *= s; + capped = true; + } + } + if (capped) { + filter.setEstimateErrorCovariance(cov); + } +} + +} // namespace + UkfWrapper::UkfWrapper( std::shared_ptr parent_node, const std::string & tf_prefix, @@ -118,7 +145,7 @@ UkfWrapper::UkfWrapper( tf_time_offset_(0ns), local_filter_(local_filter) { - parent_node_ = parent_node; + parent_node_ = parent_node.get(); tf_prefix_ = tf_prefix; plugin_name_ = plugin_name; @@ -738,6 +765,7 @@ void UkfWrapper::integrateMeasurements(const rclcpp::Time & current_time) // This will call predict and, if necessary, correct filter_.processMeasurement(*(measurement.get())); + cap_angle_variances(filter_); // Store old states and measurements if we're smoothing if (smooth_lagged_data_) { @@ -782,6 +810,7 @@ void UkfWrapper::integrateMeasurements(const rclcpp::Time & current_time) filter_.validateDelta(last_update_delta); filter_.predict(current_time, last_update_delta); + cap_angle_variances(filter_); // Update the last measurement time and last update time filter_.setLastMeasurementTime( diff --git a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt index 6c9da3a6..bdbeca1c 100644 --- a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt @@ -19,3 +19,10 @@ target_link_libraries(fusion_localizer_reconfigure_tests rclcpp_lifecycle::rclcpp_lifecycle easynav_localizer::easynav_localizer ) + +ament_add_gtest(fusion_localizer_local_filter_tests fusion_localizer_local_filter_tests.cpp) +target_link_libraries(fusion_localizer_local_filter_tests + fusion_localizer + rclcpp::rclcpp + easynav_localizer::easynav_localizer +) diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_local_filter_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_local_filter_tests.cpp new file mode 100644 index 00000000..24e983a9 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_local_filter_tests.cpp @@ -0,0 +1,273 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \file +/// \brief A local filter fusing only velocities (no absolute orientation) stays finite. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "easynav_fusion_localizer/FusionLocalizer.hpp" +#include "easynav_localizer/LocalizerNode.hpp" +#include "easynav_common/types/NavState.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" + +namespace +{ + +class FriendFusionLocalizer : public easynav::FusionLocalizer +{ +public: + using easynav::FusionLocalizer::update_rt; +}; + +bool finite(const nav_msgs::msg::Odometry & o) +{ + const auto & p = o.pose.pose; + return std::isfinite(p.position.x) && std::isfinite(p.position.y) && + std::isfinite(p.orientation.z) && std::isfinite(p.orientation.w) && + std::isfinite(o.twist.twist.linear.x) && std::isfinite(o.twist.twist.angular.z); +} + +} // namespace + +class FusionLocalFilterTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} + + // Runs the localizer for \p seconds while the odometry reports (vx, wz); returns the last + // local estimate, if any. + std::optional run( + const std::vector & params, double vx, double wz, double seconds, + bool * all_finite) + { + rclcpp::NodeOptions options; + options.parameter_overrides(params); + auto node = std::make_shared(options); + node->declare_parameter>( + "localizer_types", std::vector{}); + auto localizer = std::make_shared(); + localizer->initialize(node, "fusion"); + + auto pub_node = std::make_shared("local_filter_odom_pub"); + auto pub = pub_node->create_publisher("test_odom", 10); + auto imu_pub = pub_node->create_publisher("test_imu", 10); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + // The filter's subscriptions are in the RT callback group. + exec.add_callback_group(node->get_real_time_cbg(), node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + easynav::NavState nav_state; + std::optional last; + *all_finite = true; + const auto end = std::chrono::steady_clock::now() + std::chrono::duration(seconds); + while (std::chrono::steady_clock::now() < end) { + nav_msgs::msg::Odometry odom; + odom.header.stamp = pub_node->now(); + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_footprint"; + odom.pose.pose.orientation.w = 1.0; + odom.twist.twist.linear.x = vx; + odom.twist.twist.angular.z = wz; + for (int i = 0; i < 6; ++i) { + odom.pose.covariance[i * 7] = 0.01; + odom.twist.covariance[i * 7] = 0.001; + } + pub->publish(odom); + sensor_msgs::msg::Imu imu; + imu.header.stamp = odom.header.stamp; + imu.header.frame_id = "base_footprint"; + imu.orientation.w = 1.0; + imu.angular_velocity.z = wz; + imu.linear_acceleration.z = 9.81; + for (int i = 0; i < 3; ++i) { + imu.orientation_covariance[i * 4] = 0.01; + imu.angular_velocity_covariance[i * 4] = 0.0001; + imu.linear_acceleration_covariance[i * 4] = 0.01; + } + imu_pub->publish(imu); + exec.spin_some(); + localizer->update_rt(nav_state); + if (nav_state.has("robot_pose_local")) { + last = nav_state.get("robot_pose_local"); + *all_finite = *all_finite && finite(*last); + } + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + return last; + } + + static std::vector velocities_only() + { + return { + {"fusion.local_filter.frequency", 30.0}, + {"fusion.local_filter.two_d_mode", true}, + {"fusion.local_filter.publish_tf", false}, + {"fusion.local_filter.map_frame", "map"}, + {"fusion.local_filter.odom_frame", "odom"}, + {"fusion.local_filter.base_link_frame", "base_footprint"}, + {"fusion.local_filter.world_frame", "odom"}, + {"fusion.local_filter.odom0", "test_odom"}, + {"fusion.local_filter.odom0_config", std::vector{ + false, false, false, false, false, false, + true, true, false, false, false, true, + false, false, false}}, + }; + } + + // Plus the IMU's yaw rate (as in the Summit playground). + static std::vector with_imu_yaw_rate() + { + auto params = velocities_only(); + params.push_back({"fusion.local_filter.imu0", "test_imu"}); + params.push_back({"fusion.local_filter.imu0_config", std::vector{ + false, false, false, false, false, false, + false, false, false, false, false, true, + false, false, false}}); + params.push_back({"fusion.local_filter.imu0_remove_gravitational_acceleration", true}); + return params; + } +}; + +TEST_F(FusionLocalFilterTest, WithTheImuYawRateItStaysFinite) +{ + for (const auto & [vx, wz] : std::vector>{{0.0, 0.0}, {0.5, 0.3}}) { + bool all_finite = false; + const auto last = run(with_imu_yaw_rate(), vx, wz, 2.0, &all_finite); + ASSERT_TRUE(last.has_value()); + EXPECT_TRUE(all_finite) << vx << ", " << wz; + } +} + +TEST_F(FusionLocalFilterTest, BothFiltersStayFinite) +{ + auto params = with_imu_yaw_rate(); + for (const auto & p : std::vector{ + {"fusion.global_filter.frequency", 30.0}, + {"fusion.global_filter.two_d_mode", true}, + {"fusion.global_filter.publish_tf", false}, + {"fusion.global_filter.map_frame", "map"}, + {"fusion.global_filter.odom_frame", "odom"}, + {"fusion.global_filter.base_link_frame", "base_footprint"}, + {"fusion.global_filter.world_frame", "map"}, + {"fusion.global_filter.odom0", "test_odom"}, + {"fusion.global_filter.odom0_config", std::vector{ + false, false, false, false, false, false, + true, true, false, false, false, true, + false, false, false}}, + {"fusion.global_filter.imu0", "test_imu"}, + {"fusion.global_filter.imu0_config", std::vector{ + false, false, false, false, false, true, + false, false, false, false, false, true, + false, false, false}}}) + { + params.push_back(p); + } + bool all_finite = false; + const auto last = run(params, 0.5, 0.3, 2.0, &all_finite); + ASSERT_TRUE(last.has_value()); + EXPECT_TRUE(all_finite); +} + +// Nothing observes the yaw, so its variance grows (0.06 rad^2/s by default) past 2 rad^2, +// where the UKF's circular mean of the sigma points flips: here in ~1 s instead of ~33 s. +TEST_F(FusionLocalFilterTest, AnUnobservedYawWithAGrowingVarianceStaysFinite) +{ + auto params = with_imu_yaw_rate(); + const std::vector noise{ + 0.05, 0.05, 0.06, 0.03, 0.03, 3.0, 0.025, 0.025, 0.04, 0.01, 0.01, 0.02, 0.01, 0.01, 0.015}; + std::vector process_noise(15 * 15, 0.0); + for (int i = 0; i < 15; ++i) { + process_noise[i * 16] = noise[i]; + } + params.push_back({"fusion.local_filter.process_noise_covariance", process_noise}); + for (const auto & [vx, wz] : std::vector>{{0.0, 0.0}, {0.5, 0.3}}) { + bool all_finite = false; + const auto last = run(params, vx, wz, 3.0, &all_finite); + ASSERT_TRUE(last.has_value()); + EXPECT_TRUE(all_finite) << vx << ", " << wz; + // And the yaw did not jump by pi: it follows the turn rate. + const double yaw = 2.0 * std::atan2(last->pose.pose.orientation.z, + last->pose.pose.orientation.w); + EXPECT_LT(std::abs(std::remainder(yaw - wz * 3.0, 2.0 * M_PI)), 0.6) << vx << ", " << wz; + } +} + +TEST_F(FusionLocalFilterTest, VelocitiesOnlyStayFiniteWhenStill) +{ + bool all_finite = false; + const auto last = run(velocities_only(), 0.0, 0.0, 2.0, &all_finite); + ASSERT_TRUE(last.has_value()); + EXPECT_TRUE(all_finite); +} + +TEST_F(FusionLocalFilterTest, VelocitiesOnlyIntegrateTheMotion) +{ + bool all_finite = false; + const auto last = run(velocities_only(), 0.5, 0.0, 2.0, &all_finite); + ASSERT_TRUE(last.has_value()); + EXPECT_TRUE(all_finite); + EXPECT_GT(last->pose.pose.position.x, 0.4); // about 1 m in 2 s + EXPECT_NEAR(last->twist.twist.linear.x, 0.5, 0.1); +} + +TEST_F(FusionLocalFilterTest, VelocitiesOnlyStayFiniteWhenTurning) +{ + bool all_finite = false; + const auto last = run(velocities_only(), 0.3, 0.5, 2.0, &all_finite); + ASSERT_TRUE(last.has_value()); + EXPECT_TRUE(all_finite); + EXPECT_NEAR(last->twist.twist.angular.z, 0.5, 0.15); +} + +// The filters must not keep the node alive: the node owns the localizer, so a strong reference +// back would leak it, and its DDS threads would still run at exit (segfault on shutdown). +TEST_F(FusionLocalFilterTest, TheFiltersDoNotKeepTheNodeAlive) +{ + auto params = with_imu_yaw_rate(); + params.push_back({"fusion.global_filter.frequency", 30.0}); + params.push_back({"fusion.global_filter.odom0", "test_odom"}); + params.push_back({"fusion.global_filter.odom0_config", std::vector{ + false, false, false, false, false, false, + true, true, false, false, false, true, + false, false, false}}); + rclcpp::NodeOptions options; + options.parameter_overrides(params); + auto node = std::make_shared(options); + node->declare_parameter>( + "localizer_types", std::vector{}); + const auto before = node.use_count(); + auto localizer = std::make_shared(); + localizer->initialize(node, "fusion"); + EXPECT_EQ(node.use_count(), before); + + std::weak_ptr weak = node; + node.reset(); + localizer.reset(); + EXPECT_TRUE(weak.expired()); +} diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index ea2f32f5..dcaf19bc 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -729,16 +729,20 @@ void AMCLLocalizer::predict(NavState & nav_state) double r, p, yaw; tf2::Matrix3x3(delta.getRotation()).getRPY(r, p, yaw); double rot_len = std::abs(yaw); - std::normal_distribution n_dx(0.0, std::abs(dx) * noise_translation_); - std::normal_distribution n_dy(0.0, std::abs(dy) * noise_translation_); - std::normal_distribution n_dz(0.0, std::abs(dz) * noise_translation_); - std::normal_distribution n_yaw(0.0, - rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_); + // Zero-mean noise; a zero deviation (e.g. the robot still) is no noise: std::normal_distribution + // is undefined for it. + auto noise = [this](double stddev) { + return stddev > 0.0 ? std::normal_distribution(0.0, stddev)(rng_) : 0.0; + }; + const double sd_dx = std::abs(dx) * noise_translation_; + const double sd_dy = std::abs(dy) * noise_translation_; + const double sd_dz = std::abs(dz) * noise_translation_; + const double sd_yaw = rot_len * noise_rotation_ + trans_len * noise_translation_to_rotation_; - double noisy_y = yaw + n_yaw(rng_); + double noisy_y = yaw + noise(sd_yaw); for (auto & p : particles_) { - tf2::Vector3 noisy_t(dx + n_dx(rng_), dy + n_dy(rng_), dz + n_dz(rng_)); + tf2::Vector3 noisy_t(dx + noise(sd_dx), dy + noise(sd_dy), dz + noise(sd_dz)); tf2::Quaternion noisy_q; noisy_q.setRPY(0.0, 0.0, noisy_y); p.pose = p.pose * tf2::Transform(noisy_q, noisy_t); diff --git a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt index e6e0da16..bb86ba3f 100644 --- a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt @@ -22,3 +22,9 @@ target_link_libraries(navmap_localizer_last_known_pose_tests ${PROJECT_NAME} rclcpp::rclcpp ) + +ament_add_gtest(navmap_localizer_predict_tests navmap_localizer_predict_tests.cpp) +target_link_libraries(navmap_localizer_predict_tests + ${PROJECT_NAME} + rclcpp::rclcpp +) diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp new file mode 100644 index 00000000..b15d30e1 --- /dev/null +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp @@ -0,0 +1,171 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \file +/// \brief The motion model of the NavMap AMCL with odometry deltas that are zero in some or all +/// components (robot still, straight motion, turning in place): std::normal_distribution is +/// undefined for a zero deviation (an assertion aborts in builds with glibcxx assertions). + +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "navmap_ros/conversions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_localizer/LocalizerNode.hpp" +#include "easynav_navmap_localizer/AMCLLocalizer.hpp" + +namespace +{ + +class TestAmcl : public easynav::navmap::AMCLLocalizer +{ +public: + using AMCLLocalizer::predict; + using AMCLLocalizer::particles_; + using AMCLLocalizer::odom_; + using AMCLLocalizer::last_odom_; + using AMCLLocalizer::initialized_odom_; +}; + +} // namespace + +class NavmapAmclPredictTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + node_ = std::make_shared( + rclcpp::NodeOptions().parameter_overrides({ + rclcpp::Parameter("loc.initial_pose.x", 2.0), + rclcpp::Parameter("loc.initial_pose.y", 2.0), + rclcpp::Parameter("loc.initial_pose.yaw", 0.0), + rclcpp::Parameter("loc.initial_pose.std_dev_xy", 0.01), + rclcpp::Parameter("loc.initial_pose.std_dev_yaw", 0.01), + })); + amcl_ = std::make_shared(); + amcl_->initialize(node_, "loc"); + + // Free 4 x 4 m flat NavMap. + nav_msgs::msg::OccupancyGrid grid; + grid.header.frame_id = "map"; + grid.info.resolution = 0.1; + grid.info.width = 40; + grid.info.height = 40; + grid.info.origin.orientation.w = 1.0; + grid.data.assign(40 * 40, 0); + nav_state_.set("map.navmap", navmap_ros::from_occupancy_grid(grid)); + } + + // One prediction with the odometry moving by (dx, dy, dyaw) in the robot frame. + void predict(double dx, double dy, double dyaw) + { + amcl_->initialized_odom_ = true; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, dyaw); + amcl_->last_odom_ = amcl_->odom_; + amcl_->odom_ = amcl_->odom_ * tf2::Transform(q, tf2::Vector3(dx, dy, 0.0)); + amcl_->predict(nav_state_); + } + + bool particles_finite() const + { + for (const auto & p : amcl_->particles_) { + const auto & o = p.pose.getOrigin(); + if (!std::isfinite(o.x()) || !std::isfinite(o.y()) || + !std::isfinite(tf2::getYaw(p.pose.getRotation()))) + { + return false; + } + } + return true; + } + + // Mean x, y and yaw of the particles. + std::vector mean() const + { + double x = 0.0, y = 0.0, s = 0.0, c = 0.0; + for (const auto & p : amcl_->particles_) { + x += p.pose.getOrigin().x(); + y += p.pose.getOrigin().y(); + const double yaw = tf2::getYaw(p.pose.getRotation()); + s += std::sin(yaw); + c += std::cos(yaw); + } + const double n = static_cast(amcl_->particles_.size()); + return {x / n, y / n, std::atan2(s, c)}; + } + + std::shared_ptr node_; + std::shared_ptr amcl_; + easynav::NavState nav_state_; +}; + +TEST_F(NavmapAmclPredictTest, ARobotStandingStillDoesNotMoveTheParticles) +{ + const auto before = mean(); + for (int i = 0; i < 5; ++i) { + ASSERT_NO_FATAL_FAILURE(predict(0.0, 0.0, 0.0)); + } + ASSERT_TRUE(particles_finite()); + const auto after = mean(); + EXPECT_NEAR(after[0], before[0], 1e-6); + EXPECT_NEAR(after[1], before[1], 1e-6); + EXPECT_NEAR(after[2], before[2], 1e-6); +} + +TEST_F(NavmapAmclPredictTest, StraightMotionMovesThemAlongX) +{ + const auto before = mean(); + for (int i = 0; i < 5; ++i) { + predict(0.1, 0.0, 0.0); // dy, dz and the rotation are exactly zero + } + ASSERT_TRUE(particles_finite()); + const auto after = mean(); + EXPECT_NEAR(after[0] - before[0], 0.5, 0.1); + EXPECT_NEAR(after[1], before[1], 0.1); +} + +TEST_F(NavmapAmclPredictTest, TurningInPlaceRotatesThem) +{ + const auto before = mean(); + for (int i = 0; i < 5; ++i) { + predict(0.0, 0.0, 0.1); // no translation at all + } + ASSERT_TRUE(particles_finite()); + const auto after = mean(); + EXPECT_NEAR(after[0], before[0], 0.05); + EXPECT_NEAR(after[1], before[1], 0.05); + EXPECT_NEAR(std::remainder(after[2] - before[2] - 0.5, 2.0 * M_PI), 0.0, 0.15); +} + +TEST_F(NavmapAmclPredictTest, AMixOfStillAndMovingCyclesStaysFinite) +{ + for (int i = 0; i < 20; ++i) { + switch (i % 4) { + case 0: predict(0.0, 0.0, 0.0); break; + case 1: predict(0.05, 0.0, 0.0); break; + case 2: predict(0.0, 0.0, -0.05); break; + default: predict(0.0, 0.02, 0.0); break; + } + } + EXPECT_TRUE(particles_finite()); +}