From 175a197182fc0912a4eef48932a4fa7244f3b137 Mon Sep 17 00:00:00 2001
From: Ussama Naal <606033+Samahu@users.noreply.github.com>
Date: Wed, 19 Apr 2023 12:28:13 -0700
Subject: [PATCH] SW-4837: replace the use of ros service to retrieve sensor
 metadata with latched topics (#102)

* Working port of latched metadata topic on ros2
* Update replay and record launch files to providing metadata file an optional parameter
* Remove extra white space in replay record command
* Undo changes to the metadata-qos-override
* minor code syntax improvements
* Add missing metadata topic when bag file isn't specified
* Use concise syntax and formatting
* Reverse logic for easier read
* Apply node transition if it exists
---
 CHANGELOG.rst                                 |  3 +
 README.md                                     | 15 +++--
 ouster-ros/CMakeLists.txt                     |  2 +
 ouster-ros/config/metadata-qos-override.yaml  |  5 ++
 .../ouster_ros/os_processing_node_base.h      | 20 +++----
 ouster-ros/include/ouster_ros/os_ros.h        |  7 ++-
 .../include/ouster_ros/os_sensor_node_base.h  |  8 ++-
 ouster-ros/launch/record.composite.launch.xml | 11 +++-
 ouster-ros/launch/replay.composite.launch.xml | 19 ++++---
 ouster-ros/package.xml                        |  3 +-
 ouster-ros/src/os_cloud_node.cpp              | 27 +++++----
 ouster-ros/src/os_image_node.cpp              | 12 +++-
 ouster-ros/src/os_processing_node_base.cpp    | 57 +++----------------
 ouster-ros/src/os_replay_node.cpp             |  7 ++-
 ouster-ros/src/os_sensor_node.cpp             |  3 +
 ouster-ros/src/os_sensor_node_base.cpp        | 14 +++++
 16 files changed, 118 insertions(+), 95 deletions(-)
 create mode 100644 ouster-ros/config/metadata-qos-override.yaml

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 75029997..b2e2fafb 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -19,6 +19,9 @@ ouster_ros(2)
 * add frame_id to image topics
 * fixed a potential issue of time values within generated point clouds that could result in a value
   overflow
+* added a new ``/ouster/metadata`` topic that is consumed by os_cloud and os_image nodes and save it
+  to the bag file on record
+* make specifying metadata file optional during record and replay modes as of package version 8.1
 
 ouster_client
 -------------
diff --git a/README.md b/README.md
index 2143eeb1..4ac7ba1e 100644
--- a/README.md
+++ b/README.md
@@ -129,18 +129,25 @@ ros2 launch ouster_ros sensor.launch.xml    \
 ```
 
 #### Recording Mode
+> Note
+> As of package version 8.1, specifiying metadata file is optional since the introduction of the
+> metadata topic
 ```bash
 ros2 launch ouster_ros record.launch.xml    \
     sensor_hostname:=<sensor host name>     \
-    metadata:=<json file name>              \
-    bag_file:=<optional bag file name>
+    bag_file:=<optional bag file name>      \
+    metadata:=<json file name>              # optional
 ```
 
 #### Replay Mode
+> Note
+> As of package version 8.1, specifiying metadata file is optional if the bag file being replayed
+> already contains the metadata topic
+
 ```bash
 ros2 launch ouster_ros replay.launch.xml    \
-    metadata:=<json file name>              \
-    bag_file:=<path to rosbag file>
+    bag_file:=<path to rosbag file>         \
+    metadata:=<json file name>              # optional if bag file has /metadata topic
 ```
 
 #### Multicast Mode (experimental)
diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt
index e24829ca..d7f70c4a 100644
--- a/ouster-ros/CMakeLists.txt
+++ b/ouster-ros/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(ouster_msgs REQUIRED)
@@ -101,6 +102,7 @@ function(create_ros2_component
     class_loader
     rclcpp_components
     rclcpp_lifecycle
+    std_msgs
     sensor_msgs
     geometry_msgs
     ${additonal_dependencies}
diff --git a/ouster-ros/config/metadata-qos-override.yaml b/ouster-ros/config/metadata-qos-override.yaml
new file mode 100644
index 00000000..c11e521f
--- /dev/null
+++ b/ouster-ros/config/metadata-qos-override.yaml
@@ -0,0 +1,5 @@
+/ouster/metadata:
+  history: keep_last
+  depth: 1
+  reliability: reliable
+  durability: transient_local
diff --git a/ouster-ros/include/ouster_ros/os_processing_node_base.h b/ouster-ros/include/ouster_ros/os_processing_node_base.h
index 1a0cbeef..66237edd 100644
--- a/ouster-ros/include/ouster_ros/os_processing_node_base.h
+++ b/ouster-ros/include/ouster_ros/os_processing_node_base.h
@@ -11,8 +11,7 @@
 
 #include <chrono>
 #include <rclcpp/rclcpp.hpp>
-
-#include "ouster_srvs/srv/get_metadata.hpp"
+#include <std_msgs/msg/string.hpp>
 
 namespace ouster_ros {
 
@@ -22,24 +21,19 @@ class OusterProcessingNodeBase : public rclcpp::Node {
                                       const rclcpp::NodeOptions& options)
         : rclcpp::Node(name, options) {}
 
-   protected:
-    bool spin_till_attempts_exahused(const std::string& log_msg,
-                                     std::function<bool(void)> lambda);
-
-    std::string get_metadata();
+    void create_metadata_subscriber(
+        std::function<void(const std_msgs::msg::String::ConstSharedPtr&)>
+            on_sensor_metadata);
 
-    int get_n_returns();
+    int get_n_returns() const;
 
-    std::string topic_for_return(std::string base, int idx) {
+    static std::string topic_for_return(std::string base, int idx) {
         if (idx == 0) return base;
         return base + std::to_string(idx + 1);
     }
 
    protected:
-    // TODO: Add as node parameters?
-    static constexpr auto wait_time_per_attempt = std::chrono::seconds(10);
-    static constexpr auto total_attempts = 10;
-
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr metadata_sub;
     ouster::sensor::sensor_info info;
 };
 
diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h
index 48d933cb..7f17cd0d 100644
--- a/ouster-ros/include/ouster_ros/os_ros.h
+++ b/ouster-ros/include/ouster_ros/os_ros.h
@@ -84,7 +84,8 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm,
 [[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud_f(
     ouster::PointsF& points, const ouster::PointsF& lut_direction,
     const ouster::PointsF& lut_offset, std::chrono::nanoseconds scan_ts,
-    const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, int return_index);
+    const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud,
+    int return_index);
 
 /**
  * Populate a PCL point cloud from a LidarScan.
@@ -101,8 +102,8 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm,
 void scan_to_cloud_f(ouster::PointsF& points,
                      const ouster::PointsF& lut_direction,
                      const ouster::PointsF& lut_offset, uint64_t scan_ts,
-                     const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud,
-                     int return_index);
+                     const ouster::LidarScan& lidar_scan,
+                     ouster_ros::Cloud& cloud, int return_index);
 
 /**
  * Serialize a PCL point cloud to a ROS message
diff --git a/ouster-ros/include/ouster_ros/os_sensor_node_base.h b/ouster-ros/include/ouster_ros/os_sensor_node_base.h
index fd175adb..8ec78e07 100644
--- a/ouster-ros/include/ouster_ros/os_sensor_node_base.h
+++ b/ouster-ros/include/ouster_ros/os_sensor_node_base.h
@@ -11,6 +11,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <std_msgs/msg/string.hpp>
 
 #include "ouster_srvs/srv/get_metadata.hpp"
 
@@ -23,18 +24,23 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode {
         : rclcpp_lifecycle::LifecycleNode(name, options) {}
 
    protected:
-    bool is_arg_set(const std::string& arg) {
+    bool is_arg_set(const std::string& arg) const {
         return arg.find_first_not_of(' ') != std::string::npos;
     }
 
     void create_get_metadata_service();
 
+    void create_metadata_publisher();
+
+    void publish_metadata();
+
     void display_lidar_info(const ouster::sensor::sensor_info& info);
 
    protected:
     ouster::sensor::sensor_info info;
     rclcpp::Service<ouster_srvs::srv::GetMetadata>::SharedPtr get_metadata_srv;
     std::string cached_metadata;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr metadata_pub;
 };
 
 }  // namespace ouster_ros
\ No newline at end of file
diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml
index 9d6fe01d..d219d093 100644
--- a/ouster-ros/launch/record.composite.launch.xml
+++ b/ouster-ros/launch/record.composite.launch.xml
@@ -49,6 +49,8 @@
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterSensor" name="os_sensor">
         <param name="sensor_hostname" value="$(var sensor_hostname)"/>
         <param name="udp_dest" value="$(var udp_dest)"/>
+        <param name="mtp_dest" value=""/>
+        <param name="mtp_main" value="false"/>
         <param name="lidar_port" value="$(var lidar_port)"/>
         <param name="imu_port" value="$(var imu_port)"/>
         <param name="udp_profile_lidar" value="$(var udp_profile_lidar)"/>
@@ -83,9 +85,14 @@
 
   <executable if="$(var _use_bag_file_name)" output="screen"
     cmd="ros2 bag record --output $(var bag_file)
-         /$(var ouster_ns)/imu_packets /$(var ouster_ns)/lidar_packets"/>
+      /$(var ouster_ns)/imu_packets
+      /$(var ouster_ns)/lidar_packets
+      /$(var ouster_ns)/metadata"/>
 
   <executable unless="$(var _use_bag_file_name)" output="screen"
-    cmd="ros2 bag record /$(var ouster_ns)/imu_packets /$(var ouster_ns)/lidar_packets"/>
+    cmd="ros2 bag record
+      /$(var ouster_ns)/imu_packets
+      /$(var ouster_ns)/lidar_packets
+      /$(var ouster_ns)/metadata"/>
 
 </launch>
diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml
index 84a60a20..2941ba30 100644
--- a/ouster-ros/launch/replay.composite.launch.xml
+++ b/ouster-ros/launch/replay.composite.launch.xml
@@ -10,7 +10,7 @@
     TIME_FROM_PTP_1588,
     TIME_FROM_ROS_TIME
     }"/>
-  <arg name="metadata"
+  <arg name="metadata" default=""
     description="path to write metadata file when receiving sensor data"/>
   <arg name="bag_file"
     description="file name to use for the recorded bag file"/>
@@ -20,12 +20,14 @@
     description="optional rviz config file"/>
   <arg name="tf_prefix" default="" description="namespace for tf transforms"/>
 
+  <let name="_use_metadata_file" value="$(eval '\'$(var metadata)\' != \'\'')"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
+    <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_replay" name="os_replay" output="screen">
+      <param name="metadata" value="$(var metadata)"/>
+    </node>
     <node_container pkg="rclcpp_components" exec="component_container_mt" name="os_container" output="screen" namespace="">
-      <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterReplay" name="os_replay">
-        <param name="metadata" value="$(var metadata)"/>
-      </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterCloud" name="os_cloud">
         <param name="tf_prefix" value="$(var tf_prefix)"/>
         <param name="timestamp_mode" value="$(var timestamp_mode)"/>
@@ -36,9 +38,11 @@
 
   <!-- HACK: configure and activate the replay node via a process execute since state
     transition is currently not availabe through launch.xml format -->
-  <executable cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_replay configure"
+  <executable if="$(var _use_metadata_file)"
+    cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_replay configure"
     launch-prefix="bash -c 'sleep 0; $0 $@'" output="screen"/>
-  <executable cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_replay activate"
+  <executable if="$(var _use_metadata_file)"
+    cmd="$(find-exec ros2) lifecycle set /$(var ouster_ns)/os_replay activate"
     launch-prefix="bash -c 'sleep 1; $0 $@'" output="screen"/>
 
   <include if="$(var viz)" file="$(find-pkg-share ouster_ros)/launch/rviz.launch.xml">
@@ -51,6 +55,7 @@
 
   <executable if="$(var _use_bag_file_name)" output="screen"
     launch-prefix="bash -c 'sleep 3; $0 $@'"
-    cmd="ros2 bag play $(var bag_file) --clock"/>
+    cmd="ros2 bag play $(var bag_file) --clock
+      --qos-profile-overrides-path $(find-pkg-share ouster_ros)/config/metadata-qos-override.yaml"/>
 
 </launch>
diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml
index c0a715b1..e6855890 100644
--- a/ouster-ros/package.xml
+++ b/ouster-ros/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.8.0</version>
+  <version>0.8.1</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>
@@ -13,6 +13,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>tf2_ros</depend>
diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp
index 2d1040e1..d4a451c4 100644
--- a/ouster-ros/src/os_cloud_node.cpp
+++ b/ouster-ros/src/os_cloud_node.cpp
@@ -29,7 +29,6 @@
 
 namespace sensor = ouster::sensor;
 using ouster_msgs::msg::PacketMsg;
-using ouster_srvs::srv::GetMetadata;
 
 namespace {
 
@@ -86,15 +85,9 @@ class OusterCloud : public OusterProcessingNodeBase {
     void on_init() {
         declare_parameters();
         parse_parameters();
-        auto metadata = get_metadata();
-        info = sensor::parse_metadata(metadata);
-        n_returns = get_n_returns();
-        create_lidarscan_objects();
-        compute_scan_ts = [this](const auto& ts_v) {
-            return compute_scan_ts_0(ts_v);
-        };
-        create_publishers();
-        create_subscriptions();
+        create_metadata_subscriber(
+            [this](const auto& msg) { metadata_handler(msg); });
+        RCLCPP_INFO(get_logger(), "OusterCloud: node initialized!");
     }
 
     void declare_parameters() {
@@ -120,6 +113,20 @@ class OusterCloud : public OusterProcessingNodeBase {
         return one_sec_in_ns / (scan_width * scan_frequency);
     }
 
+    void metadata_handler(
+        const std_msgs::msg::String::ConstSharedPtr& metadata_msg) {
+        RCLCPP_INFO(get_logger(),
+                    "OusterCloud: retrieved new sensor metadata!");
+        info = sensor::parse_metadata(metadata_msg->data);
+        n_returns = get_n_returns();
+        create_lidarscan_objects();
+        compute_scan_ts = [this](const auto& ts_v) {
+            return compute_scan_ts_0(ts_v);
+        };
+        create_publishers();
+        create_subscriptions();
+    }
+
     void create_lidarscan_objects() {
         // The ouster_ros drive currently only uses single precision when it
         // produces the point cloud. So it isn't of a benefit to compute point
diff --git a/ouster-ros/src/os_image_node.cpp b/ouster-ros/src/os_image_node.cpp
index 522d2d77..4289adf8 100644
--- a/ouster-ros/src/os_image_node.cpp
+++ b/ouster-ros/src/os_image_node.cpp
@@ -36,7 +36,6 @@
 
 namespace sensor = ouster::sensor;
 namespace viz = ouster::viz;
-using ouster_srvs::srv::GetMetadata;
 
 using pixel_type = uint16_t;
 const size_t pixel_value_max = std::numeric_limits<pixel_type>::max();
@@ -53,8 +52,15 @@ class OusterImage : public OusterProcessingNodeBase {
 
    private:
     void on_init() {
-        auto metadata = get_metadata();
-        info = sensor::parse_metadata(metadata);
+        create_metadata_subscriber(
+            [this](const auto& msg) { metadata_handler(msg); });
+        RCLCPP_INFO(get_logger(), "OusterImage: node initialized!");
+    }
+
+    void metadata_handler(const std_msgs::msg::String::ConstPtr& metadata_msg) {
+        RCLCPP_INFO(get_logger(),
+                    "OusterImage: retrieved new sensor metadata!");
+        info = sensor::parse_metadata(metadata_msg->data);
         create_cloud_object();
         const int n_returns = get_n_returns();
         create_publishers(n_returns);
diff --git a/ouster-ros/src/os_processing_node_base.cpp b/ouster-ros/src/os_processing_node_base.cpp
index 4024620b..a066980d 100644
--- a/ouster-ros/src/os_processing_node_base.cpp
+++ b/ouster-ros/src/os_processing_node_base.cpp
@@ -8,60 +8,21 @@
 
 #include "ouster_ros/os_processing_node_base.h"
 
-using namespace std::chrono_literals;
 using ouster::sensor::UDPProfileLidar;
-using ouster_srvs::srv::GetMetadata;
-using rclcpp::FutureReturnCode;
 
 namespace ouster_ros {
 
-bool OusterProcessingNodeBase::spin_till_attempts_exahused(
-    const std::string& log_msg, std::function<bool(void)> lambda) {
-    int remaining_attempts = total_attempts;
-    bool done;
-    do {
-        RCLCPP_INFO_STREAM(get_logger(),
-                           log_msg << "; attempt no: "
-                                   << total_attempts - remaining_attempts + 1
-                                   << "/" << total_attempts);
-        done = lambda();
-    } while (rclcpp::ok() && !done && --remaining_attempts > 0);
-    return done;
+void OusterProcessingNodeBase::create_metadata_subscriber(
+    std::function<void(const std_msgs::msg::String::ConstSharedPtr&)>
+        on_sensor_metadata) {
+    auto latching_qos = rclcpp::QoS(rclcpp::KeepLast(1));
+    latching_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    latching_qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+    metadata_sub = create_subscription<std_msgs::msg::String>(
+        "metadata", latching_qos, on_sensor_metadata);
 }
 
-std::string OusterProcessingNodeBase::get_metadata() {
-    auto client = create_client<GetMetadata>("get_metadata");
-    if (!spin_till_attempts_exahused(
-            "contacting get_metadata service", [this, &client]() -> bool {
-                return client->wait_for_service(wait_time_per_attempt);
-            })) {
-        auto error_msg = "get_metadata service is unavailable";
-        RCLCPP_ERROR_STREAM(get_logger(), error_msg);
-        throw std::runtime_error(error_msg);
-    }
-    auto request = std::make_shared<GetMetadata::Request>();
-    auto result = client->async_send_request(request);
-
-    rclcpp::FutureReturnCode return_code;
-    spin_till_attempts_exahused(
-        "waiting for get_metadata service to respond",
-        [this, &return_code, &result]() -> bool {
-            return_code = rclcpp::spin_until_future_complete(
-                get_node_base_interface(), result, wait_time_per_attempt);
-            return return_code != FutureReturnCode::TIMEOUT;
-        });
-
-    if (return_code != FutureReturnCode::SUCCESS) {
-        auto error_msg = "get_metadata service timed out or interrupted";
-        RCLCPP_ERROR_STREAM(get_logger(), error_msg);
-        throw std::runtime_error(error_msg);
-    }
-
-    RCLCPP_INFO(get_logger(), "retrieved sensor metadata!");
-    return result.get()->metadata;
-}
-
-int OusterProcessingNodeBase::get_n_returns() {
+int OusterProcessingNodeBase::get_n_returns() const {
     return info.format.udp_profile_lidar ==
                    UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL
                ? 2
diff --git a/ouster-ros/src/os_replay_node.cpp b/ouster-ros/src/os_replay_node.cpp
index 831d8942..c13b1b24 100644
--- a/ouster-ros/src/os_replay_node.cpp
+++ b/ouster-ros/src/os_replay_node.cpp
@@ -30,7 +30,9 @@ class OusterReplay : public OusterSensorNodeBase {
 
         try {
             auto meta_file = parse_parameters();
-            populate_metadata_from_json(meta_file);
+            create_metadata_publisher();
+            load_metadata_from_file(meta_file);
+            publish_metadata();
             create_get_metadata_service();
             RCLCPP_INFO(get_logger(), "Running in replay mode");
         } catch (const std::exception& ex) {
@@ -110,7 +112,7 @@ class OusterReplay : public OusterSensorNodeBase {
         return meta_file;
     }
 
-    void populate_metadata_from_json(const std::string& meta_file) {
+    void load_metadata_from_file(const std::string& meta_file) {
         try {
             std::ifstream in_file(meta_file);
             std::stringstream buffer;
@@ -129,7 +131,6 @@ class OusterReplay : public OusterSensorNodeBase {
     void cleanup() {
         get_metadata_srv.reset();
     }
-
 };
 
 }  // namespace ouster_ros
diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp
index 08de1be1..ffe3c716 100644
--- a/ouster-ros/src/os_sensor_node.cpp
+++ b/ouster-ros/src/os_sensor_node.cpp
@@ -63,7 +63,9 @@ class OusterSensor : public OusterSensorNodeBase {
             sensor_client = create_sensor_client(sensor_hostname, config);
             if (!sensor_client)
                 return LifecycleNodeInterface::CallbackReturn::FAILURE;
+            create_metadata_publisher();
             update_config_and_metadata(*sensor_client);
+            publish_metadata();
             save_metadata();
             create_reset_service();
             create_get_metadata_service();
@@ -340,6 +342,7 @@ class OusterSensor : public OusterSensorNodeBase {
         connection_loop_timer->cancel();
         reset_last_init_id = init_id_reset;
         update_config_and_metadata(*sensor_client);
+        publish_metadata();
         save_metadata();
         auto request_transitions = std::vector<uint8_t>{
             lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE,
diff --git a/ouster-ros/src/os_sensor_node_base.cpp b/ouster-ros/src/os_sensor_node_base.cpp
index faee0154..729a28f4 100644
--- a/ouster-ros/src/os_sensor_node_base.cpp
+++ b/ouster-ros/src/os_sensor_node_base.cpp
@@ -28,6 +28,20 @@ void OusterSensorNodeBase::create_get_metadata_service() {
     RCLCPP_INFO(get_logger(), "get_metadata service created");
 }
 
+void OusterSensorNodeBase::create_metadata_publisher() {
+    auto latching_qos = rclcpp::QoS(rclcpp::KeepLast(1));
+    latching_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    latching_qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+    metadata_pub =
+        create_publisher<std_msgs::msg::String>("metadata", latching_qos);
+}
+
+void OusterSensorNodeBase::publish_metadata() {
+    std_msgs::msg::String metadata_msg;
+    metadata_msg.data = cached_metadata;
+    metadata_pub->publish(metadata_msg);
+}
+
 void OusterSensorNodeBase::display_lidar_info(const sensor::sensor_info& info) {
     auto lidar_profile = info.format.udp_profile_lidar;
     RCLCPP_INFO_STREAM(