From ae1eafe1c3d6ec48a5809e4a54d30420e621e858 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 1 Nov 2026 09:01:16 +0100 Subject: [PATCH 1/2] FusionLocalizer: GNSS fixes in east-north-up maps and from the antenna MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The projection parameter chooses how fixes become map coordinates: utm (default, as before) or enu, east-north-up at the origin, as Gazebo's spherical_coordinates and most local maps define it. Through UTM, a map defined east-north-up was off by the grid's convergence: 1.9 m at 300 m from the origin at Sonoma Raceway. The first fix placed the robot at the antenna, and later fixes reached the filter with an identity orientation, so it removed the antenna's mounting as if the robot faced east. Now the first fix removes the mounting turned with the IMU's attitude, and every fix carries that attitude. Signed-off-by: Francisco Martín Rico --- localizers/easynav_fusion_localizer/README.md | 7 + .../FusionLocalizer.hpp | 12 + .../easynav_fusion_localizer/ukf_wrapper.hpp | 6 + .../FusionLocalizer.cpp | 88 ++++-- .../tests/CMakeLists.txt | 7 + .../tests/fusion_localizer_gps_tests.cpp | 257 ++++++++++++++++++ 6 files changed, 361 insertions(+), 16 deletions(-) create mode 100644 localizers/easynav_fusion_localizer/tests/fusion_localizer_gps_tests.cpp diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md index 60faec87..bf492399 100644 --- a/localizers/easynav_fusion_localizer/README.md +++ b/localizers/easynav_fusion_localizer/README.md @@ -35,6 +35,13 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.initial_pose.x` | `double` | `0.0` | Initial X position (m). | | `.initial_pose.y` | `double` | `0.0` | Initial Y position (m). | | `.initial_pose.yaw` | `double` | `0.0` | Initial yaw (rad). | +| `.latitude_origin` | `double` | `0.0` | Latitude (deg) of the map's origin, for GNSS fixes (global filter). | +| `.longitude_origin` | `double` | `0.0` | Longitude (deg) of the map's origin. | +| `.altitude_origin` | `double` | `0.0` | Altitude (m) of the map's origin. | +| `.projection` | `string` | `utm` | How fixes become map coordinates: `utm`, the UTM grid shifted to the origin (its axes turn up to a few degrees from east and north away from the zone's central meridian), or `enu`, east-north-up at the origin, as Gazebo's `spherical_coordinates` and most local maps define it. | +| `.navsatfix_topic` | `string` | `gps/filtered` | Where the global estimate is published as a fix. | + +A fix is the position of the GNSS antenna, in the frame of its `header.frame_id`. With the IMU's attitude and the antenna's mounting (TF from the filter's `base_link_frame` to that frame), the first fix places the robot, not the antenna, and the filter removes the mounting from the later ones. Without IMU or without that TF, the robot starts where the antenna is. For additional robot_localization-related configuration, see the upstream documentation: https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp index a3f2a7bd..9f949c68 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -27,6 +27,7 @@ #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "GeographicLib/LocalCartesian.hpp" namespace easynav { @@ -103,6 +104,17 @@ class FusionLocalizer : public easynav::LocalizerMethodBase sensor_msgs::msg::NavSatFix odom_to_navsatfix(const nav_msgs::msg::Odometry & odom_msg); + /// @brief Map axes: "utm" (the UTM grid) or "enu" (east and north at the origin). + std::string projection_{"utm"}; + /// @brief The local east-north-up projection at the origin (projection "enu"). + std::unique_ptr enu_; + + /// @brief Moves \p pose from the GNSS antenna (\p antenna_frame) to the frame the global + /// filter estimates, with the robot's attitude in \p pose. Unchanged if the antenna's mounting + /// is unknown. + void antenna_to_robot( + geometry_msgs::msg::PoseWithCovarianceStamped & pose, const std::string & antenna_frame); + double latitude_origin_{0.0}; double longitude_origin_{0.0}; double altitude_origin_{0.0}; 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 27ed1e05..7157740e 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 @@ -360,6 +360,12 @@ class UkfWrapper return gps_callbackData_arr_; } + //! @brief The robot frame the filter estimates (base_link_frame) + const std::string & getBaseLinkFrameId() const + { + return base_link_frame_id_; + } + protected: //! @brief Finds the latest filter state before the given timestamp and makes //! it the current state again. diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 8a2805fb..7593dcaf 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -136,10 +136,16 @@ void FusionLocalizer::on_initialize() easynav::declare_parameter_if_absent( *localizer_node, plugin_name + ".navsatfix_topic", std::string("gps/filtered")); + easynav::declare_parameter_if_absent( + *localizer_node, plugin_name + ".projection", projection_); localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_); localizer_node->get_parameter(plugin_name + ".longitude_origin", longitude_origin_); localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_); localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_); + localizer_node->get_parameter(plugin_name + ".projection", projection_); + if (projection_ != "utm" && projection_ != "enu") { + throw std::runtime_error("projection '" + projection_ + "' is not utm or enu"); + } navsat_pub_ = localizer_node->create_publisher( navsatfix_topic_, rclcpp::QoS(10)); } @@ -156,6 +162,13 @@ void FusionLocalizer::on_initialize() UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_); UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S"); UTM_origin_z_ = altitude_origin_; + if (projection_ == "enu") { + // The map's origin is the projection's: offsets only move it (initial pose) + enu_ = std::make_unique( + latitude_origin_, longitude_origin_, altitude_origin_); + UTM_origin_x_ = 0.0; + UTM_origin_y_ = 0.0; + } n_gps_sensors_ = static_cast(ukf_global_->getGpsCallbackDataArr().size()); last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME)); @@ -227,13 +240,19 @@ void FusionLocalizer::update_rt(NavState & nav_state) std::make_shared( navsatfix_to_pose( gps_data[i]->data)); + // The fix is the antenna's position: the filter moves it to the robot with this + // attitude (a fix carries none) + const bool has_attitude = nav_state.has("imu"); + if (has_attitude) { + pose->pose.pose.orientation = + nav_state.get(std::string("imu")).data.orientation; + } if (!first_pose_received_) { RCLCPP_INFO( get_node()->get_logger(), "First valid GPS fix received. Initializing filter state."); - if (nav_state.has("imu")) { - auto imu_data = nav_state.get(std::string("imu")); - pose->pose.pose.orientation = imu_data.data.orientation; + if (has_attitude) { + antenna_to_robot(*pose, gps_data[i]->data.header.frame_id); } ukf_global_->setPoseCallback(pose); first_pose_received_ = true; @@ -307,6 +326,34 @@ void FusionLocalizer::place_on_terrain(nav_msgs::msg::Odometry & odom) odom.pose.pose.orientation = tf2::toMsg(placed->orientation); } +void FusionLocalizer::antenna_to_robot( + geometry_msgs::msg::PoseWithCovarianceStamped & pose, const std::string & antenna_frame) +{ + // The frame the filter estimates (its base_link_frame) + const std::string & robot_frame = ukf_global_->getBaseLinkFrameId(); + if (antenna_frame.empty() || antenna_frame == robot_frame) { + return; + } + geometry_msgs::msg::TransformStamped mount; + try { + mount = RTTFBuffer::getInstance()->lookupTransform( + robot_frame, antenna_frame, tf2::TimePointZero); + } catch (const tf2::TransformException & e) { + RCLCPP_WARN( + get_node()->get_logger(), "GNSS antenna mounting unknown (%s): the first fix places the " + "robot at the antenna", e.what()); + return; + } + // robot = antenna - R(attitude) * antenna offset in the robot frame + tf2::Quaternion q; + tf2::fromMsg(pose.pose.pose.orientation, q); + const auto & t = mount.transform.translation; + const tf2::Vector3 offset = tf2::quatRotate(q.normalized(), tf2::Vector3(t.x, t.y, t.z)); + pose.pose.pose.position.x -= offset.x(); + pose.pose.pose.position.y -= offset.y(); + pose.pose.pose.position.z -= offset.z(); +} + geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose( const sensor_msgs::msg::NavSatFix & navsat_msg) { @@ -318,17 +365,21 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose pose_msg.header.frame_id = tf_info.map_frame; double utm_x, utm_y; - int zone; - bool northp; - - GeographicLib::UTMUPS::Forward( - navsat_msg.latitude, - navsat_msg.longitude, - zone, - northp, - utm_x, - utm_y, - UTM_zone_number_); + if (enu_) { + double up; + enu_->Forward(navsat_msg.latitude, navsat_msg.longitude, navsat_msg.altitude, utm_x, utm_y, up); + } else { + int zone; + bool northp; + GeographicLib::UTMUPS::Forward( + navsat_msg.latitude, + navsat_msg.longitude, + zone, + northp, + utm_x, + utm_y, + UTM_zone_number_); + } pose_msg.pose.pose.position.x = utm_x - UTM_origin_x_; pose_msg.pose.pose.position.y = utm_y - UTM_origin_y_; @@ -386,8 +437,13 @@ sensor_msgs::msg::NavSatFix FusionLocalizer::odom_to_navsatfix( double latitude = 0.0; double longitude = 0.0; - GeographicLib::UTMUPS::Reverse( - UTM_zone_number_, UTM_zone_northp_, utm_x, utm_y, latitude, longitude); + if (enu_) { + double altitude; + enu_->Reverse(utm_x, utm_y, 0.0, latitude, longitude, altitude); + } else { + GeographicLib::UTMUPS::Reverse( + UTM_zone_number_, UTM_zone_northp_, utm_x, utm_y, latitude, longitude); + } navsat_msg.latitude = latitude; navsat_msg.longitude = longitude; diff --git a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt index fc535e50..664ab8a3 100644 --- a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt @@ -45,3 +45,10 @@ target_link_libraries(fusion_localizer_terrain_tests rclcpp::rclcpp easynav_localizer::easynav_localizer ) + +ament_add_gtest(fusion_localizer_gps_tests fusion_localizer_gps_tests.cpp) +target_link_libraries(fusion_localizer_gps_tests + fusion_localizer + rclcpp::rclcpp + easynav_localizer::easynav_localizer +) diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_gps_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_gps_tests.cpp new file mode 100644 index 00000000..0b2d5080 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_gps_tests.cpp @@ -0,0 +1,257 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 GNSS fixes in the map: the projection (UTM grid or east-north-up) and the antenna's +/// mounting on the robot. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "GeographicLib/LocalCartesian.hpp" +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_fusion_localizer/FusionLocalizer.hpp" +#include "easynav_localizer/LocalizerNode.hpp" +#include "easynav_sensors/types/GNSSPerception.hpp" +#include "easynav_sensors/types/IMUPerception.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + +namespace +{ + +class FriendFusionLocalizer : public easynav::FusionLocalizer +{ +public: + using easynav::FusionLocalizer::update_rt; +}; + +// Sonoma Raceway: 0.54 deg east of UTM zone 10's central meridian, so the UTM grid is turned +// 0.34 deg from east-north there +constexpr double kLat0 = 38.162767; +constexpr double kLon0 = -122.457945; +constexpr double kAlt0 = 10.3; + +} // namespace + +class FusionGpsTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override + { + localizer_.reset(); + node_.reset(); + rclcpp::shutdown(); + } + + void make(std::optional projection) + { + std::vector params = { + {"fusion.latitude_origin", kLat0}, + {"fusion.longitude_origin", kLon0}, + {"fusion.altitude_origin", kAlt0}, + {"fusion.navmap_terrain", false}, + {"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.gps0", "gps_test_fix"}, + {"fusion.global_filter.gps0_config", std::vector{ + true, true, false, false, false, false, + false, false, false, false, false, false, + false, false, false}}, + }; + if (projection) { + params.push_back({"fusion.projection", *projection}); + } + rclcpp::NodeOptions options; + options.parameter_overrides(params); + node_ = std::make_shared(options); + node_->declare_parameter>( + "localizer_types", std::vector{}); + localizer_ = std::make_shared(); + localizer_->initialize(node_, "fusion"); + } + + /// The fix of an antenna at (x, y) east-north-up from the origin, in \p frame. + easynav::GNSSPerception fix(double x, double y, const std::string & frame) + { + GeographicLib::LocalCartesian enu(kLat0, kLon0, kAlt0); + easynav::GNSSPerception p; + enu.Reverse(x, y, 0.0, p.data.latitude, p.data.longitude, p.data.altitude); + p.data.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + p.data.header.frame_id = frame; + p.data.header.stamp = node_->now(); + return p; + } + + void set_imu_yaw(double yaw) + { + easynav::IMUPerception imu; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + imu.data.orientation.x = q.x(); + imu.data.orientation.y = q.y(); + imu.data.orientation.z = q.z(); + imu.data.orientation.w = q.w(); + nav_state_.set("imu", imu); + } + + /// Feeds the same antenna position for \p seconds (one fix per cycle); the last estimate. + std::optional feed( + double x, double y, const std::string & frame, double seconds) + { + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node_->get_node_base_interface()); + std::optional last; + const auto end = std::chrono::steady_clock::now() + std::chrono::duration(seconds); + do { + nav_state_.set("gps", fix(x, y, frame)); + nav_state_.set_group("gnss", {"gps"}); + exec.spin_some(); + localizer_->update_rt(nav_state_); + if (nav_state_.has("robot_pose")) { + last = nav_state_.get("robot_pose"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } while (std::chrono::steady_clock::now() < end); + return last; + } + + static void mount(const std::string & frame, double x, double y, double z) + { + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = "base_footprint"; + t.child_frame_id = frame; + t.transform.translation.x = x; + t.transform.translation.y = y; + t.transform.translation.z = z; + t.transform.rotation.w = 1.0; + easynav::RTTFBuffer::getInstance()->setTransform(t, "fusion_gps_test", true); + } + + easynav::LocalizerNode::SharedPtr node_; + std::shared_ptr localizer_; + easynav::NavState nav_state_; +}; + +TEST_F(FusionGpsTest, EnuPutsTheFixWhereTheMapHasIt) +{ + make("enu"); + const auto pose = feed(250.0, -150.0, "base_footprint", 0.5); + ASSERT_TRUE(pose); + EXPECT_NEAR(pose->pose.pose.position.x, 250.0, 0.05); + EXPECT_NEAR(pose->pose.pose.position.y, -150.0, 0.05); +} + +TEST_F(FusionGpsTest, UtmIsTheDefaultAndItsGridIsTurnedFromEastNorth) +{ + make(std::nullopt); + const auto pose = feed(250.0, -150.0, "base_footprint", 0.5); + ASSERT_TRUE(pose); + const double error = std::hypot( + pose->pose.pose.position.x - 250.0, pose->pose.pose.position.y + 150.0); + // 0.34 deg at 290 m from the origin: 1.7 m (and the UTM scale, 0.1 m) + EXPECT_GT(error, 1.0) << "the default keeps the UTM grid, as before"; + EXPECT_LT(error, 2.5); +} + +TEST_F(FusionGpsTest, UnknownProjectionFailsToInitialize) +{ + EXPECT_THROW(make("mercator"), std::runtime_error); +} + +TEST_F(FusionGpsTest, TheFirstFixPlacesTheRobotAndNotTheAntenna) +{ + // Antenna 1.5 m ahead and 1 m above; the robot faces north + mount("gps_ahead", 1.5, 0.0, 1.0); + make("enu"); + set_imu_yaw(M_PI / 2.0); + const auto first = feed(100.0, 50.0, "gps_ahead", 0.0); + ASSERT_TRUE(first); + EXPECT_NEAR(first->pose.pose.position.x, 100.0, 0.02); + EXPECT_NEAR(first->pose.pose.position.y, 48.5, 0.02) << "1.5 m behind the antenna"; +} + +TEST_F(FusionGpsTest, LaterFixesKeepTheRobotBehindTheAntenna) +{ + // Antenna 1.5 m ahead, 0.5 m to the left; the robot faces west + mount("gps_left", 1.5, 0.5, 1.0); + make("enu"); + set_imu_yaw(M_PI); + const auto pose = feed(-40.0, 20.0, "gps_left", 1.5); + ASSERT_TRUE(pose); + EXPECT_NEAR(pose->pose.pose.position.x, -38.5, 0.1); + EXPECT_NEAR(pose->pose.pose.position.y, 20.5, 0.1); +} + +TEST_F(FusionGpsTest, WithoutTheAntennaMountingTheRobotIsAtTheAntenna) +{ + make("enu"); + set_imu_yaw(0.3); + const auto first = feed(10.0, -5.0, "gps_unmounted", 0.0); + ASSERT_TRUE(first); + EXPECT_NEAR(first->pose.pose.position.x, 10.0, 0.02); + EXPECT_NEAR(first->pose.pose.position.y, -5.0, 0.02); +} + +TEST_F(FusionGpsTest, WithoutImuTheFirstFixIsTheAntennas) +{ + // No attitude to turn the mounting with: the antenna's position, as before + mount("gps_no_imu", 1.5, 0.0, 1.0); + make("enu"); + const auto first = feed(30.0, 30.0, "gps_no_imu", 0.0); + ASSERT_TRUE(first); + EXPECT_NEAR(first->pose.pose.position.x, 30.0, 0.02); + EXPECT_NEAR(first->pose.pose.position.y, 30.0, 0.02); +} + +TEST_F(FusionGpsTest, ThePublishedFixOfTheEstimateIsInTheSameProjection) +{ + make("enu"); + sensor_msgs::msg::NavSatFix::SharedPtr published; + auto listener = std::make_shared("gps_filtered_listener"); + auto sub = listener->create_subscription( + "gps/filtered", 10, [&published](sensor_msgs::msg::NavSatFix::SharedPtr m) {published = m;}); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(listener); + const auto in = fix(250.0, -150.0, "base_footprint"); + const auto pose = feed(250.0, -150.0, "base_footprint", 0.5); + ASSERT_TRUE(pose); + const auto end = std::chrono::steady_clock::now() + std::chrono::seconds(2); + while (!published && std::chrono::steady_clock::now() < end) { + feed(250.0, -150.0, "base_footprint", 0.0); + exec.spin_some(); + } + ASSERT_TRUE(published); + EXPECT_NEAR(published->latitude, in.data.latitude, 1e-6); // ~0.1 m + EXPECT_NEAR(published->longitude, in.data.longitude, 1e-6); +} From d457a29df69898671d229e49dc4e2fea71628535 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 3 Nov 2026 12:21:25 +0100 Subject: [PATCH 2/2] FusionLocalizer: robot attitude from the IMU without its mounting; no NaN without orientation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The IMU's orientation is its frame's: the robot's attitude now removes the IMU's mounting (TF). An IMU without orientation (covariance -1, null or non-finite quaternion) or without a known mounting gives no attitude instead of a NaN first fix; later fixes then use the estimated heading. Later fixes carry the antenna frame's orientation, which is what the filter expects. Signed-off-by: Francisco Martín Rico --- localizers/easynav_fusion_localizer/README.md | 4 +- .../FusionLocalizer.hpp | 26 +++- .../FusionLocalizer.cpp | 92 ++++++++---- .../tests/fusion_localizer_gps_tests.cpp | 135 ++++++++++++++++-- 4 files changed, 217 insertions(+), 40 deletions(-) diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md index bf492399..02d29ab5 100644 --- a/localizers/easynav_fusion_localizer/README.md +++ b/localizers/easynav_fusion_localizer/README.md @@ -41,7 +41,9 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | `.projection` | `string` | `utm` | How fixes become map coordinates: `utm`, the UTM grid shifted to the origin (its axes turn up to a few degrees from east and north away from the zone's central meridian), or `enu`, east-north-up at the origin, as Gazebo's `spherical_coordinates` and most local maps define it. | | `.navsatfix_topic` | `string` | `gps/filtered` | Where the global estimate is published as a fix. | -A fix is the position of the GNSS antenna, in the frame of its `header.frame_id`. With the IMU's attitude and the antenna's mounting (TF from the filter's `base_link_frame` to that frame), the first fix places the robot, not the antenna, and the filter removes the mounting from the later ones. Without IMU or without that TF, the robot starts where the antenna is. +A fix is the position of the GNSS antenna, in the frame of its `header.frame_id`. With the robot's attitude and the antenna's mounting (TF from the filter's `base_link_frame` to that frame), the first fix places the robot, not the antenna, and the filter removes the mounting from the later ones. Without attitude or without that TF, the robot starts where the antenna is. + +The attitude comes from the IMU of the sensor named `imu`: its orientation without its own mounting (TF from `base_link_frame` to the IMU's `header.frame_id`). An IMU that gives no orientation (`orientation_covariance[0] = -1`, or a null quaternion) or whose mounting is not in TF gives none; later fixes then use the filter's estimated heading. The IMU's yaw must be absolute (east-north-up, as a magnetometer or Gazebo gives it): one relative to where the IMU started turns the mounting the wrong way. For additional robot_localization-related configuration, see the upstream documentation: https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp index 9f949c68..4cbdb664 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -15,12 +15,16 @@ #pragma once -#include #include +#include #include #include +#include #include "easynav_core/LocalizerMethodBase.hpp" +#include "easynav_sensors/types/IMUPerception.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Transform.hpp" #include "easynav_fusion_localizer/NavMapTerrain.hpp" #include "easynav_fusion_localizer/ukf_wrapper.hpp" #include "sensor_msgs/msg/imu.hpp" @@ -109,11 +113,21 @@ class FusionLocalizer : public easynav::LocalizerMethodBase /// @brief The local east-north-up projection at the origin (projection "enu"). std::unique_ptr enu_; - /// @brief Moves \p pose from the GNSS antenna (\p antenna_frame) to the frame the global - /// filter estimates, with the robot's attitude in \p pose. Unchanged if the antenna's mounting - /// is unknown. - void antenna_to_robot( - geometry_msgs::msg::PoseWithCovarianceStamped & pose, const std::string & antenna_frame); + /// @brief The robot's attitude from \p imu: its orientation without the IMU's mounting. Empty + /// if the IMU reports no orientation or its mounting is unknown. + std::optional robot_attitude(const IMUPerception & imu); + + /// @brief The mounting of \p frame on the frame the global filter estimates (identity for that + /// frame or an empty one). Empty if TF does not have it. + std::optional mounting(const std::string & frame); + /// @brief Mountings already found (they are fixed). + std::unordered_map mountings_; + + /// @brief Moves \p pose from the GNSS antenna, mounted as \p antenna_mount, to the robot with + /// the robot's \p attitude. + static void antenna_to_robot( + geometry_msgs::msg::PoseWithCovarianceStamped & pose, const tf2::Transform & antenna_mount, + const tf2::Quaternion & attitude); double latitude_origin_{0.0}; double longitude_origin_{0.0}; diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 7593dcaf..65f35546 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -13,6 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "easynav_common/Parameters.hpp" #include "easynav_fusion_localizer/FusionLocalizer.hpp" @@ -240,24 +242,42 @@ void FusionLocalizer::update_rt(NavState & nav_state) std::make_shared( navsatfix_to_pose( gps_data[i]->data)); - // The fix is the antenna's position: the filter moves it to the robot with this - // attitude (a fix carries none) - const bool has_attitude = nav_state.has("imu"); - if (has_attitude) { - pose->pose.pose.orientation = - nav_state.get(std::string("imu")).data.orientation; + // The fix is the antenna's position (it carries no attitude) + const std::string & antenna_frame = gps_data[i]->data.header.frame_id; + const auto antenna_mount = mounting(antenna_frame); + std::optional attitude; + if (nav_state.has("imu")) { + attitude = robot_attitude(nav_state.get(std::string("imu"))); } if (!first_pose_received_) { RCLCPP_INFO( get_node()->get_logger(), "First valid GPS fix received. Initializing filter state."); - if (has_attitude) { - antenna_to_robot(*pose, gps_data[i]->data.header.frame_id); + if (attitude) { + pose->pose.pose.orientation = tf2::toMsg(*attitude); + } + if (attitude && antenna_mount) { + antenna_to_robot(*pose, *antenna_mount, *attitude); + } else if (!antenna_mount || antenna_mount->getOrigin().length2() > 0.0) { + RCLCPP_WARN( + get_node()->get_logger(), "GNSS antenna mounting (%s) or robot attitude (IMU) " + "unknown: the first fix places the robot at the antenna", antenna_frame.c_str()); } ukf_global_->setPoseCallback(pose); first_pose_received_ = true; continue; } + // The filter removes the mounting with the antenna's orientation: the robot's attitude + // from the IMU or, without it, the estimated one + tf2::Quaternion antenna_q = attitude.value_or(tf2::Quaternion::getIdentity()); + nav_msgs::msg::Odometry estimate; + if (!attitude && ukf_global_->getFilteredOdometryMessage(&estimate)) { + tf2::fromMsg(estimate.pose.pose.orientation, antenna_q); + } + if (antenna_mount) { + antenna_q *= antenna_mount->getRotation(); + } + pose->pose.pose.orientation = tf2::toMsg(antenna_q.normalized()); ukf_global_->poseCallback( pose, gps_cb_arr[i], @@ -326,29 +346,51 @@ void FusionLocalizer::place_on_terrain(nav_msgs::msg::Odometry & odom) odom.pose.pose.orientation = tf2::toMsg(placed->orientation); } -void FusionLocalizer::antenna_to_robot( - geometry_msgs::msg::PoseWithCovarianceStamped & pose, const std::string & antenna_frame) +std::optional FusionLocalizer::robot_attitude(const IMUPerception & imu) +{ + // orientation_covariance[0] == -1: the IMU gives no orientation (sensor_msgs/Imu) + const auto & o = imu.data.orientation; + tf2::Quaternion q(o.x, o.y, o.z, o.w); + if (imu.data.orientation_covariance[0] == -1.0 || !std::isfinite(q.length2()) || + q.length2() < 1e-6) + { + return std::nullopt; + } + const auto imu_mount = mounting(imu.data.header.frame_id); + if (!imu_mount) { + return std::nullopt; + } + // The IMU's orientation is its frame's: robot = imu * mounting^-1 + return (q.normalized() * imu_mount->getRotation().inverse()).normalized(); +} + +std::optional FusionLocalizer::mounting(const std::string & frame) { - // The frame the filter estimates (its base_link_frame) const std::string & robot_frame = ukf_global_->getBaseLinkFrameId(); - if (antenna_frame.empty() || antenna_frame == robot_frame) { - return; + if (frame.empty() || frame == robot_frame) { + return tf2::Transform::getIdentity(); + } + if (const auto found = mountings_.find(frame); found != mountings_.end()) { + return found->second; } - geometry_msgs::msg::TransformStamped mount; try { - mount = RTTFBuffer::getInstance()->lookupTransform( - robot_frame, antenna_frame, tf2::TimePointZero); - } catch (const tf2::TransformException & e) { - RCLCPP_WARN( - get_node()->get_logger(), "GNSS antenna mounting unknown (%s): the first fix places the " - "robot at the antenna", e.what()); - return; + tf2::Transform mount; + tf2::fromMsg( + RTTFBuffer::getInstance()->lookupTransform(robot_frame, frame, tf2::TimePointZero).transform, + mount); + mountings_[frame] = mount; + return mount; + } catch (const tf2::TransformException &) { + return std::nullopt; } +} + +void FusionLocalizer::antenna_to_robot( + geometry_msgs::msg::PoseWithCovarianceStamped & pose, const tf2::Transform & antenna_mount, + const tf2::Quaternion & attitude) +{ // robot = antenna - R(attitude) * antenna offset in the robot frame - tf2::Quaternion q; - tf2::fromMsg(pose.pose.pose.orientation, q); - const auto & t = mount.transform.translation; - const tf2::Vector3 offset = tf2::quatRotate(q.normalized(), tf2::Vector3(t.x, t.y, t.z)); + const tf2::Vector3 offset = tf2::quatRotate(attitude, antenna_mount.getOrigin()); pose.pose.pose.position.x -= offset.x(); pose.pose.pose.position.y -= offset.y(); pose.pose.pose.position.z -= offset.z(); diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_gps_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_gps_tests.cpp index 0b2d5080..1031dbb6 100644 --- a/localizers/easynav_fusion_localizer/tests/fusion_localizer_gps_tests.cpp +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_gps_tests.cpp @@ -113,18 +113,35 @@ class FusionGpsTest : public ::testing::Test return p; } - void set_imu_yaw(double yaw) + /// An IMU in \p frame (empty: the robot's) whose frame has \p yaw. + void set_imu_yaw(double yaw, const std::string & frame = "") { - easynav::IMUPerception imu; tf2::Quaternion q; q.setRPY(0.0, 0.0, yaw); - imu.data.orientation.x = q.x(); - imu.data.orientation.y = q.y(); - imu.data.orientation.z = q.z(); - imu.data.orientation.w = q.w(); + set_imu(q.x(), q.y(), q.z(), q.w(), 0.0, frame); + } + + void set_imu( + double x, double y, double z, double w, double covariance0, const std::string & frame = "") + { + easynav::IMUPerception imu; + imu.data.header.frame_id = frame; + imu.data.orientation.x = x; + imu.data.orientation.y = y; + imu.data.orientation.z = z; + imu.data.orientation.w = w; + imu.data.orientation_covariance[0] = covariance0; nav_state_.set("imu", imu); } + static bool finite(const std::optional & pose) + { + return pose && std::isfinite(pose->pose.pose.position.x) && + std::isfinite(pose->pose.pose.position.y) && + std::isfinite(pose->pose.pose.position.z) && + std::isfinite(pose->pose.pose.orientation.w); + } + /// Feeds the same antenna position for \p seconds (one fix per cycle); the last estimate. std::optional feed( double x, double y, const std::string & frame, double seconds) @@ -146,7 +163,7 @@ class FusionGpsTest : public ::testing::Test return last; } - static void mount(const std::string & frame, double x, double y, double z) + static void mount(const std::string & frame, double x, double y, double z, double yaw = 0.0) { geometry_msgs::msg::TransformStamped t; t.header.frame_id = "base_footprint"; @@ -154,7 +171,12 @@ class FusionGpsTest : public ::testing::Test t.transform.translation.x = x; t.transform.translation.y = y; t.transform.translation.z = z; - t.transform.rotation.w = 1.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); easynav::RTTFBuffer::getInstance()->setTransform(t, "fusion_gps_test", true); } @@ -234,6 +256,103 @@ TEST_F(FusionGpsTest, WithoutImuTheFirstFixIsTheAntennas) EXPECT_NEAR(first->pose.pose.position.y, 30.0, 0.02); } +TEST_F(FusionGpsTest, AnImuMountedTurnedGivesTheRobotsAttitude) +{ + // IMU turned 90 deg left on the robot; the robot faces north, so the IMU's frame faces west + mount("imu_turned", 0.2, 0.0, 0.3, M_PI / 2.0); + mount("gps_ahead_t", 1.5, 0.0, 1.0); + make("enu"); + set_imu_yaw(M_PI, "imu_turned"); + const auto first = feed(100.0, 50.0, "gps_ahead_t", 0.0); + ASSERT_TRUE(first); + EXPECT_NEAR(first->pose.pose.position.x, 100.0, 0.02); + EXPECT_NEAR(first->pose.pose.position.y, 48.5, 0.02) << "1.5 m south: the robot faces north"; + const auto later = feed(100.0, 50.0, "gps_ahead_t", 1.5); + ASSERT_TRUE(later); + EXPECT_NEAR(later->pose.pose.position.x, 100.0, 0.1); + EXPECT_NEAR(later->pose.pose.position.y, 48.5, 0.1); +} + +TEST_F(FusionGpsTest, AnAntennaMountedTurnedOnlyCountsItsOffset) +{ + // The antenna's frame turned 90 deg changes nothing: 1.5 m ahead, 0.5 m left; robot faces west + mount("gps_turned", 1.5, 0.5, 1.0, M_PI / 2.0); + make("enu"); + set_imu_yaw(M_PI); + const auto first = feed(-40.0, 20.0, "gps_turned", 0.0); + ASSERT_TRUE(first); + EXPECT_NEAR(first->pose.pose.position.x, -38.5, 0.02); + EXPECT_NEAR(first->pose.pose.position.y, 20.5, 0.02); + const auto later = feed(-40.0, 20.0, "gps_turned", 1.5); + ASSERT_TRUE(later); + EXPECT_NEAR(later->pose.pose.position.x, -38.5, 0.1); + EXPECT_NEAR(later->pose.pose.position.y, 20.5, 0.1); +} + +TEST_F(FusionGpsTest, AnImuWithoutOrientationIsNoAttitude) +{ + // orientation_covariance[0] = -1 and a null quaternion, as sensor_msgs/Imu says + mount("gps_no_orientation", 1.5, 0.0, 1.0); + make("enu"); + set_imu(0.0, 0.0, 0.0, 0.0, -1.0); + const auto first = feed(30.0, 30.0, "gps_no_orientation", 0.0); + ASSERT_TRUE(finite(first)); + EXPECT_NEAR(first->pose.pose.position.x, 30.0, 0.02) << "the robot starts at the antenna"; + EXPECT_NEAR(first->pose.pose.position.y, 30.0, 0.02); + EXPECT_TRUE(finite(feed(30.0, 30.0, "gps_no_orientation", 1.0))); +} + +TEST_F(FusionGpsTest, ANullOrNonFiniteQuaternionIsNoAttitude) +{ + mount("gps_null_q", 1.5, 0.0, 1.0); + make("enu"); + set_imu(0.0, 0.0, 0.0, 0.0, 0.0); + const auto first = feed(30.0, 30.0, "gps_null_q", 0.0); + ASSERT_TRUE(finite(first)); + EXPECT_NEAR(first->pose.pose.position.x, 30.0, 0.02); + EXPECT_NEAR(first->pose.pose.position.y, 30.0, 0.02); + set_imu(NAN, 0.0, 0.0, 1.0, 0.0); + EXPECT_TRUE(finite(feed(30.0, 30.0, "gps_null_q", 1.0))); + set_imu(0.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_TRUE(finite(feed(30.0, 30.0, "gps_null_q", 0.5))); +} + +TEST_F(FusionGpsTest, AnImuWithAnUnknownMountingIsNoAttitude) +{ + // Its yaw would put the robot 1.5 m west of the antenna; without its mounting, it is not used + mount("gps_imu_unknown", 1.5, 0.0, 1.0); + make("enu"); + set_imu_yaw(0.0, "imu_not_in_tf"); + const auto first = feed(30.0, 30.0, "gps_imu_unknown", 0.0); + ASSERT_TRUE(finite(first)); + EXPECT_NEAR(first->pose.pose.position.x, 30.0, 0.02); + EXPECT_NEAR(first->pose.pose.position.y, 30.0, 0.02); +} + +TEST_F(FusionGpsTest, WithoutImuLaterFixesUseTheEstimatedHeading) +{ + // No IMU: the filter's heading (east here, nothing turns it) removes the mounting + mount("gps_estimated", 1.5, 0.0, 1.0); + make("enu"); + const auto pose = feed(30.0, 30.0, "gps_estimated", 2.0); + ASSERT_TRUE(finite(pose)); + EXPECT_NEAR(pose->pose.pose.position.x, 28.5, 0.15); + EXPECT_NEAR(pose->pose.pose.position.y, 30.0, 0.15); +} + +TEST_F(FusionGpsTest, AnImuThatAppearsLaterGivesTheAttitudeFromThen) +{ + // First fix without IMU (robot at the antenna), then the IMU says the robot faces north + mount("gps_imu_late", 1.5, 0.0, 1.0); + make("enu"); + ASSERT_TRUE(finite(feed(100.0, 50.0, "gps_imu_late", 0.0))); + set_imu_yaw(M_PI / 2.0); + const auto pose = feed(100.0, 50.0, "gps_imu_late", 2.0); + ASSERT_TRUE(finite(pose)); + EXPECT_NEAR(pose->pose.pose.position.x, 100.0, 0.15); + EXPECT_NEAR(pose->pose.pose.position.y, 48.5, 0.15); +} + TEST_F(FusionGpsTest, ThePublishedFixOfTheEstimateIsInTheSameProjection) { make("enu");