From d659dcc7ebcae3b69179e06395d840971b7f33f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 6 Oct 2026 18:20:38 +0200 Subject: [PATCH 1/2] Remove deprecated GPS localizer MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_gps_localizer/CHANGELOG.rst | 24 -- .../easynav_gps_localizer/CMakeLists.txt | 95 -------- localizers/easynav_gps_localizer/README.md | 69 ------ .../easynav_gps_localizer_plugins.xml | 9 - .../easynav_gps_localizer/GpsLocalizer.hpp | 194 ---------------- localizers/easynav_gps_localizer/package.xml | 33 --- .../easynav_gps_localizer/GpsLocalizer.cpp | 212 ------------------ .../tests/CMakeLists.txt | 18 -- .../tests/gps_localizer_reconfigure_tests.cpp | 83 ------- .../tests/gps_localizer_tests.cpp | 186 --------------- 10 files changed, 923 deletions(-) delete mode 100644 localizers/easynav_gps_localizer/CHANGELOG.rst delete mode 100644 localizers/easynav_gps_localizer/CMakeLists.txt delete mode 100644 localizers/easynav_gps_localizer/README.md delete mode 100644 localizers/easynav_gps_localizer/easynav_gps_localizer_plugins.xml delete mode 100644 localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp delete mode 100644 localizers/easynav_gps_localizer/package.xml delete mode 100644 localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp delete mode 100644 localizers/easynav_gps_localizer/tests/CMakeLists.txt delete mode 100644 localizers/easynav_gps_localizer/tests/gps_localizer_reconfigure_tests.cpp delete mode 100644 localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp diff --git a/localizers/easynav_gps_localizer/CHANGELOG.rst b/localizers/easynav_gps_localizer/CHANGELOG.rst deleted file mode 100644 index 730a123d..00000000 --- a/localizers/easynav_gps_localizer/CHANGELOG.rst +++ /dev/null @@ -1,24 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package easynav_gps_localizer -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.4.2 (2026-07-26) ------------------- -* Complete deps -* Update README.md for all localizers -* Reset pose from RViz2 for every localizer -* GPLv3 -> Apache 2.0 -* Add a base_footprint frame in TFInfo -* Remove C++20/C++23 features and update to new MethodBase interface -* Merge branch 'set_robot_frame' into frames-fix-pr-40 -* TFInfo in RTTFBuffer -* Refactor to use TFInfo -* linting docs -* Cleanup unused headers -* Update sheets -* Contributors: Francisco Martín Rico, Francisco Miguel Moreno, Juan S. Cely, Miguel, migueldm - -0.0.2 (2025-10-12) ------------------- -* Reorganization initial -* Contributors: Francisco Martín Rico diff --git a/localizers/easynav_gps_localizer/CMakeLists.txt b/localizers/easynav_gps_localizer/CMakeLists.txt deleted file mode 100644 index 9151bdad..00000000 --- a/localizers/easynav_gps_localizer/CMakeLists.txt +++ /dev/null @@ -1,95 +0,0 @@ -cmake_minimum_required(VERSION 3.20) -project(easynav_gps_localizer) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(easynav_core REQUIRED) -find_package(easynav_common REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) - -# Geographiclib installs FindGeographicLib.cmake to this non-standard location -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") -find_package(GeographicLib REQUIRED) - -include_directories(${GeographicLib_INCLUDE_DIRS}) - - -set(dependencies - rclcpp - rclcpp_lifecycle - easynav_core - easynav_common - pluginlib - nav_msgs - sensor_msgs - geometry_msgs - tf2 - tf2_ros - tf2_geometry_msgs -) - -add_library(gps_localizer SHARED - src/easynav_gps_localizer/GpsLocalizer.cpp -) -target_include_directories(gps_localizer PUBLIC - $ - $ -) -target_link_libraries(gps_localizer - easynav_core::easynav_core - easynav_common::easynav_common - pluginlib::pluginlib - tf2::tf2 - tf2_ros::tf2_ros - tf2_geometry_msgs::tf2_geometry_msgs - ${nav_msgs_TARGETS} - ${sensor_msgs_TARGETS} - ${geometry_msgs_TARGETS} - GeographicLib - ${GeographicLib_LIBRARIES} -) - -install(DIRECTORY include/ - DESTINATION include/ -) - -install(TARGETS - gps_localizer - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(tests) -endif() - -ament_export_include_directories(include) -ament_export_libraries( - gps_localizer -) -ament_export_targets( - export_${PROJECT_NAME} -) -# Register the localization plugins -pluginlib_export_plugin_description_file(easynav_core easynav_gps_localizer_plugins.xml) -ament_export_dependencies(${dependencies}) -ament_package() diff --git a/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md deleted file mode 100644 index 97640b1f..00000000 --- a/localizers/easynav_gps_localizer/README.md +++ /dev/null @@ -1,69 +0,0 @@ -# easynav_gps_localizer - -## Description - -GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom-like frame. - -## Authors and Maintainers - -- **Authors:** Intelligent Robotics Lab -- **Maintainers:** Francisco Martín Rico - -## Supported ROS 2 Distributions - -| Distribution | Status | -|---|---| -| humble | ![kilted](https://img.shields.io/badge/humble-supported-brightgreen) | -| jazzy | ![kilted](https://img.shields.io/badge/jazzy-supported-brightgreen) | -| kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | -| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | - -## Plugin (pluginlib) - -- **Plugin Name:** `easynav_gps_localizer/GpsLocalizer` -- **Type:** `easynav::GpsLocalizer` -- **Base Class:** `easynav::LocalizerMethodBase` -- **Library:** `gps_localizer` -- **Description:** GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom-like frame. - -## Parameters - -All parameters are declared under the plugin namespace, i.e., `//easynav_gps_localizer/GpsLocalizer/...`. - -| Name | Type | Default | Description | -|---|---|---:|---| -| `.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). | - -## Interfaces (Topics and Services) - -### Subscriptions and Publications - -| Direction | Topic | Type | Purpose | QoS | -|---|---|---|---|---| -| Subscription | `robot/gps/fix` | `sensor_msgs/msg/NavSatFix` | Raw GPS fix. | SensorDataQoS (reliable) | -| Subscription | `imu/data` | `sensor_msgs/msg/Imu` | IMU orientation for yaw fusion. | SensorDataQoS (reliable) | -| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Seed/reset the localizer pose from an external initial pose estimate. | depth=10 | -| Publisher | `robot/odom_gps` | `nav_msgs/msg/Odometry` | Odometry fused from GPS + IMU (UTM-projected). | SensorDataQoS | - -### Services - -This package does not create service servers or clients. - -## NavState Keys - -| Key | Type | Access | Notes | -|---|---|---|---| -| `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | GPS-based odometry estimate. | - -## TF Frames - -| Role | Transform | Notes | -|---|---|---| -| Publishes | `map (or world) -> odom_gps` | Static or slowly varying transform aligning UTM to local odometry frame. | -| Optional | `base_link -> imu` | Used implicitly via IMU orientation; not looked up explicitly in this plugin. | - -## License - -Apache-2.0 diff --git a/localizers/easynav_gps_localizer/easynav_gps_localizer_plugins.xml b/localizers/easynav_gps_localizer/easynav_gps_localizer_plugins.xml deleted file mode 100644 index b1f10a46..00000000 --- a/localizers/easynav_gps_localizer/easynav_gps_localizer_plugins.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - A GPS implementation for the Localizer Method. - - - - diff --git a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp deleted file mode 100644 index f37a3065..00000000 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ /dev/null @@ -1,194 +0,0 @@ -// Copyright 2025 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 Declaration of the GpsLocalizer class, a default plugin implementation for localization. - -#ifndef EASYNAV_LOCALIZER__GPSLOCALIZER_HPP_ -#define EASYNAV_LOCALIZER__GPSLOCALIZER_HPP_ - -#include "nav_msgs/msg/odometry.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -#include "easynav_core/LocalizerMethodBase.hpp" -#include "tf2_ros/static_transform_broadcaster.hpp" -#include "sensor_msgs/msg/imu.hpp" -#include -#include - -namespace easynav -{ - -/** - * @class GpsLocalizer - * @brief A default "do-nothing" implementation of the LocalizerMethodBase. - * - * This class implements the interface required by the Easy Navigation framework - * for localization but does not perform any actual computation. It is useful as - * a placeholder, example, or fallback when no real localization plugin is configured. - */ -class GpsLocalizer : public easynav::LocalizerMethodBase -{ -public: - /** - * @brief Default constructor. - */ - GpsLocalizer() = default; - - /** - * @brief Default destructor. - */ - ~GpsLocalizer() = default; - - /** - * @brief Initialize the localization method. - * - * This override may be used to set up internal resources. By default, it simply succeeds. - * - * @throws std::runtime_error if initialization fails. - */ - virtual void on_initialize() override; - - /** - * @brief Updates the localization estimate based on the current navigation state. - * - * This method is intended to run the localization logic and update the odometry. - * In this implementation, the method updates the odometry based on the latest GPS data. - * GPS data is used to set the robot's position in the UTM coordinate system. - * The origin of the UTM coordinates is set when the first GPS message is received. - * - * @param nav_state The current navigation state of the system. - */ - virtual void update_rt(NavState & nav_state) override; - - /** - * @brief Updates the localization estimate based on the current navigation state. - * - * This method is intended to run the localization logic and update the odometry. - * In this implementation, the method updates the odometry based on the latest GPS data. - * GPS data is used to set the robot's position in the UTM coordinate system. - * The origin of the UTM coordinates is set when the first GPS message is received. - * - * @param nav_state The current navigation state of the system. - */ - virtual void update(NavState & nav_state) override; - -protected: - /// Subscriber for the initial pose. - rclcpp::Subscription::SharedPtr init_pose_sub_; - - /// Initial pose received but not yet applied (e.g. waiting for a valid GPS fix). - std::optional pending_init_pose_; - - /// Callback for /initialpose. - void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - /// @brief Starts from the last known pose after a reconfiguration ("initial_pose.use_last_known"). - void on_last_known_pose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) override; - - /// @brief Whether to start from the last known pose after a reconfiguration. - bool use_last_known_pose_ {true}; - -private: - /** - * @brief Internal representation of the robot's current odometry. - * - * Stores the estimated position and velocity of the robot. - */ - nav_msgs::msg::Odometry odom_; - - /** - * @brief Origin UTM coordinates. - * - * Stores the first UTM coordinates obtained from the GPS data. - * It is used to calculate the relative position of the robot in the UTM coordinate system. - */ - geometry_msgs::msg::Point origin_utm_; - - /** - * @brief odometry publisher - * - * This publisher publsh in a topic the odom value - * It is used to validate the localization - */ - rclcpp::Publisher::SharedPtr odom_pub_; - - /** - * @brief Subscriber for GPS data. - * - * This subscriber listens to the GPS data published on the "/robot/gps/fix" topic. - * It is used to obtain the robot's position in the UTM coordinate system. - */ - rclcpp::Subscription::SharedPtr gps_subscriber_; - - /** - * @brief Subscriber to the IMU data. - * - * This subscriber listens to the IMU data published on the "imu/data" topic. - * It is used to obtain the robot's orientation. - * The IMU data is used to calculate the yaw angle of the robot. - */ - rclcpp::Subscription::SharedPtr imu_subscriber_; - - /** - * @brief GPS message. - * - * Stores the latest GPS data received from the subscriber. - * It contains information about the robot's position and status. - */ - sensor_msgs::msg::NavSatFix gps_msg_; - - /** - * @brief IMU message. - * - * Stores the latest IMU data received from the subscriber. - * It contains information about the robot's orientation. - */ - sensor_msgs::msg::Imu imu_msg_; - - /** - * @brief Static broadcaster for static transforms. - * - * This broadcaster is used to broadcast static transforms between coordinate frames. - * It is used to broadcast the static transform between the "map" frame and the "odom" frame. - */ - std::shared_ptr static_broadcaster_; - - /** - * @brief Callback function for GPS data. - * - * This function is called whenever a new GPS message is received. - * It updates the internal GPS message with the latest data. - * - * @param msg The received GPS message. - */ - void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); - - /** - * @brief Callback function for the IMU data. - * - * This function is called whenever a new IMU data message is received. - * It updates the internal yaw angle of the robot based on the IMU data. - * - * @param msg The IMU data message. - */ - void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); - - double alpha_, dt_, yaw_1_, time_1_; -}; - -} // namespace easynav - -#endif // EASYNAV_LOCALIZER__GPSLOCALIZER_HPP_ diff --git a/localizers/easynav_gps_localizer/package.xml b/localizers/easynav_gps_localizer/package.xml deleted file mode 100644 index 2ba779c7..00000000 --- a/localizers/easynav_gps_localizer/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - easynav_gps_localizer - 0.4.2 - Easy Navigation: GPS Localizer package. - Jose Miguel Guerrero Hernandez - - Apache-2.0 - - ament_cmake - - rclcpp - rclcpp_lifecycle - easynav_core - easynav_common - pluginlib - geographiclib - nav_msgs - sensor_msgs - geometry_msgs - tf2 - tf2_ros - tf2_geometry_msgs - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - ament_cmake - - diff --git a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp deleted file mode 100644 index 65c2b730..00000000 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ /dev/null @@ -1,212 +0,0 @@ -// Copyright 2025 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 Implementation of the GpsLocalizer class. - -#include "easynav_common/Parameters.hpp" -#include "easynav_gps_localizer/GpsLocalizer.hpp" - -#include "easynav_common/RTTFBuffer.hpp" - -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/LinearMath/Quaternion.hpp" - -namespace easynav -{ - -void GpsLocalizer::on_initialize() -{ - auto node = get_node(); - const auto & plugin_name = get_plugin_name(); - - // Initialize the odometry message - odom_.header.stamp = get_node()->now(); - const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - odom_.header.frame_id = tf_info.map_frame; - odom_.child_frame_id = tf_info.robot_footprint_frame; - - // Create subscriber to GPS data - gps_subscriber_ = node->create_subscription( - "robot/gps/fix", rclcpp::SensorDataQoS().reliable(), - std::bind(&GpsLocalizer::gps_callback, this, std::placeholders::_1)); - - // Create static broadcaster - static_broadcaster_ = std::make_shared(*node); - - // Create subscriber to IMU data - imu_subscriber_ = node->create_subscription( - "imu/data", rclcpp::SensorDataQoS().reliable(), - std::bind(&GpsLocalizer::imu_callback, this, std::placeholders::_1)); - - // Subscribe to initial pose - init_pose_sub_ = node->create_subscription( - "initialpose", 10, - std::bind(&GpsLocalizer::init_pose_callback, this, std::placeholders::_1)); - - // Optional initial pose from parameters (kept consistent with AMCL parameter names) - easynav::declare_parameter_if_absent(*node, plugin_name + ".initial_pose.x", 0.0); - easynav::declare_parameter_if_absent(*node, plugin_name + ".initial_pose.y", 0.0); - easynav::declare_parameter_if_absent(*node, plugin_name + ".initial_pose.yaw", 0.0); - - double init_x = 0.0; - double init_y = 0.0; - double init_yaw = 0.0; - node->get_parameter(plugin_name + ".initial_pose.x", init_x); - node->get_parameter(plugin_name + ".initial_pose.y", init_y); - node->get_parameter(plugin_name + ".initial_pose.yaw", init_yaw); - - easynav::declare_parameter_if_absent(*node, plugin_name + ".initial_pose.use_last_known", - use_last_known_pose_); - node->get_parameter(plugin_name + ".initial_pose.use_last_known", use_last_known_pose_); - - if (std::abs(init_x) > 1e-12 || std::abs(init_y) > 1e-12 || std::abs(init_yaw) > 1e-12) { - geometry_msgs::msg::PoseWithCovarianceStamped init_pose; - init_pose.header.stamp = node->now(); - const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - init_pose.header.frame_id = tf_info.map_frame; - init_pose.pose.pose.position.x = init_x; - init_pose.pose.pose.position.y = init_y; - init_pose.pose.pose.position.z = 0.0; - tf2::Quaternion q; - q.setRPY(0.0, 0.0, init_yaw); - init_pose.pose.pose.orientation = tf2::toMsg(q); - init_pose.pose.covariance.fill(0.0); - pending_init_pose_ = init_pose; - } - - // Create publisher - odom_pub_ = node->create_publisher( - "robot/odom_gps", rclcpp::SensorDataQoS().reliable()); - - // Create static transform - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = node->now(); - transform.header.frame_id = tf_info.map_frame; - transform.child_frame_id = tf_info.odom_frame; - transform.transform.translation.x = 0.0; - transform.transform.translation.y = 0.0; - transform.transform.translation.z = 0.0; - transform.transform.rotation.x = 0.0; - transform.transform.rotation.y = 0.0; - transform.transform.rotation.z = 0.0; - transform.transform.rotation.w = 1.0; - static_broadcaster_->sendTransform(transform); - - RTTFBuffer::getInstance()->setTransform(transform, "easynav", true); - - time_1_ = get_node()->now().seconds(); - alpha_ = 0.99; -} - -void GpsLocalizer::gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) -{ - gps_msg_ = std::move(*msg); -} - - -void GpsLocalizer::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) -{ - imu_msg_ = std::move(*msg); -} - -void GpsLocalizer::on_last_known_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose) -{ - if (use_last_known_pose_) { - pending_init_pose_ = pose; // Applied with the next GPS fix. - } -} - -void GpsLocalizer::init_pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) -{ - pending_init_pose_ = *msg; -} - - -void GpsLocalizer::update_rt([[maybe_unused]] NavState & nav_state) -{ -} - -void GpsLocalizer::update(NavState & nav_state) -{ - // Convert GPS coordinates to UTM - double lat = gps_msg_.latitude; - double lon = gps_msg_.longitude; - double utm_x, utm_y, roll, pitch, yaw_imu, yaw_gyro, yaw_filtered; - int zone; - bool northp; - - GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, utm_x, utm_y); - std::string utm_zone = std::to_string(zone) + (northp ? "N" : "S"); - - // If an initial pose was provided, align the UTM origin so that the current - // GPS fix maps to that pose in the map frame. - if (pending_init_pose_.has_value() && gps_msg_ != sensor_msgs::msg::NavSatFix()) { - origin_utm_.x = utm_x - pending_init_pose_->pose.pose.position.x; - origin_utm_.y = utm_y - pending_init_pose_->pose.pose.position.y; - pending_init_pose_.reset(); - } - - if (origin_utm_ == geometry_msgs::msg::Point() && - gps_msg_ != sensor_msgs::msg::NavSatFix()) - { - // Get first UTM coordinates - origin_utm_.x = utm_x; - origin_utm_.y = utm_y; - } - - // Get XY cartesian coordinates respect to the origin - odom_.header.stamp = gps_msg_.header.stamp; - const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); - odom_.header.frame_id = tf_info.map_frame; - odom_.child_frame_id = tf_info.robot_footprint_frame; - odom_.pose.pose.position.x = utm_x - origin_utm_.x; - odom_.pose.pose.position.y = utm_y - origin_utm_.y; - - // Extract the yaw angle from the IMU data - dt_ = get_node()->now().seconds() - time_1_; - yaw_gyro = yaw_1_ + imu_msg_.angular_velocity.z * dt_; - - tf2::Quaternion q( - imu_msg_.orientation.x, - imu_msg_.orientation.y, - imu_msg_.orientation.z, - imu_msg_.orientation.w); - tf2::Matrix3x3 m(q); - m.getRPY(roll, pitch, yaw_imu); - - // Yaw angle filtered using a complementary filter - yaw_filtered = (yaw_gyro * alpha_) + (yaw_imu * (1 - alpha_)); - - // Extract the yaw angle from the IMU data - tf2::Quaternion q_filtered; - q_filtered.setRPY(roll, pitch, yaw_filtered); - odom_.pose.pose.orientation.x = q_filtered.x(); - odom_.pose.pose.orientation.y = q_filtered.y(); - odom_.pose.pose.orientation.z = q_filtered.z(); - odom_.pose.pose.orientation.w = q_filtered.w(); - yaw_1_ = yaw_gyro; - time_1_ = get_node()->now().seconds(); - - nav_state.set("robot_pose", odom_); - odom_pub_->publish(odom_); -} - -} // namespace easynav - -#include -PLUGINLIB_EXPORT_CLASS(easynav::GpsLocalizer, easynav::LocalizerMethodBase) diff --git a/localizers/easynav_gps_localizer/tests/CMakeLists.txt b/localizers/easynav_gps_localizer/tests/CMakeLists.txt deleted file mode 100644 index be182147..00000000 --- a/localizers/easynav_gps_localizer/tests/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) - -ament_add_gtest(gps_localizer_tests gps_localizer_tests.cpp) - -target_link_libraries(gps_localizer_tests - gps_localizer - rclcpp::rclcpp - rclcpp_lifecycle::rclcpp_lifecycle -) - -ament_add_gtest(gps_localizer_reconfigure_tests gps_localizer_reconfigure_tests.cpp) - -target_link_libraries(gps_localizer_reconfigure_tests - gps_localizer - rclcpp::rclcpp - rclcpp_lifecycle::rclcpp_lifecycle -) diff --git a/localizers/easynav_gps_localizer/tests/gps_localizer_reconfigure_tests.cpp b/localizers/easynav_gps_localizer/tests/gps_localizer_reconfigure_tests.cpp deleted file mode 100644 index 265cc961..00000000 --- a/localizers/easynav_gps_localizer/tests/gps_localizer_reconfigure_tests.cpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2025 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 Regression test: a plugin must tolerate initialize() being -/// called twice on the same node (as happens across a cleanup/reconfigure -/// cycle) without throwing. - -#include -#include -#include "easynav_gps_localizer/GpsLocalizer.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "gtest/gtest.h" - -class GpsLocalizerReconfigureTest : public ::testing::Test -{ -protected: - void SetUp() override - { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } - } -}; - -TEST_F(GpsLocalizerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) -{ - auto node = rclcpp_lifecycle::LifecycleNode::make_shared("gps_localizer_reconfigure_test"); - - auto plugin1 = std::make_shared(); - ASSERT_NO_THROW(plugin1->initialize(node, "test_localizer")); - - auto plugin2 = std::make_shared(); - ASSERT_NO_THROW(plugin2->initialize(node, "test_localizer")); -} - -TEST_F(GpsLocalizerReconfigureTest, InitializeWhenAnotherPluginLeftSomeOfItsParameters) -{ - // A plugin of another type under the same name may have left any subset of them declared. - std::vector names; - std::vector values; - { - auto node = rclcpp_lifecycle::LifecycleNode::make_shared("gps_localizer_reconfigure_test"); - - auto plugin = std::make_shared(); - plugin->initialize(node, "test_localizer"); - for (const auto & n : node->list_parameters({"test_localizer"}, 10).names) { - try { - values.push_back(node->get_parameter(n).get_parameter_value()); - names.push_back(n); - } catch (const rclcpp::exceptions::ParameterUninitializedException &) { - // Declared by type only, without a value: nothing another plugin could have left. - } - } - } - ASSERT_FALSE(names.empty()); - - for (std::size_t i = 0; i < names.size(); ++i) { - auto node = rclcpp_lifecycle::LifecycleNode::make_shared("gps_localizer_reconfigure_test"); - - node->declare_parameter(names[i], values[i]); - auto plugin = std::make_shared(); - ASSERT_NO_THROW(plugin->initialize(node, "test_localizer")) << "left declared: " << names[i]; - for (const auto & n : names) { - EXPECT_TRUE(node->has_parameter(n)) << "left declared: " << names[i] << ", missing: " << n; - } - } -} diff --git a/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp deleted file mode 100644 index b594eaee..00000000 --- a/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp +++ /dev/null @@ -1,186 +0,0 @@ -// Copyright 2025 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. - -#include - -#include -#include -#include -#include - -#include "easynav_gps_localizer/GpsLocalizer.hpp" - -#include "easynav_common/types/NavState.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/LinearMath/Quaternion.hpp" - -namespace -{ - -class FriendGpsLocalizer : public easynav::GpsLocalizer -{ -public: - using easynav::GpsLocalizer::init_pose_sub_; - using easynav::GpsLocalizer::update; -}; - -class GpsLocalizerInitialPoseTest : public ::testing::Test -{ -protected: - void SetUp() override {rclcpp::init(0, nullptr);} - void TearDown() override {rclcpp::shutdown();} -}; - -} // namespace - -TEST_F(GpsLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) -{ - const double x0 = 1.1; - const double y0 = -0.7; - - const double x1 = -2.3; - const double y1 = 0.4; - - rclcpp::NodeOptions options; - options.parameter_overrides({ - rclcpp::Parameter("test.initial_pose.x", x0), - rclcpp::Parameter("test.initial_pose.y", y0), - rclcpp::Parameter("test.initial_pose.yaw", 0.0), - }); - - auto node = std::make_shared("test_gps_localizer_node", options); - auto localizer = std::make_shared(); - - localizer->initialize(node, "test"); - - ASSERT_NE(localizer->init_pose_sub_, nullptr); - - const auto infos = node->get_subscriptions_info_by_topic("initialpose"); - ASSERT_FALSE(infos.empty()); - - const bool has_expected_type = std::any_of( - infos.begin(), infos.end(), - [](const rclcpp::TopicEndpointInfo & info) { - return info.topic_type() == "geometry_msgs/msg/PoseWithCovarianceStamped"; - }); - EXPECT_TRUE(has_expected_type); - - auto pub_node = std::make_shared("gps_test_pub_node"); - auto initialpose_pub = pub_node->create_publisher( - "initialpose", 10); - auto gps_pub = pub_node->create_publisher( - "robot/gps/fix", rclcpp::SensorDataQoS().reliable()); - auto imu_pub = pub_node->create_publisher( - "imu/data", rclcpp::SensorDataQoS().reliable()); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node->get_node_base_interface()); - exec.add_node(pub_node->get_node_base_interface()); - - { - const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); - while (std::chrono::steady_clock::now() < connect_deadline && - (initialpose_pub->get_subscription_count() == 0 || - gps_pub->get_subscription_count() == 0 || - imu_pub->get_subscription_count() == 0)) - { - exec.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - } - - sensor_msgs::msg::Imu imu_msg; - imu_msg.header.stamp = pub_node->now(); - imu_msg.header.frame_id = ""; - imu_msg.angular_velocity.z = 0.0; - imu_msg.orientation.w = 1.0; - imu_pub->publish(imu_msg); - - sensor_msgs::msg::NavSatFix gps_msg; - gps_msg.header.stamp = pub_node->now(); - gps_msg.header.frame_id = ""; - gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; - gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; - gps_msg.latitude = 42.0; - gps_msg.longitude = -1.0; - gps_msg.altitude = 0.0; - gps_pub->publish(gps_msg); - - easynav::NavState nav_state; - - // Deliver GPS/IMU callbacks and run update() until the initial pose is applied. - { - const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); - while (std::chrono::steady_clock::now() < deadline) { - exec.spin_some(); - localizer->update(nav_state); - - if (nav_state.has("robot_pose")) { - const auto odom = nav_state.get("robot_pose"); - const bool ok = - std::abs(odom.pose.pose.position.x - x0) < 1e-9 && - std::abs(odom.pose.pose.position.y - y0) < 1e-9; - if (ok) { - break; - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - } - - ASSERT_TRUE(nav_state.has("robot_pose")); - { - const auto odom = nav_state.get("robot_pose"); - EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); - EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); - } - - geometry_msgs::msg::PoseWithCovarianceStamped init_pose_msg; - init_pose_msg.header.stamp = pub_node->now(); - init_pose_msg.header.frame_id = "map"; - init_pose_msg.pose.pose.position.x = x1; - init_pose_msg.pose.pose.position.y = y1; - init_pose_msg.pose.pose.position.z = 0.0; - init_pose_msg.pose.pose.orientation.w = 1.0; - init_pose_msg.pose.covariance.fill(0.0); - initialpose_pub->publish(init_pose_msg); - - const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); - while (std::chrono::steady_clock::now() < deadline) { - exec.spin_some(); - localizer->update(nav_state); - - const auto odom = nav_state.get("robot_pose"); - const bool ok = - std::abs(odom.pose.pose.position.x - x1) < 1e-9 && - std::abs(odom.pose.pose.position.y - y1) < 1e-9; - - if (ok) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - { - const auto odom = nav_state.get("robot_pose"); - EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); - EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); - } -} From 5c14ab0beb734e7b470bb8eca053989794811d30 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Tue, 6 Oct 2026 18:51:24 +0200 Subject: [PATCH 2/2] Remove deprecated GPS localizer from CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .github/workflows/rolling.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index 2364a80b..91ed8e56 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -23,7 +23,7 @@ jobs: - name: build and test uses: ros-tooling/action-ros-ci@0.4.8 with: - package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_mhamcl_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller + package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_mhamcl_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller target-ros2-distro: rolling vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos skip-tests: false