From 8b8134749b7ec67f581c652084b231d9a9df4000 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Tue, 20 Aug 2024 12:49:07 -0700 Subject: [PATCH] ROS-227: Set LIDAR FOV on startup and add an option to persist the config [NOETIC] (#356) * Set LIDAR FOV on startup * Add persist flags option to launch files. * Add a breaking note about azimuth window config reset --- CHANGELOG.rst | 5 +++++ launch/driver.launch | 11 +++++++++++ launch/record.launch | 11 +++++++++++ launch/replay.launch | 9 ++++++++- launch/sensor.launch | 11 +++++++++++ launch/sensor_mtp.launch | 11 +++++++++++ package.xml | 2 +- src/os_sensor_nodelet.cpp | 30 ++++++++++++++++++++++++++++-- src/os_sensor_nodelet.h | 4 ++-- 9 files changed, 88 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index fe3b6b38..8f10b501 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -17,6 +17,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`` file. ouster_ros v0.10.0 diff --git a/launch/driver.launch b/launch/driver.launch index 0f6f1002..23506b9e 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -65,6 +65,14 @@ xyzir }"/> + + + + + @@ -92,6 +100,9 @@ + + + diff --git a/launch/record.launch b/launch/record.launch index 806a233e..ad49ed2f 100644 --- a/launch/record.launch +++ b/launch/record.launch @@ -68,6 +68,14 @@ xyzir }"/> + + + + + @@ -86,6 +94,9 @@ + + + diff --git a/launch/replay.launch b/launch/replay.launch index 02c09a39..ed99213d 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -2,6 +2,13 @@ + + + + + + + @@ -93,6 +100,6 @@ + args="--clock $(arg bag_file) $(arg _loop)"/> diff --git a/launch/sensor.launch b/launch/sensor.launch index a8b2f15e..46839d4e 100644 --- a/launch/sensor.launch +++ b/launch/sensor.launch @@ -73,6 +73,14 @@ xyzir }"/> + + + + + @@ -90,6 +98,9 @@ + + + diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index ae061e55..963781cc 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -77,6 +77,14 @@ xyzir }"/> + + + + + + + + diff --git a/package.xml b/package.xml index d8e66bff..f91d6bc5 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ ouster_ros - 0.12.4 + 0.12.5 Ouster ROS driver ouster developers BSD diff --git a/src/os_sensor_nodelet.cpp b/src/os_sensor_nodelet.cpp index 18a24bab..c193cda8 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 = @@ -313,6 +317,14 @@ 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 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; @@ -325,6 +337,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; } @@ -362,6 +384,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; } @@ -450,9 +477,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..d45fc1a6 100644 --- a/src/os_sensor_nodelet.h +++ b/src/os_sensor_nodelet.h @@ -138,11 +138,11 @@ class OusterSensor : public OusterSensorNodeletBase { std::atomic lidar_packets_processing_thread_active = {false}; std::unique_ptr 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 last_init_id; // TODO: add as a ros parameter const int max_poll_client_error_count = 10;