SW-6906: publish sensor telemetry in ouster ros (#413)

* Refactor packet time handling code in ouster-ros * Always write the timestamp value even when no point is null * Define and implement the telemetry msg
ouster-lidar · Jan 18, 2025 · c4b4f87 · c4b4f87
1 parent 0469977
commit c4b4f87
Show file tree

Hide file tree

Showing 19 changed files with 431 additions and 240 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -9,6 +9,8 @@ Changelog
 * Added support to enable **loop** for pcap replay + other replay config
 * Added a new launch file parameter ``pub_static_tf`` that allows users to turn off the broadcast
   of sensor TF transforms.
+* Introduced a new topic ``/ouster/telemetry`` that publishes ``ouster_ros::Telemetry`` messages,
+  the topic can be turned on/off by including the token ``TLM`` in the flag ``proc_mask`` launch arg.
 
 
 ouster_ros v0.13.0

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -31,7 +31,7 @@ add_compile_options(-Wall -Wextra)
 option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON)
 
 # ==== Catkin ====
-add_message_files(FILES PacketMsg.msg)
+add_message_files(FILES PacketMsg.msg Telemetry.msg)
 add_service_files(FILES GetConfig.srv SetConfig.srv GetMetadata.srv)
 generate_messages(DEPENDENCIES std_msgs sensor_msgs geometry_msgs)
 

diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h
@@ -24,6 +24,7 @@
 #include <string>
 
 #include "ouster_ros/PacketMsg.h"
+#include "ouster_ros/Telemetry.h"
 #include "ouster_ros/os_point.h"
 
 namespace ouster_ros {
@@ -116,6 +117,20 @@ sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg(
     const uint16_t ring, const std::vector<int>& pixel_shift_by_row,
     const int return_index);
 
+
+/**
+ * Parse a LidarPacket and generate the Telemetry message
+ * @param[in] lidar_packet lidar packet to parse telemetry data from
+ * @param[in] timestamp the timestamp to give the resulting ROS message
+ * @param[in] pf the packet format
+ * @return ROS sensor message with fields populated from the packet
+ */
+Telemetry lidar_packet_to_telemetry_msg(
+    const ouster::sensor::LidarPacket& lidar_packet,
+    const ros::Time& timestamp,
+    const ouster::sensor::packet_format& pf);
+
+
 namespace impl {
 sensor::ChanField suitable_return(sensor::ChanField input_field, bool second);
 

diff --git a/launch/driver.launch b/launch/driver.launch
@@ -56,7 +56,7 @@
   <arg if="$(arg no_bond)" name="_no_bond" value="--no-bond"/>
   <arg unless="$(arg no_bond)" name="_no_bond" value=" "/>
 
-  <arg name="proc_mask" default="IMU|PCL|SCAN|IMG|RAW" doc="
+  <arg name="proc_mask" default="IMU|PCL|SCAN|IMG|RAW|TLM" doc="
     use any combination of the 4 flags to enable or disable specific processors"/>
 
   <arg name="scan_ring" default="0" doc="

diff --git a/launch/record.launch b/launch/record.launch
@@ -54,7 +54,7 @@
   <arg name="dynamic_transforms_broadcast_rate" default="1.0"
     doc="set the rate (Hz) of broadcast when using dynamic broadcast; minimum value is 1 Hz"/>
 
-  <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" doc="
+  <arg name="proc_mask" default="IMG|PCL|IMU|SCAN|TLM" doc="
     The IMG flag here is not supported and does not affect anything,
     to disable image topics you would need to omit the os_image node
     from the launch file"/>
@@ -116,7 +116,10 @@
       <param name="~/udp_profile_lidar" type="str" value="$(arg udp_profile_lidar)"/>
       <param name="~/lidar_mode" type="str" value="$(arg lidar_mode)"/>
       <param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
+      <param name="~/ptp_utc_tai_offset" type="double"
+        value="$(arg ptp_utc_tai_offset)"/>
       <param name="~/metadata" type="str" value="$(arg metadata)"/>
+      <param name="~/proc_mask" type="str" value="$(arg proc_mask)"/>
       <param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
       <param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
       <param name="~/persist_config" value="$(arg persist_config)"/>

diff --git a/launch/sensor.launch b/launch/sensor.launch
@@ -62,7 +62,7 @@
   <arg name="dynamic_transforms_broadcast_rate" default="1.0"
     doc="set the rate (Hz) of broadcast when using dynamic broadcast; minimum value is 1 Hz"/>
 
-  <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" doc="
+  <arg name="proc_mask" default="IMG|PCL|IMU|SCAN|TLM" doc="
     The IMG flag here is not supported and does not affect anything,
     to disable image topics you would need to omit the os_image node
     from the launch file"/>
@@ -123,7 +123,10 @@
       <param name="~/udp_profile_lidar" type="str" value="$(arg udp_profile_lidar)"/>
       <param name="~/lidar_mode" type="str" value="$(arg lidar_mode)"/>
       <param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
+      <param name="~/ptp_utc_tai_offset" type="double"
+        value="$(arg ptp_utc_tai_offset)"/>
       <param name="~/metadata" type="str" value="$(arg metadata)"/>
+      <param name="~/proc_mask" type="str" value="$(arg proc_mask)"/>
       <param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
       <param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
       <param name="~/persist_config" value="$(arg persist_config)"/>

diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch
@@ -66,7 +66,7 @@
   <arg name="dynamic_transforms_broadcast_rate" default="1.0"
     doc="set the rate (Hz) of broadcast when using dynamic broadcast; minimum value is 1 Hz"/>
 
-  <arg name="proc_mask" default="IMG|PCL|IMU|SCAN" doc="
+  <arg name="proc_mask" default="IMG|PCL|IMU|SCAN|TLM" doc="
     The IMG flag here is not supported and does not affect anything,
     to disable image topics you would need to omit the os_image node
     from the launch file"/>
@@ -129,7 +129,10 @@
       <param name="~/udp_profile_lidar" type="str" value="$(arg udp_profile_lidar)"/>
       <param name="~/lidar_mode" type="str" value="$(arg lidar_mode)"/>
       <param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
+      <param name="~/ptp_utc_tai_offset" type="double"
+        value="$(arg ptp_utc_tai_offset)"/>
       <param name="~/metadata" type="str" value="$(arg metadata)"/>
+      <param name="~/proc_mask" type="str" value="$(arg proc_mask)"/>
       <param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
       <param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
       <param name="~/persist_config" value="$(arg persist_config)"/>

diff --git a/msg/Telemetry.msg b/msg/Telemetry.msg
@@ -0,0 +1,9 @@
+# This message defines the telemetry data for Ouster sensors.
+# Message header
+std_msgs/Header header
+# Telemetry fields for more information on these fields and their meaning, please review:
+# https://static.ouster.dev/sensor-docs/image_route1/image_route2/thermal_int_guide/therm_int_guide.html
+uint16 countdown_thermal_shutdown # the thermal shutdown countdown.
+uint16 countdown_shot_limiting    # the shot limiting countdown.
+uint8 thermal_shutdown            # the thermal shutdown status. 0 = NORMAL, 1 = SHUTDOWN IMMINENT.
+uint8 shot_limiting               # the shot limiting status. 0 = NORMAL OPERATION.
diff --git a/package.xml b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.13.2</version>
+  <version>0.13.3</version>
   <description>Ouster ROS driver</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

diff --git a/src/imu_packet_handler.h b/src/imu_packet_handler.h
@@ -22,7 +22,7 @@ class ImuPacketHandler {
     using HandlerType = std::function<HandlerOutput(const sensor::ImuPacket&)>;
 
    public:
-    static HandlerType create_handler(const sensor::sensor_info& info,
+    static HandlerType create(const sensor::sensor_info& info,
                                       const std::string& frame,
                                       const std::string& timestamp_mode,
                                       int64_t ptp_utc_tai_offset) {

diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h
@@ -57,7 +57,8 @@ using LidarScanProcessor =
     std::function<void(const ouster::LidarScan&, uint64_t, const ros::Time&)>;
 
 class LidarPacketHandler {
-    using LidarPacketAccumlator = std::function<bool(const sensor::LidarPacket&)>;
+    using LidarPacketAccumlator =
+        std::function<bool(const sensor::LidarPacket&)>;
 
    public:
     using HandlerOutput = ouster::LidarScan;
@@ -69,7 +70,9 @@ class LidarPacketHandler {
                        const std::vector<LidarScanProcessor>& handlers,
                        const std::string& timestamp_mode,
                        int64_t ptp_utc_tai_offset)
-        : ring_buffer(LIDAR_SCAN_COUNT), lidar_scan_handlers{handlers} {
+        : ring_buffer(LIDAR_SCAN_COUNT),
+          lidar_scan_handlers{handlers},
+          ptp_utc_tai_offset_(ptp_utc_tai_offset) {
         // initialize lidar_scan processor and buffer
         scan_batcher = std::make_unique<ouster::ScanBatcher>(info);
 
@@ -98,23 +101,39 @@ class LidarPacketHandler {
 
         const sensor::packet_format& pf = sensor::get_format(info);
 
+        std::function<bool(LidarPacketHandler&, const sensor::packet_format&,
+                           const sensor::LidarPacket&, ouster::LidarScan&)>
+            lidar_handler;
+
         if (timestamp_mode == "TIME_FROM_ROS_TIME") {
-            lidar_packet_accumlator =
-                LidarPacketAccumlator{[this, pf](const sensor::LidarPacket& lidar_packet) {
-                    return lidar_handler_ros_time(pf, lidar_packet);
-                }};
+            lidar_handler =
+                std::mem_fn(&LidarPacketHandler::lidar_handler_ros_time);
         } else if (timestamp_mode == "TIME_FROM_PTP_1588") {
-            lidar_packet_accumlator = LidarPacketAccumlator{
-                [this, pf, ptp_utc_tai_offset](const sensor::LidarPacket& lidar_packet) {
-                    return lidar_handler_sensor_time_ptp(pf, lidar_packet,
-                                                         ptp_utc_tai_offset);
-                }};
-        } else {
-            lidar_packet_accumlator =
-                LidarPacketAccumlator{[this, pf](const sensor::LidarPacket& lidar_packet) {
-                    return lidar_handler_sensor_time(pf, lidar_packet);
-                }};
+            lidar_handler =
+                std::mem_fn(&LidarPacketHandler::lidar_handler_sensor_time_ptp);
+        } else /*SENSOR TIME (INTERNAL_OSC, SYNC_PULSE_IN)*/ {
+            lidar_handler =
+                std::mem_fn(&LidarPacketHandler::lidar_handler_sensor_time);
         }
+
+        lidar_packet_accumlator = LidarPacketAccumlator{
+            [this, pf, lidar_handler](const sensor::LidarPacket& lidar_packet) {
+                if (ring_buffer.full()) {
+                    NODELET_WARN("lidar_scans full, DROPPING PACKET");
+                    return false;
+                }
+                bool result = false;
+                {
+                    std::unique_lock<std::mutex> lock(
+                        *(mutexes[ring_buffer.write_head()]));
+                    auto& lidar_scan = *lidar_scans[ring_buffer.write_head()];
+                    result = lidar_handler(*this, pf, lidar_packet, lidar_scan);
+                }
+                if (result) {
+                    ring_buffer.write();
+                }
+                return result;
+            }};
     }
 
     LidarPacketHandler(const LidarPacketHandler&) = delete;
@@ -134,7 +153,7 @@ class LidarPacketHandler {
     void clear_registered_lidar_scan_handlers() { lidar_scan_handlers.clear(); }
 
    public:
-    static HandlerType create_handler(
+    static HandlerType create(
         const sensor::sensor_info& info,
         const std::vector<LidarScanProcessor>& handlers,
         const std::string& timestamp_mode, int64_t ptp_utc_tai_offset) {
@@ -150,13 +169,11 @@ class LidarPacketHandler {
     const std::string getName() const { return "lidar_packet_hander"; }
 
     void process_scans() {
-
         {
             using namespace std::chrono;
             std::unique_lock<std::mutex> index_lock(ring_buffer_mutex);
-            ring_buffer_has_elements.wait_for(index_lock, 1s, [this] {
-                return !ring_buffer.empty();
-            });
+            ring_buffer_has_elements.wait_for(
+                index_lock, 1s, [this] { return !ring_buffer.empty(); });
 
             if (ring_buffer.empty()) return;
         }
@@ -165,14 +182,14 @@ class LidarPacketHandler {
 
         for (auto h : lidar_scan_handlers) {
             h(*lidar_scans[ring_buffer.read_head()], lidar_scan_estimated_ts,
-                lidar_scan_estimated_msg_ts);
+              lidar_scan_estimated_msg_ts);
         }
 
-        // why we hit percent amount of the ring_buffer capacity throttle
+        // when we hit percent amount of the ring_buffer capacity throttle
         size_t read_step = 1;
         if (ring_buffer.size() > THROTTLE_PERCENT * ring_buffer.capacity()) {
             NODELET_WARN("lidar_scans %d%% full, THROTTLING",
-                         static_cast<int>(100* THROTTLE_PERCENT));
+                         static_cast<int>(100 * THROTTLE_PERCENT));
             read_step = 2;
         }
         ring_buffer.read(read_step);
@@ -265,75 +282,48 @@ class LidarPacketHandler {
     }
 
     bool lidar_handler_sensor_time(const sensor::packet_format&,
-                                   const sensor::LidarPacket& lidar_packet) {
-
-        if (ring_buffer.full()) {
-            NODELET_WARN("lidar_scans full, DROPPING PACKET");
-            return false;
-        }
-
-        std::unique_lock<std::mutex> lock(*(mutexes[ring_buffer.write_head()]));
-
-        if (!(*scan_batcher)(lidar_packet, *lidar_scans[ring_buffer.write_head()])) return false;
-        lidar_scan_estimated_ts = compute_scan_ts(lidar_scans[ring_buffer.write_head()]->timestamp());
-        lidar_scan_estimated_msg_ts = impl::ts_to_ros_time(lidar_scan_estimated_ts);
-
-        ring_buffer.write();
+                                   const sensor::LidarPacket& lidar_packet,
+                                   ouster::LidarScan& lidar_scan) {
+        if (!(*scan_batcher)(lidar_packet, lidar_scan)) return false;
+        lidar_scan_estimated_ts = compute_scan_ts(lidar_scan.timestamp());
+        lidar_scan_estimated_msg_ts =
+            impl::ts_to_ros_time(lidar_scan_estimated_ts);
 
         return true;
     }
 
     bool lidar_handler_sensor_time_ptp(const sensor::packet_format&,
                                        const sensor::LidarPacket& lidar_packet,
-                                       int64_t ptp_utc_tai_offset) {
-
-        if (ring_buffer.full()) {
-            NODELET_WARN("lidar_scans full, DROPPING PACKET");
-            return false;
-        }
-
-        std::unique_lock<std::mutex> lock(
-            *(mutexes[ring_buffer.write_head()]));
-
-        if (!(*scan_batcher)(lidar_packet, *lidar_scans[ring_buffer.write_head()])) return false;
-        auto ts_v = lidar_scans[ring_buffer.write_head()]->timestamp();
+                                       ouster::LidarScan& lidar_scan) {
+        if (!(*scan_batcher)(lidar_packet, lidar_scan)) return false;
+        auto ts_v = lidar_scan.timestamp();
         for (int i = 0; i < ts_v.rows(); ++i)
-            ts_v[i] = impl::ts_safe_offset_add(ts_v[i], ptp_utc_tai_offset);
+            ts_v[i] = impl::ts_safe_offset_add(ts_v[i], ptp_utc_tai_offset_);
         lidar_scan_estimated_ts = compute_scan_ts(ts_v);
         lidar_scan_estimated_msg_ts =
             impl::ts_to_ros_time(lidar_scan_estimated_ts);
 
-        ring_buffer.write();
-
         return true;
     }
 
     bool lidar_handler_ros_time(const sensor::packet_format& pf,
-                                const sensor::LidarPacket& lidar_packet) {
-        auto packet_receive_time = impl::ts_to_ros_time(lidar_packet.host_timestamp);
-        if (!lidar_handler_ros_time_frame_ts_initialized) {
-            lidar_handler_ros_time_frame_ts = extrapolate_frame_ts(
-                pf, lidar_packet.buf.data(), packet_receive_time);  // first point cloud time
-            lidar_handler_ros_time_frame_ts_initialized = true;
-        }
+                                const sensor::LidarPacket& lidar_packet,
+                                ouster::LidarScan& lidar_scan) {
+        auto packet_receive_time =
+            impl::ts_to_ros_time(lidar_packet.host_timestamp);
 
-        if (ring_buffer.full()) {
-            NODELET_WARN("lidar_scans full, DROPPING PACKET");
-            return false;
+        if (!lidar_handler_ros_time_frame_ts) {
+            lidar_handler_ros_time_frame_ts = extrapolate_frame_ts(
+                pf, lidar_packet.buf.data(),
+                packet_receive_time);  // first point cloud time
         }
 
-        std::unique_lock<std::mutex> lock(
-            *(mutexes[ring_buffer.write_head()]));
-
-        if (!(*scan_batcher)(lidar_packet, *lidar_scans[ring_buffer.write_head()])) return false;
-        lidar_scan_estimated_ts = compute_scan_ts(lidar_scans[ring_buffer.write_head()]->timestamp());
-        lidar_scan_estimated_msg_ts = lidar_handler_ros_time_frame_ts;
+        if (!(*scan_batcher)(lidar_packet, lidar_scan)) return false;
+        lidar_scan_estimated_ts = compute_scan_ts(lidar_scan.timestamp());
+        lidar_scan_estimated_msg_ts = lidar_handler_ros_time_frame_ts.value();
         // set time for next point cloud msg
-        lidar_handler_ros_time_frame_ts =
-            extrapolate_frame_ts(pf, lidar_packet.buf.data(), packet_receive_time);
-
-        ring_buffer.write();
-
+        lidar_handler_ros_time_frame_ts = extrapolate_frame_ts(
+            pf, lidar_packet.buf.data(), packet_receive_time);
         return true;
     }
 
@@ -356,8 +346,7 @@ class LidarPacketHandler {
     uint64_t lidar_scan_estimated_ts;
     ros::Time lidar_scan_estimated_msg_ts;
 
-    bool lidar_handler_ros_time_frame_ts_initialized = false;
-    ros::Time lidar_handler_ros_time_frame_ts;
+    std::optional<ros::Time> lidar_handler_ros_time_frame_ts;
 
     int last_scan_last_nonzero_idx = -1;
     uint64_t last_scan_last_nonzero_value = 0;
@@ -375,6 +364,8 @@ class LidarPacketHandler {
     bool lidar_scans_processing_active = true;
     std::unique_ptr<std::thread> lidar_scans_processing_thread;
     std::condition_variable ring_buffer_has_elements;
+
+    int64_t ptp_utc_tai_offset_;
 };
 
 }  // namespace ouster_ros