diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index cd1a81b5..576beb51 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -72,7 +72,7 @@ This package does not create service servers or clients. | Key | Type | Access | Notes | |---|---|---|---| -| `imu` | `IMUPerceptions` | **Read** | Used to get the latest IMU orientation when available. | +| `imu` | `IMUPerceptions` | **Read** | When available, the particles take its roll and pitch, and turn by the change of its yaw (not by the odometry's); its absolute yaw is never used (without a magnetometer it is arbitrary and drifts). Without it, the orientation follows the NavMap surface and the odometry's turns. | | `points` | `PointPerceptions` | **Read** | 3D point cloud bundles used in the correction step. | | `map.navmap` | `::navmap::NavMap` | **Read** | Triangle mesh map providing surface geometry and elevation. | | `map.bonxai` | `Bonxai::ProbabilisticMap` | **Read** | Probabilistic occupancy map used for scoring. | diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp index 55ef72ac..280f771d 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "Eigen/Geometry" #include "bonxai/probabilistic_map.hpp" @@ -233,6 +234,8 @@ class AMCLLocalizer : public LocalizerMethodBase /// Previous odometry transform (used to compute deltas). tf2::Transform last_odom_{tf2::Transform::getIdentity()}; + /// IMU yaw at the last prediction: only its change is used. + std::optional last_imu_yaw_; /// Flag indicating if the odometry has been initialized. bool initialized_odom_ = false; 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 db27d392..8a890c66 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -736,6 +736,18 @@ void AMCLLocalizer::predict(NavState & nav_state) double trans_len = std::sqrt(dx * dx + dy * dy + dz * dz); double r, p, yaw; tf2::Matrix3x3(delta.getRotation()).getRPY(r, p, yaw); + // The IMU: roll and pitch (gravity) are absolute; of its yaw only the change is used (a gyro + // turns far better than skid-steer odometry, and without a magnetometer its absolute yaw is + // arbitrary and drifts) + double imu_roll = 0.0, imu_pitch = 0.0; + if (imu_q_opt.has_value()) { + double imu_yaw; + tf2::Matrix3x3(*imu_q_opt).getRPY(imu_roll, imu_pitch, imu_yaw); + if (last_imu_yaw_.has_value()) { + yaw = std::remainder(imu_yaw - *last_imu_yaw_, 2.0 * M_PI); + } + last_imu_yaw_ = imu_yaw; + } double rot_len = std::abs(yaw); // Zero-mean noise; a zero deviation (e.g. the robot still) is no noise: std::normal_distribution @@ -748,12 +760,15 @@ void AMCLLocalizer::predict(NavState & nav_state) 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 + noise(sd_yaw); - for (auto & p : particles_) { - 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); + // Each particle its own noise: they must spread in heading too for the correction to fix it + double pr, pp, py; tf2::Matrix3x3(p.pose.getRotation()).getRPY(pr, pp, py); + const tf2::Vector3 noisy_t(dx + noise(sd_dx), dy + noise(sd_dy), dz + noise(sd_dz)); + tf2::Vector3 Pw = p.pose * noisy_t; // along the robot's current (tilted) heading + const double new_yaw = py + yaw + noise(sd_yaw); + tf2::Quaternion flat_q; flat_q.setRPY(0.0, 0.0, new_yaw); + p.pose.setOrigin(Pw); + p.pose.setRotation(flat_q); if (have_navmap) { ::navmap::NavMap::LocateOpts opts; @@ -762,7 +777,6 @@ void AMCLLocalizer::predict(NavState & nav_state) } opts.planar_eps = 1e-4f; opts.height_eps = 0.50f; opts.use_downward_ray = true; - tf2::Vector3 Pw = p.pose.getOrigin(); std::size_t sidx = 0; ::navmap::NavCelId cid = std::numeric_limits::max(); Eigen::Vector3f bary, hit_eig; @@ -776,21 +790,22 @@ void AMCLLocalizer::predict(NavState & nav_state) tf2::Vector3 hit = to_tf(hit_eig); p.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), hit.z())); if (imu_q_opt.has_value()) { - p.pose.setRotation(*imu_q_opt); + tf2::Quaternion q; q.setRPY(imu_roll, imu_pitch, new_yaw); + p.pose.setRotation(q); } else { const auto & cel = navmap.navcels[cid]; tf2::Vector3 n_world = to_tf(cel.normal); double nlen = n_world.length(); if (nlen > 1e-9) { n_world /= nlen; - double rr, pp, yy; tf2::Matrix3x3(p.pose.getRotation()).getRPY(rr, pp, yy); - p.pose.setRotation(frame_from_normal_and_yaw(n_world, yy)); + if (n_world.z() < 0.0) {n_world = -n_world;} + p.pose.setRotation(frame_from_normal_and_yaw(n_world, new_yaw)); } } p.last_cid = cid; p.last_surface = sidx; - } else { - p.pose.setOrigin(tf2::Vector3(Pw.x(), Pw.y(), Pw.z())); - if (imu_q_opt.has_value()) {p.pose.setRotation(*imu_q_opt);} + } else if (imu_q_opt.has_value()) { + tf2::Quaternion q; q.setRPY(imu_roll, imu_pitch, new_yaw); + p.pose.setRotation(q); } } } diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp index ea01604b..5e94c461 100644 --- a/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp @@ -27,6 +27,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.hpp" #include "easynav_common/types/NavState.hpp" +#include "easynav_sensors/types/IMUPerception.hpp" #include "easynav_localizer/LocalizerNode.hpp" #include "easynav_navmap_localizer/AMCLLocalizer.hpp" @@ -115,6 +116,34 @@ class NavmapAmclPredictTest : public ::testing::Test return {x / n, y / n, std::atan2(s, c)}; } + // Spread of the particles' yaw (circular standard deviation) + double yaw_spread() const + { + double s = 0.0, c = 0.0; + for (const auto & p : amcl_->particles_) { + const double yaw = tf2::getYaw(p.pose.getRotation()); + s += std::sin(yaw); + c += std::cos(yaw); + } + const double n = static_cast(amcl_->particles_.size()); + const double r = std::hypot(s, c) / n; + return std::sqrt(std::max(0.0, -2.0 * std::log(std::max(r, 1e-12)))); + } + + // An IMU reading with this orientation + void imu(double roll, double pitch, double yaw) + { + easynav::IMUPerception p; + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + p.data.orientation.x = q.x(); + p.data.orientation.y = q.y(); + p.data.orientation.z = q.z(); + p.data.orientation.w = q.w(); + p.valid = true; + nav_state_.set("imu", p); + } + std::shared_ptr node_; std::shared_ptr amcl_; easynav::NavState nav_state_; @@ -170,3 +199,59 @@ TEST_F(NavmapAmclPredictTest, AMixOfStillAndMovingCyclesStaysFinite) } EXPECT_TRUE(particles_finite()); } + +TEST_F(NavmapAmclPredictTest, EachParticleGetsItsOwnHeadingNoise) +{ + // Odometry turning: the particles must spread in yaw, so the correction can choose among them + node_->set_parameter(rclcpp::Parameter("loc.noise_rotation", 0.3)); + amcl_->initialize(node_, "loc"); + const double before = yaw_spread(); + for (int i = 0; i < 20; ++i) { + predict(0.0, 0.0, 0.1); + } + EXPECT_GT(yaw_spread(), 2.0 * before + 0.01); +} + +TEST_F(NavmapAmclPredictTest, TheImuGivesRollPitchAndTurnsButNotItsAbsoluteYaw) +{ + // An IMU whose yaw is 1.2 rad off the map (no magnetometer) and tilted on a slope + imu(0.1, -0.2, 1.2); + const double yaw0 = mean()[2]; + predict(0.0, 0.0, 0.0); + EXPECT_NEAR(std::remainder(mean()[2] - yaw0, 2.0 * M_PI), 0.0, 0.05); // not snapped to 1.2 + for (const auto & p : amcl_->particles_) { + double r, pi, y; + tf2::Matrix3x3(p.pose.getRotation()).getRPY(r, pi, y); + ASSERT_NEAR(r, 0.1, 1e-6); + ASSERT_NEAR(pi, -0.2, 1e-6); + } + // The IMU turns 0.5 rad while the (slipping) odometry says 0.1: the particles follow the IMU + for (int i = 1; i <= 5; ++i) { + imu(0.1, -0.2, 1.2 + 0.1 * i); + predict(0.0, 0.0, 0.02); + } + EXPECT_NEAR(std::remainder(mean()[2] - yaw0 - 0.5, 2.0 * M_PI), 0.0, 0.1); + // Across the IMU's +-pi wrap too + imu(0.0, 0.0, M_PI - 0.05); + predict(0.0, 0.0, 0.0); + const double y1 = mean()[2]; + imu(0.0, 0.0, -M_PI + 0.05); + predict(0.0, 0.0, 0.0); + EXPECT_NEAR(std::remainder(mean()[2] - y1 - 0.1, 2.0 * M_PI), 0.0, 0.05); +} + +TEST_F(NavmapAmclPredictTest, MovingOnATiltedRobotStaysOnTheSurface) +{ + imu(0.0, -0.3, 0.0); // pitched up 0.3 rad (an incline the flat map does not have) + const auto before = mean(); + for (int i = 0; i < 5; ++i) { + predict(0.1, 0.0, 0.0); + } + ASSERT_TRUE(particles_finite()); + const auto after = mean(); + // Along the tilted heading: about cos(0.3) of the distance in XY, z back on the map surface + EXPECT_NEAR(after[0] - before[0], 0.5 * std::cos(0.3), 0.1); + for (const auto & p : amcl_->particles_) { + ASSERT_NEAR(p.pose.getOrigin().z(), 0.0, 1e-3); + } +}