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)"/> + + + + + + + @@ -82,6 +99,9 @@ + + + @@ -93,6 +113,7 @@ + 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 @@ + + + + + + + @@ -97,8 +104,8 @@ 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 }"/> + + + + + @@ -95,6 +103,9 @@ + + + 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 }"/> + + + + + @@ -89,6 +97,9 @@ + + + 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 }"/> + + + + + @@ -103,6 +111,9 @@ + + + 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 @@ ouster_ros - 0.12.5 + 0.12.6 Ouster ROS2 driver ouster developers BSD 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 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; @@ -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