Merge pull request EasyNavigation#100 from Butakus/odometry_perception

Add Odometry perception

Author: fmrico

easynav_localizer/include/easynav_localizer/DummyLocalizer.hpp

@@ -65,9 +65,6 @@ class DummyLocalizer : public easynav::LocalizerMethodBase
   virtual void update(NavState & nav_state) override;
 
 private:
-  /// @brief Internal pose placeholder.
-  nav_msgs::msg::Odometry robot_pose_;
-
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
 
   double cycle_time_rt_ {0.0};

easynav_localizer/src/easynav_localizer/DummyLocalizer.cpp

@@ -50,8 +50,6 @@ void DummyLocalizer::update_rt([[maybe_unused]] NavState & nav_state)
   RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false);
   // tf_broadcaster_->sendTransform(tf_msg);
 
-  nav_state.set("robot_pose", robot_pose_);
-
   // Busy wait to simulate processing time
   while (chr::duration<double>(chr::steady_clock::now() - start).count() < cycle_time_rt_) {}
 }
@@ -71,8 +69,6 @@ void DummyLocalizer::update([[maybe_unused]] NavState & nav_state)
   RTTFBuffer::getInstance()->setTransform(tf_msg, "easynav", false);
   // tf_broadcaster_->sendTransform(tf_msg);
 
-  nav_state.set("robot_pose", robot_pose_);
-
   // Busy wait to simulate processing time
   while (chr::duration<double>(chr::steady_clock::now() - start).count() < cycle_time_nort_) {}
 }

easynav_localizer/src/easynav_localizer/LocalizerNode.cpp

@@ -19,6 +19,8 @@
 
 #include "lifecycle_msgs/msg/transition.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "tf2/LinearMath/Transform.hpp"
 
 #include "easynav_localizer/LocalizerNode.hpp"
 
@@ -35,6 +37,30 @@ LocalizerNode::LocalizerNode(
 
   localizer_loader_ = std::make_unique<pluginlib::ClassLoader<easynav::LocalizerMethodBase>>(
     "easynav_core", "easynav::LocalizerMethodBase");
+
+  NavState::register_printer<nav_msgs::msg::Odometry>(
+    [](const nav_msgs::msg::Odometry & odom) {
+      const double x = odom.pose.pose.position.x;
+      const double y = odom.pose.pose.position.y;
+      const double z = odom.pose.pose.position.z;
+
+      const tf2::Quaternion q(
+        odom.pose.pose.orientation.x,
+        odom.pose.pose.orientation.y,
+        odom.pose.pose.orientation.z,
+        odom.pose.pose.orientation.w);
+
+      double roll, pitch, yaw;
+      tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
+
+      std::ostringstream ret;
+      ret << std::fixed << std::setprecision(3);
+      ret << "{" << rclcpp::Time(odom.header.stamp).seconds() << " } Odometry with pose: (x: " <<
+        x << ", y: " << y << ", z: " << z << ", yaw: " << yaw << ")";
+      return ret.str();
+    });
+
+
 }
 
 LocalizerNode::~LocalizerNode()

easynav_sensors/CMakeLists.txt

@@ -40,6 +40,7 @@ add_library(${PROJECT_NAME} SHARED
   src/easynav_sensors/types/PointPerception.cpp
   src/easynav_sensors/types/IMUPerception.cpp
   src/easynav_sensors/types/GNSSPerception.cpp
+  src/easynav_sensors/types/OdometryPerception.cpp
   src/easynav_sensors/types/DetectionsPerception.cpp
 )
 target_include_directories(${PROJECT_NAME} PUBLIC

easynav_sensors/easynav_sensors_perception_handler_plugins.xml

@@ -25,6 +25,14 @@
         Perceptions are stored in NavState under the "gnss" group.
       </description>
     </class>
+    <class name="easynav_sensors/OdometryPerceptionHandler"
+           type="easynav::OdometryPerceptionHandler"
+           base_class_type="easynav::PerceptionHandler">
+      <description>
+        Handler for odometry sensors. Supports nav_msgs/msg/Odometry message type.
+        Perceptions are stored in NavState under the "odom" group.
+      </description>
+    </class>
     <class name="easynav_sensors/ImagePerceptionHandler"
            type="easynav::ImagePerceptionHandler"
            base_class_type="easynav::PerceptionHandler">

easynav_sensors/include/easynav_sensors/types/OdometryPerception.hpp

@@ -0,0 +1,130 @@
+// Copyright 2026 Intelligent Robotics Lab
+//
+// 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 Defines data structures and utilities for representing and processing odometry perceptions.
+///
+/// This file contains the definition of the OdometryPerception class, which holds odometry sensor data,
+/// and the OdometryPerceptionHandler class, which handles subscriptions to odometry messages and
+/// transforms them into OdometryPerception instances. It also defines an alias for a collection of such
+/// perceptions.
+
+#ifndef EASYNAV_SENSORS_TYPES__ODOMETRYPERCEPTIONS_HPP_
+#define EASYNAV_SENSORS_TYPES__ODOMETRYPERCEPTIONS_HPP_
+
+#include <string_view>
+#include <vector>
+
+#include "nav_msgs/msg/odometry.hpp"
+
+#include "easynav_sensors/types/Perceptions.hpp"
+
+namespace easynav
+{
+
+/// \class OdometryPerception
+/// \brief Represents a single odometry perception from a sensor.
+///
+/// Inherits from PerceptionBase and stores a nav_msgs::msg::Odometry message.
+class OdometryPerception : public PerceptionBase
+{
+public:
+  /// \brief Group identifier for odometry perceptions.
+  static constexpr std::string_view default_group_ = "odom";
+
+  /// \brief Returns whether the given ROS 2 type name is supported by this perception.
+  /// \param t Fully qualified message type name (e.g., "nav_msgs/msg/Odometry").
+  /// \return true if \p t equals "nav_msgs/msg/Odometry", otherwise false.
+  static inline bool supports_msg_type(std::string_view t)
+  {
+    return t == "nav_msgs/msg/Odometry";
+  }
+
+  OdometryPerception()
+  {
+    [[maybe_unused]] static const bool _ = [] {
+        ::easynav::NavState::register_printer<OdometryPerception>(
+          [](const OdometryPerception & perception) {
+            std::ostringstream ret;
+            const auto & pose = perception.data.pose.pose;
+            const auto & twist = perception.data.twist.twist;
+            ret << "{ " << perception.stamp.seconds()
+                << " } OdometryPerception pose = ("
+                << pose.position.x << ", "
+                << pose.position.y << ", "
+                << pose.position.z << "), twist = ("
+                << twist.linear.x << ", "
+                << twist.linear.y << ", "
+                << twist.linear.z
+                << ") in frame [" << perception.frame_id
+                << "] with ts " << perception.stamp.seconds() << "\n";
+            return ret.str();
+        });
+        return true;
+      }();
+  }
+
+  /// \brief Odometry data received from the sensor.
+  nav_msgs::msg::Odometry data;
+};
+
+/// \class OdometryPerceptionHandler
+/// \brief Handles the creation and updating of OdometryPerception instances from nav_msgs::msg::Odometry messages.
+///
+/// This class provides methods to register subscriptions to odometry topics and update OdometryPerception objects.
+class OdometryPerceptionHandler : public PerceptionHandler
+{
+public:
+  /// \brief Optional post-initialization hook for subclasses.
+  /// Here, the handler must reserve memory to store the perception data
+  /// and create any Subscription or similar objects to read the data.
+  void on_initialize() override;
+
+  /// @brief Run one real-time sensor processing cycle.
+  /// This method is called by the SensorsNode before executing its cycle_rt.
+  /// Here the handler should update the NavState with the sensor data.
+  /// If new data arrived before this call and the state is updated, it must return true.
+  ///
+  /// @param nav_state Pointer to the NavState to store the sensor data.
+  /// @return True if new data was stored (to trigger processing).
+  bool cycle_rt([[maybe_unused]] std::shared_ptr<NavState> nav_state) override;
+
+private:
+  /// \brief pointer to the perception data
+  std::shared_ptr<OdometryPerception> perception_data_ {nullptr};
+
+  /// \brief pointer to the subscription object
+  rclcpp::SubscriptionBase::SharedPtr perception_sub_;
+
+  /// \brief NavState key to store the odometry value (defaults to sensor name)
+  std::string odom_ns_key_;
+};
+
+/**
+ * @typedef OdometryPerceptions
+ * @brief Alias for a vector of shared pointers to OdometryPerception objects.
+ *
+ * The container can represent a time-ordered or batched collection, depending on producer logic.
+ */
+using OdometryPerceptions =
+  std::vector<std::shared_ptr<OdometryPerception>>;
+
+/// @brief Retrieves the latest timestamp among a set of odometry perceptions.
+/// @param perceptions Container of odometry perceptions.
+/// @return The most recent timestamp found in \p perceptions, or a default-constructed \c rclcpp::Time if \p perceptions is empty.
+rclcpp::Time get_latest_odometry_perceptions_stamp(const OdometryPerceptions & perceptions);
+
+}  // namespace easynav
+
+#endif  // EASYNAV_SENSORS_TYPES__ODOMETRYPERCEPTIONS_HPP_

easynav_sensors/src/easynav_sensors/SensorsNode.cpp

@@ -63,6 +63,7 @@ SensorsNode::SensorsNode(const rclcpp::NodeOptions & options)
     {"sensor_msgs/msg/LaserScan", "easynav_sensors/PointPerceptionHandler"},
     {"sensor_msgs/msg/Imu", "easynav_sensors/IMUPerceptionHandler"},
     {"sensor_msgs/msg/NavSatFix", "easynav_sensors/GNSSPerceptionHandler"},
+    {"nav_msgs/msg/Odometry", "easynav_sensors/OdometryPerceptionHandler"},
     {"sensor_msgs/msg/Image", "easynav_sensors/ImagePerceptionHandler"},
     {"vision_msgs/msg/Detection3DArray", "easynav_sensors/DetectionsPerceptionHandler"},
   };

easynav_sensors/src/easynav_sensors/types/OdometryPerception.cpp

@@ -0,0 +1,110 @@
+// Copyright 2026 Intelligent Robotics Lab
+//
+// 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 <string>
+
+#include "nav_msgs/msg/odometry.hpp"
+
+#include "rclcpp/time.hpp"
+
+#include "easynav_sensors/types/OdometryPerception.hpp"
+
+namespace easynav
+{
+
+void OdometryPerceptionHandler::on_initialize()
+{
+  // Create the perception data instance
+  perception_data_ = std::make_shared<OdometryPerception>();
+
+  // Get sensor parameters
+  auto node = get_node();
+  std::string topic, msg_type;
+
+  if (!node->has_parameter(get_sensor_name() + ".topic")) {
+    node->declare_parameter(get_sensor_name() + ".topic", std::string{});
+  }
+  if (!node->has_parameter(get_sensor_name() + ".type")) {
+    node->declare_parameter(get_sensor_name() + ".type", std::string{});
+  }
+  if (!node->has_parameter(get_sensor_name() + ".nav_state_key")) {
+    node->declare_parameter(get_sensor_name() + ".nav_state_key", get_sensor_name());
+  }
+
+  node->get_parameter(get_sensor_name() + ".topic", topic);
+  node->get_parameter(get_sensor_name() + ".type", msg_type);
+  node->get_parameter(get_sensor_name() + ".nav_state_key", odom_ns_key_);
+
+  // Setup subscription
+  auto options = rclcpp::SubscriptionOptions();
+  options.callback_group = get_realtime_cbg();
+
+  const auto clock_type = node->get_clock()->get_clock_type();
+
+  if (msg_type != "nav_msgs/msg/Odometry") {
+    throw std::runtime_error("Unsupported message type for OdometryPerceptionHandler: " + msg_type);
+  }
+
+  perception_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
+    topic, rclcpp::QoS(1),
+    [this, clock_type](const nav_msgs::msg::Odometry::SharedPtr msg)
+    {
+      perception_data_->stamp = rclcpp::Time(msg->header.stamp, clock_type);
+      perception_data_->frame_id = msg->header.frame_id;
+      perception_data_->new_data = true;
+      perception_data_->data = *msg;
+      perception_data_->valid = true;
+    },
+    options);
+}
+
+bool OdometryPerceptionHandler::cycle_rt(std::shared_ptr<NavState> nav_state)
+{
+  // Store the data in the NavState
+  // NOTE: We store the actual Odometry message, not the Perception object.
+  //       This is so this handler can be a replacement of the localizer plugin.
+  nav_state->set(odom_ns_key_, perception_data_->data);
+  // Check if there was new data to trigger process and reset new_data state
+  const bool should_trigger = perception_data_->new_data;
+  perception_data_->new_data = false;
+  return should_trigger;
+}
+
+rclcpp::Time get_latest_odometry_perceptions_stamp(const OdometryPerceptions & perceptions)
+{
+  auto is_newer = [](const rclcpp::Time & a, const rclcpp::Time & b) {
+      if (a.get_clock_type() == b.get_clock_type()) {
+        return a > b;
+      }
+      return a.nanoseconds() > b.nanoseconds();
+    };
+
+  rclcpp::Time latest_stamp;
+  bool inited = false;
+
+  for (const auto & perception : perceptions) {
+    if (!inited || is_newer(perception->stamp, latest_stamp)) {
+      latest_stamp = perception->stamp;
+      inited = true;
+    }
+  }
+
+  return latest_stamp;
+}
+
+}  // namespace easynav
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(easynav::OdometryPerceptionHandler, easynav::PerceptionHandler)

easynav_sensors/tests/plugin_tests.cpp

@@ -54,6 +54,7 @@ static const std::vector<std::string> kAllPlugins = {
   "easynav_sensors/PointPerceptionHandler",
   "easynav_sensors/IMUPerceptionHandler",
   "easynav_sensors/GNSSPerceptionHandler",
+  "easynav_sensors/OdometryPerceptionHandler",
   "easynav_sensors/ImagePerceptionHandler",
   "easynav_sensors/DetectionsPerceptionHandler",
 };