From 341706f2ecc24164ed69845a690321a0be5a7106 Mon Sep 17 00:00:00 2001
From: Ussama Naal <ussama.naal@ouster.io>
Date: Mon, 19 Aug 2024 10:55:31 -0700
Subject: [PATCH] Add persist flags option to launch files.

---
 launch/driver.launch      |  4 ++++
 launch/record.launch      |  4 ++++
 launch/sensor.launch      |  4 ++++
 launch/sensor_mtp.launch  |  4 ++++
 src/os_sensor_nodelet.cpp | 12 ++++++++++++
 5 files changed, 28 insertions(+)

diff --git a/launch/driver.launch b/launch/driver.launch
index a4bf97df..8a9ed863 100644
--- a/launch/driver.launch
+++ b/launch/driver.launch
@@ -70,6 +70,9 @@
   <arg name="azimuth_window_end" default="360000" doc="azimuth window end;
     values range [0, 360000] millideggrees"/>
 
+  <arg name="persist_config" default="false"
+    doc="request the sensor to persist settings"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -99,6 +102,7 @@
       <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)"/>
+      <param name="~/persist_config" value="$(arg persist_config)"/>
     </node>
   </group>
 
diff --git a/launch/record.launch b/launch/record.launch
index 10d1937b..fa325b94 100644
--- a/launch/record.launch
+++ b/launch/record.launch
@@ -73,6 +73,9 @@
   <arg name="azimuth_window_end" default="360000" doc="azimuth window end,
     values range [0, 360000] millideggrees"/>
 
+  <arg name="persist_config" default="false"
+    doc="request the sensor to persist settings"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -93,6 +96,7 @@
       <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)"/>
+      <param name="~/persist_config" value="$(arg persist_config)"/>
     </node>
   </group>
 
diff --git a/launch/sensor.launch b/launch/sensor.launch
index e473169d..ba7c9d1d 100644
--- a/launch/sensor.launch
+++ b/launch/sensor.launch
@@ -78,6 +78,9 @@
   <arg name="azimuth_window_end" default="360000" doc="azimuth window end;
     values range [0, 360000] millideggrees"/>
 
+  <arg name="persist_config" default="false"
+    doc="request the sensor to persist settings"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true" args="manager"/>
@@ -97,6 +100,7 @@
       <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)"/>
+      <param name="~/persist_config" value="$(arg persist_config)"/>
     </node>
   </group>
 
diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch
index e2c32306..894e7b28 100644
--- a/launch/sensor_mtp.launch
+++ b/launch/sensor_mtp.launch
@@ -82,6 +82,9 @@
   <arg name="azimuth_window_end" default="360000" doc="azimuth window end,
     values range [0, 360000] millideggrees"/>
 
+  <arg name="persist_config" default="false"
+    doc="request the sensor to persist settings"/>
+
   <group ns="$(arg ouster_ns)">
     <node pkg="nodelet" type="nodelet" name="os_nodelet_mgr"
       output="screen" required="true"
@@ -104,6 +107,7 @@
       <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)"/>
+      <param name="~/persist_config" value="$(arg persist_config)"/>
     </node>
   </group>
 
diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp
index 9b1b8e49..75a15a21 100644
--- a/src/os_sensor_nodelet.cpp
+++ b/src/os_sensor_nodelet.cpp
@@ -317,6 +317,13 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() {
         config.udp_port_imu = imu_port;
     }
 
+    persist_config = nh.param("persist_config", false);
+    if (persist_config && (lidar_port == 0 || imu_port == 0)) {
+        NODELET_WARN("When using persist_config it is recommended to not "
+        "use 0 for port value");
+    }
+
+
     config.udp_profile_lidar = udp_profile_lidar;
     config.operating_mode = sensor::OPERATING_NORMAL;
     if (lidar_mode) config.ld_mode = lidar_mode;
@@ -376,6 +383,11 @@ uint8_t OusterSensor::compose_config_flags(
         config_flags |= ouster::sensor::CONFIG_FORCE_REINIT;
     }
 
+    if (persist_config) {
+        NODELET_INFO("Configuration will be persisted");
+        config_flags |= ouster::sensor::CONFIG_PERSIST;
+    }
+
     return config_flags;
 }