From 4ec21f5d71d81fd1520e069666fdbd38167091c6 Mon Sep 17 00:00:00 2001
From: Ussama Naal <606033+Samahu@users.noreply.github.com>
Date: Wed, 21 Aug 2024 15:17:13 -0700
Subject: [PATCH] Port changing lidar fov to foxy (#358)

---
 CHANGELOG.rst                                 |  5 +++
 ouster-ros/config/driver_params.yaml          |  8 ++++-
 .../config/os_sensor_cloud_image_params.yaml  |  3 ++
 .../launch/record.independent.launch.xml      | 21 +++++++++++++
 .../launch/replay.independent.launch.xml      | 11 +++++--
 ouster-ros/launch/sensor.composite.launch.xml | 11 +++++++
 .../launch/sensor.independent.launch.xml      | 11 +++++++
 ouster-ros/launch/sensor_mtp.launch.xml       | 11 +++++++
 ouster-ros/package.xml                        |  2 +-
 ouster-ros/src/os_sensor_node.cpp             | 31 +++++++++++++++++--
 ouster-ros/src/os_sensor_node.h               |  7 +++--
 11 files changed, 113 insertions(+), 8 deletions(-)

diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index e1ba1270..89a8bfb9 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -13,6 +13,11 @@ Changelog
 * [BREAKING]: Set xyz values of individual points in the PointCloud to NaNs when range is zero.
 * Added support to replay pcap format direclty from ouster-ros. The feature needs to be enabled
   explicitly by turning on the ``BUILD_PCAP`` cmake option and having ``libpcap-dev`` installed.
+* [BREAKING] Added new launch files args ``azimuth_window_start`` and ``azimuth_window_end`` to
+  allow users to set LIDAR FOV on startup. The new options will reset the current azimuth window
+  to the default
+* Added a new launch ``persist_config`` option to request the sensor persist the current config
+* Added a new ``loop`` option to the ``replay.launch.xml`` file.
 
 ouster_ros v0.12.0
 ==================
diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml
index a537ffac..bab4451a 100644
--- a/ouster-ros/config/driver_params.yaml
+++ b/ouster-ros/config/driver_params.yaml
@@ -86,4 +86,10 @@ ouster/os_driver:
     # - ouster_ros/os_point.h
     # - ouster_ros/sensor_point_types.h
     # - ouster_ros/common_point_types.h.
-    point_type: original
\ No newline at end of file
+    point_type: original
+    # azimuth window start[optional]: values range [0, 360000] millidegrees
+    azimuth_window_start: 0
+    # azimuth_window_end[optional]: values range [0, 360000] millidegrees
+    azimuth_window_end: 360000
+    # persist_config[optional]: request the sensor to persist settings
+    persist_config: false
diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml
index 063774f4..76999cfa 100644
--- a/ouster-ros/config/os_sensor_cloud_image_params.yaml
+++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml
@@ -15,6 +15,9 @@ ouster/os_sensor:
     lidar_port: 0
     imu_port: 0
     use_system_default_qos: False
+    azimuth_window_start: 0
+    azimuth_window_end: 360000
+    persist_config: false
 ouster/os_cloud:
   ros__parameters:
     sensor_frame: os_sensor
diff --git a/ouster-ros/launch/record.independent.launch.xml b/ouster-ros/launch/record.independent.launch.xml
index 6e6786f4..f0242b09 100644
--- a/ouster-ros/launch/record.independent.launch.xml
+++ b/ouster-ros/launch/record.independent.launch.xml
@@ -68,6 +68,23 @@
     use this parameter in conjunction with the SCAN flag
     and choose a value the range [0, sensor_beams_count)"/>
 
+  <arg name="point_type" default="original" description="point type for the generated point cloud;
+   available options: {
+    original,
+    native,
+    xyz,
+    xyzi,
+    xyzir
+    }"/>
+
+  <arg name="azimuth_window_start" default="0" description="azimuth window start;
+    values range [0, 360000] millidegrees"/>
+  <arg name="azimuth_window_end" default="360000" description="azimuth window end;
+    values range [0, 360000] millidegrees"/>
+
+  <arg name="persist_config" default="false"
+    description="request the sensor to persist settings"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node pkg="ouster_ros" exec="os_sensor" name="os_sensor" output="screen">
@@ -82,6 +99,9 @@
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
       <param name="metadata" value="$(var metadata)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+      <param name="azimuth_window_start" value="$(var azimuth_window_start)"/>
+      <param name="azimuth_window_end" value="$(var azimuth_window_end)"/>
+      <param name="persist_config" value="$(var persist_config)"/>
     </node>
     <node pkg="ouster_ros" exec="os_cloud" name="os_cloud" output="screen">
       <param name="sensor_frame" value="$(var sensor_frame)"/>
@@ -93,6 +113,7 @@
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>
+      <param name="point_type" value="$(var point_type)"/>
     </node>
     <node pkg="ouster_ros" exec="os_image" name="os_image" output="screen">
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
diff --git a/ouster-ros/launch/replay.independent.launch.xml b/ouster-ros/launch/replay.independent.launch.xml
index cb0c44d1..189f4d42 100644
--- a/ouster-ros/launch/replay.independent.launch.xml
+++ b/ouster-ros/launch/replay.independent.launch.xml
@@ -2,6 +2,13 @@
 
   <set_parameter name="use_sim_time" value="true" />
 
+  <arg name="loop" default="false" description="request loop playback"/>
+  <arg if="$(arg loop)" name="_loop" value="--loop"/>
+  <arg unless="$(arg loop)" name="_loop" value=" "/>
+
+  <remap from="/os_node/imu_packets" to="/ouster/imu_packets"/>
+  <remap from="/os_node/lidar_packets" to="/ouster/lidar_packets"/>
+
   <arg name="ouster_ns" default="ouster"
     description="Override the default namespace of all ouster nodes"/>
   <!-- TODO: revisit the proper behaviour of allowing users override the default timestamp_mode during replay -->
@@ -97,8 +104,8 @@
 
   <executable if="$(var _use_bag_file_name)" output="screen"
     launch-prefix="bash -c 'sleep 3; $0 $@'"
-    cmd="ros2 bag play $(var bag_file)
-      --qos-profile-overrides-path 
+    cmd="ros2 bag play $(var bag_file) --clock $(arg _loop)
+      --qos-profile-overrides-path
       $(find-pkg-share ouster_ros)/config/metadata-qos-override.yaml"/>
 
 </launch>
diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml
index 0940c6ec..19119494 100644
--- a/ouster-ros/launch/sensor.composite.launch.xml
+++ b/ouster-ros/launch/sensor.composite.launch.xml
@@ -73,6 +73,14 @@
     xyzir
     }"/>
 
+  <arg name="azimuth_window_start" default="0" description="azimuth window start;
+    values range [0, 360000] millidegrees"/>
+  <arg name="azimuth_window_end" default="360000" description="azimuth window end;
+    values range [0, 360000] millidegrees"/>
+
+  <arg name="persist_config" default="false"
+    description="request the sensor to persist settings"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node pkg="ouster_ros" exec="os_driver" name="os_driver" output="screen">
@@ -95,6 +103,9 @@
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>
       <param name="point_type" value="$(var point_type)"/>
+      <param name="azimuth_window_start" value="$(var azimuth_window_start)"/>
+      <param name="azimuth_window_end" value="$(var azimuth_window_end)"/>
+      <param name="persist_config" value="$(var persist_config)"/>
     </node>
   </group>
 
diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml
index b483d6fd..43f26b4a 100644
--- a/ouster-ros/launch/sensor.independent.launch.xml
+++ b/ouster-ros/launch/sensor.independent.launch.xml
@@ -75,6 +75,14 @@
     xyzir
     }"/>
 
+  <arg name="azimuth_window_start" default="0" description="azimuth window start;
+    values range [0, 360000] millidegrees"/>
+  <arg name="azimuth_window_end" default="360000" description="azimuth window end;
+    values range [0, 360000] millidegrees"/>
+
+  <arg name="persist_config" default="false"
+    description="request the sensor to persist settings"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node pkg="ouster_ros" exec="os_sensor" name="os_sensor" output="screen">
@@ -89,6 +97,9 @@
       <param name="timestamp_mode" value="$(var timestamp_mode)"/>
       <param name="metadata" value="$(var metadata)"/>
       <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+      <param name="azimuth_window_start" value="$(var azimuth_window_start)"/>
+      <param name="azimuth_window_end" value="$(var azimuth_window_end)"/>
+      <param name="persist_config" value="$(var persist_config)"/>
     </node>
     <node pkg="ouster_ros" exec="os_cloud" name="os_cloud" output="screen">
       <param name="sensor_frame" value="$(var sensor_frame)"/>
diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml
index a6c56277..97892956 100644
--- a/ouster-ros/launch/sensor_mtp.launch.xml
+++ b/ouster-ros/launch/sensor_mtp.launch.xml
@@ -81,6 +81,14 @@
     xyzir
     }"/>
 
+  <arg name="azimuth_window_start" default="0" description="azimuth window start;
+    values range [0, 360000] millidegrees"/>
+  <arg name="azimuth_window_end" default="360000" description="azimuth window end;
+    values range [0, 360000] millidegrees"/>
+
+  <arg name="persist_config" default="false"
+    description="request the sensor to persist settings"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node pkg="ouster_ros" exec="os_driver" name="os_driver" output="screen">
@@ -103,6 +111,9 @@
       <param name="proc_mask" value="$(var proc_mask)"/>
       <param name="scan_ring" value="$(var scan_ring)"/>
       <param name="point_type" value="$(var point_type)"/>
+      <param name="azimuth_window_start" value="$(var azimuth_window_start)"/>
+      <param name="azimuth_window_end" value="$(var azimuth_window_end)"/>
+      <param name="persist_config" value="$(var persist_config)"/>
     </node>
   </group>
 
diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml
index 258523bc..d32bcd86 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.12.5</version>
+  <version>0.12.6</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>
diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp
index 1d66203a..38866708 100644
--- a/ouster-ros/src/os_sensor_node.cpp
+++ b/ouster-ros/src/os_sensor_node.cpp
@@ -22,6 +22,7 @@ using ouster_sensor_msgs::srv::SetConfig;
 
 using namespace std::chrono_literals;
 using namespace std::string_literals;
+using std::to_string;
 
 namespace ouster_ros {
 
@@ -60,6 +61,9 @@ void OusterSensor::declare_parameters() {
     declare_parameter("timestamp_mode", "");
     declare_parameter("udp_profile_lidar", "");
     declare_parameter("use_system_default_qos", false);
+    declare_parameter("azimuth_window_start", MIN_AZW);
+    declare_parameter("azimuth_window_end", MAX_AZW);
+    declare_parameter("persist_config", false);
 }
 
 LifecycleNodeInterface::CallbackReturn OusterSensor::on_configure(
@@ -445,6 +449,8 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
     auto lidar_mode_arg = get_parameter("lidar_mode").as_string();
     auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string();
     auto udp_profile_lidar_arg = get_parameter("udp_profile_lidar").as_string();
+    auto azimuth_window_start = get_parameter("azimuth_window_start").as_int();
+    auto azimuth_window_end = get_parameter("azimuth_window_end").as_int();
 
     if (lidar_port < 0 || lidar_port > 65535) {
         auto error_msg =
@@ -527,6 +533,13 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
         config.udp_port_imu = imu_port;
     }
 
+    persist_config = get_parameter("persist_config").as_bool();
+    if (persist_config && (lidar_port == 0 || imu_port == 0)) {
+        RCLCPP_WARN(get_logger(), "When using persist_config it is recommended "
+        " to not use 0 for port values as this currently will trigger sensor "
+        " reinit event each time");
+    }
+
     config.udp_profile_lidar = udp_profile_lidar;
     config.operating_mode = sensor::OPERATING_NORMAL;
     if (lidar_mode) config.ld_mode = lidar_mode;
@@ -539,6 +552,16 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
         }
     }
 
+    if (azimuth_window_start < MIN_AZW || azimuth_window_start > MAX_AZW ||
+        azimuth_window_end < MIN_AZW || azimuth_window_end > MAX_AZW) {
+        auto error_msg = "azimuth window values must be between " +
+                    to_string(MIN_AZW) + " and " + to_string(MAX_AZW);
+        RCLCPP_ERROR_STREAM(get_logger(), error_msg);
+        throw std::runtime_error(error_msg);
+    }
+
+    config.azimuth_window = {azimuth_window_start, azimuth_window_end};
+
     return config;
 }
 
@@ -578,6 +601,11 @@ uint8_t OusterSensor::compose_config_flags(
         config_flags |= ouster::sensor::CONFIG_FORCE_REINIT;
     }
 
+    if (persist_config) {
+        RCLCPP_INFO(get_logger(), "Configuration will be persisted");
+        config_flags |= ouster::sensor::CONFIG_PERSIST;
+    }
+
     return config_flags;
 }
 
@@ -675,9 +703,8 @@ void OusterSensor::allocate_buffers() {
 bool OusterSensor::init_id_changed(const sensor::packet_format& pf,
                                    const uint8_t* lidar_buf) {
     uint32_t current_init_id = pf.init_id(lidar_buf);
-    if (!last_init_id_initialized) {
+    if (!last_init_id) {
         last_init_id = current_init_id + 1;
-        last_init_id_initialized = true;
     }
     if (reset_last_init_id && last_init_id != current_init_id) {
         last_init_id = current_init_id;
diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h
index 0715a2d2..0dec1bd4 100644
--- a/ouster-ros/src/os_sensor_node.h
+++ b/ouster-ros/src/os_sensor_node.h
@@ -172,11 +172,11 @@ class OusterSensor : public OusterSensorNodeBase {
     std::atomic<bool> lidar_packets_processing_thread_active = {false};
     std::unique_ptr<std::thread> lidar_packets_processing_thread;
 
+    bool persist_config = false;
     bool force_sensor_reinit = false;
     bool reset_last_init_id = true;
 
-    bool last_init_id_initialized = false;
-    uint32_t last_init_id;
+    nonstd::optional<uint32_t> last_init_id;
 
     // TODO: add as a ros parameter
     const int max_poll_client_error_count = 10;
@@ -187,6 +187,9 @@ class OusterSensor : public OusterSensorNodeBase {
     // TODO: add as a ros parameter
     const int max_read_imu_packet_errors = 60;
     int read_imu_packet_errors = 0;
+
+    const int MIN_AZW = 0;
+    const int MAX_AZW = 360000;
 };
 
 }  // namespace ouster_ros
\ No newline at end of file