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;