Initpose from rviz2

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}")

localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp

@@ -15,143 +15,169 @@
 
 #include <gtest/gtest.h>
 
+#include <algorithm>
+#include <chrono>
+#include <memory>
+#include <string>
+#include <thread>
+
 #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 <memory>
-#include <fstream>
+#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<rclcpp_lifecycle::LifecycleNode>("test_node");
-  auto manager = std::make_shared<easynav::AMCLLocalizer>();
-  manager->initialize(node, "test");
-
-  auto static_map = std::make_shared<easynav::Costmap2D>();
-  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<easynav::Perception>();
-  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<easynav::Perceptions>("perceptions")->push_back(perception);
-  navstate.set("map.base", *static_map);
-
-  manager->update(navstate);
-
-  auto map_ptr = std::dynamic_pointer_cast<easynav::Costmap2D>(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<rclcpp_lifecycle::LifecycleNode>("test_node2");
-  auto manager = std::make_shared<easynav::AMCLLocalizer>();
-  manager->initialize(node, "test2");
-
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.add_node(node->get_node_base_interface());
-
-  auto pub = node->create_publisher<nav_msgs::msg::OccupancyGrid>(
-    "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<easynav::Costmap2D>(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<rclcpp_lifecycle::LifecycleNode>("test_savemap_node");
-  auto manager = std::make_shared<easynav::AMCLLocalizer>();
-  manager->initialize(node, "test_savemap");
-
-  auto static_map = std::make_shared<easynav::Costmap2D>();
-  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<FriendAMCLLocalizer>(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<std_srvs::srv::Trigger>(
-    "/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<easynav::LocalizerNode>(options);
+  auto localizer = std::make_shared<FriendAMCLLocalizer>();
+
+  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<std_srvs::srv::Trigger::Request>();
-  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<rclcpp::Node>("initialpose_pub_node");
+  auto pub = pub_node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "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);
+  }
 }

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)

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., `/<node_fqn>/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 |
+|---|---|---:|---|
+| `<plugin>.initial_pose.x` | `double` | `0.0` | Initial X position (m). |
+| `<plugin>.initial_pose.y` | `double` | `0.0` | Initial Y position (m). |
+| `<plugin>.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

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

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<geometry_msgs::msg::PoseWithCovarianceStamped>::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.