From f456a922e302dc32734054159404dce4631c9679 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Fri, 17 Nov 2023 18:32:46 -0800 Subject: [PATCH] Bump up ouster_client to 20231031 release (#263) * Bump up ouster_client to 20231031 release * fix: gracefully stop the driver when shutdown is requested. --- CHANGELOG.rst | 43 ++++++++++++++++++++++++++++--- ouster-ros/ouster-sdk | 2 +- ouster-ros/package.xml | 2 +- ouster-ros/src/os_driver_node.cpp | 5 ++++ ouster-ros/src/os_sensor_node.cpp | 22 ++++++++++------ ouster-ros/src/os_sensor_node.h | 5 ++-- ouster-sensor-msgs/package.xml | 2 +- 7 files changed, 65 insertions(+), 16 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 6e5766ec..cbb6c97c 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,9 +2,13 @@ Changelog ========= -[unreleased] -============ -* breaking: publish PCL point clouds destaggered. +ouster_ros v0.12.0 +================== + +ouster_ros(2) +------------- +* [BREAKING]: updated ouster_client to the release of ``20231031`` [v0.10.0]; changes listed below. +* [BREAKING]: publish PCL point clouds destaggered. * introduced a new launch file parameter ``ptp_utc_tai_offset`` which represent offset in seconds to be applied to all ROS messages the driver generates when ``TIME_FROM_PTP_1588`` timestamp mode is used. @@ -16,6 +20,39 @@ Changelog * added the ability to customize the published point clouds(s) to velodyne point cloud format and other common pcl point types. * ouster_image_compoenent can operate separately from ouster_cloud_component. +* fix: gracefully stop the driver when shutdown is requested. + +ouster_client +------------- +* [BREAKING] Updates to ``sensor_info`` include: + * new fields added: ``build_date``, ``image_rev``, ``prod_pn``, ``status``, ``cal`` (representing + the value stored in the ``calibration_status`` metadata JSON key), ``config`` (representing the + value of the ``sensor_config`` metadata JSON key) + * the original JSON string is accessible via the ``original_string()`` method + * The ``updated_metadata_string()`` now returns a JSON string reflecting any modifications to + ``sensor_info`` + * ``to_string`` is now marked as deprecated +* [BREAKING] The RANGE field defined in `parsing.cpp`, for the low data rate profile, is now 32 bits + wide (originally 16 bits). + * Please note this fixes a SDK bug. The underlying UDP format is unchanged. +* [BREAKING] The NEAR_IR field defined in `parsing.cpp`, for the low data rate profile, is now 16 + bits wide (originally 8 bits). + * Plase note this fixes a SDK bug. The underlying UDP format is unchanged. +* [BREAKING] changed frame_id return size to 32 bits from 16 bits +* An array of per-packet timestamps (called ``packet_timestamp``) is added to ``LidarScan`` +* The client now retries failed requests to an Ouster sensor's HTTP API +* Increased the default timeout for HTTP requests to 40s +* Added FuSA UDP profile to support Ouster FW 3.1+ +* Improved ``ScanBatcher`` performance by roughly 3x (depending on hardware) +* Receive buffer size increased from 256KB to 1MB +* [bugfix] Fixed an issue that caused incorrect Cartesian point computation in the ``viz.Cloud`` + Python class +* [bugfix] Fixed an issue that resulted in some ``packet_format`` methods returning an uninitialized + value +* [bugfix] Fixed a libpcap-related linking issue +* [bugfix] Fixed an eigen 3.3-related linking issue +* [bugfix] Fixed a zero beam angle calculation issue +* [bugfix] Fixed dropped columns issue with 4096x5 and 2048x10 ouster_ros v0.10.0 ================== diff --git a/ouster-ros/ouster-sdk b/ouster-ros/ouster-sdk index d7307986..2898060f 160000 --- a/ouster-ros/ouster-sdk +++ b/ouster-ros/ouster-sdk @@ -1 +1 @@ -Subproject commit d73079865ebadd961d7af81b235bb34747f7cb6a +Subproject commit 2898060fb47a69b4fbf9807d3c17f6fdc8249379 diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index b6796fbd..6609bf52 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.11.2 + 0.12.0 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 18746827..49971f5c 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -40,6 +40,11 @@ class OusterDriver : public OusterSensor { declare_parameter("point_type", "original"); } + ~OusterDriver() override { + RCLCPP_DEBUG(get_logger(), "OusterDriver::~OusterDriver() called"); + halt(); + } + LifecycleNodeInterface::CallbackReturn on_activate( const rclcpp_lifecycle::State& state) { RCLCPP_DEBUG(get_logger(), "os_driver::on_activate() is called."); diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 43d1f648..3c76d6cf 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -36,6 +36,16 @@ OusterSensor::OusterSensor(const std::string& name, OusterSensor::OusterSensor(const rclcpp::NodeOptions& options) : OusterSensor("os_sensor", options) {} +OusterSensor::~OusterSensor() { + RCLCPP_DEBUG(get_logger(), "OusterSensor::~OusterSensor() called"); + halt(); +} + +void OusterSensor::halt() { + stop_packet_processing_threads(); + stop_sensor_connection_thread(); +} + void OusterSensor::declare_parameters() { declare_parameter("sensor_hostname", ""); declare_parameter("lidar_ip", ""); // community driver param @@ -325,7 +335,6 @@ void OusterSensor::reactivate_sensor(bool init_id_reset) { reset_last_init_id = init_id_reset; active_config.clear(); cached_metadata.clear(); - imu_packets_skip = lidar_packets_skip = true; auto request_transitions = std::vector{ lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE}; @@ -789,24 +798,21 @@ void OusterSensor::stop_sensor_connection_thread() { } void OusterSensor::start_packet_processing_threads() { - imu_packets_skip = false; imu_packets_processing_thread_active = true; imu_packets_processing_thread = std::make_unique([this]() { - auto read_packet = [this](const uint8_t* buffer) { - if (!imu_packets_skip) on_imu_packet_msg(buffer); - }; while (imu_packets_processing_thread_active) { - imu_packets->read(read_packet); + imu_packets->read([this](const uint8_t* buffer) { + on_imu_packet_msg(buffer); + }); } RCLCPP_DEBUG(get_logger(), "imu_packets_processing_thread done."); }); - lidar_packets_skip = false; lidar_packets_processing_thread_active = true; lidar_packets_processing_thread = std::make_unique([this]() { while (lidar_packets_processing_thread_active) { lidar_packets->read([this](const uint8_t* buffer) { - if (!lidar_packets_skip) on_lidar_packet_msg(buffer); + on_lidar_packet_msg(buffer); }); } RCLCPP_DEBUG(get_logger(), "lidar_packets_processing_thread done."); diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h index 4e5dbcb8..0715a2d2 100644 --- a/ouster-ros/src/os_sensor_node.h +++ b/ouster-ros/src/os_sensor_node.h @@ -39,6 +39,7 @@ class OusterSensor : public OusterSensorNodeBase { OUSTER_ROS_PUBLIC OusterSensor(const std::string& name, const rclcpp::NodeOptions& options); explicit OusterSensor(const rclcpp::NodeOptions& options); + ~OusterSensor() override; LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State& state); @@ -66,6 +67,8 @@ class OusterSensor : public OusterSensorNodeBase { virtual void cleanup(); + void halt(); + private: void declare_parameters(); @@ -164,11 +167,9 @@ class OusterSensor : public OusterSensorNodeBase { std::unique_ptr sensor_connection_thread; std::atomic imu_packets_processing_thread_active = {false}; - std::atomic imu_packets_skip; std::unique_ptr imu_packets_processing_thread; std::atomic lidar_packets_processing_thread_active = {false}; - std::atomic lidar_packets_skip; std::unique_ptr lidar_packets_processing_thread; bool force_sensor_reinit = false; diff --git a/ouster-sensor-msgs/package.xml b/ouster-sensor-msgs/package.xml index 22e829d1..08cbc873 100644 --- a/ouster-sensor-msgs/package.xml +++ b/ouster-sensor-msgs/package.xml @@ -2,7 +2,7 @@ ouster_sensor_msgs - 0.11.2 + 0.12.0 ouster_ros message and service definitions ouster developers BSD