Skip to content

Commit

Permalink
SW-5623: Bump up ouster_client to 20231031 release (#262)
Browse files Browse the repository at this point in the history
* Bump ouster-client to 2023103 release
* fix: gracefully stop the driver when shutdown is requested.
  • Loading branch information
Samahu authored Nov 18, 2023
1 parent 3d82e79 commit 726b0d5
Show file tree
Hide file tree
Showing 7 changed files with 62 additions and 16 deletions.
40 changes: 37 additions & 3 deletions CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,10 @@
Changelog
=========

[unreleased]
============
* breaking: publish PCL point clouds destaggered.
ouster_ros v0.12.0
==================
* [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.
Expand All @@ -16,6 +17,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
==================
Expand Down
2 changes: 1 addition & 1 deletion ouster-ros/ouster-sdk
2 changes: 1 addition & 1 deletion ouster-ros/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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.11.2</version>
<version>0.12.0</version>
<description>Ouster ROS2 driver</description>
<maintainer email="[email protected]">ouster developers</maintainer>
<license file="LICENSE">BSD</license>
Expand Down
5 changes: 5 additions & 0 deletions ouster-ros/src/os_driver_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,11 @@ class OusterDriver : public OusterSensor {
declare_parameter("point_type", "original");
}

~OusterDriver() override {
RCLCPP_INFO(get_logger(), "OusterDriver::~OusterDriver() called");
halt();
}

virtual void on_metadata_updated(const sensor::sensor_info& info) override {
OusterSensor::on_metadata_updated(info);
tf_bcast.broadcast_transforms(info);
Expand Down
22 changes: 14 additions & 8 deletions ouster-ros/src/os_sensor_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,16 @@ OusterSensor::OusterSensor(const std::string& name,
OusterSensor::OusterSensor(const rclcpp::NodeOptions& options)
: OusterSensor("os_sensor", options) {}

OusterSensor::~OusterSensor() {
RCLCPP_INFO(get_logger(), "OusterDriver::~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
Expand Down Expand Up @@ -323,7 +333,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<uint8_t>{
lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE,
lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE};
Expand Down Expand Up @@ -787,24 +796,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<std::thread>([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<std::thread>([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.");
Expand Down
5 changes: 3 additions & 2 deletions ouster-ros/src/os_sensor_node.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -66,6 +67,8 @@ class OusterSensor : public OusterSensorNodeBase {

virtual void cleanup();

void halt();

private:
void declare_parameters();

Expand Down Expand Up @@ -162,11 +165,9 @@ class OusterSensor : public OusterSensorNodeBase {
std::unique_ptr<std::thread> sensor_connection_thread;

std::atomic<bool> imu_packets_processing_thread_active = {false};
std::atomic<bool> imu_packets_skip;
std::unique_ptr<std::thread> imu_packets_processing_thread;

std::atomic<bool> lidar_packets_processing_thread_active = {false};
std::atomic<bool> lidar_packets_skip;
std::unique_ptr<std::thread> lidar_packets_processing_thread;

bool force_sensor_reinit = false;
Expand Down
2 changes: 1 addition & 1 deletion ouster-sensor-msgs/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ouster_sensor_msgs</name>
<version>0.11.2</version>
<version>0.12.0</version>
<description>ouster_ros message and service definitions</description>
<maintainer email="[email protected]">ouster developers</maintainer>
<license>BSD</license>
Expand Down

0 comments on commit 726b0d5

Please sign in to comment.