diff --git a/.gitignore b/.gitignore index 8586665df..d9730f311 100644 --- a/.gitignore +++ b/.gitignore @@ -43,3 +43,13 @@ CMakeLists.txt.user # pre-commit node_modules/ + +# Local notes / scratch files +claude.txt + +# Pasted screenshots and videos used for debugging +Pasted image*.png +Pasted image*.jpg +*.MOV +*.mov +*.mp4 diff --git a/README.md b/README.md index ee575b556..edd7af4ad 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,7 @@ Nebula works with ROS 2 and is the recommended sensor driver for the [Autoware]( - [Quick start](#quick-start) - [Agnocast](#agnocast) - [Building only specific vendors](#building-only-specific-vendors) + - [Ouster](#ouster) - [Migration to Nebula 0.3.0](#migration-to-nebula-030) ## Documentation @@ -178,8 +179,37 @@ Available vendor packages are: - `nebula_hesai` - Hesai LiDARs (Pandar series, AT128, OT128, etc.) - `nebula_velodyne` - Velodyne LiDARs (VLP-16, VLP-32, VLS-128) - `nebula_robosense` - Robosense LiDARs (Bpearl, Helios) +- `nebula_ouster` - Ouster LiDARs (OS0, OS1, OS2 — all beam counts; native decoder, no external SDK) - `nebula_continental` - Continental radars (ARS548, SRR520) +## Ouster + +The `nebula_ouster` package is a native Nebula driver for Ouster OS-0 / OS-1 / OS-2 sensors +(any beam count, including OS-128). It decodes Ouster UDP packets directly and does not depend +on `ouster-sdk`. + +Setup steps: + +1. Build the package: + + ```bash + colcon build --packages-up-to nebula_ouster --cmake-args -DCMAKE_BUILD_TYPE=Release + ``` + +2. Edit `src/nebula_ouster/nebula_ouster/config/ouster_sensor.param.yaml` and set + `connection.sensor_ip` and `connection.host_ip` to match your environment. Update + `frame_id` if needed. + +3. Launch: + + ```bash + ros2 launch nebula_ouster ouster_launch_all_hw.xml + ``` + +The decoder publishes point clouds on `/points`, IMU samples on `/imu`, and raw packets on +`/packets`. For offline rosbag replay without the sensor, set `metadata_file` to a cached +metadata JSON path and launch with `launch_hw:=false`. + ## Migration to Nebula 0.3.0 Version 0.3.0 separates vendor-specific functionality into individual packages. This allows users diff --git a/src/nebula/package.xml b/src/nebula/package.xml index 2c5fe1d81..7e37515db 100644 --- a/src/nebula/package.xml +++ b/src/nebula/package.xml @@ -13,6 +13,7 @@ nebula_continental nebula_hesai + nebula_ouster nebula_robosense nebula_velodyne diff --git a/src/nebula_ouster/README.md b/src/nebula_ouster/README.md new file mode 100644 index 000000000..20d18c5d7 --- /dev/null +++ b/src/nebula_ouster/README.md @@ -0,0 +1,62 @@ +# Nebula Ouster sensor package + +Native Nebula driver for Ouster LiDAR sensors (OS-0 / OS-1 / OS-2 at 32, 64, or 128 beams). +The driver decodes Ouster UDP packets directly — it does **not** depend on `ouster-sdk`. + +## Features + +- Native packet parsing for the `RNG19_RFL8_SIG16_NIR16` (single return), + `RNG19_RFL8_SIG16_NIR16_DUAL` (dual return), and `LEGACY` UDP profiles. +- Dual return support — points from both returns are published with the correct `return_type` + field (`FIRST` and `LAST`). +- IMU output — Ouster IMU packets (~100 Hz) are decoded and published as `sensor_msgs/Imu`. +- Metadata caching — sensor metadata JSON can be cached to a file so offline rosbag replay + does not require the sensor to be reachable. +- No external SDK dependencies; uses only Nebula's built-in HTTP and UDP clients. + +## Package structure + +The driver is split into the four standard Nebula sub-packages: + +- **nebula_ouster_common** — sensor configuration structs +- **nebula_ouster_decoders** — packet parsing, XYZ lookup, metadata JSON parsing +- **nebula_ouster_hw_interfaces** — UDP receive socket wrapper +- **nebula_ouster** — ROS 2 wrapper node, launch files, diagnostics + +## Building + +```bash +colcon build --packages-up-to nebula_ouster +``` + +## Running + +Edit `config/ouster_sensor.param.yaml` and set `connection.sensor_ip` and `connection.host_ip` +to match your network. Then: + +```bash +# Live hardware +ros2 launch nebula_ouster ouster_launch_all_hw.xml + +# Offline replay (uses cached metadata_file; subscribes to NebulaPackets on the 'packets' topic) +ros2 launch nebula_ouster ouster_launch_all_hw.xml launch_hw:=false +``` + +## Topics + +| Topic | Type | Description | +|-------|------|-------------| +| `points` | `sensor_msgs/PointCloud2` | Decoded point cloud (`PointXYZIRCAEDT`) | +| `imu` | `sensor_msgs/Imu` | IMU sample (~100 Hz) | +| `packets` | `nebula_msgs/NebulaPackets` | Raw packet stream for rosbag recording | + +## Parameters + +See `config/ouster_sensor.param.yaml` for the full list. Key parameters: + +- `connection.sensor_ip` / `connection.host_ip` / `connection.data_port` +- `connection.imu_port` — dedicated UDP socket for IMU packets (set to 0 to disable) +- `launch_hw` — `true` for live hardware, `false` for rosbag replay via `NebulaPackets` +- `metadata_file` — optional cache path; enables offline replay without the sensor +- `fov.azimuth.min_deg` / `max_deg`, `fov.elevation.min_deg` / `max_deg` — FoV crop +- `frame_id` — TF frame used for the point cloud and IMU topics diff --git a/src/nebula_ouster/nebula_ouster/CMakeLists.txt b/src/nebula_ouster/nebula_ouster/CMakeLists.txt new file mode 100644 index 000000000..dcd767650 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.20) +project(nebula_ouster) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +add_library( + ouster_ros_wrapper SHARED + src/ouster_ros_wrapper.cpp) + +target_include_directories( + ouster_ros_wrapper + PUBLIC $ + $) + +target_link_libraries( + ouster_ros_wrapper + PUBLIC diagnostic_updater::diagnostic_updater + nebula_ouster_decoders::nebula_ouster_decoders + nebula_ouster_hw_interfaces::nebula_ouster_hw_interfaces + nebula_core_ros::nebula_core_ros) +ament_target_dependencies( + ouster_ros_wrapper + PUBLIC + diagnostic_msgs + nebula_msgs + rclcpp + rclcpp_components + sensor_msgs + std_msgs) + +rclcpp_components_register_node(ouster_ros_wrapper PLUGIN "nebula::ros::OusterRosWrapper" + EXECUTABLE ouster_ros_wrapper_node) + +install( + TARGETS ouster_ros_wrapper + EXPORT export_ouster_ros_wrapper + LIBRARY DESTINATION lib) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) + +install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_ouster_ros_wrapper) + +ament_export_dependencies( + diagnostic_msgs + diagnostic_updater + nebula_core_common + nebula_ouster_common + nebula_core_decoders + nebula_ouster_decoders + nebula_core_hw_interfaces + nebula_ouster_hw_interfaces + nebula_core_ros + nebula_msgs + rclcpp_components + sensor_msgs + std_msgs) + +ament_package() diff --git a/src/nebula_ouster/nebula_ouster/config/ouster_sensor.param.yaml b/src/nebula_ouster/nebula_ouster/config/ouster_sensor.param.yaml new file mode 100644 index 000000000..c925353cb --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/config/ouster_sensor.param.yaml @@ -0,0 +1,46 @@ +# Ouster Sensor ROS 2 Parameters +# Example configuration for an OS-128. Adjust `connection.*` and `frame_id` to match your setup. + +/**: + ros__parameters: + # ROS wrapper configuration + sensor_model: OS-128 + launch_hw: true + frame_id: ouster_lidar + + # Optional: cache sensor metadata JSON. When launch_hw is true and this path is set, + # the wrapper saves metadata fetched via HTTP here. When launch_hw is false, it loads + # metadata from this file instead of contacting the sensor. Leave empty to always use HTTP. + metadata_file: "" + + # Network configuration. `data_port` 7502 is the Ouster default for lidar data; the sensor + # also sends IMU packets to the same port. + connection: + sensor_ip: 192.168.1.201 + host_ip: 192.168.1.100 + data_port: 7502 + # Set to 0 to disable the separate IMU socket. Ouster's factory default is 7503. + imu_port: 7503 + filter_sender_ip: true + + # Decoder configuration — points outside this azimuth/elevation range are filtered out. + fov: + azimuth: + min_deg: 0.0 + max_deg: 360.0 + elevation: + min_deg: -45.0 + max_deg: 45.0 + + # Diagnostics configuration + diagnostics: + pointcloud_publish_rate: + frequency_ok: + min_hz: 9.0 + max_hz: 11.0 + frequency_warn: + min_hz: 8.0 + max_hz: 12.0 + num_frame_transition: 1 + packet_liveness: + timeout_ms: 1000 diff --git a/src/nebula_ouster/nebula_ouster/include/nebula_ouster/ouster_ros_wrapper.hpp b/src/nebula_ouster/nebula_ouster/include/nebula_ouster/ouster_ros_wrapper.hpp new file mode 100644 index 000000000..8ac5e52c1 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/include/nebula_ouster/ouster_ros_wrapper.hpp @@ -0,0 +1,183 @@ +// Copyright 2026 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_OUSTER_ROS_WRAPPER_HPP +#define NEBULA_OUSTER_ROS_WRAPPER_HPP + +#include "nebula_ouster_decoders/ouster_decoder.hpp" +#include "nebula_ouster_hw_interfaces/ouster_hw_interface.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::ros +{ + +struct ConfigError +{ + enum class Code : uint8_t { + PARAMETER_DECLARATION_FAILED, ///< Parameter declaration/read failed. + PARAMETER_VALIDATION_FAILED, ///< Parameter value failed semantic validation. + }; + + Code code; + std::string message; +}; + +/// @brief Read and validate Ouster driver configuration from ROS parameters. +/// @param node Node used to declare/read parameters. +/// @return Parsed OusterSensorConfiguration or ConfigError on validation failure. +util::expected load_config_from_ros_parameters( + rclcpp::Node & node); + +/// @brief ROS 2 wrapper for the Ouster LiDAR driver +/// @details This node bridges the C++ driver with ROS 2. +/// Responsibilities: +/// - Turn ROS 2 parameters into sensor configuration +/// - Initialize decoder and hardware interface +/// - Forward packets from HW interface and pass to decoder +/// - Convert decoded point clouds to ROS messages +/// - Publish point clouds on ROS topics +/// - Optionally: provide services for runtime configuration +class OusterRosWrapper : public rclcpp::Node +{ +public: + struct Error + { + enum class Code : uint8_t { + HW_INTERFACE_NOT_INITIALIZED, ///< Stream start requested while HW interface is absent. + HW_STREAM_START_FAILED, ///< Underlying HW interface failed to start. + }; + + Code code; + std::string message; + }; + + /// @brief Construct the ROS 2 node and initialize decoder + optional HW stream. + /// @param options Standard ROS 2 component/node options. + /// @throws std::runtime_error on invalid configuration or startup failures. + explicit OusterRosWrapper(const rclcpp::NodeOptions & options); + ~OusterRosWrapper() override; + +private: + /// @brief Resources used only when launch_hw is true. + struct OnlineMode + { + explicit OnlineMode(drivers::ConnectionConfiguration connection_configuration) + : hw_interface(std::move(connection_configuration)) + { + } + + drivers::OusterHwInterface hw_interface; + rclcpp::Publisher::SharedPtr packets_pub; + std::unique_ptr current_scan_packets_msg{ + std::make_unique()}; + /// @brief Serializes access to current_scan_packets_msg across lidar+imu socket threads. + std::mutex packets_mutex; + }; + + /// @brief Resources used only when launch_hw is false. + struct OfflineMode + { + rclcpp::Subscription::SharedPtr packets_sub; + }; + + struct Publishers + { + rclcpp::Publisher::SharedPtr points; + rclcpp::Publisher::SharedPtr imu; + rclcpp::Publisher::SharedPtr receive_duration_ms; + rclcpp::Publisher::SharedPtr decode_duration_ms; + rclcpp::Publisher::SharedPtr publish_duration_ms; + }; + + struct Diagnostics + { + explicit Diagnostics(rclcpp::Node * node) : updater(node) {} + + diagnostic_updater::Updater updater; + std::optional publish_rate; + std::optional packet_liveness; + }; + + /// @brief Publish a decoded pointcloud to ROS. + /// @param pointcloud Decoded pointcloud from the decoder. + /// @param timestamp_s Scan timestamp in seconds, epoch time. + void publish_pointcloud_callback( + const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s); + + /// @brief Publish a decoded IMU sample to ROS. + void publish_imu_callback(const drivers::OusterImuSample & sample); + + /// @brief Process one received UDP packet through the decoder pipeline. + /// @param packet Raw packet payload. + /// @param metadata Transport metadata provided by the UDP receiver. + void receive_cloud_packet_callback( + std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata); + + /// @brief Process one replayed NebulaPackets message. + /// @param packets_msg Packed scan data used for software-only replay. + void receive_packets_message_callback( + std::unique_ptr packets_msg); + + /// @brief Configure common diagnostics used by most sensor integrations. + void initialize_diagnostics(); + + /// @brief Decode one packet and publish relevant outputs. + /// @param packet Raw packet payload. + /// @param receive_duration_ns Time spent in transport receive path for this packet. + void process_packet(const std::vector & packet, uint64_t receive_duration_ns); + + /// @brief Publish receive/decode/publish debug durations. + /// @param receive_duration_ns Time spent in transport receive path. + /// @param decode_duration_ns Time spent decoding one packet. + /// @param publish_duration_ns Time spent publishing one completed scan callback. + void publish_debug_durations( + uint64_t receive_duration_ns, uint64_t decode_duration_ns, uint64_t publish_duration_ns) const; + + static const char * to_cstr(Error::Code code); + + drivers::OusterSensorConfiguration config_; + std::string frame_id_; + Publishers publishers_; + Diagnostics diagnostics_; + + std::optional decoder_; + /// @brief Exactly one runtime mode is active: offline replay or online hardware mode. + /// @details During construction, neither mode is active. + std::variant runtime_mode_; +}; + +} // namespace nebula::ros + +#endif // NEBULA_OUSTER_ROS_WRAPPER_HPP diff --git a/src/nebula_ouster/nebula_ouster/launch/nebula_ouster.launch.xml b/src/nebula_ouster/nebula_ouster/launch/nebula_ouster.launch.xml new file mode 100644 index 000000000..89388b9bf --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/launch/nebula_ouster.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/src/nebula_ouster/nebula_ouster/launch/ouster_launch_all_hw.xml b/src/nebula_ouster/nebula_ouster/launch/ouster_launch_all_hw.xml new file mode 100644 index 000000000..b151d0b94 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/launch/ouster_launch_all_hw.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + diff --git a/src/nebula_ouster/nebula_ouster/package.xml b/src/nebula_ouster/nebula_ouster/package.xml new file mode 100644 index 000000000..e69ef8c8d --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/package.xml @@ -0,0 +1,31 @@ + + + + nebula_ouster + 0.3.0 + Nebula Ouster ROS 2 Package + David Wong + Max Schmeller + + Apache 2 + TIER IV + + autoware_cmake + + diagnostic_msgs + diagnostic_updater + nebula_core_common + nebula_core_ros + nebula_msgs + nebula_ouster_common + nebula_ouster_decoders + nebula_ouster_hw_interfaces + rclcpp + rclcpp_components + sensor_msgs + std_msgs + + + ament_cmake + + diff --git a/src/nebula_ouster/nebula_ouster/src/ouster_ros_wrapper.cpp b/src/nebula_ouster/nebula_ouster/src/ouster_ros_wrapper.cpp new file mode 100644 index 000000000..16f70ec39 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/src/ouster_ros_wrapper.cpp @@ -0,0 +1,540 @@ +// Copyright 2026 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nebula_ouster/ouster_ros_wrapper.hpp" + +#include "nebula_ouster_decoders/ouster_metadata.hpp" +#include "nebula_ouster_decoders/ouster_packet.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::ros +{ + +namespace +{ +constexpr double k_ns_to_ms = 1e-6; + +uint64_t current_system_time_ns() +{ + return static_cast(std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count()); +} + +template +util::expected declare_required_parameter( + rclcpp::Node & node, const std::string & name, + const rcl_interfaces::msg::ParameterDescriptor & descriptor) +{ + try { + return node.declare_parameter(name, descriptor); + } catch (const std::exception & e) { + return ConfigError{ + ConfigError::Code::PARAMETER_DECLARATION_FAILED, + "Failed to declare/read parameter '" + name + "': " + e.what()}; + } +} + +util::expected validate_fov( + const drivers::FieldOfView & fov) +{ + if (fov.azimuth.start < 0.0F || fov.azimuth.start >= 360.0F) { + return ConfigError{ + ConfigError::Code::PARAMETER_VALIDATION_FAILED, + "Parameter 'fov.azimuth.min_deg' must be in [0, 360), got " + + std::to_string(fov.azimuth.start)}; + } + if (fov.azimuth.end <= fov.azimuth.start || fov.azimuth.end > 360.0F) { + return ConfigError{ + ConfigError::Code::PARAMETER_VALIDATION_FAILED, + "Parameter 'fov.azimuth.max_deg' must be in (" + std::to_string(fov.azimuth.start) + + ", 360], got " + std::to_string(fov.azimuth.end)}; + } + if (fov.elevation.start < -90.0F || fov.elevation.start >= 90.0F) { + return ConfigError{ + ConfigError::Code::PARAMETER_VALIDATION_FAILED, + "Parameter 'fov.elevation.min_deg' must be in [-90, 90), got " + + std::to_string(fov.elevation.start)}; + } + if (fov.elevation.end <= fov.elevation.start || fov.elevation.end > 90.0F) { + return ConfigError{ + ConfigError::Code::PARAMETER_VALIDATION_FAILED, + "Parameter 'fov.elevation.max_deg' must be in (" + std::to_string(fov.elevation.start) + + ", 90], got " + std::to_string(fov.elevation.end)}; + } + return std::monostate{}; +} + +std::string load_metadata_from_file(const std::string & path) +{ + std::ifstream ifs(path); + if (!ifs.is_open()) { + throw std::runtime_error("Cannot open metadata_file for reading: " + path); + } + std::stringstream buffer; + buffer << ifs.rdbuf(); + return buffer.str(); +} + +void save_metadata_to_file(const std::string & path, const std::string & json) +{ + std::ofstream ofs(path); + if (!ofs.is_open()) { + // Non-fatal: log at wrapper level instead. + return; + } + ofs << json; +} +} // namespace + +util::expected load_config_from_ros_parameters( + rclcpp::Node & node) +{ + drivers::OusterSensorConfiguration config{}; + + const auto host_ip = + declare_required_parameter(node, "connection.host_ip", param_read_only()); + if (!host_ip.has_value()) return host_ip.error(); + config.connection.host_ip = host_ip.value(); + if (!drivers::connections::parse_ip(config.connection.host_ip).has_value()) { + return ConfigError{ + ConfigError::Code::PARAMETER_VALIDATION_FAILED, + "Parameter 'connection.host_ip' must be a valid IPv4 address, got '" + + config.connection.host_ip + "'"}; + } + + const auto sensor_ip = + declare_required_parameter(node, "connection.sensor_ip", param_read_only()); + if (!sensor_ip.has_value()) return sensor_ip.error(); + config.connection.sensor_ip = sensor_ip.value(); + if (!drivers::connections::parse_ip(config.connection.sensor_ip).has_value()) { + return ConfigError{ + ConfigError::Code::PARAMETER_VALIDATION_FAILED, + "Parameter 'connection.sensor_ip' must be a valid IPv4 address, got '" + + config.connection.sensor_ip + "'"}; + } + + const auto data_port = + declare_required_parameter(node, "connection.data_port", param_read_only()); + if (!data_port.has_value()) return data_port.error(); + if (data_port.value() <= 0 || data_port.value() > 65535) { + return ConfigError{ + ConfigError::Code::PARAMETER_VALIDATION_FAILED, + "Parameter 'connection.data_port' must be in [1, 65535], got " + + std::to_string(data_port.value())}; + } + config.connection.data_port = static_cast(data_port.value()); + + const auto imu_port = + node.declare_parameter("connection.imu_port", 0, param_read_only()); + if (imu_port < 0 || imu_port > 65535) { + return ConfigError{ + ConfigError::Code::PARAMETER_VALIDATION_FAILED, + "Parameter 'connection.imu_port' must be in [0, 65535], got " + std::to_string(imu_port)}; + } + config.connection.imu_port = static_cast(imu_port); + + config.connection.filter_sender_ip = + node.declare_parameter("connection.filter_sender_ip", true, param_read_only()); + + const auto azimuth_min = + declare_required_parameter(node, "fov.azimuth.min_deg", param_read_write()); + if (!azimuth_min.has_value()) return azimuth_min.error(); + const auto azimuth_max = + declare_required_parameter(node, "fov.azimuth.max_deg", param_read_write()); + if (!azimuth_max.has_value()) return azimuth_max.error(); + const auto elevation_min = + declare_required_parameter(node, "fov.elevation.min_deg", param_read_write()); + if (!elevation_min.has_value()) return elevation_min.error(); + const auto elevation_max = + declare_required_parameter(node, "fov.elevation.max_deg", param_read_write()); + if (!elevation_max.has_value()) return elevation_max.error(); + + config.fov.azimuth.start = static_cast(azimuth_min.value()); + config.fov.azimuth.end = static_cast(azimuth_max.value()); + config.fov.elevation.start = static_cast(elevation_min.value()); + config.fov.elevation.end = static_cast(elevation_max.value()); + + const auto fov_validation = validate_fov(config.fov); + if (!fov_validation.has_value()) return fov_validation.error(); + + return config; +} + +OusterRosWrapper::OusterRosWrapper(const rclcpp::NodeOptions & options) +: Node("nebula_ouster_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + diagnostics_(this), + runtime_mode_(std::monostate{}) +{ + const bool launch_hw = declare_parameter("launch_hw", true, param_read_only()); + declare_parameter("sensor_model", "OS-128", param_read_only()); + frame_id_ = declare_parameter("frame_id", "ouster_lidar", param_read_write()); + const std::string metadata_file = + declare_parameter("metadata_file", "", param_read_only()); + + const auto config_or_error = load_config_from_ros_parameters(*this); + if (!config_or_error.has_value()) { + throw std::runtime_error( + "Invalid ouster sensor configuration: " + config_or_error.error().message); + } + config_ = config_or_error.value(); + + publishers_.points = + create_publisher("points", rclcpp::SensorDataQoS()); + publishers_.imu = create_publisher("imu", rclcpp::SensorDataQoS()); + publishers_.receive_duration_ms = + create_publisher("debug/receive_duration_ms", 10); + publishers_.decode_duration_ms = + create_publisher("debug/decode_duration_ms", 10); + publishers_.publish_duration_ms = + create_publisher("debug/publish_duration_ms", 10); + + initialize_diagnostics(); + + // Metadata acquisition: file (offline) or HTTP (online, with optional file cache). + std::string metadata_json; + if (!metadata_file.empty() && !launch_hw) { + RCLCPP_INFO(get_logger(), "Loading Ouster metadata from file: %s", metadata_file.c_str()); + metadata_json = load_metadata_from_file(metadata_file); + } else { + RCLCPP_INFO( + get_logger(), "Fetching Ouster metadata via HTTP from %s", + config_.connection.sensor_ip.c_str()); + metadata_json = drivers::fetch_ouster_metadata_http(config_.connection.sensor_ip); + if (!metadata_file.empty()) { + save_metadata_to_file(metadata_file, metadata_json); + RCLCPP_INFO(get_logger(), "Cached metadata to %s", metadata_file.c_str()); + } + } + RCLCPP_INFO(get_logger(), "Got Ouster metadata (%zu bytes)", metadata_json.size()); + + auto metadata = drivers::parse_ouster_metadata(metadata_json); + config_.connection.receiver_mtu_bytes = + static_cast(metadata.lidar_packet_size_bytes); + + RCLCPP_INFO( + get_logger(), + "Ouster UDP: listening on %s:%u filter_sender_ip=%s (sensor_ip=%s) receiver_mtu=%u " + "(profile=%d, %ux%u beams, %u columns_per_packet)", + config_.connection.host_ip.c_str(), config_.connection.data_port, + config_.connection.filter_sender_ip ? "true" : "false", config_.connection.sensor_ip.c_str(), + config_.connection.receiver_mtu_bytes, static_cast(metadata.udp_profile_lidar), + metadata.pixels_per_column, metadata.columns_per_frame, metadata.columns_per_packet); + + decoder_.emplace( + config_.fov, std::move(metadata), + [this](const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { + publish_pointcloud_callback(pointcloud, timestamp_s); + }); + decoder_->set_imu_callback( + [this](const drivers::OusterImuSample & sample) { publish_imu_callback(sample); }); + + if (launch_hw) { + runtime_mode_.emplace(config_.connection); + auto & online_mode = std::get(runtime_mode_); + online_mode.packets_pub = + create_publisher("packets", rclcpp::SensorDataQoS()); + const auto callback_result = online_mode.hw_interface.register_scan_callback( + [this]( + std::vector & raw_packet, + const drivers::connections::UdpSocket::RxMetadata & metadata) { + receive_cloud_packet_callback(raw_packet, metadata); + }); + if (!callback_result.has_value()) { + throw std::runtime_error( + "Failed to register ouster sensor packet callback: " + callback_result.error().message); + } + + // Register a separate callback for the IMU socket (when imu_port is set). This keeps the + // IMU thread completely isolated from the lidar thread — no shared state, no races. + if (config_.connection.imu_port != 0 && + config_.connection.imu_port != config_.connection.data_port) { + const auto imu_result = online_mode.hw_interface.register_imu_callback( + [this]( + std::vector & raw_packet, + const drivers::connections::UdpSocket::RxMetadata & /*metadata*/) { + if (!decoder_) return; + if (raw_packet.size() != decoder_->metadata().imu_packet_size_bytes) return; + const auto raw = drivers::ouster_packet::parse_imu_packet(raw_packet.data()); + drivers::OusterImuSample s{}; + s.timestamp_ns = raw.sys_timestamp_ns; + constexpr float k_g = 9.80665f; + constexpr float k_deg_to_rad = static_cast(M_PI) / 180.0f; + s.accel_x = raw.accel_x_g * k_g; + s.accel_y = raw.accel_y_g * k_g; + s.accel_z = raw.accel_z_g * k_g; + s.gyro_x = raw.gyro_x_dps * k_deg_to_rad; + s.gyro_y = raw.gyro_y_dps * k_deg_to_rad; + s.gyro_z = raw.gyro_z_dps * k_deg_to_rad; + publish_imu_callback(s); + }); + if (!imu_result.has_value()) { + throw std::runtime_error( + "Failed to register ouster IMU callback: " + imu_result.error().message); + } + } + + const auto stream_start_result = online_mode.hw_interface.sensor_interface_start(); + if (!stream_start_result.has_value()) { + throw std::runtime_error( + "Failed to start ouster sensor stream: " + stream_start_result.error().message); + } + } else { + runtime_mode_.emplace(); + auto & offline_mode = std::get(runtime_mode_); + offline_mode.packets_sub = create_subscription( + "packets", rclcpp::SensorDataQoS(), + [this](std::unique_ptr packets_msg) { + receive_packets_message_callback(std::move(packets_msg)); + }); + RCLCPP_INFO( + get_logger(), "Hardware connection disabled, listening for packets on %s", + offline_mode.packets_sub->get_topic_name()); + } +} + +OusterRosWrapper::~OusterRosWrapper() +{ + auto * online_mode = std::get_if(&runtime_mode_); + if (!online_mode) return; + + const auto stop_result = online_mode->hw_interface.sensor_interface_stop(); + if (!stop_result.has_value()) { + RCLCPP_WARN( + get_logger(), "Failed to stop ouster sensor stream cleanly: %s", + stop_result.error().message.c_str()); + } +} + +void OusterRosWrapper::initialize_diagnostics() +{ + const double min_ok_hz = declare_parameter( + "diagnostics.pointcloud_publish_rate.frequency_ok.min_hz", param_read_only()); + const double max_ok_hz = declare_parameter( + "diagnostics.pointcloud_publish_rate.frequency_ok.max_hz", param_read_only()); + const double min_warn_hz = declare_parameter( + "diagnostics.pointcloud_publish_rate.frequency_warn.min_hz", param_read_only()); + const double max_warn_hz = declare_parameter( + "diagnostics.pointcloud_publish_rate.frequency_warn.max_hz", param_read_only()); + const auto num_frame_transition = static_cast(declare_parameter( + "diagnostics.pointcloud_publish_rate.num_frame_transition", param_read_only())); + const auto packet_liveness_timeout_ms = + declare_parameter("diagnostics.packet_liveness.timeout_ms", param_read_only()); + + if (min_warn_hz >= min_ok_hz) { + throw std::runtime_error( + "Invalid diagnostics config: frequency_warn.min_hz must be smaller than " + "frequency_ok.min_hz."); + } + if (max_warn_hz <= max_ok_hz) { + throw std::runtime_error( + "Invalid diagnostics config: frequency_warn.max_hz must be greater than " + "frequency_ok.max_hz."); + } + if (packet_liveness_timeout_ms <= 0) { + throw std::runtime_error( + "Invalid diagnostics config: packet_liveness.timeout_ms must be positive."); + } + const auto packet_liveness_timeout = std::chrono::milliseconds(packet_liveness_timeout_ms); + + diagnostics_.publish_rate.emplace( + this, custom_diagnostic_tasks::RateBoundStatusParam{min_ok_hz, max_ok_hz}, + custom_diagnostic_tasks::RateBoundStatusParam{min_warn_hz, max_warn_hz}, + std::max(1, num_frame_transition), false, true, "pointcloud publish rate"); + diagnostics_.packet_liveness.emplace( + "packet_receive", this, + rclcpp::Duration::from_seconds( + std::chrono::duration(packet_liveness_timeout).count() * 1e-3)); + + diagnostics_.updater.setHardwareID(frame_id_); + diagnostics_.updater.add(diagnostics_.publish_rate.value()); + diagnostics_.updater.add(diagnostics_.packet_liveness.value()); + diagnostics_.updater.force_update(); +} + +void OusterRosWrapper::publish_pointcloud_callback( + const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) +{ + if (!pointcloud) return; + + auto ros_pc_msg_ptr = + std::make_unique(nebula::ros::to_ros_msg(*pointcloud)); + ros_pc_msg_ptr->header.stamp = rclcpp::Time(static_cast(timestamp_s * 1e9)); + ros_pc_msg_ptr->header.frame_id = frame_id_; + publishers_.points->publish(std::move(ros_pc_msg_ptr)); + + diagnostics_.publish_rate->tick(); +} + +void OusterRosWrapper::publish_imu_callback(const drivers::OusterImuSample & sample) +{ + if (!publishers_.imu) return; + + auto msg = std::make_unique(); + msg->header.stamp = rclcpp::Time(static_cast(sample.timestamp_ns)); + msg->header.frame_id = frame_id_; + + msg->linear_acceleration.x = sample.accel_x; + msg->linear_acceleration.y = sample.accel_y; + msg->linear_acceleration.z = sample.accel_z; + msg->angular_velocity.x = sample.gyro_x; + msg->angular_velocity.y = sample.gyro_y; + msg->angular_velocity.z = sample.gyro_z; + // Orientation is not provided by the Ouster IMU; mark covariance[0] = -1 per ROS convention. + msg->orientation_covariance[0] = -1.0; + + publishers_.imu->publish(std::move(msg)); +} + +void OusterRosWrapper::receive_cloud_packet_callback( + std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata) +{ + diagnostics_.packet_liveness->tick(); + + if (metadata.truncated) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 2000, + "Ouster UDP payload was truncated at %zu bytes (kernel/datagram is larger than " + "connection.receiver_mtu). Decoding is skipped for this datagram.", + packet.size()); + return; + } + + auto * online_mode = std::get_if(&runtime_mode_); + if (online_mode && online_mode->packets_pub) { + // Two socket threads (lidar+imu) can enter this function concurrently — serialize the + // packets_msg append below so the vector isn't corrupted. + std::lock_guard guard(online_mode->packets_mutex); + if (!online_mode->current_scan_packets_msg) { + online_mode->current_scan_packets_msg = std::make_unique(); + } + + const auto packet_timestamp_ns = metadata.timestamp_ns.value_or(current_system_time_ns()); + nebula_msgs::msg::NebulaPacket packet_msg{}; + packet_msg.stamp.sec = static_cast(packet_timestamp_ns / 1'000'000'000ULL); + packet_msg.stamp.nanosec = static_cast(packet_timestamp_ns % 1'000'000'000ULL); + packet_msg.data = packet; + if (online_mode->current_scan_packets_msg->packets.empty()) { + online_mode->current_scan_packets_msg->header.stamp = packet_msg.stamp; + online_mode->current_scan_packets_msg->header.frame_id = frame_id_; + } + online_mode->current_scan_packets_msg->packets.emplace_back(std::move(packet_msg)); + } + + process_packet(packet, metadata.packet_perf_counters.receive_duration_ns); +} + +void OusterRosWrapper::receive_packets_message_callback( + std::unique_ptr packets_msg) +{ + if (!packets_msg) return; + + if (!std::holds_alternative(runtime_mode_)) { + RCLCPP_ERROR_ONCE( + get_logger(), + "Ignoring NebulaPackets. Launch with launch_hw:=false to enable NebulaPackets replay."); + return; + } + + for (const auto & packet_msg : packets_msg->packets) { + diagnostics_.packet_liveness->tick(); + process_packet(packet_msg.data, 0U); + } +} + +void OusterRosWrapper::process_packet( + const std::vector & packet, const uint64_t receive_duration_ns) +{ + if (!decoder_) return; + + const auto decode_result = decoder_->unpack(packet); + publish_debug_durations( + receive_duration_ns, decode_result.performance_counters.decode_time_ns, + decode_result.performance_counters.callback_time_ns); + + if (!decode_result.metadata_or_error.has_value()) { + RCLCPP_DEBUG_THROTTLE( + get_logger(), *get_clock(), 1000, "Packet decode failed: %s.", + drivers::to_cstr(decode_result.metadata_or_error.error())); + return; + } + + auto * online_mode = std::get_if(&runtime_mode_); + if ( + online_mode && online_mode->packets_pub && + decode_result.metadata_or_error.value().did_scan_complete && + online_mode->current_scan_packets_msg && + !online_mode->current_scan_packets_msg->packets.empty()) { + online_mode->packets_pub->publish(std::move(online_mode->current_scan_packets_msg)); + online_mode->current_scan_packets_msg = std::make_unique(); + } +} + +void OusterRosWrapper::publish_debug_durations( + uint64_t receive_duration_ns, uint64_t decode_duration_ns, uint64_t publish_duration_ns) const +{ + const auto publish_metric = [](const auto & publisher, const uint64_t duration_ns) { + if (!publisher) return; + if ( + publisher->get_subscription_count() == 0 && + publisher->get_intra_process_subscription_count() == 0) { + return; + } + std_msgs::msg::Float64 msg; + msg.data = static_cast(duration_ns) * k_ns_to_ms; + publisher->publish(msg); + }; + + publish_metric(publishers_.receive_duration_ms, receive_duration_ns); + publish_metric(publishers_.decode_duration_ms, decode_duration_ns); + publish_metric(publishers_.publish_duration_ms, publish_duration_ns); +} + +const char * OusterRosWrapper::to_cstr(const Error::Code code) +{ + switch (code) { + case Error::Code::HW_INTERFACE_NOT_INITIALIZED: + return "hardware interface not initialized"; + case Error::Code::HW_STREAM_START_FAILED: + return "hardware stream start failed"; + } + return "unknown wrapper error"; +} + +} // namespace nebula::ros + +RCLCPP_COMPONENTS_REGISTER_NODE(nebula::ros::OusterRosWrapper) diff --git a/src/nebula_ouster/nebula_ouster_common/CMakeLists.txt b/src/nebula_ouster/nebula_ouster_common/CMakeLists.txt new file mode 100644 index 000000000..148da22ce --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_common/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.20) +project(nebula_ouster_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +add_library(nebula_ouster_common INTERFACE) + +target_include_directories( + nebula_ouster_common + INTERFACE $ + $) +target_link_libraries(nebula_ouster_common + INTERFACE nebula_core_common::nebula_core_common + nebula_core_decoders::nebula_core_decoders) + +install(TARGETS nebula_ouster_common EXPORT export_nebula_ouster_common) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) + +ament_export_targets(export_nebula_ouster_common) +ament_export_dependencies(nebula_core_common nebula_core_decoders) + +ament_package() diff --git a/src/nebula_ouster/nebula_ouster_common/include/nebula_ouster_common/ouster_configuration.hpp b/src/nebula_ouster/nebula_ouster_common/include/nebula_ouster_common/ouster_configuration.hpp new file mode 100644 index 000000000..1102cf2a4 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_common/include/nebula_ouster_common/ouster_configuration.hpp @@ -0,0 +1,117 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#include + +namespace nebula::drivers +{ + +/// @brief Network endpoint settings used by the ouster driver. +/// @details These values define where the UDP packets are received from and on which local port. +struct ConnectionConfiguration +{ + /// IP address of the host interface that receives LiDAR packets. + std::string host_ip; + /// IP address assigned to the sensor. + std::string sensor_ip; + /// UDP destination port on the host for sensor data. + uint16_t data_port; + /// UDP destination port on the host for IMU data. Set to 0 to disable the IMU socket (IMU + /// packets may still arrive on @c data_port if the sensor is configured that way). + uint16_t imu_port{0}; + /// Maximum UDP payload we allocate for recv (Ouster frames are often 12k–64kB; default was 1500). + uint32_t receiver_mtu_bytes{65527}; + /// If true, require LiDAR packets from @c sensor_ip (Nebula UDP filter checks IP only). + bool filter_sender_ip{true}; +}; + +// JSON: extra keys optional so older config files keep working. +inline void to_json(nlohmann::json & j, const ConnectionConfiguration & c) +{ + j = nlohmann::json{ + {"host_ip", c.host_ip}, + {"sensor_ip", c.sensor_ip}, + {"data_port", c.data_port}, + {"receiver_mtu_bytes", c.receiver_mtu_bytes}, + {"filter_sender_ip", c.filter_sender_ip}, + }; +} + +inline void from_json(const nlohmann::json & j, ConnectionConfiguration & c) +{ + j.at("host_ip").get_to(c.host_ip); + j.at("sensor_ip").get_to(c.sensor_ip); + j.at("data_port").get_to(c.data_port); + if (j.contains("imu_port")) { + j.at("imu_port").get_to(c.imu_port); + } + if (j.contains("receiver_mtu_bytes")) { + j.at("receiver_mtu_bytes").get_to(c.receiver_mtu_bytes); + } + if (j.contains("filter_sender_ip")) { + j.at("filter_sender_ip").get_to(c.filter_sender_ip); + } +} + +/// @brief Sensor-specific configuration for the Ouster LiDAR +/// @details Minimal tutorial configuration that combines connection settings and angular filtering. +struct OusterSensorConfiguration +{ + ConnectionConfiguration connection; + FieldOfView fov{}; +}; + +/// @brief Serialize OusterSensorConfiguration to JSON. +/// @details The JSON schema contains: +/// - `connection` (ConnectionConfiguration) +/// - `fov` (FieldOfView) +inline void to_json(nlohmann::json & j, const OusterSensorConfiguration & config) +{ + j = nlohmann::json{}; + j["connection"] = config.connection; + + nlohmann::json azimuth_fov; + azimuth_fov["min_deg"] = config.fov.azimuth.start; + azimuth_fov["max_deg"] = config.fov.azimuth.end; + + nlohmann::json elevation_fov; + elevation_fov["min_deg"] = config.fov.elevation.start; + elevation_fov["max_deg"] = config.fov.elevation.end; + + j["fov"] = {{"azimuth", azimuth_fov}, {"elevation", elevation_fov}}; +} + +/// @brief Parse OusterSensorConfiguration from JSON. +/// @throws nlohmann::json::exception if required fields are missing or have incompatible types. +inline void from_json(const nlohmann::json & j, OusterSensorConfiguration & config) +{ + j.at("connection").get_to(config.connection); + + const auto & fov_json = j.at("fov"); + const auto & azimuth_fov = fov_json.at("azimuth"); + azimuth_fov.at("min_deg").get_to(config.fov.azimuth.start); + azimuth_fov.at("max_deg").get_to(config.fov.azimuth.end); + + const auto & elevation_fov = fov_json.at("elevation"); + elevation_fov.at("min_deg").get_to(config.fov.elevation.start); + elevation_fov.at("max_deg").get_to(config.fov.elevation.end); +} + +} // namespace nebula::drivers diff --git a/src/nebula_ouster/nebula_ouster_common/package.xml b/src/nebula_ouster/nebula_ouster_common/package.xml new file mode 100644 index 000000000..058a4a71a --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_common/package.xml @@ -0,0 +1,22 @@ + + + + nebula_ouster_common + 0.3.0 + Nebula Ouster Common Libraries and Headers + David Wong + Max Schmeller + + Apache 2 + TIER IV + + autoware_cmake + ros_environment + + nebula_core_common + nebula_core_decoders + + + ament_cmake + + diff --git a/src/nebula_ouster/nebula_ouster_decoders/CMakeLists.txt b/src/nebula_ouster/nebula_ouster_decoders/CMakeLists.txt new file mode 100644 index 000000000..4d0f1980c --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.20) +project(nebula_ouster_decoders) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(nlohmann_json REQUIRED) + +add_library( + nebula_ouster_decoders SHARED + src/ouster_decoder.cpp + src/ouster_metadata.cpp) + +target_include_directories( + nebula_ouster_decoders + PUBLIC $ + $) + +target_link_libraries( + nebula_ouster_decoders + PUBLIC nlohmann_json::nlohmann_json) + +ament_target_dependencies( + nebula_ouster_decoders + PUBLIC + nebula_core_common + nebula_core_decoders + nebula_core_hw_interfaces + nebula_ouster_common) + +install(TARGETS nebula_ouster_decoders EXPORT export_nebula_ouster_decoders) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_nebula_ouster_decoders) + +ament_export_dependencies( + nebula_core_common + nebula_core_decoders + nebula_core_hw_interfaces + nebula_ouster_common + nlohmann_json) + +ament_package() diff --git a/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_decoder.hpp b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_decoder.hpp new file mode 100644 index 000000000..968378511 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_decoder.hpp @@ -0,0 +1,121 @@ +// Copyright 2026 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_OUSTER_DECODER_HPP +#define NEBULA_OUSTER_DECODER_HPP + +#include "nebula_ouster_decoders/ouster_metadata.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula::drivers +{ + +/// @brief Error codes returned during packet decoding. +enum class DecodeError : uint8_t { + PACKET_FORMAT_INVALID, ///< Packet size did not match expected lidar or IMU packet. + CALLBACK_NOT_SET, ///< Decoder cannot publish scans because callback is missing. + EMPTY_PACKET, ///< Received packet had zero bytes. +}; + +/// @brief Convert DecodeError to a stable string literal for logging. +const char * to_cstr(DecodeError error); + +/// @brief Metadata extracted from a decoded packet. +struct PacketMetadata +{ + uint64_t packet_timestamp_ns{}; ///< Sensor-local timestamp of the packet (ns). + bool did_scan_complete{false}; ///< True if this packet closed out a scan. +}; + +/// @brief Performance metrics for packet decoding. +struct PerformanceCounters +{ + uint64_t decode_time_ns{0}; ///< Time spent decoding the packet (ns). + uint64_t callback_time_ns{0}; ///< Time spent in the pointcloud callback (ns). +}; + +/// @brief Result of decoding a single packet. +struct PacketDecodeResult +{ + PerformanceCounters performance_counters; + util::expected metadata_or_error; + + PacketDecodeResult() : performance_counters{}, metadata_or_error(DecodeError::CALLBACK_NOT_SET) {} +}; + +/// @brief Decoded IMU sample in SI units (m/s^2 for accel, rad/s for gyro). +struct OusterImuSample +{ + uint64_t timestamp_ns{}; + float accel_x{}, accel_y{}, accel_z{}; // m/s^2 + float gyro_x{}, gyro_y{}, gyro_z{}; // rad/s +}; + +/// @brief Decoder that parses Ouster UDP packets and publishes Nebula point clouds and IMU data. +/// Implemented with a custom packet parser — no dependency on ouster-sdk. +class OusterDecoder +{ +public: + /// @brief Callback type for publishing complete point clouds. + using pointcloud_callback_t = + std::function; + + /// @brief Callback type for IMU samples. Fires once per IMU UDP packet (~100 Hz). + using imu_callback_t = std::function; + + /// @param fov Angular crop in sensor spherical coordinates (degrees). + /// @param metadata Parsed Ouster metadata (beam angles, packet geometry, profile). + /// @param pointcloud_cb Callback invoked when a full scan is assembled. + OusterDecoder( + FieldOfView fov, OusterMetadata metadata, + pointcloud_callback_t pointcloud_cb); + + ~OusterDecoder(); + + OusterDecoder(const OusterDecoder &) = delete; + OusterDecoder & operator=(const OusterDecoder &) = delete; + OusterDecoder(OusterDecoder && other) noexcept; + OusterDecoder & operator=(OusterDecoder && other) noexcept; + + /// @brief Decode a single UDP packet. + /// @param packet Raw packet bytes from the sensor. + /// @return PacketDecodeResult with metadata on success, or DecodeError on failure. + /// @post performance_counters.decode_time_ns is always set. + [[nodiscard]] PacketDecodeResult unpack(const std::vector & packet); + + /// @brief Replace the callback used for completed scans. + void set_pointcloud_callback(pointcloud_callback_t pointcloud_cb); + + /// @brief Register an IMU callback. Pass nullptr to disable IMU output. + void set_imu_callback(imu_callback_t imu_cb); + + /// @brief Access the metadata this decoder was configured with. + [[nodiscard]] const OusterMetadata & metadata() const; + +private: + struct Impl; + std::unique_ptr impl_; +}; + +} // namespace nebula::drivers + +#endif // NEBULA_OUSTER_DECODER_HPP diff --git a/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_metadata.hpp b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_metadata.hpp new file mode 100644 index 000000000..319ab9bd8 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_metadata.hpp @@ -0,0 +1,87 @@ +// Copyright 2026 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_OUSTER_METADATA_HPP +#define NEBULA_OUSTER_METADATA_HPP + +#include "nebula_ouster_decoders/ouster_packet.hpp" + +#include +#include +#include + +namespace nebula::drivers +{ + +/// @brief Sensor metadata parsed from Ouster's sensor_info JSON document. +/// @details Only the fields actually needed for decoding are kept. The JSON layout is documented +/// in Ouster's software user manual (sensor_info endpoint). +struct OusterMetadata +{ + // Beam geometry (length == pixels_per_column, i.e. 128 for OS-128). + std::vector beam_altitude_angles_deg; + std::vector beam_azimuth_angles_deg; + + // Column stagger: integer column offset applied per beam when reassembling a scan. + std::vector pixel_shift_by_row; + + // Sensor frame origin offset (mm) — added to each XYZ along +Z of lidar frame. + double lidar_origin_to_beam_origin_mm{0.0}; + + // Scan geometry + uint32_t columns_per_frame{1024}; ///< e.g. 512, 1024, 2048 + uint32_t columns_per_packet{16}; ///< typically 16 for modern firmware + uint32_t pixels_per_column{128}; ///< number of beams (rows) + + // UDP packet format + ouster_packet::UdpProfileLidar udp_profile_lidar{ + ouster_packet::UdpProfileLidar::RNG19_RFL8_SIG16_NIR16}; + + // Derived packet sizes (bytes). + size_t lidar_packet_size_bytes{0}; + size_t imu_packet_size_bytes{ouster_packet::k_imu_packet_size}; + + // Informational — not required for decoding. + std::string sensor_prod_line; ///< e.g. "OS-1-128" + std::string lidar_mode; ///< e.g. "1024x10" + std::string firmware_version; + + /// @brief Number of returns encoded per pixel (1 for single, 2 for dual). + [[nodiscard]] uint8_t num_returns() const + { + return ouster_packet::get_layout(udp_profile_lidar).returns; + } + + /// @brief Recompute lidar_packet_size_bytes from the profile, columns-per-packet, and pixels. + void update_derived_sizes() + { + lidar_packet_size_bytes = + ouster_packet::lidar_packet_size(udp_profile_lidar, columns_per_packet, pixels_per_column); + } +}; + +/// @brief Parse a sensor_info JSON document (the output of /api/v1/sensor/metadata/sensor_info) into +/// an OusterMetadata struct. +/// @throws std::runtime_error when required fields are missing or malformed. +OusterMetadata parse_ouster_metadata(const std::string & sensor_info_json); + +/// @brief Fetch sensor metadata from a live Ouster sensor via HTTP (uses Nebula's HTTP client). +/// @param sensor_ip IPv4 address (or hostname) of the sensor. +/// @param timeout_ms HTTP receive timeout in milliseconds. +/// @return JSON string as returned by the sensor. +std::string fetch_ouster_metadata_http(const std::string & sensor_ip, int timeout_ms = 2000); + +} // namespace nebula::drivers + +#endif // NEBULA_OUSTER_METADATA_HPP diff --git a/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_packet.hpp b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_packet.hpp new file mode 100644 index 000000000..2ec4c8d41 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_packet.hpp @@ -0,0 +1,237 @@ +// Copyright 2026 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_OUSTER_PACKET_HPP +#define NEBULA_OUSTER_PACKET_HPP + +#include +#include +#include + +namespace nebula::drivers::ouster_packet +{ + +// Ouster uses little-endian wire format for all multibyte fields. + +/// @brief Ouster UDP lidar profile identifiers from metadata JSON. +enum class UdpProfileLidar : uint8_t { + LEGACY, + RNG19_RFL8_SIG16_NIR16, + RNG19_RFL8_SIG16_NIR16_DUAL, + RNG15_RFL8_NIR8, +}; + +/// @brief Byte sizes of packet-level headers and footers by profile. +struct ProfileLayout +{ + size_t packet_header_size; + size_t column_header_size; + size_t pixel_size; + size_t packet_footer_size; + uint8_t returns; ///< Returns per pixel (1 for single, 2 for dual). +}; + +/// @brief Layout constants for LEGACY profile. +/// Legacy packet has no dedicated header/footer; every column carries its own 16-byte header and +/// 16-byte footer, with 12-byte pixels. +constexpr ProfileLayout k_legacy_layout{ + /*packet_header_size=*/0, + /*column_header_size=*/16, + /*pixel_size=*/12, + /*packet_footer_size=*/0, + /*returns=*/1}; + +/// @brief Layout constants for RNG19_RFL8_SIG16_NIR16 (single return). +/// 32-byte packet header + 12-byte column header + 12-byte column footer + 32-byte packet footer. +/// Each pixel is 12 bytes: range(4) + reflectivity(1) + reserved(1) + signal(2) + near_ir(2) + pad(2). +constexpr ProfileLayout k_rng19_single_layout{ + /*packet_header_size=*/32, + /*column_header_size=*/12, + /*pixel_size=*/12, + /*packet_footer_size=*/32, + /*returns=*/1}; + +/// @brief Layout constants for RNG19_RFL8_SIG16_NIR16_DUAL (dual return). +/// Same framing as single-return profile but each pixel contains two returns: range(4)+refl(1)+ +/// pad(1)+signal(2) repeated twice, plus near_ir(2) and alignment pad. +constexpr ProfileLayout k_rng19_dual_layout{ + /*packet_header_size=*/32, + /*column_header_size=*/12, + /*pixel_size=*/24, + /*packet_footer_size=*/32, + /*returns=*/2}; + +/// @brief Pick the layout for a given profile. +inline ProfileLayout get_layout(UdpProfileLidar profile) +{ + switch (profile) { + case UdpProfileLidar::LEGACY: + return k_legacy_layout; + case UdpProfileLidar::RNG19_RFL8_SIG16_NIR16: + return k_rng19_single_layout; + case UdpProfileLidar::RNG19_RFL8_SIG16_NIR16_DUAL: + return k_rng19_dual_layout; + case UdpProfileLidar::RNG15_RFL8_NIR8: + // Not supported for OS-128 default modes; fall back to single-return framing. + return k_rng19_single_layout; + } + return k_rng19_single_layout; +} + +/// @brief Size of the per-column footer in the LEGACY profile (4 bytes). Modern profiles have no +/// per-column footer; they use a single packet-level footer instead. +constexpr size_t k_legacy_column_footer_size = 4; + +/// @brief Total bytes of a lidar packet for a given profile + columns per packet + beams. +inline size_t lidar_packet_size( + UdpProfileLidar profile, size_t columns_per_packet, size_t pixels_per_column) +{ + const auto layout = get_layout(profile); + if (profile == UdpProfileLidar::LEGACY) { + return columns_per_packet * + (layout.column_header_size + layout.pixel_size * pixels_per_column + + k_legacy_column_footer_size); + } + return layout.packet_header_size + + columns_per_packet * (layout.column_header_size + layout.pixel_size * pixels_per_column) + + layout.packet_footer_size; +} + +constexpr size_t k_imu_packet_size = 48; + +/// @brief Read a little-endian integer from a raw byte buffer. +template +inline T read_le(const uint8_t * src) +{ + T v{}; + std::memcpy(&v, src, sizeof(T)); + return v; +} + +/// @brief Column header fields shared across modern Ouster profiles. +struct ColumnHeader +{ + uint64_t timestamp_ns; ///< Sensor-local timestamp (ns). + uint16_t measurement_id; ///< Column index [0, columns_per_frame). + uint16_t status; ///< Bit 0 = valid measurement. +}; + +/// @brief Parse a column header from the packet buffer (modern profiles, 12 bytes). +inline ColumnHeader parse_column_header(const uint8_t * src) +{ + ColumnHeader h{}; + h.timestamp_ns = read_le(src); + h.measurement_id = read_le(src + 8); + h.status = read_le(src + 10); + return h; +} + +/// @brief Parse a LEGACY column header (16 bytes) — timestamp, measurement_id, encoder, status. +inline ColumnHeader parse_legacy_column_header(const uint8_t * src) +{ + ColumnHeader h{}; + h.timestamp_ns = read_le(src); + h.measurement_id = read_le(src + 8); + // bytes 10..11 frame_id, bytes 12..15 encoder_count, status bit lives in packet footer for + // legacy; approximate validity by a non-zero timestamp. + h.status = h.timestamp_ns != 0U ? 0x01U : 0x00U; + return h; +} + +/// @brief Frame id for modern profiles. +inline uint16_t parse_packet_frame_id(const uint8_t * packet_begin) +{ + // byte 2..3: frame_id (little endian) + return read_le(packet_begin + 2); +} + +/// @brief Pixel accessor helpers for RNG19 single-return profile. +struct Rng19Single +{ + /// @brief Range in millimeters; the low 19 bits of the 32-bit word hold range, upper bits hold + /// flags. Ouster defines range unit = 1 mm. + static uint32_t range_mm(const uint8_t * pixel) + { + return read_le(pixel) & 0x7FFFFU; + } + static uint8_t reflectivity(const uint8_t * pixel) { return pixel[4]; } + static uint16_t signal(const uint8_t * pixel) { return read_le(pixel + 6); } + static uint16_t near_ir(const uint8_t * pixel) { return read_le(pixel + 8); } +}; + +/// @brief Pixel accessor helpers for RNG19 dual-return profile. +struct Rng19Dual +{ + static uint32_t range1_mm(const uint8_t * pixel) + { + return read_le(pixel) & 0x7FFFFU; + } + static uint8_t reflectivity1(const uint8_t * pixel) { return pixel[4]; } + static uint16_t signal1(const uint8_t * pixel) { return read_le(pixel + 6); } + + static uint32_t range2_mm(const uint8_t * pixel) + { + return read_le(pixel + 8) & 0x7FFFFU; + } + static uint8_t reflectivity2(const uint8_t * pixel) { return pixel[12]; } + static uint16_t signal2(const uint8_t * pixel) { return read_le(pixel + 14); } + + static uint16_t near_ir(const uint8_t * pixel) { return read_le(pixel + 18); } +}; + +/// @brief Pixel accessor helpers for LEGACY profile (12 bytes per pixel). +/// Layout: range(4, only 20 low bits) + reflectivity(2) + signal(2) + near_ir(2) + reserved(2). +struct Legacy +{ + static uint32_t range_mm(const uint8_t * pixel) + { + return read_le(pixel) & 0xFFFFFU; // 20 bits of range in legacy + } + static uint16_t reflectivity(const uint8_t * pixel) { return read_le(pixel + 4); } + static uint16_t signal(const uint8_t * pixel) { return read_le(pixel + 6); } + static uint16_t near_ir(const uint8_t * pixel) { return read_le(pixel + 8); } +}; + +/// @brief Parsed IMU packet (48 bytes, always the same format). +struct ImuPacket +{ + uint64_t sys_timestamp_ns; + uint64_t accel_timestamp_ns; + uint64_t gyro_timestamp_ns; + float accel_x_g; ///< Acceleration in units of g + float accel_y_g; + float accel_z_g; + float gyro_x_dps; ///< Angular velocity in degrees per second + float gyro_y_dps; + float gyro_z_dps; +}; + +inline ImuPacket parse_imu_packet(const uint8_t * src) +{ + ImuPacket p{}; + p.sys_timestamp_ns = read_le(src); + p.accel_timestamp_ns = read_le(src + 8); + p.gyro_timestamp_ns = read_le(src + 16); + std::memcpy(&p.accel_x_g, src + 24, 4); + std::memcpy(&p.accel_y_g, src + 28, 4); + std::memcpy(&p.accel_z_g, src + 32, 4); + std::memcpy(&p.gyro_x_dps, src + 36, 4); + std::memcpy(&p.gyro_y_dps, src + 40, 4); + std::memcpy(&p.gyro_z_dps, src + 44, 4); + return p; +} + +} // namespace nebula::drivers::ouster_packet + +#endif // NEBULA_OUSTER_PACKET_HPP diff --git a/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_xyz_lut.hpp b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_xyz_lut.hpp new file mode 100644 index 000000000..578ec6ff5 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_xyz_lut.hpp @@ -0,0 +1,146 @@ +// Copyright 2026 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_OUSTER_XYZ_LUT_HPP +#define NEBULA_OUSTER_XYZ_LUT_HPP + +#include "nebula_ouster_decoders/ouster_metadata.hpp" + +#include +#include +#include +#include + +namespace nebula::drivers +{ + +/// @brief Precomputed per-beam / per-column trig tables for XYZ conversion. +/// @details Given a range measurement r at (beam, measurement_id), the Cartesian coordinates are: +/// encoder = -2π * measurement_id / columns_per_frame (Ouster convention — CW rotation) +/// theta = encoder + beam_azimuth_offset[beam] +/// phi = beam_altitude[beam] +/// r_m = range_mm * 1e-3 +/// x = r_m * cos(phi) * cos(theta) + beam_origin * cos(theta) +/// y = r_m * cos(phi) * sin(theta) + beam_origin * sin(theta) +/// z = r_m * sin(phi) +/// The beam_origin offset corresponds to lidar_origin_to_beam_origin_mm (converted to meters) and +/// accounts for the horizontal offset of the beam optical center from the sensor frame origin. +class OusterXyzLut +{ +public: + /// @param metadata Parsed sensor metadata. Requires valid beam angles and columns_per_frame. + explicit OusterXyzLut(const OusterMetadata & metadata) + : columns_per_frame_(metadata.columns_per_frame), + pixels_per_column_(metadata.pixels_per_column), + beam_origin_m_(metadata.lidar_origin_to_beam_origin_mm * 1e-3), + pixel_shift_(metadata.pixel_shift_by_row) + { + constexpr double k_deg_to_rad = M_PI / 180.0; + beam_cos_elev_.resize(pixels_per_column_); + beam_sin_elev_.resize(pixels_per_column_); + beam_azimuth_rad_.resize(pixels_per_column_); + beam_cos_az_.resize(pixels_per_column_); + beam_sin_az_.resize(pixels_per_column_); + for (size_t b = 0; b < pixels_per_column_; ++b) { + const double elev_rad = metadata.beam_altitude_angles_deg[b] * k_deg_to_rad; + beam_cos_elev_[b] = std::cos(elev_rad); + beam_sin_elev_[b] = std::sin(elev_rad); + // Match Ouster SDK's make_xyz_lut: the per-beam azimuth is subtracted (not added) from the + // encoder angle. Negating at LUT build time lets the downstream formula remain a plain + // "encoder + offset" addition. + beam_azimuth_rad_[b] = -metadata.beam_azimuth_angles_deg[b] * k_deg_to_rad; + beam_cos_az_[b] = std::cos(beam_azimuth_rad_[b]); + beam_sin_az_[b] = std::sin(beam_azimuth_rad_[b]); + } + + cos_encoder_.resize(columns_per_frame_); + sin_encoder_.resize(columns_per_frame_); + const double k_two_pi = 2.0 * M_PI; + for (size_t c = 0; c < columns_per_frame_; ++c) { + // Negative sign: Ouster measurement_id increases with clockwise rotation, but ROS right-hand + // coordinate frame expects counter-clockwise azimuth. + const double encoder = -k_two_pi * static_cast(c) / + static_cast(columns_per_frame_); + cos_encoder_[c] = std::cos(encoder); + sin_encoder_[c] = std::sin(encoder); + } + } + + /// @brief Compute sensor-frame XYZ for a single measurement. + /// @param range_mm Raw range value in millimeters. + /// @param beam_idx Beam (row) index in [0, pixels_per_column). + /// @param measurement_id Column index in [0, columns_per_frame). + /// @param[out] x, y, z Point in sensor frame, meters. + /// @param[out] azimuth_rad, elevation_rad Spherical coordinates of the measured direction. + inline void compute( + uint32_t range_mm, size_t beam_idx, size_t measurement_id, float & x, float & y, float & z, + float & azimuth_rad, float & elevation_rad) const + { + // Reference formula from Ouster's Software User Manual (sensor frame XYZ): + // theta_encoder = -2π * measurement_id / columns_per_frame + // theta = theta_encoder + beam_azimuth_angles[row] + // phi = beam_altitude_angles[row] + // n = lidar_origin_to_beam_origin_mm * 1e-3 (beam-origin offset) + // r = range_mm * 1e-3 + // + // x = (r - n) * cos(theta) * cos(phi) + n * cos(theta_encoder) + // y = (r - n) * sin(theta) * cos(phi) + n * sin(theta_encoder) + // z = (r - n) * sin(phi) + // Note the "+ n * cos(theta_encoder)" term uses the ENCODER angle, not the (encoder + beam) + // angle — that's the subtle difference that matters for matching the Ouster SDK output. + const double r_m = static_cast(range_mm) * 1e-3; + const double n = beam_origin_m_; + const double r_minus_n = r_m - n; + + const double cos_e = cos_encoder_[measurement_id]; + const double sin_e = sin_encoder_[measurement_id]; + const double cos_off = beam_cos_az_[beam_idx]; + const double sin_off = beam_sin_az_[beam_idx]; + // cos/sin of (theta_encoder + beam_azimuth_offset) via angle-addition. + const double cos_theta = cos_e * cos_off - sin_e * sin_off; + const double sin_theta = sin_e * cos_off + cos_e * sin_off; + + const double cos_phi = beam_cos_elev_[beam_idx]; + const double sin_phi = beam_sin_elev_[beam_idx]; + + x = static_cast(r_minus_n * cos_theta * cos_phi + n * cos_e); + // Ouster SDK negates y to convert from the sensor's internal rotation convention to the + // +X forward / +Y left right-handed output frame. + y = static_cast(-(r_minus_n * sin_theta * cos_phi) - n * sin_e); + z = static_cast(r_minus_n * sin_phi); + + azimuth_rad = static_cast(std::atan2(sin_theta, cos_theta)); + elevation_rad = static_cast(std::asin(sin_phi)); + } + + [[nodiscard]] size_t columns_per_frame() const { return columns_per_frame_; } + [[nodiscard]] size_t pixels_per_column() const { return pixels_per_column_; } + +private: + size_t columns_per_frame_; + size_t pixels_per_column_; + double beam_origin_m_; + std::vector pixel_shift_; + std::vector beam_cos_elev_; + std::vector beam_sin_elev_; + std::vector beam_azimuth_rad_; + std::vector beam_cos_az_; + std::vector beam_sin_az_; + std::vector cos_encoder_; + std::vector sin_encoder_; +}; + +} // namespace nebula::drivers + +#endif // NEBULA_OUSTER_XYZ_LUT_HPP diff --git a/src/nebula_ouster/nebula_ouster_decoders/package.xml b/src/nebula_ouster/nebula_ouster_decoders/package.xml new file mode 100644 index 000000000..f7b5daf46 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/package.xml @@ -0,0 +1,24 @@ + + + + nebula_ouster_decoders + 0.3.0 + Nebula Ouster Decoders Library + Ussama Naal + + Apache 2 + Ouster + + autoware_cmake + ros_environment + + nebula_core_common + nebula_core_decoders + nebula_core_hw_interfaces + nebula_ouster_common + nlohmann-json-dev + + + ament_cmake + + diff --git a/src/nebula_ouster/nebula_ouster_decoders/src/ouster_decoder.cpp b/src/nebula_ouster/nebula_ouster_decoders/src/ouster_decoder.cpp new file mode 100644 index 000000000..5c441c7f7 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/src/ouster_decoder.cpp @@ -0,0 +1,321 @@ +// Copyright 2026 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nebula_ouster_decoders/ouster_decoder.hpp" + +#include "nebula_ouster_decoders/ouster_packet.hpp" +#include "nebula_ouster_decoders/ouster_xyz_lut.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers +{ + +namespace +{ +constexpr float k_g_to_m_s2 = 9.80665f; +constexpr float k_deg_to_rad = static_cast(M_PI) / 180.0f; + +uint64_t now_steady_ns(std::chrono::steady_clock::time_point start) +{ + return static_cast( + std::chrono::duration_cast( + std::chrono::steady_clock::now() - start) + .count()); +} + +/// @brief Wrap azimuth into [0, 360). +float normalize_azimuth_deg(float az_deg) +{ + az_deg = std::fmod(az_deg, 360.0f); + if (az_deg < 0.0f) az_deg += 360.0f; + return az_deg; +} +} // namespace + +const char * to_cstr(const DecodeError error) +{ + switch (error) { + case DecodeError::PACKET_FORMAT_INVALID: + return "packet format invalid"; + case DecodeError::CALLBACK_NOT_SET: + return "pointcloud callback is not set"; + case DecodeError::EMPTY_PACKET: + return "packet is empty"; + } + return "unknown decode error"; +} + +struct OusterDecoder::Impl +{ + FieldOfView fov; + OusterMetadata metadata; + OusterXyzLut lut; + pointcloud_callback_t pointcloud_callback; + imu_callback_t imu_callback; + + // Packet processing must be serialized — the HW interface may fire the callback from two + // different socket threads (lidar + IMU ports). Shared mutable state (scan_cloud, + // prev_measurement_id, prev_frame_id, scan_first_ts_ns) would otherwise race. + std::mutex decode_mutex; + + // Current scan state. + NebulaPointCloudPtr scan_cloud; ///< Accumulating cloud. + uint64_t scan_first_ts_ns{0}; ///< Sensor timestamp of first valid column in the scan. + int32_t prev_measurement_id{-1}; ///< Last column index seen, -1 before first packet. + int32_t prev_frame_id{-1}; ///< Last frame_id from packet header, -1 before first packet. + + Impl(FieldOfView fov_in, OusterMetadata md, pointcloud_callback_t cb) + : fov(fov_in), + metadata(std::move(md)), + lut(metadata), + pointcloud_callback(std::move(cb)), + scan_cloud(std::make_shared()) + { + // Pre-allocate for a typical OS-128 frame (1024 columns x 128 beams x up to 2 returns). + scan_cloud->reserve( + metadata.columns_per_frame * metadata.pixels_per_column * metadata.num_returns()); + } + + /// @brief Start a fresh scan, emitting the previous one via callback. + /// @param completing_column_id The measurement_id that triggered scan completion (for logging). + void emit_and_reset_scan() + { + if (pointcloud_callback && !scan_cloud->empty()) { + const double timestamp_s = static_cast(scan_first_ts_ns) * 1e-9; + pointcloud_callback(scan_cloud, timestamp_s); + } + scan_cloud = std::make_shared(); + scan_cloud->reserve( + metadata.columns_per_frame * metadata.pixels_per_column * metadata.num_returns()); + scan_first_ts_ns = 0; + } + + /// @brief Append one pixel to the current scan, applying FoV filter and return-type tag. + void push_point( + uint32_t range_mm, size_t beam_idx, size_t measurement_id, uint8_t intensity, + ReturnType return_type, uint32_t point_time_offset_ns) + { + if (range_mm == 0U) return; + + float x{}, y{}, z{}, azimuth_rad{}, elevation_rad{}; + lut.compute( + range_mm, beam_idx, measurement_id, x, y, z, azimuth_rad, elevation_rad); + + const float azimuth_deg = normalize_azimuth_deg(azimuth_rad / k_deg_to_rad); + if (azimuth_deg < fov.azimuth.start || azimuth_deg > fov.azimuth.end) return; + + const float elevation_deg = elevation_rad / k_deg_to_rad; + if (elevation_deg < fov.elevation.start || elevation_deg > fov.elevation.end) return; + + NebulaPoint pt{}; + pt.x = x; + pt.y = y; + pt.z = z; + pt.intensity = intensity; + pt.return_type = static_cast(return_type); + pt.channel = static_cast(beam_idx); + pt.azimuth = azimuth_rad; + pt.elevation = elevation_rad; + pt.distance = static_cast(range_mm) * 1e-3f; + pt.time_stamp = point_time_offset_ns; + scan_cloud->push_back(pt); + } + + /// @brief Parse one lidar UDP packet and add its points to the current scan. + /// @return true if this packet closed out a scan. + bool process_lidar_packet(const std::vector & packet) + { + using namespace ouster_packet; + const auto layout = get_layout(metadata.udp_profile_lidar); + const size_t cpp = metadata.columns_per_packet; + const size_t ppc = metadata.pixels_per_column; + const bool dual = metadata.num_returns() == 2; + const bool legacy = metadata.udp_profile_lidar == UdpProfileLidar::LEGACY; + + // Parse packet header (modern profiles only — LEGACY has no packet header). + int32_t this_frame_id = -1; + if (!legacy && packet.size() >= layout.packet_header_size) { + this_frame_id = + static_cast(ouster_packet::parse_packet_frame_id(packet.data())); + } + + const uint8_t * p = packet.data(); + p += layout.packet_header_size; + + bool scan_completed = false; + + // Detect frame boundary via frame_id change (most reliable for Ouster firmware). + if (this_frame_id >= 0 && prev_frame_id >= 0 && this_frame_id != prev_frame_id) { + emit_and_reset_scan(); + scan_completed = true; + prev_measurement_id = -1; + } + if (this_frame_id >= 0) prev_frame_id = this_frame_id; + + for (size_t col = 0; col < cpp; ++col) { + const ColumnHeader header = + legacy ? parse_legacy_column_header(p) : parse_column_header(p); + p += layout.column_header_size; + + const uint8_t * pixel_data = p; + p += layout.pixel_size * ppc; + + // Skip legacy per-column footer (4 bytes). Modern profiles have a single packet footer at + // the end handled after the loop. + if (legacy) { + p += 4; + } + + const bool valid = (header.status & 0x01) != 0U; + if (!valid) continue; + + // Detect frame boundary: measurement_id wrapped back to a lower value. + const int32_t mid = static_cast(header.measurement_id); + if (prev_measurement_id >= 0 && mid < prev_measurement_id) { + emit_and_reset_scan(); + scan_completed = true; + } + prev_measurement_id = mid; + + if (scan_first_ts_ns == 0) scan_first_ts_ns = header.timestamp_ns; + + const uint32_t point_time_offset_ns = + (header.timestamp_ns >= scan_first_ts_ns) + ? static_cast((header.timestamp_ns - scan_first_ts_ns) & 0xFFFFFFFFU) + : 0U; + + // Iterate beams (rows) within this column. + for (size_t beam = 0; beam < ppc; ++beam) { + const uint8_t * pixel = pixel_data + beam * layout.pixel_size; + + if (!dual) { + uint32_t range_mm{}; + uint8_t refl{}; + if (legacy) { + range_mm = Legacy::range_mm(pixel); + refl = static_cast(Legacy::reflectivity(pixel) & 0xFFU); + } else { + range_mm = Rng19Single::range_mm(pixel); + refl = Rng19Single::reflectivity(pixel); + } + push_point( + range_mm, beam, static_cast(mid), refl, ReturnType::STRONGEST, + point_time_offset_ns); + } else { + const uint32_t r1 = Rng19Dual::range1_mm(pixel); + const uint8_t refl1 = Rng19Dual::reflectivity1(pixel); + push_point( + r1, beam, static_cast(mid), refl1, ReturnType::FIRST, point_time_offset_ns); + + const uint32_t r2 = Rng19Dual::range2_mm(pixel); + const uint8_t refl2 = Rng19Dual::reflectivity2(pixel); + push_point( + r2, beam, static_cast(mid), refl2, ReturnType::LAST, point_time_offset_ns); + } + } + } + + return scan_completed; + } +}; + +OusterDecoder::OusterDecoder( + FieldOfView fov, OusterMetadata metadata, + pointcloud_callback_t pointcloud_cb) +: impl_(std::make_unique(fov, std::move(metadata), std::move(pointcloud_cb))) +{ +} + +OusterDecoder::~OusterDecoder() = default; +OusterDecoder::OusterDecoder(OusterDecoder &&) noexcept = default; +OusterDecoder & OusterDecoder::operator=(OusterDecoder &&) noexcept = default; + +PacketDecodeResult OusterDecoder::unpack(const std::vector & packet) +{ + const auto decode_begin = std::chrono::steady_clock::now(); + PacketDecodeResult result; + + if (!impl_->pointcloud_callback) { + result.metadata_or_error = DecodeError::CALLBACK_NOT_SET; + result.performance_counters.decode_time_ns = now_steady_ns(decode_begin); + return result; + } + + if (packet.empty()) { + result.metadata_or_error = DecodeError::EMPTY_PACKET; + result.performance_counters.decode_time_ns = now_steady_ns(decode_begin); + return result; + } + + const size_t lidar_size = impl_->metadata.lidar_packet_size_bytes; + const size_t imu_size = impl_->metadata.imu_packet_size_bytes; + + PacketMetadata metadata{}; + + if (packet.size() == lidar_size) { + metadata.did_scan_complete = impl_->process_lidar_packet(packet); + metadata.packet_timestamp_ns = impl_->scan_first_ts_ns; + } else if (packet.size() == imu_size) { + if (impl_->imu_callback) { + const auto raw = ouster_packet::parse_imu_packet(packet.data()); + OusterImuSample s{}; + s.timestamp_ns = raw.sys_timestamp_ns; + s.accel_x = raw.accel_x_g * k_g_to_m_s2; + s.accel_y = raw.accel_y_g * k_g_to_m_s2; + s.accel_z = raw.accel_z_g * k_g_to_m_s2; + s.gyro_x = raw.gyro_x_dps * k_deg_to_rad; + s.gyro_y = raw.gyro_y_dps * k_deg_to_rad; + s.gyro_z = raw.gyro_z_dps * k_deg_to_rad; + impl_->imu_callback(s); + } + metadata.packet_timestamp_ns = 0; + metadata.did_scan_complete = false; + } else { + result.metadata_or_error = DecodeError::PACKET_FORMAT_INVALID; + result.performance_counters.decode_time_ns = now_steady_ns(decode_begin); + return result; + } + + result.metadata_or_error = metadata; + result.performance_counters.decode_time_ns = now_steady_ns(decode_begin); + return result; +} + +void OusterDecoder::set_pointcloud_callback(pointcloud_callback_t pointcloud_cb) +{ + impl_->pointcloud_callback = std::move(pointcloud_cb); +} + +void OusterDecoder::set_imu_callback(imu_callback_t imu_cb) +{ + impl_->imu_callback = std::move(imu_cb); +} + +const OusterMetadata & OusterDecoder::metadata() const +{ + return impl_->metadata; +} + +} // namespace nebula::drivers diff --git a/src/nebula_ouster/nebula_ouster_decoders/src/ouster_metadata.cpp b/src/nebula_ouster/nebula_ouster_decoders/src/ouster_metadata.cpp new file mode 100644 index 000000000..7a6caf814 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/src/ouster_metadata.cpp @@ -0,0 +1,152 @@ +// Copyright 2026 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nebula_ouster_decoders/ouster_metadata.hpp" + +#include "nebula_ouster_decoders/ouster_packet.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace nebula::drivers +{ + +namespace +{ +ouster_packet::UdpProfileLidar parse_profile(const std::string & s) +{ + if (s == "LEGACY") return ouster_packet::UdpProfileLidar::LEGACY; + if (s == "RNG19_RFL8_SIG16_NIR16") return ouster_packet::UdpProfileLidar::RNG19_RFL8_SIG16_NIR16; + if (s == "RNG19_RFL8_SIG16_NIR16_DUAL") { + return ouster_packet::UdpProfileLidar::RNG19_RFL8_SIG16_NIR16_DUAL; + } + if (s == "RNG15_RFL8_NIR8") return ouster_packet::UdpProfileLidar::RNG15_RFL8_NIR8; + throw std::runtime_error("Unsupported Ouster UDP profile: " + s); +} + +uint32_t columns_per_frame_from_mode(const std::string & mode) +{ + const auto x = mode.find('x'); + if (x == std::string::npos) { + throw std::runtime_error("Invalid lidar_mode: " + mode); + } + return static_cast(std::stoul(mode.substr(0, x))); +} + +/// @brief Fetch a sub-object from the JSON root or a nested sensor_info wrapper. +/// @details Ouster's /api/v1/sensor/metadata/sensor_info returns a flat object; some firmware +/// versions or exported metadata files wrap everything under a "sensor_info" key. Handle both. +const nlohmann::json & section(const nlohmann::json & root, const std::string & key) +{ + if (root.contains(key)) return root.at(key); + if (root.contains("sensor_info") && root.at("sensor_info").contains(key)) { + return root.at("sensor_info").at(key); + } + throw std::runtime_error("Ouster metadata: missing required section '" + key + "'"); +} +} // namespace + +OusterMetadata parse_ouster_metadata(const std::string & sensor_info_json) +{ + using nlohmann::json; + const json doc = json::parse(sensor_info_json); + + OusterMetadata md{}; + + // Beam geometry — always present. + const json & bi = section(doc, "beam_intrinsics"); + md.beam_altitude_angles_deg = bi.at("beam_altitude_angles").get>(); + md.beam_azimuth_angles_deg = bi.at("beam_azimuth_angles").get>(); + if (bi.contains("lidar_origin_to_beam_origin_mm")) { + md.lidar_origin_to_beam_origin_mm = bi.at("lidar_origin_to_beam_origin_mm").get(); + } + + // Scan geometry. Modern firmware exposes `lidar_data_format`; legacy sensors do not. + const bool has_ldf = + doc.contains("lidar_data_format") || + (doc.contains("sensor_info") && doc.at("sensor_info").contains("lidar_data_format")); + + if (has_ldf) { + const json & ldf = section(doc, "lidar_data_format"); + md.columns_per_frame = ldf.at("columns_per_frame").get(); + md.columns_per_packet = ldf.at("columns_per_packet").get(); + md.pixels_per_column = ldf.at("pixels_per_column").get(); + if (ldf.contains("pixel_shift_by_row")) { + md.pixel_shift_by_row = ldf.at("pixel_shift_by_row").get>(); + } + if (ldf.contains("udp_profile_lidar")) { + md.udp_profile_lidar = parse_profile(ldf.at("udp_profile_lidar").get()); + } + } else { + md.pixels_per_column = static_cast(md.beam_altitude_angles_deg.size()); + md.udp_profile_lidar = ouster_packet::UdpProfileLidar::LEGACY; + md.columns_per_packet = 16; // legacy default + } + + // Optional informational fields. Also provides columns_per_frame fallback for legacy sensors. + const json * cfg = nullptr; + if (doc.contains("config_params")) { + cfg = &doc.at("config_params"); + } else if (doc.contains("sensor_info") && doc.at("sensor_info").contains("config_params")) { + cfg = &doc.at("sensor_info").at("config_params"); + } + if (cfg != nullptr && cfg->contains("lidar_mode")) { + md.lidar_mode = cfg->at("lidar_mode").get(); + if (!has_ldf) md.columns_per_frame = columns_per_frame_from_mode(md.lidar_mode); + } + + // Prod line / firmware rev live at different levels depending on firmware version. + if (doc.contains("prod_line")) md.sensor_prod_line = doc.at("prod_line").get(); + else if (doc.contains("sensor_info") && doc.at("sensor_info").contains("prod_line")) { + md.sensor_prod_line = doc.at("sensor_info").at("prod_line").get(); + } + if (doc.contains("firmware_rev")) md.firmware_version = doc.at("firmware_rev").get(); + + // Sanity checks. + if (md.beam_altitude_angles_deg.size() != md.pixels_per_column || + md.beam_azimuth_angles_deg.size() != md.pixels_per_column) { + throw std::runtime_error( + "Ouster metadata: beam angle arrays length does not match pixels_per_column"); + } + if (!md.pixel_shift_by_row.empty() && md.pixel_shift_by_row.size() != md.pixels_per_column) { + throw std::runtime_error( + "Ouster metadata: pixel_shift_by_row length does not match pixels_per_column"); + } + if (md.pixel_shift_by_row.empty()) { + md.pixel_shift_by_row.assign(md.pixels_per_column, 0); + } + + md.update_derived_sizes(); + return md; +} + +std::string fetch_ouster_metadata_http(const std::string & sensor_ip, int timeout_ms) +{ + connections::HttpClient client(sensor_ip, 80); + // Modern firmware: /api/v1/sensor/metadata returns the full document (all sections). + // The /api/v1/sensor/metadata/sensor_info sub-endpoint only returns the sensor_info section — + // do NOT use that one. Older firmware exposes /metadata as a legacy alias. + try { + return client.get("/api/v1/sensor/metadata", timeout_ms); + } catch (const connections::SocketError &) { + return client.get("/metadata", timeout_ms); + } +} + +} // namespace nebula::drivers diff --git a/src/nebula_ouster/nebula_ouster_hw_interfaces/CMakeLists.txt b/src/nebula_ouster/nebula_ouster_hw_interfaces/CMakeLists.txt new file mode 100644 index 000000000..f92c6ce92 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_hw_interfaces/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.20) +project(nebula_ouster_hw_interfaces) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +add_library(nebula_ouster_hw_interfaces SHARED src/ouster_hw_interface.cpp) + +target_include_directories( + nebula_ouster_hw_interfaces + PUBLIC $ + $) + +ament_target_dependencies( + nebula_ouster_hw_interfaces PUBLIC nebula_core_common nebula_ouster_common + nebula_core_hw_interfaces) + +install(TARGETS nebula_ouster_hw_interfaces + EXPORT export_nebula_ouster_hw_interfaces) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_nebula_ouster_hw_interfaces) +ament_export_dependencies(nebula_core_common nebula_ouster_common nebula_core_hw_interfaces) + +ament_package() diff --git a/src/nebula_ouster/nebula_ouster_hw_interfaces/include/nebula_ouster_hw_interfaces/ouster_hw_interface.hpp b/src/nebula_ouster/nebula_ouster_hw_interfaces/include/nebula_ouster_hw_interfaces/ouster_hw_interface.hpp new file mode 100644 index 000000000..e670f474c --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_hw_interfaces/include/nebula_ouster_hw_interfaces/ouster_hw_interface.hpp @@ -0,0 +1,88 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_OUSTER_HW_INTERFACE_HPP +#define NEBULA_OUSTER_HW_INTERFACE_HPP + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers +{ + +/// @brief Receives raw sensor packets and forwards them to a registered callback. +/// @details This class owns the transport-facing state used by the ouster driver. +class OusterHwInterface +{ +public: + /// @brief Rich error payload for hardware interface operations. + struct Error + { + /// @brief Coarse category for quick handling and branching. + enum class Code : uint8_t { + CALLBACK_NOT_REGISTERED, ///< Start requested before a callback was registered. + INVALID_CALLBACK, ///< Empty callback passed to register_scan_callback. + INVALID_OPERATION, ///< Operation is invalid in the current interface state. + SOCKET_OPEN_FAILED, ///< UDP socket creation/bind/subscribe failed. + SOCKET_CLOSE_FAILED, ///< UDP socket unsubscribe/teardown failed. + }; + + Code code; + std::string message; + }; + + static const char * to_cstr(Error::Code code); + + /// @brief Construct the hardware interface with connection settings. + /// @param connection_configuration Network addresses and ports used by the sensor stream. + explicit OusterHwInterface(ConnectionConfiguration connection_configuration); + + /// @brief Start packet reception. + /// @return std::monostate on success, Error on failure. + /// @post On success, incoming packets are delivered through the registered callback. + util::expected sensor_interface_start(); + + /// @brief Stop packet reception. + /// @return std::monostate on success, Error on failure. + util::expected sensor_interface_stop(); + + /// @brief Register the callback invoked for each incoming LiDAR packet (data_port). + util::expected register_scan_callback( + connections::UdpSocket::callback_t scan_callback); + + /// @brief Register the callback invoked for each incoming IMU packet (imu_port). Optional — + /// leave unset if imu_port is 0. + util::expected register_imu_callback( + connections::UdpSocket::callback_t imu_callback); + +private: + ConnectionConfiguration connection_configuration_; + std::optional udp_socket_; + std::optional imu_socket_; + std::shared_ptr packet_callback_; + std::shared_ptr imu_callback_; + std::mutex callback_mutex_; +}; + +} // namespace nebula::drivers + +#endif // NEBULA_OUSTER_HW_INTERFACE_HPP diff --git a/src/nebula_ouster/nebula_ouster_hw_interfaces/package.xml b/src/nebula_ouster/nebula_ouster_hw_interfaces/package.xml new file mode 100644 index 000000000..6264e89e9 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_hw_interfaces/package.xml @@ -0,0 +1,21 @@ + + + + nebula_ouster_hw_interfaces + 0.3.0 + Nebula Ouster HW Interfaces + Ussama Naal + + Apache 2 + Ouster + + autoware_cmake + + nebula_core_common + nebula_core_hw_interfaces + nebula_ouster_common + + + ament_cmake + + diff --git a/src/nebula_ouster/nebula_ouster_hw_interfaces/src/ouster_hw_interface.cpp b/src/nebula_ouster/nebula_ouster_hw_interfaces/src/ouster_hw_interface.cpp new file mode 100644 index 000000000..5656b4890 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_hw_interfaces/src/ouster_hw_interface.cpp @@ -0,0 +1,177 @@ +// Copyright 2026 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nebula_ouster_hw_interfaces/ouster_hw_interface.hpp" + +#include +#include +#include +#include +#include + +namespace nebula::drivers +{ + +OusterHwInterface::OusterHwInterface(ConnectionConfiguration connection_configuration) +: connection_configuration_(std::move(connection_configuration)) +{ + // The ouster driver is UDP-only. Real integrations can initialize control/sync channels here. +} + +util::expected OusterHwInterface::sensor_interface_start() +{ + std::lock_guard lock(callback_mutex_); + + if (!packet_callback_) { + return Error{ + Error::Code::CALLBACK_NOT_REGISTERED, + "Cannot start sensor interface before registering a packet callback"}; + } + + if (udp_socket_ && udp_socket_->is_subscribed()) { + return std::monostate{}; + } + + try { + connections::UdpSocket::Builder builder( + connection_configuration_.host_ip, connection_configuration_.data_port); + builder.set_mtu(static_cast(connection_configuration_.receiver_mtu_bytes)); + + if (connection_configuration_.filter_sender_ip) { + builder.limit_to_sender( + connection_configuration_.sensor_ip, connection_configuration_.data_port); + } + udp_socket_.emplace(std::move(builder).bind()); + + // Callback can only be set while udp_socket_ is nullopt, so we don't need locking here + udp_socket_->subscribe( + [this](std::vector & packet, const connections::UdpSocket::RxMetadata & metadata) { + if (this->packet_callback_ && *this->packet_callback_) { + (*this->packet_callback_)(packet, metadata); + } + }); + + // Open an optional second socket for IMU packets when imu_port is configured. Each socket + // has its own dedicated callback — no shared mutable state between lidar and IMU threads. + const auto imu_port = connection_configuration_.imu_port; + if (imu_port != 0 && imu_port != connection_configuration_.data_port && imu_callback_) { + connections::UdpSocket::Builder imu_builder( + connection_configuration_.host_ip, imu_port); + imu_builder.set_mtu(128); // IMU packets are fixed 48 bytes + if (connection_configuration_.filter_sender_ip) { + imu_builder.limit_to_sender(connection_configuration_.sensor_ip, imu_port); + } + imu_socket_.emplace(std::move(imu_builder).bind()); + imu_socket_->subscribe( + [this]( + std::vector & packet, const connections::UdpSocket::RxMetadata & metadata) { + if (this->imu_callback_ && *this->imu_callback_) { + (*this->imu_callback_)(packet, metadata); + } + }); + } + } catch (const connections::SocketError & e) { + return Error{ + Error::Code::SOCKET_OPEN_FAILED, std::string("Failed to open UDP socket: ") + e.what()}; + } catch (const connections::UsageError & e) { + return Error{ + Error::Code::SOCKET_OPEN_FAILED, + std::string("Invalid UDP socket configuration: ") + e.what()}; + } catch (const std::exception & e) { + return Error{ + Error::Code::SOCKET_OPEN_FAILED, + std::string("Failed to open UDP socket due to unexpected error: ") + e.what()}; + } + + return std::monostate{}; +} + +util::expected OusterHwInterface::sensor_interface_stop() +{ + if (!udp_socket_) { + return std::monostate{}; + } + + try { + if (imu_socket_) { + imu_socket_->unsubscribe(); + imu_socket_.reset(); + } + udp_socket_->unsubscribe(); + udp_socket_.reset(); + } catch (const connections::SocketError & e) { + return Error{ + Error::Code::SOCKET_CLOSE_FAILED, std::string("Failed to close UDP socket: ") + e.what()}; + } catch (const std::exception & e) { + return Error{ + Error::Code::SOCKET_CLOSE_FAILED, + std::string("Failed to close UDP socket due to unexpected error: ") + e.what()}; + } + + return std::monostate{}; +} + +util::expected OusterHwInterface::register_scan_callback( + connections::UdpSocket::callback_t scan_callback) +{ + if (!scan_callback) { + return Error{Error::Code::INVALID_CALLBACK, "Cannot register an empty packet callback"}; + } + + std::lock_guard lock(callback_mutex_); + if (udp_socket_) { + return Error{ + Error::Code::INVALID_OPERATION, + "Cannot replace packet callback while sensor interface is active"}; + } + packet_callback_ = std::make_shared(std::move(scan_callback)); + return std::monostate{}; +} + +util::expected OusterHwInterface::register_imu_callback( + connections::UdpSocket::callback_t imu_callback) +{ + if (!imu_callback) { + return Error{Error::Code::INVALID_CALLBACK, "Cannot register an empty IMU callback"}; + } + + std::lock_guard lock(callback_mutex_); + if (imu_socket_) { + return Error{ + Error::Code::INVALID_OPERATION, + "Cannot replace IMU callback while sensor interface is active"}; + } + imu_callback_ = std::make_shared(std::move(imu_callback)); + return std::monostate{}; +} + +const char * OusterHwInterface::to_cstr(OusterHwInterface::Error::Code code) +{ + switch (code) { + case Error::Code::CALLBACK_NOT_REGISTERED: + return "callback not registered"; + case Error::Code::INVALID_CALLBACK: + return "invalid callback"; + case Error::Code::INVALID_OPERATION: + return "invalid operation"; + case Error::Code::SOCKET_OPEN_FAILED: + return "failed to open UDP socket"; + case Error::Code::SOCKET_CLOSE_FAILED: + return "failed to close UDP socket"; + default: + return "unknown hardware error"; + } +} + +} // namespace nebula::drivers