diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index aabb1b11..15c2642b 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/CMakeLists.txt @@ -58,7 +58,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp index 8f7bd0ac..6259a598 100644 --- a/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp @@ -15,143 +15,169 @@ #include +#include +#include +#include +#include +#include + #include "easynav_costmap_localizer/AMCLLocalizer.hpp" -#include "easynav_costmap_common/costmap_2d.hpp" +#include "easynav_localizer/LocalizerNode.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include -#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test +namespace { -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - void TearDown() override - { - rclcpp::shutdown(); - } -}; - -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) { - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared(); - static_map->resizeMap(30, 30, 0.1, -1.5, -1.5); - static_map->setCost(15, 15, 254); // Occupied cell - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - perception->data.points.resize(2); - perception->data.points[0].x = 0.0; - perception->data.points[0].y = 0.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = 1.5; - perception->data.points[1].y = 1.5; - perception->data.points[1].z = 0.2; - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", easynav::Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.base", *static_map); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(map_ptr != nullptr); - EXPECT_EQ(map_ptr->getCost(15, 15), 254u); // Cell (15, 15) + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; } -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) +double yaw_from_tf(const tf2::Transform & tf) { - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto pub = node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = "map"; - grid.info.width = 10; - grid.info.height = 10; - grid.info.resolution = 0.2; - grid.info.origin.position.x = -1.0; - grid.info.origin.position.y = -0.6; - grid.data.assign(100, 0); - grid.data[55] = 100; - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - EXPECT_EQ(static_map->getCost(5, 5), 254u); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; } -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { +class FriendAMCLLocalizer : public easynav::AMCLLocalizer +{ public: - void force_path(const std::string & path) {map_path_ = path;} + using easynav::AMCLLocalizer::init_pose_sub_; }; -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) +class AMCLLocalizerInitialPoseTest : public ::testing::Test { - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared(); - static_map->resizeMap(4, 4, 0.5, -1.0, -1.0); - static_map->setCost(1, 1, 254); - static_map->setCost(2, 2, 254); - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map"; - std::static_pointer_cast(manager)->force_path(test_map_file); +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); +} // namespace - auto client = node->create_client( - "/test_savemap_node/test_savemap/savemap"); +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.25; + const double y0 = -2.5; + const double yaw0 = 0.4; + + const double x1 = -0.75; + const double y1 = 0.9; + const double yaw1 = -1.2; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), + rclcpp::Parameter("test.min_noise_xy", 0.0), + rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.compute_odom_from_tf", true), + }); + + auto node = std::make_shared(options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + } - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + } - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); + 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("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); - std::ifstream yamlfile(test_map_file + ".yaml"); - ASSERT_TRUE(yamlfile.is_open()); + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } - std::string line; - std::getline(yamlfile, line); - EXPECT_NE(line.find("image:"), std::string::npos); - yamlfile.close(); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } - std::remove((test_map_file + ".yaml").c_str()); - std::remove((test_map_file + ".pgm").c_str()); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } } diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt index 94db9550..c0755cd4 100644 --- a/localizers/easynav_fusion_localizer/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -81,6 +81,9 @@ if(BUILD_TESTING) 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) diff --git a/localizers/easynav_fusion_localizer/README.md b/localizers/easynav_fusion_localizer/README.md index e48542e7..c641ef72 100644 --- a/localizers/easynav_fusion_localizer/README.md +++ b/localizers/easynav_fusion_localizer/README.md @@ -1,4 +1,4 @@ -# easynav_gps_localizer +# easynav_fusion_localizer [![ROS 2: kilted](https://img.shields.io/badge/ROS%202-kilted-blue)](#) [![ROS 2: rolling](https://img.shields.io/badge/ROS%202-rolling-blue)](#) @@ -23,15 +23,23 @@ Odometry fusion localizer based on Robot Localization that fuses any n odometry ## Parameters -TODO +All parameters are declared under the plugin namespace, i.e., `//easynav_fusion_localizer/FusionLocalizer/...`. -See [robot_localization documentation](https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html) +| 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). | + +For additional robot_localization-related configuration, see the upstream documentation: +https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html ## Interfaces (Topics and Services) ### Subscriptions and Publications | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Seed/reset the filter pose from an external initial pose estimate. | depth=10 | | Publisher | `odometry/filtered` | `nav_msgs/msg/Odometry` | Odometry fused from all sources. | SensorDataQoS | @@ -41,13 +49,13 @@ 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. | +| `robot_pose` | `nav_msgs::msg::Odometry` | **Write** | Fused odometry estimate. | ## TF Frames | Role | Transform | Notes | |---|---|---| -| Publishes | `map -> odom` | Static or slowly varying transform aligning UTM to local odometry frame. | +| Publishes | `map -> odom` | Transform that aligns the odometry frame with the global frame. | ## License diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml index b7afad57..7ec21016 100644 --- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml +++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml @@ -267,6 +267,7 @@ sensors_node: gps: topic: /gps/fix type: sensor_msgs/msg/NavSatFix + group: gnss laser1: topic: /front_laser/points type: sensor_msgs/msg/PointCloud2 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 1f49c3f8..740a03df 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,13 @@ class FusionLocalizer : public easynav::LocalizerMethodBase FusionLocalizer() = default; virtual ~FusionLocalizer() = default; +protected: + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + + /// Callback for /initialpose. + void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + protected: /** * @brief Hook de inicialización de MethodBase. 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 f1cd27f5..8488f319 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -12,6 +12,9 @@ #include "easynav_common/YTSession.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + namespace easynav { @@ -22,6 +25,11 @@ void FusionLocalizer::on_initialize() auto node = get_node(); + // Subscribe to initial pose + init_pose_sub_ = node->create_subscription( + "initialpose", 10, + std::bind(&FusionLocalizer::init_pose_callback, this, std::placeholders::_1)); + auto localizer_node = std::dynamic_pointer_cast(node); const std::string & plugin_name = this->get_plugin_name(); @@ -120,13 +128,43 @@ void FusionLocalizer::on_initialize() RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); } +void FusionLocalizer::init_pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) +{ + if (has_global_filter_) { + nav_msgs::msg::Odometry global_odom; + // Get current position to calculate the offset + if (ukf_global_->getFilteredOdometryMessage(&global_odom)) { + double current_x = global_odom.pose.pose.position.x; + double current_y = global_odom.pose.pose.position.y; + + // Shift the UTM origin so that the current GPS position will now map to msg's position + UTM_origin_x_ += (current_x - msg->pose.pose.position.x); + UTM_origin_y_ += (current_y - msg->pose.pose.position.y); + + RCLCPP_INFO(get_node()->get_logger(), + "Initial pose reset UTM origin. Shifted X by %.2f, Y by %.2f to match pose (%.2f, %.2f)", + (current_x - msg->pose.pose.position.x), (current_y - msg->pose.pose.position.y), + msg->pose.pose.position.x, msg->pose.pose.position.y); + } + + // Forward initial pose to global filter to make the state jump immediately + ukf_global_->setPoseCallback(msg); + first_pose_received_ = true; + } + + if (has_local_filter_) { + ukf_local_->setPoseCallback(msg); + } +} + void FusionLocalizer::update_rt(NavState & nav_state) { const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); if (has_global_filter_) { - if (n_gps_sensors_ && nav_state.has("gnss")) { - auto gps_data = nav_state.get(std::string("gnss")); + if (n_gps_sensors_ && nav_state.has_group("gnss")) { + auto gps_data = nav_state.get_group(std::string("gnss")); const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr(); for (int i = 0; i < n_gps_sensors_; ++i) { if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) { diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp index 6164b4de..5ce5bd30 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -2538,10 +2538,29 @@ void UkfWrapper::setPoseCallback( update_vector, measurement, measurement_covariance); // For the state + const auto now = parent_node_->now(); + + // Deterministically set the internal state to the requested pose. + // This ensures a /set_pose (or /initialpose) immediately changes outputs. filter_.setState(measurement); filter_.setEstimateErrorCovariance(measurement_covariance); - - filter_.setLastMeasurementTime(parent_node_->now()); + filter_.setLastMeasurementTime(now); + + // If the filter was not initialized yet, feed an equivalent measurement + // through the filter to flip its initialized flag. + if (!filter_.getInitializedStatus()) { + Measurement pose_measurement; + pose_measurement.topic_name_ = topic_name; + pose_measurement.measurement_ = measurement; + pose_measurement.covariance_ = measurement_covariance; + pose_measurement.update_vector_ = update_vector; + pose_measurement.time_ = now; + pose_measurement.mahalanobis_thresh_ = std::numeric_limits::max(); + pose_measurement.latest_control_ = latest_control_; + pose_measurement.latest_control_time_ = latest_control_time_; + + filter_.processMeasurement(pose_measurement); + } RF_DEBUG("\n------ /UkfWrapper::setPoseCallback ------\n"); } diff --git a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt new file mode 100644 index 00000000..8d470925 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt @@ -0,0 +1,12 @@ +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(easynav_localizer REQUIRED) + +ament_add_gtest(fusion_localizer_tests fusion_localizer_tests.cpp) + +target_link_libraries(fusion_localizer_tests + fusion_localizer + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + easynav_localizer::easynav_localizer +) diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp new file mode 100644 index 00000000..d010b352 --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp @@ -0,0 +1,157 @@ +// 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 + +#include "easynav_fusion_localizer/FusionLocalizer.hpp" +#include "easynav_localizer/LocalizerNode.hpp" + +#include "easynav_common/types/NavState.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" + +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ + +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; +} + +class FriendFusionLocalizer : public easynav::FusionLocalizer +{ +public: + using easynav::FusionLocalizer::init_pose_sub_; + using easynav::FusionLocalizer::update_rt; +}; + +class FusionLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(FusionLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 1.5; + const double y0 = -0.25; + const double yaw0 = 0.7; + + const double x1 = -0.8; + const double y1 = 2.1; + const double yaw1 = -1.2; + + // Provide at least one parameter override so FusionLocalizer considers a filter configured. + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.global_filter.frequency", 30.0), + }); + + auto node = std::make_shared(options); + node->declare_parameter>("localizer_types", std::vector{}); + + auto localizer = std::make_shared(); + + ASSERT_NO_THROW(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("fusion_test_pub_node"); + auto initialpose_pub = pub_node->create_publisher( + "initialpose", 10); + + 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) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + easynav::NavState nav_state; + + 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; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + init_pose_msg.pose.pose.orientation = tf2::toMsg(q); + 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_rt(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 - x1) < 1e-6 && + std::abs(odom.pose.pose.position.y - y1) < 1e-6 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-3; + + 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, x1, 1e-6); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-6); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-3); + } +} diff --git a/localizers/easynav_gps_localizer/CMakeLists.txt b/localizers/easynav_gps_localizer/CMakeLists.txt index f18114bc..b44c29b4 100644 --- a/localizers/easynav_gps_localizer/CMakeLists.txt +++ b/localizers/easynav_gps_localizer/CMakeLists.txt @@ -59,6 +59,9 @@ if(BUILD_TESTING) 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) diff --git a/localizers/easynav_gps_localizer/README.md b/localizers/easynav_gps_localizer/README.md index 5306d1fb..97640b1f 100644 --- a/localizers/easynav_gps_localizer/README.md +++ b/localizers/easynav_gps_localizer/README.md @@ -28,7 +28,13 @@ GPS-based localizer that fuses NavSatFix and IMU to publish odometry in an odom- ## Parameters -This plugin does not declare configurable ROS 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) @@ -38,6 +44,7 @@ This plugin does not declare configurable ROS parameters. |---|---|---|---|---| | 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 diff --git a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp index 03d1fcdc..2628d6a9 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -21,10 +21,12 @@ #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 { @@ -83,6 +85,16 @@ class GpsLocalizer : public easynav::LocalizerMethodBase */ 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); + private: /** * @brief Internal representation of the robot's current odometry. diff --git a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp index c4eec332..a0ab2cf9 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -20,12 +20,16 @@ #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(); @@ -46,6 +50,38 @@ void GpsLocalizer::on_initialize() "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) + node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); + node->declare_parameter(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); + + 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()); @@ -81,6 +117,12 @@ void GpsLocalizer::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) imu_msg_ = std::move(*msg); } +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) { @@ -98,6 +140,14 @@ void GpsLocalizer::update(NavState & nav_state) 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()) { diff --git a/localizers/easynav_gps_localizer/tests/CMakeLists.txt b/localizers/easynav_gps_localizer/tests/CMakeLists.txt new file mode 100644 index 00000000..0e8b0048 --- /dev/null +++ b/localizers/easynav_gps_localizer/tests/CMakeLists.txt @@ -0,0 +1,10 @@ +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 +) diff --git a/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp new file mode 100644 index 00000000..b594eaee --- /dev/null +++ b/localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp @@ -0,0 +1,186 @@ +// 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); + } +} diff --git a/localizers/easynav_navmap_localizer/CMakeLists.txt b/localizers/easynav_navmap_localizer/CMakeLists.txt index 12e237f4..8bd6781e 100644 --- a/localizers/easynav_navmap_localizer/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/CMakeLists.txt @@ -64,7 +64,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/localizers/easynav_navmap_localizer/README.md b/localizers/easynav_navmap_localizer/README.md index b984b01b..827a6c1c 100644 --- a/localizers/easynav_navmap_localizer/README.md +++ b/localizers/easynav_navmap_localizer/README.md @@ -59,6 +59,7 @@ All parameters are declared under the plugin name namespace, i.e., `// | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `/odom` | `nav_msgs/msg/Odometry` | Used only when `.compute_odom_from_tf = false` | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Initialize particles pose to the received pose, using covariance | depth=10 | | Publisher | `//particles` | `geometry_msgs/msg/PoseArray` | Publishes the current particle set | depth=10 | | Publisher | `//pose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Publishes the estimated pose with covariance | depth=10 | diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp index c166f876..55a666d3 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -163,6 +163,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Subscriber for odometry messages. rclcpp::Subscription::SharedPtr odom_sub_; + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + /** * @brief Callback for receiving odometry updates. * @@ -170,6 +173,13 @@ class AMCLLocalizer : public LocalizerMethodBase */ void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg); + /** + * @brief Callback for receiving the initial pose. + * + * @param msg The incoming initial pose with covariance. + */ + void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); + /** * @brief Update odom from TFs instead of a odom topic * diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index 0371eab1..2b6dc641 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -449,7 +449,8 @@ void AMCLLocalizer::on_initialize() tf_broadcaster_ = std::make_unique(get_node()); auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); + auto rt_cbg = node_typed ? node_typed->get_real_time_cbg() : + get_node()->get_node_base_interface()->get_default_callback_group(); rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; if (!compute_odom_from_tf_) { @@ -463,6 +464,10 @@ void AMCLLocalizer::on_initialize() estimate_pub_ = get_node()->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + init_pose_sub_ = get_node()->create_subscription( + "initialpose", 10, + std::bind(&AMCLLocalizer::init_pose_callback, this, std::placeholders::_1)); + last_reseed_ = get_node()->now(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); } @@ -475,6 +480,153 @@ void AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) if (!initialized_odom_) {last_odom_ = odom_; initialized_odom_ = true;} } +void +AMCLLocalizer::init_pose_callback( + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg) +{ + if (particles_.empty()) { + return; + } + + auto logger = get_node()->get_logger(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + // Check expected frame (map) + const std::string expected_frame = tf_info.map_frame; + if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { + RCLCPP_WARN( + logger, + "AMCLLocalizer::init_pose_callback: received initial pose in frame '%s' but expected '%s'. " + "Ignoring message.", + msg->header.frame_id.c_str(), expected_frame.c_str()); + return; + } + + last_input_time_ = msg->header.stamp; + + // Extract pose mean (x, y, yaw) expressed in map frame + const auto & pose = msg->pose.pose; + const double mean_x = pose.position.x; + const double mean_y = pose.position.y; + + tf2::Quaternion q( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w); + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + const double mean_yaw = yaw; + + // Extract 6x6 covariance (ROS order: x, y, z, roll, pitch, yaw) + const auto & cov = msg->pose.covariance; + + // Positional covariance terms + double var_x = cov[0]; + double cov_xy = cov[1]; + double var_y = cov[7]; + + // Yaw variance + double var_yaw = cov[35]; + + // Ensure non-negative variances + var_x = std::max(var_x, 0.0); + var_y = std::max(var_y, 0.0); + var_yaw = std::max(var_yaw, 0.0); + + // 2D Cholesky decomposition of covariance(x,y): + // [ var_x cov_xy ] + // [ cov_xy var_y ] + double l00 = std::sqrt(var_x); + double l10 = 0.0; + double l11 = 0.0; + + if (l00 > 0.0) { + l10 = cov_xy / l00; + double tmp = var_y - l10 * l10; + if (tmp < 0.0) { + tmp = 0.0; + } + l11 = std::sqrt(tmp); + } else { + // Degenerate case: fallback to diagonal stddevs + l00 = std::sqrt(var_x); + l10 = 0.0; + l11 = std::sqrt(var_y); + } + + // Enforce minimum translational noise if covariance is too small + const double std_xy = std::sqrt(0.5 * std::max(0.0, var_x + var_y)); + if (std_xy < min_noise_xy_) { + const double safe_std_xy = (std_xy > 1e-6) ? std_xy : 1.0; + const double scale = min_noise_xy_ / safe_std_xy; + l00 *= scale; + l10 *= scale; + l11 *= scale; + } + + // Yaw noise with minimum enforced + double yaw_stddev = std::sqrt(var_yaw); + yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); + + std::normal_distribution standard_normal(0.0, 1.0); + std::normal_distribution yaw_noise(0.0, yaw_stddev); + + const std::size_t N = particles_.size(); + for (std::size_t i = 0; i < N; ++i) { + const double z0 = standard_normal(rng_); + const double z1 = standard_normal(rng_); + + const double dx = l00 * z0; + const double dy = l10 * z0 + l11 * z1; + + const double new_x = mean_x + dx; + const double new_y = mean_y + dy; + const double new_yaw = mean_yaw + yaw_noise(rng_); + + tf2::Quaternion q_particle; + q_particle.setRPY(0.0, 0.0, new_yaw); + + particles_[i].pose.setOrigin(tf2::Vector3(new_x, new_y, 0.0)); + particles_[i].pose.setRotation(q_particle); + + particles_[i].hits = 0; + particles_[i].possible_hits = 0; + particles_[i].weight = 1.0 / static_cast(N); + } + + // Normalize particle weights (safety) + double total_weight = 0.0; + for (const auto & p : particles_) { + total_weight += p.weight; + } + if (total_weight > 0.0) { + for (auto & p : particles_) { + p.weight /= total_weight; + } + } + + // Prevent immediate reseed after initialization + last_reseed_ = get_node()->now(); + + // Compute map->basefootprint estimate + tf2::Transform map2bf = getEstimatedPose(); + + // Publish TF if odom transform is already known + if (initialized_odom_) { + tf2::Transform map2odom = map2bf * odom_.inverse(); + publishTF(map2odom); + } + + publishEstimatedPose(map2bf); + publishParticles(); + + RCLCPP_INFO( + logger, + "AMCLLocalizer::init_pose_callback: reinitialized %zu particles around (%.3f, %.3f, %.3f)", + N, mean_x, mean_y, mean_yaw); +} + void AMCLLocalizer::update_odom_from_tf() { const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); diff --git a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt index b874c059..400b4523 100644 --- a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt @@ -2,8 +2,8 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(std_srvs REQUIRED) -ament_add_gtest(costmap_localizer_tests costmap_localizer_tests.cpp) -target_link_libraries(costmap_localizer_tests +ament_add_gtest(navmap_localizer_tests navmap_localizer_tests.cpp) +target_link_libraries(navmap_localizer_tests ${PROJECT_NAME} rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle diff --git a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp deleted file mode 100644 index 7e9166a0..00000000 --- a/localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp +++ /dev/null @@ -1,157 +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 "easynav_navmap_localizer/AMCLLocalizer.hpp" -#include "easynav_navmap_common/costmap_2d.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "std_srvs/srv/trigger.hpp" - -#include -#include - -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test -{ -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - - void TearDown() override - { - rclcpp::shutdown(); - } -}; - -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) -{ - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared<::navmap::NavMap>(); - static_map->resizeMap(30, 30, 0.1, -1.5, -1.5); - static_map->setCost(15, 15, 254); // Occupied cell - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - perception->data.points.resize(2); - perception->data.points[0].x = 0.0; - perception->data.points[0].y = 0.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = 1.5; - perception->data.points[1].y = 1.5; - perception->data.points[1].z = 0.2; - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", easynav::Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - navstate.set("map.static", *static_map); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast<::navmap::NavMap>(manager->get_static_map()); - ASSERT_TRUE(map_ptr != nullptr); - EXPECT_EQ(map_ptr->getCost(15, 15), 254u); // Cell (15, 15) -} - -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) -{ - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto pub = node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = "map"; - grid.info.width = 10; - grid.info.height = 10; - grid.info.resolution = 0.2; - grid.info.origin.position.x = -1.0; - grid.info.origin.position.y = -0.6; - grid.data.assign(100, 0); - grid.data[55] = 100; - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast<::navmap::NavMap>(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - EXPECT_EQ(static_map->getCost(5, 5), 254u); -} - -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { -public: - void force_path(const std::string & path) {map_path_ = path;} -}; - -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) -{ - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared<::navmap::NavMap>(); - static_map->resizeMap(4, 4, 0.5, -1.0, -1.0); - static_map->setCost(1, 1, 254); - static_map->setCost(2, 2, 254); - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map"; - std::static_pointer_cast(manager)->force_path(test_map_file); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - auto client = node->create_client( - "/test_savemap_node/test_savemap/savemap"); - - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); - - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); - - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); - - std::ifstream yamlfile(test_map_file + ".yaml"); - ASSERT_TRUE(yamlfile.is_open()); - - std::string line; - std::getline(yamlfile, line); - EXPECT_NE(line.find("image:"), std::string::npos); - yamlfile.close(); - - std::remove((test_map_file + ".yaml").c_str()); - std::remove((test_map_file + ".pgm").c_str()); -} diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp new file mode 100644 index 00000000..89dfed8a --- /dev/null +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp @@ -0,0 +1,182 @@ +// 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_navmap_localizer/AMCLLocalizer.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" + +namespace +{ + +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; +} + +double yaw_from_tf(const tf2::Transform & tf) +{ + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; +} + +class FriendAMCLLocalizer : public easynav::navmap::AMCLLocalizer +{ +public: + using easynav::navmap::AMCLLocalizer::init_pose_sub_; +}; + +class AMCLLocalizerInitialPoseTest : public ::testing::Test +{ +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; + +} // namespace + +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = -3.0; + const double y0 = 0.25; + const double yaw0 = 0.2; + + const double x1 = 0.8; + const double y1 = 3.3; + const double yaw1 = 2.0; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), + rclcpp::Parameter("test.min_noise_xy", 0.0), + rclcpp::Parameter("test.min_noise_yaw", 0.0), + rclcpp::Parameter("test.compute_odom_from_tf", true), + }); + + auto node = std::make_shared( + "test_navmap_localizer_node", options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); + + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + } + + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + } + + 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("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); + + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } + + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } +} diff --git a/localizers/easynav_simple_localizer/CMakeLists.txt b/localizers/easynav_simple_localizer/CMakeLists.txt index 5b61ef63..a26bb848 100644 --- a/localizers/easynav_simple_localizer/CMakeLists.txt +++ b/localizers/easynav_simple_localizer/CMakeLists.txt @@ -58,7 +58,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - # add_subdirectory(tests) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/localizers/easynav_simple_localizer/README.md b/localizers/easynav_simple_localizer/README.md index bee15a05..8a06b1be 100644 --- a/localizers/easynav_simple_localizer/README.md +++ b/localizers/easynav_simple_localizer/README.md @@ -52,6 +52,7 @@ All parameters are declared under the plugin namespace, i.e., `//easyn | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Subscription | `/odom` | `nav_msgs/msg/Odometry` | Read odometry when compute_odom_from_tf=false (not present here). | SensorDataQoS (reliable) | +| Subscription | `initialpose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Initialize particles pose to the received pose, using covariance. | depth=10 | | Publisher | `//particles` | `geometry_msgs/msg/PoseArray` | Publishes the current particle set. | depth=10 | | Publisher | `//pose` | `geometry_msgs/msg/PoseWithCovarianceStamped` | Estimated pose with covariance. | depth=10 | diff --git a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp index 9ba5ebab..caf6fdd7 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -155,6 +155,9 @@ class AMCLLocalizer : public LocalizerMethodBase /// Subscriber for odometry messages. rclcpp::Subscription::SharedPtr odom_sub_; + /// Subscriber for the initial pose. + rclcpp::Subscription::SharedPtr init_pose_sub_; + /** * @brief Callback for receiving odometry updates. * @@ -162,6 +165,13 @@ class AMCLLocalizer : public LocalizerMethodBase */ void odom_callback(nav_msgs::msg::Odometry::UniquePtr msg); + /** + * @brief Callback for receiving the initial pose. + * + * @param msg The incoming initial pose with covariance. + */ + void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); + /// List of particles representing the belief distribution. std::vector particles_; diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index 42165823..d391cb4b 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -243,7 +243,8 @@ AMCLLocalizer::on_initialize() tf_broadcaster_ = std::make_unique(get_node()); auto node_typed = std::dynamic_pointer_cast(get_node()); - auto rt_cbg = node_typed->get_real_time_cbg(); + auto rt_cbg = node_typed ? node_typed->get_real_time_cbg() : + get_node()->get_node_base_interface()->get_default_callback_group(); rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; @@ -256,6 +257,10 @@ AMCLLocalizer::on_initialize() estimate_pub_ = get_node()->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/pose", 10); + init_pose_sub_ = get_node()->create_subscription( + "initialpose", 10, + std::bind(&AMCLLocalizer::init_pose_callback, this, std::placeholders::_1)); + last_reseed_ = get_node()->now(); last_input_time_ = get_node()->now(); @@ -314,6 +319,154 @@ AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) } } +void +AMCLLocalizer::init_pose_callback( + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg) +{ + if (particles_.empty()) { + return; + } + + auto logger = get_node()->get_logger(); + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + + // Check expected frame (map) + const std::string expected_frame = tf_info.map_frame; + if (!msg->header.frame_id.empty() && msg->header.frame_id != expected_frame) { + RCLCPP_WARN( + logger, + "AMCLLocalizer::init_pose_callback: received initial pose in frame '%s' but expected '%s'. " + "Ignoring message.", + msg->header.frame_id.c_str(), expected_frame.c_str()); + return; + } + + last_input_time_ = msg->header.stamp; + + // Extract pose mean (x, y, yaw) expressed in map frame + const auto & pose = msg->pose.pose; + const double mean_x = pose.position.x; + const double mean_y = pose.position.y; + + tf2::Quaternion q( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w); + + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + const double mean_yaw = yaw; + + // Extract 6x6 covariance (ROS order: x, y, z, roll, pitch, yaw) + const auto & cov = msg->pose.covariance; + + // Positional covariance terms + double var_x = cov[0]; + double cov_xy = cov[1]; + double var_y = cov[7]; + + // Yaw variance + double var_yaw = cov[35]; + + // Ensure non-negative variances + var_x = std::max(var_x, 0.0); + var_y = std::max(var_y, 0.0); + var_yaw = std::max(var_yaw, 0.0); + + // 2D Cholesky decomposition of covariance(x,y): + // [ var_x cov_xy ] + // [ cov_xy var_y ] + double l00 = std::sqrt(var_x); + double l10 = 0.0; + double l11 = 0.0; + + if (l00 > 0.0) { + l10 = cov_xy / l00; + double tmp = var_y - l10 * l10; + if (tmp < 0.0) { + tmp = 0.0; + } + l11 = std::sqrt(tmp); + } else { + // Degenerate case: fallback to diagonal stddevs + l00 = std::sqrt(var_x); + l10 = 0.0; + l11 = std::sqrt(var_y); + } + + // Enforce minimum translational noise if covariance is too small + const double std_xy = std::sqrt(0.5 * std::max(0.0, var_x + var_y)); + if (std_xy < min_noise_xy_) { + const double safe_std_xy = (std_xy > 1e-6) ? std_xy : 1.0; + const double scale = min_noise_xy_ / safe_std_xy; + l00 *= scale; + l10 *= scale; + l11 *= scale; + } + + // Yaw noise with minimum enforced + double yaw_stddev = std::sqrt(var_yaw); + yaw_stddev = std::max(yaw_stddev, min_noise_yaw_); + + std::normal_distribution standard_normal(0.0, 1.0); + std::normal_distribution yaw_noise(0.0, yaw_stddev); + + const std::size_t N = particles_.size(); + for (std::size_t i = 0; i < N; ++i) { + const double z0 = standard_normal(rng_); + const double z1 = standard_normal(rng_); + + const double dx = l00 * z0; + const double dy = l10 * z0 + l11 * z1; + + const double new_x = mean_x + dx; + const double new_y = mean_y + dy; + const double new_yaw = mean_yaw + yaw_noise(rng_); + + tf2::Quaternion q_particle; + q_particle.setRPY(0.0, 0.0, new_yaw); + + particles_[i].pose.setOrigin(tf2::Vector3(new_x, new_y, 0.0)); + particles_[i].pose.setRotation(q_particle); + + particles_[i].hits = 0; + particles_[i].possible_hits = 0; + particles_[i].weight = 1.0 / static_cast(N); + } + + // Normalize particle weights (safety) + double total_weight = 0.0; + for (const auto & p : particles_) { + total_weight += p.weight; + } + if (total_weight > 0.0) { + for (auto & p : particles_) { + p.weight /= total_weight; + } + } + + // Prevent immediate reseed after initialization + last_reseed_ = get_node()->now(); + + // Compute map->basefootprint estimate + tf2::Transform map2bf = getEstimatedPose(); + + // Publish TF if odom transform is already known + if (initialized_odom_) { + tf2::Transform map2odom = map2bf * odom_.inverse(); + publishTF(map2odom); + } + + publishEstimatedPose(map2bf); + publishParticles(); + + RCLCPP_INFO( + logger, + "AMCLLocalizer::init_pose_callback: reinitialized %zu particles around (%.3f, %.3f, %.3f)", + N, mean_x, mean_y, mean_yaw); +} + void AMCLLocalizer::predict([[maybe_unused]] NavState & nav_state) { diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp index d353bbea..36efd008 100644 --- a/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp @@ -15,173 +15,167 @@ #include -#include "easynav_simple_common/SimpleMap.hpp" +#include +#include +#include +#include + #include "easynav_simple_localizer/AMCLLocalizer.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "std_srvs/srv/trigger.hpp" - -#include -#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -/// \brief Fixture for AMCLLocalizer tests (minimal) -class AMCLLocalizerTest : public ::testing::Test +namespace { -protected: - void SetUp() override - { - rclcpp::init(0, nullptr); - } - void TearDown() override - { - rclcpp::shutdown(); - } -}; - - -/// \brief Dynamic map update tests -TEST_F(AMCLLocalizerTest, BasicDynamicUpdate) +double yaw_from_quat(const geometry_msgs::msg::Quaternion & q) { - auto node = std::make_shared("test_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test"); - - auto static_map = std::make_shared(); - static_map->initialize(30, 30, 0.1, -1.5, -1.5, 0.0); - manager->set_static_map(static_map); - - easynav::NavState navstate; - auto perception = std::make_shared(); - - perception->data.points.resize(6); - perception->data.points[0].x = 1.0; - perception->data.points[0].y = 1.0; - perception->data.points[0].z = 0.2; - perception->data.points[1].x = -1.0; - perception->data.points[1].y = -1.0; - perception->data.points[1].z = 0.2; - perception->data.points[2].x = -10.0; - perception->data.points[2].y = -1.0; - perception->data.points[2].z = 0.2; - perception->data.points[3].x = 10.0; - perception->data.points[3].y = -1.0; - perception->data.points[3].z = 0.2; - perception->data.points[4].x = 1.0; - perception->data.points[4].y = -10.0; - perception->data.points[4].z = 0.2; - perception->data.points[5].x = 1.0; - perception->data.points[5].y = 10.0; - perception->data.points[5].z = 0.2; - - perception->stamp = rclcpp::Time(0); - perception->frame_id = "map"; - perception->valid = true; - - navstate.set("perceptions", Perceptions()); - navstate.get_mutable("perceptions")->push_back(perception); - - manager->update(navstate); - - auto map_ptr = std::dynamic_pointer_cast(manager->get_dynamyc_map()); - ASSERT_TRUE(map_ptr != nullptr); - - auto cell1 = map_ptr->metric_to_cell(1.0, 1.0); - EXPECT_TRUE(map_ptr->at(cell1.first, cell1.second)); - - auto cell2 = map_ptr->metric_to_cell(-1.0, -1.0); - EXPECT_TRUE(map_ptr->at(cell2.first, cell2.second)); + tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); + return yaw; } -/// \brief Map loading via subscription test -TEST_F(AMCLLocalizerTest, IncomingOccupancyGridUpdatesMaps) +double yaw_from_tf(const tf2::Transform & tf) { - auto node = std::make_shared("test_node2"); - auto manager = std::make_shared(); - easynav::TFInfo tf_info; - tf_info.map_frame = "world_map"; - tf_info.odom_frame = "world_odom"; - tf_info.robot_frame = "world_base"; - tf_info.robot_footprint_frame = "world_footprint_base"; - easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - - manager->initialize(node, "test2"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - - rclcpp::Publisher::SharedPtr pub = - node->create_publisher( - "test_node2/test2/incoming_map", rclcpp::QoS(1).transient_local().reliable()); - - nav_msgs::msg::OccupancyGrid grid; - grid.header.frame_id = tf_info.map_frame; - grid.info.width = 10; - grid.info.height = 10; - grid.info.resolution = 0.2; - grid.info.origin.position.x = -1.0; - grid.info.origin.position.y = -0.6; - grid.data.assign(100, 0); - grid.data[55] = 100; // Mark cell (5,5) as occupied - - pub->publish(grid); - - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - auto static_map = std::dynamic_pointer_cast(manager->get_static_map()); - ASSERT_TRUE(static_map != nullptr); - - EXPECT_EQ(static_map->at(5, 5), 1); - EXPECT_EQ(static_map->at(1, 1), 0); + double roll = 0.0, pitch = 0.0, yaw = 0.0; + tf2::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); + return yaw; } -class FriendAMCLLocalizer : public easynav::AMCLLocalizer { +class FriendAMCLLocalizer : public easynav::AMCLLocalizer +{ public: - void force_path(const std::string & path) {map_path_ = path;} + using easynav::AMCLLocalizer::init_pose_sub_; }; -TEST_F(AMCLLocalizerTest, SavemapServiceWorks) +class AMCLLocalizerInitialPoseTest : public ::testing::Test { - auto node = std::make_shared("test_savemap_node"); - auto manager = std::make_shared(); - manager->initialize(node, "test_savemap"); - - auto static_map = std::make_shared(); - static_map->initialize(4, 4, 0.5, -1.0, -1.0, 0); - static_map->at(1, 1) = true; - static_map->at(2, 2) = true; - manager->set_static_map(static_map); - - const std::string test_map_file = "/tmp/savemap_test_map.txt"; - const std::string service_name = "/test_savemap_node/test_savemap/savemap"; +protected: + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; - std::static_pointer_cast(manager)->force_path(test_map_file); +} // namespace - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); +TEST_F(AMCLLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) +{ + const double x0 = 0.3; + const double y0 = 1.7; + const double yaw0 = -0.8; + + const double x1 = 2.2; + const double y1 = -0.4; + const double yaw1 = 1.1; + + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test.num_particles", 100), + rclcpp::Parameter("test.initial_pose.x", x0), + rclcpp::Parameter("test.initial_pose.y", y0), + rclcpp::Parameter("test.initial_pose.yaw", yaw0), + rclcpp::Parameter("test.initial_pose.std_dev_xy", 0.0), + rclcpp::Parameter("test.initial_pose.std_dev_yaw", 0.0), + rclcpp::Parameter("test.min_noise_xy", 0.0), + rclcpp::Parameter("test.min_noise_yaw", 0.0), + }); + + auto node = std::make_shared( + "test_simple_localizer_node", options); + auto localizer = std::make_shared(); + + localizer->initialize(node, "test"); + + ASSERT_NE(localizer->init_pose_sub_, nullptr); - auto client = node->create_client(service_name); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x0, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y0, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw0, 1e-9); + } - auto request = std::make_shared(); - auto future = client->async_send_request(request); - executor.spin_until_future_complete(future); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x0, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y0, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw0, 1e-9); + } - auto response = future.get(); - EXPECT_TRUE(response->success); - EXPECT_NE(response->message.find("saved"), std::string::npos); + 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("initialpose_pub_node"); + auto pub = pub_node->create_publisher( + "initialpose", 10); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node->get_node_base_interface()); + exec.add_node(pub_node->get_node_base_interface()); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = pub_node->now(); + msg.header.frame_id = "map"; + msg.pose.pose.position.x = x1; + msg.pose.pose.position.y = y1; + msg.pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw1); + msg.pose.pose.orientation = tf2::toMsg(q); + msg.pose.covariance.fill(0.0); - std::ifstream infile(test_map_file); - ASSERT_TRUE(infile.is_open()); + { + const auto connect_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(200); + while (std::chrono::steady_clock::now() < connect_deadline && + pub->get_subscription_count() == 0) + { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + pub->publish(msg); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::steady_clock::now() < deadline) { + exec.spin_some(); + const tf2::Transform tf = localizer->getEstimatedPose(); + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + + const bool pose_ok = + std::abs(tf.getOrigin().x() - x1) < 1e-9 && + std::abs(tf.getOrigin().y() - y1) < 1e-9 && + std::abs(yaw_from_tf(tf) - yaw1) < 1e-9 && + std::abs(odom.pose.pose.position.x - x1) < 1e-9 && + std::abs(odom.pose.pose.position.y - y1) < 1e-9 && + std::abs(yaw_from_quat(odom.pose.pose.orientation) - yaw1) < 1e-9; + + if (pose_ok) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } - std::string first_line; - std::getline(infile, first_line); - EXPECT_NE(first_line.find("4 4"), std::string::npos); - EXPECT_NE(first_line.find("0.5"), std::string::npos); + { + const tf2::Transform tf = localizer->getEstimatedPose(); + EXPECT_NEAR(tf.getOrigin().x(), x1, 1e-9); + EXPECT_NEAR(tf.getOrigin().y(), y1, 1e-9); + EXPECT_NEAR(yaw_from_tf(tf), yaw1, 1e-9); + } - infile.close(); - std::remove(test_map_file.c_str()); + { + const nav_msgs::msg::Odometry odom = localizer->get_pose(); + EXPECT_NEAR(odom.pose.pose.position.x, x1, 1e-9); + EXPECT_NEAR(odom.pose.pose.position.y, y1, 1e-9); + EXPECT_NEAR(yaw_from_quat(odom.pose.pose.orientation), yaw1, 1e-9); + } } diff --git a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp index 75691590..76751621 100644 --- a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp +++ b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp @@ -149,9 +149,6 @@ SimpleMapsManager::update(NavState & nav_state) return; } - std::cout << "perceptions size: " << perceptions.size() << std::endl; - std::cout << "perception[0] size: " << perceptions[0]->data.points.size() << std::endl; - std::cout << "perception[0] valid: " << perceptions[0]->valid << std::endl; const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); rclcpp::Time stamp;