SW-4837: replace the use of ros service to retrieve sensor metadata w…

…ith 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
ouster-lidar · Apr 19, 2023 · 175a197 · 175a197
1 parent 592e316
commit 175a197
Show file tree

Hide file tree

Showing 16 changed files with 118 additions and 95 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
diff --git 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
@@ -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
@@ -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="[email protected]">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
@@ -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
@@ -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
@@ -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