Repository: ouster-lidar/ouster-ros (language: cpp)

Issue:
Changing the FOV of the Lidar in ROS

I want to change the FOV of the Ouster OS0-32 from [0-360] to [0, 180000] and in the Software User guide i see  the use of ``` set_config_param azimuth_window``` How can i set this param in ros? I don't see this particular rosparam in rosparam list.

**Platform :**
- Ouster Sensor: OS-0-32
- ROS version/distro Melodic
- Operating System? Ubuntu 18



Discussion / hints:
We don't expose the ability to set azimuth window via ros launch file. We have `/ouster/set_config/` (described [here](https://github.com/ouster-lidar/ouster-ros#setconfig-experimental) which could be used for this purpose but due to sensor re-initialization this is unlikely to work for ROS1 driver. It does work though for the ROS2 implementation since it implements Node LifeCycle.
Hi @Samahu 
Could you please further elaborate how we can change the configuration to set the azimuth window up to 180 ° in ROS-2?

Sure, create a config file in some known path and add the following content:
```json
{
    "azimuth_window": 
    [
        0,
        180000
    ],
}
```
This config would set the azimuth window to 180 degrees. Let's name the file `os-xxx.json`. Now to apply this config file you can do so by invoking the command:
```bash
ros2 service call /ouster/set_config ouster_sensor_msgs/srv/SetConfig "{config_file: /path/to/file/os-xxx.json}"
```
Make sure to set the correct path of your file. If the call was successful you should get the following response:
```bash
ouster_sensor_msgs.srv.SetConfig_Response(config='{\n    "azimuth_window": \n    [\n        0,\n        180000\n    ]\n}')
```
The driver will stop momentarily and and restart with new the new azimuth window configuration. 
Hi @Samahu,

Thank you for your response. I would like to know whether the above configuration will only limit the field of view of the point cloud or image information  (signal image, etc.) as well?


Best regards,
It will affect both simultaneously 

Fix patch:
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


Test patch: