From a7709c33bbca9999e1d48690d76b81488df9e9a4 Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Sat, 17 Aug 2024 18:31:44 -0700
Subject: [PATCH] Set LIDAR FOV on startup

---
 launch/driver.launch      |  7 +++++++
 launch/record.launch      |  7 +++++++
 launch/replay.launch      |  5 ++++-
 launch/sensor.launch      |  7 +++++++
 launch/sensor_mtp.launch  |  7 +++++++
 src/os_sensor_nodelet.cpp | 17 +++++++++++++++--
 src/os_sensor_nodelet.h   |  3 +--
 7 files changed, 48 insertions(+), 5 deletions(-)

diff --git a/launch/driver.launch b/launch/driver.launch
index 0f6f1002..a4bf97df 100644
--- a/launch/driver.launch
+++ b/launch/driver.launch
@@ -65,6 +65,11 @@
     xyzir
     }"/>
 
+  <arg name="azimuth_window_start" default="0" doc="azimuth window start;
+    values range [0, 360000] millideggrees"/>
+  <arg name="azimuth_window_end" default="360000" doc="azimuth window end;
+    values range [0, 360000] millideggrees"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -92,6 +97,8 @@
       <param name="~/proc_mask" value="$(arg proc_mask)"/>
       <param name="~/scan_ring" value="$(arg scan_ring)"/>
       <param name="~/point_type" value="$(arg point_type)"/>
+      <param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
+      <param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
     </node>
   </group>
 
diff --git a/launch/record.launch b/launch/record.launch
index 806a233e..10d1937b 100644
--- a/launch/record.launch
+++ b/launch/record.launch
@@ -68,6 +68,11 @@
     xyzir
     }"/>
 
+  <arg name="azimuth_window_start" default="0" doc="azimuth window start,
+    values range [0, 360000] millideggrees"/>
+  <arg name="azimuth_window_end" default="360000" doc="azimuth window end,
+    values range [0, 360000] millideggrees"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -86,6 +91,8 @@
       <param name="~/lidar_mode" type="str" value="$(arg lidar_mode)"/>
       <param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
       <param name="~/metadata" type="str" value="$(arg metadata)"/>
+      <param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
+      <param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
     </node>
   </group>
 
diff --git a/launch/replay.launch b/launch/replay.launch
index 02c09a39..d5e7fcd3 100644
--- a/launch/replay.launch
+++ b/launch/replay.launch
@@ -2,6 +2,9 @@
 
   <param name="use_sim_time" value="true"/>
 
+  <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" doc="Override the default namespace of all ouster nodes"/>
   <arg name="metadata" default="" doc="path to read metadata file when replaying sensor data"/>
   <arg name="bag_file" doc="file name to use for the recorded bag file"/>
@@ -93,6 +96,6 @@
   <node if="$(arg _use_bag_file_name)" pkg="rosbag" type="play" name="rosbag_play_recording"
     launch-prefix="bash -c 'sleep 3; $0 $@' "
     output="screen" required="true"
-    args="--clock $(arg bag_file)"/>
+    args="--clock $(arg bag_file) -l"/>
 
 </launch>
diff --git a/launch/sensor.launch b/launch/sensor.launch
index a8b2f15e..e473169d 100644
--- a/launch/sensor.launch
+++ b/launch/sensor.launch
@@ -73,6 +73,11 @@
     xyzir
     }"/>
 
+  <arg name="azimuth_window_start" default="0" doc="azimuth window start;
+    values range [0, 360000] millideggrees"/>
+  <arg name="azimuth_window_end" default="360000" doc="azimuth window end;
+    values range [0, 360000] millideggrees"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -90,6 +95,8 @@
       <param name="~/lidar_mode" type="str" value="$(arg lidar_mode)"/>
       <param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
       <param name="~/metadata" type="str" value="$(arg metadata)"/>
+      <param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
+      <param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
     </node>
   </group>
 
diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch
index ae061e55..e2c32306 100644
--- a/launch/sensor_mtp.launch
+++ b/launch/sensor_mtp.launch
@@ -77,6 +77,11 @@
     xyzir
     }"/>
 
+  <arg name="azimuth_window_start" default="0" doc="azimuth window start,
+    values range [0, 360000] millideggrees"/>
+  <arg name="azimuth_window_end" default="360000" doc="azimuth window end,
+    values range [0, 360000] millideggrees"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true"
@@ -97,6 +102,8 @@
       <param name="~/lidar_mode" type="str" value="$(arg lidar_mode)"/>
       <param name="~/timestamp_mode" type="str" value="$(arg timestamp_mode)"/>
       <param name="~/metadata" type="str" value="$(arg metadata)"/>
+      <param name="~/azimuth_window_start" value="$(arg azimuth_window_start)"/>
+      <param name="~/azimuth_window_end" value="$(arg azimuth_window_end)"/>
     </node>
   </group>
 
diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp
index 18a24bab..9b1b8e49 100644
--- a/src/os_sensor_nodelet.cpp
+++ b/src/os_sensor_nodelet.cpp
@@ -25,6 +25,7 @@ using nonstd::optional;
 
 using namespace std::chrono_literals;
 using namespace std::string_literals;
+using std::to_string;
 
 namespace ouster_ros {
 
@@ -232,6 +233,9 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
     auto lidar_mode_arg = nh.param("lidar_mode", std::string{});
     auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{});
     auto udp_profile_lidar_arg = nh.param("udp_profile_lidar", std::string{});
+    const int MIN_AZW = 0, MAX_AZW = 360000;
+    auto azimuth_window_start = nh.param("azimuth_window_start", MIN_AZW);
+    auto azimuth_window_end = nh.param("azimuth_window_end", MAX_AZW);
 
     if (lidar_port < 0 || lidar_port > 65535) {
         auto error_msg =
@@ -325,6 +329,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);
+        NODELET_ERROR_STREAM(error_msg);
+        throw std::runtime_error(error_msg);
+    }
+
+    config.azimuth_window = {azimuth_window_start, azimuth_window_end};
+
     return config;
 }
 
@@ -450,9 +464,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/src/os_sensor_nodelet.h b/src/os_sensor_nodelet.h
index fcaab051..39d1e3ee 100644
--- a/src/os_sensor_nodelet.h
+++ b/src/os_sensor_nodelet.h
@@ -141,8 +141,7 @@ class OusterSensor : public OusterSensorNodeletBase {
     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;