diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md index 60faec87..02d29ab5 100644 --- a/localizers/easynav_fusion_localizer/README.md +++ b/localizers/easynav_fusion_localizer/README.md @@ -35,6 +35,15 @@ 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 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 a3f2a7bd..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,18 +15,23 @@ #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" #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 +108,27 @@ 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 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}; 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..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" @@ -136,10 +138,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 +164,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,18 +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 (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 (nav_state.has("imu")) { - auto imu_data = nav_state.get(std::string("imu")); - pose->pose.pose.orientation = imu_data.data.orientation; + 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], @@ -307,6 +346,56 @@ void FusionLocalizer::place_on_terrain(nav_msgs::msg::Odometry & odom) odom.pose.pose.orientation = tf2::toMsg(placed->orientation); } +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) +{ + const std::string & robot_frame = ukf_global_->getBaseLinkFrameId(); + if (frame.empty() || frame == robot_frame) { + return tf2::Transform::getIdentity(); + } + if (const auto found = mountings_.find(frame); found != mountings_.end()) { + return found->second; + } + try { + 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 + 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(); +} + geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose( const sensor_msgs::msg::NavSatFix & navsat_msg) { @@ -318,17 +407,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 +479,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..1031dbb6 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_gps_tests.cpp @@ -0,0 +1,376 @@ +// 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; + } + + /// An IMU in \p frame (empty: the robot's) whose frame has \p yaw. + void set_imu_yaw(double yaw, const std::string & frame = "") + { + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + 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) + { + 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, double yaw = 0.0) + { + 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; + 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); + } + + 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, 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"); + 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); +}