diff --git a/.codecov.yml b/.codecov.yml index aaab0287c..791fca4bd 100644 --- a/.codecov.yml +++ b/.codecov.yml @@ -85,6 +85,18 @@ flag_management: - name: nebula_robosense_hw_interfaces paths: - src/nebula_robosense/nebula_robosense_hw_interfaces + - name: nebula_ouster + paths: + - src/nebula_ouster/nebula_ouster + - name: nebula_ouster_common + paths: + - src/nebula_ouster/nebula_ouster_common + - name: nebula_ouster_decoders + paths: + - src/nebula_ouster/nebula_ouster_decoders + - name: nebula_ouster_hw_interfaces + paths: + - src/nebula_ouster/nebula_ouster_hw_interfaces - name: nebula_velodyne paths: - src/nebula_velodyne/nebula_velodyne diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index bfee62fdd..24fb0d2db 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -27,6 +27,7 @@ jobs: uses: actions/checkout@v3 with: fetch-depth: 0 + submodules: recursive - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -119,6 +120,7 @@ jobs: with: fetch-depth: 0 ref: ${{ github.event.pull_request.head.sha }} + submodules: recursive - name: Get modified packages id: get-modified-packages @@ -134,6 +136,7 @@ jobs: with: fetch-depth: 0 ref: ${{ matrix.ref }} + submodules: recursive - name: Restore .github folder run: | diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 55295fb55..0daa37b40 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -29,6 +29,8 @@ jobs: steps: - name: Check out repository uses: actions/checkout@v3 + with: + submodules: recursive - name: Free disk space (Ubuntu) uses: jlumbroso/free-disk-space@v1.2.0 diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..0c429e16e --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "ouster-sdk"] + path = src/nebula_ouster/lib/ouster-sdk + url = https://github.com/ouster-lidar/ouster-sdk.git diff --git a/README.md b/README.md index ee575b556..7f144cea5 100644 --- a/README.md +++ b/README.md @@ -55,7 +55,7 @@ To build Nebula run the following commands in your workspace: ```bash # In workspace -git clone https://github.com/tier4/nebula.git +git clone --recurse-submodules https://github.com/tier4/nebula.git cd nebula # Import dependencies vcs import < build_depends-${ROS_DISTRO}.repos diff --git a/docs/installation.md b/docs/installation.md index c4365e42a..ef6874835 100644 --- a/docs/installation.md +++ b/docs/installation.md @@ -20,7 +20,7 @@ To build Nebula run the following commands in your workspace: ```bash # In workspace -git clone https://github.com/tier4/nebula.git +git clone --recurse-submodules https://github.com/tier4/nebula.git cd nebula # Import dependencies vcs import < build_depends-${ROS_DISTRO}.repos diff --git a/src/nebula_ouster/README.md b/src/nebula_ouster/README.md new file mode 100644 index 000000000..392ac76a7 --- /dev/null +++ b/src/nebula_ouster/README.md @@ -0,0 +1,66 @@ +# Nebula ouster sensor package + +A module extends the support Nebula driver framework to Ouster lidars and sensors. + +## Purpose + +This package integrates support for Ouster lidars into the Nebula framework. + +The ouster decoder receive packets from the sensor converts them into a Lidar scan and then into +a pointcloud of Nebula point type before publishing them as a rostopic. + +## Package structure + +The ouster sensor consists of four packages: + +- **nebula_ouster_common** - Common definitions and configuration structures +- **nebula_ouster_decoders** - Packet decoder and driver implementation +- **nebula_ouster_hw_interfaces** - Hardware interface for sensor communication +- **nebula_ouster** - ROS 2 wrapper and launch files + +## Building + +When cloning the Nebula driver make sure to clone as a recursive submodule or if you already have +the project cloned then simply invoke the following command to init the submodules before proceeding: + +```bash +git submodule update --init --recursive src/nebula_ouster/lib/ouster-sdk. +``` + +Now invoke the following command to build nebula_ouster package + +```bash +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to nebula_ouster +``` + +## Running + +```bash +# Online mode (with hardware) +ros2 launch nebula_ouster nebula_ouster.launch.xml +# Offline mode (replay from rosbag) +ros2 launch nebula_ouster nebula_ouster.launch.xml launch_hw:=false +``` + +## Key components + +### Configuration (`*_common`) + +- `OusterSensorConfiguration` - Minimal sensor-specific settings for an IP-based sensor + +### Decoder (`*_decoders`) + +- `OusterDecoder` - Decodes packets into Ouster LidarScan object before converting them into pointclouds +- `PacketDecodeResult` - Decoder output containing metadata/error and performance counters +- `DecodeError` - Decoder error codes for packet handling failures + +### Hardware interface (`*_hw_interfaces`) + +- `OusterHwInterface` - Communicates with Ouster sensors via their API + +### ROS wrapper + +- `OusterRosWrapper` - ROS 2 node wrapping the driver +- Point cloud publisher on `/points` +- Packet publish/replay topic on `/packets` (`nebula_msgs/msg/NebulaPackets`) depending on + runtime mode (`launch_hw` parameter) diff --git a/src/nebula_ouster/lib/ouster-sdk b/src/nebula_ouster/lib/ouster-sdk new file mode 160000 index 000000000..af4e7d2a4 --- /dev/null +++ b/src/nebula_ouster/lib/ouster-sdk @@ -0,0 +1 @@ +Subproject commit af4e7d2a49d711a709212a06238bd8acdb2d234f diff --git a/src/nebula_ouster/nebula_ouster/CMakeLists.txt b/src/nebula_ouster/nebula_ouster/CMakeLists.txt new file mode 100644 index 000000000..bd6592673 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/CMakeLists.txt @@ -0,0 +1,96 @@ +cmake_minimum_required(VERSION 3.20) +project(nebula_ouster) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Ouster +add_library( + ouster_ros_wrapper SHARED + src/ouster_ros_wrapper.cpp) + +set(_nebula_ouster_sdk "${CMAKE_CURRENT_SOURCE_DIR}/../lib/ouster-sdk") + +if(NOT EXISTS "${_nebula_ouster_sdk}/CMakeLists.txt") + message(FATAL_ERROR + "nebula_ouster_decoders: ouster-sdk is missing! init submodules using:" + "\n\tgit submodule update --init --recursive src/nebula_ouster/lib/ouster-sdk. + \nChecked:\n\t${_nebula_ouster_sdk}") +endif() + +set(BUILD_SENSOR ON CACHE BOOL "Ouster SDK: sensor module" FORCE) +set(BUILD_PCAP OFF CACHE BOOL "Ouster SDK: pcap" FORCE) +set(BUILD_OSF OFF CACHE BOOL "Ouster SDK: osf" FORCE) +set(BUILD_VIZ OFF CACHE BOOL "Ouster SDK: visualizer" FORCE) +set(BUILD_MAPPING OFF CACHE BOOL "Ouster SDK: mapping" FORCE) +set(BUILD_TESTING OFF CACHE BOOL "Ouster SDK: tests" FORCE) +set(BUILD_EXAMPLES OFF CACHE BOOL "Ouster SDK: examples" FORCE) +set(BUILD_PYTHON_MODULE OFF CACHE BOOL "Ouster SDK: python" FORCE) + +add_subdirectory("${_nebula_ouster_sdk}" "${CMAKE_CURRENT_BINARY_DIR}/ouster-sdk-build") + +if(TARGET ouster_client) + target_compile_options(ouster_client PRIVATE -Wno-pedantic) + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + target_compile_options(ouster_client PRIVATE -Wno-error=stringop-overflow) # cspell:ignore stringop + endif() +endif() +if(TARGET ouster_sensor) + target_compile_options(ouster_sensor PRIVATE -Wno-pedantic) +endif() + +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 + PRIVATE ouster_client + ouster_sensor) +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..4e5e9ade6 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/config/ouster_sensor.param.yaml @@ -0,0 +1,40 @@ +# Ouster Sensor ROS 2 Parameters +# This file contains example parameters for the Ouster LiDAR sensor. +# Copy and modify this file for your specific sensor model. + +/**: + ros__parameters: + # ROS wrapper configuration + sensor_model: OS0-64 + launch_hw: true + frame_id: ouster_lidar + use_sensor_extrinsics: false + + # Network configuration + connection: + sensor_ip: 169.254.119.211 + host_ip: 169.254.242.38 + data_port: 7502 + filter_sender_ip: true + + # Decoder configuration + fov: + azimuth: + min_deg: 0.0 + max_deg: 360.0 + elevation: + min_deg: -90.0 + max_deg: 90.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..293b77757 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/include/nebula_ouster/ouster_ros_wrapper.hpp @@ -0,0 +1,175 @@ +// 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 + +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 Resources used only when launch_hw is false. + struct OfflineMode + { + rclcpp::Subscription::SharedPtr packets_sub; + }; + + struct Publishers + { + rclcpp::Publisher::SharedPtr points; + 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 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..f10d50d42 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/launch/ouster_launch_all_hw.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + diff --git a/src/nebula_ouster/nebula_ouster/package.xml b/src/nebula_ouster/nebula_ouster/package.xml new file mode 100644 index 000000000..692c99cbd --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/package.xml @@ -0,0 +1,31 @@ + + + + nebula_ouster + 1.0.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..191e81fa6 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster/src/ouster_ros_wrapper.cpp @@ -0,0 +1,482 @@ +// 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 +#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; + +/// Fetch Ouster `SensorInfo` JSON from a sensor or replay HTTP API. Respects `http_proxy` / +/// `https_proxy`. +std::string fetch_ouster_metadata_via_http(const std::string & sensor_url) +{ + auto sensor_http = ouster::sdk::sensor::SensorHttp::create(sensor_url); + std::string meta = sensor_http->metadata(); + return meta; +} + +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{}; +} +} // 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()); + + 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", "OusterSensor", param_read_only()); + frame_id_ = declare_parameter("frame_id", "ouster_lidar", param_read_write()); + + 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_.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(); + + const auto sensor_ip = config_.connection.sensor_ip; + const auto sensor_metadata_json = fetch_ouster_metadata_via_http(sensor_ip); + RCLCPP_INFO( + get_logger(), "Loaded Ouster metadata via HTTP (%zu bytes)", sensor_metadata_json.size()); + + const bool use_sensor_extrinsics = + declare_parameter("use_sensor_extrinsics", false, param_read_only()); + + auto sensor_info = std::make_shared(sensor_metadata_json); + auto packet_format = std::make_shared(*sensor_info); + config_.connection.receiver_mtu_bytes = packet_format->lidar_packet_size; + + RCLCPP_INFO( + get_logger(), + "Ouster UDP: listening on %s:%u filter_sender_ip=%s (sensor_ip=%s) receiver_mtu=%u", + 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); + + decoder_.emplace( + config_.fov, sensor_info, use_sensor_extrinsics, + [this](const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { + publish_pointcloud_callback(pointcloud, timestamp_s); + }); + + 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()) { + const auto error = callback_result.error(); + throw std::runtime_error( + "Failed to register ouster sensor packet callback: " + error.message); + } + + const auto stream_start_result = online_mode.hw_interface.sensor_interface_start(); + if (!stream_start_result.has_value()) { + const auto error = stream_start_result.error(); + throw std::runtime_error("Failed to start ouster sensor stream: " + 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()) { + const auto error = stop_result.error(); + RCLCPP_WARN( + get_logger(), "Failed to stop ouster sensor stream cleanly: %s", 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; + } + + // Always publish: subscription_count can be 0 briefly after startup or in some executor + // setups, which makes `ros2 topic echo /points` miss data unexpectedly. + 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::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). Increase 'connection.receiver_mtu' (e.g. 65527). Decoding is " + "skipped for this datagram.", + packet.size()); + return; + } + + auto * online_mode = std::get_if(&runtime_mode_); + if (online_mode && online_mode->packets_pub) { + 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"; + default: + 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_calibration_data.hpp b/src/nebula_ouster/nebula_ouster_common/include/nebula_ouster_common/ouster_calibration_data.hpp new file mode 100644 index 000000000..989ffcfc0 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_common/include/nebula_ouster_common/ouster_calibration_data.hpp @@ -0,0 +1,86 @@ +// 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 +#include + +namespace nebula::drivers +{ + +/// @brief Calibration data for the Ouster LiDAR (required for some sensors) +/// @details Real sensor integrations can replace this stub with calibration tables +/// such as per-laser angle offsets, distance corrections, or timing offsets. +struct OusterCalibrationData +{ + enum class ErrorCode : uint8_t { + OPEN_FOR_READ_FAILED, ///< Opening a calibration file for reading failed. + OPEN_FOR_WRITE_FAILED, ///< Opening a calibration file for writing failed. + }; + + /// @brief Error payload for calibration file operations. + struct Error + { + ErrorCode code; + std::string message; + }; + + /// @brief Load calibration data from a file, e.g. for offline decoding + /// @param calibration_file Path to the calibration file + /// @return Parsed calibration data on success, Error on failure. + static util::expected load_from_file( + const std::string & calibration_file) + { + std::ifstream file; + file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + + try { + file.open(calibration_file); + } catch (const std::ifstream::failure & e) { + return Error{ + ErrorCode::OPEN_FOR_READ_FAILED, + "Failed to open calibration file: " + calibration_file + " Error: " + e.what()}; + } + + // Implement: Parse and validate sensor calibration fields from the opened file. + return OusterCalibrationData{}; + } + + /// @brief Save calibration data to a file + /// @param calibration_file Path to save the calibration file + /// @return std::monostate on success, Error on failure. + util::expected save_to_file(const std::string & calibration_file) + { + std::ofstream file; + file.exceptions(std::ofstream::failbit | std::ofstream::badbit); + + try { + file.open(calibration_file); + } catch (const std::ofstream::failure & e) { + return Error{ + ErrorCode::OPEN_FOR_WRITE_FAILED, + "Failed to open calibration file for writing: " + calibration_file + " Error: " + e.what()}; + } + + // Implement: Serialize sensor calibration fields to the opened file. + return std::monostate{}; + } +}; + +} // namespace nebula::drivers 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..1f1bf6b63 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_common/include/nebula_ouster_common/ouster_configuration.hpp @@ -0,0 +1,111 @@ +// 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; + /// 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("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..7e5879adb --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_common/package.xml @@ -0,0 +1,22 @@ + + + + nebula_ouster_common + 1.0.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..29f4a5088 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.20) +project(nebula_ouster_decoders) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(_nebula_ouster_sdk "${CMAKE_CURRENT_SOURCE_DIR}/../lib/ouster-sdk") + +if(NOT EXISTS "${_nebula_ouster_sdk}/CMakeLists.txt") + message(FATAL_ERROR + "nebula_ouster_decoders: ouster-sdk is missing! init submodules using:" + "\n\tgit submodule update --init --recursive src/nebula_ouster/lib/ouster-sdk. + \nChecked:\n\t${_nebula_ouster_sdk}") +endif() + +set(BUILD_SENSOR OFF CACHE BOOL "Ouster SDK: sensor module" FORCE) +set(BUILD_PCAP OFF CACHE BOOL "Ouster SDK: pcap" FORCE) +set(BUILD_OSF OFF CACHE BOOL "Ouster SDK: osf" FORCE) +set(BUILD_VIZ OFF CACHE BOOL "Ouster SDK: visualizer" FORCE) +set(BUILD_MAPPING OFF CACHE BOOL "Ouster SDK: mapping" FORCE) +set(BUILD_TESTING OFF CACHE BOOL "Ouster SDK: tests" FORCE) +set(BUILD_EXAMPLES OFF CACHE BOOL "Ouster SDK: examples" FORCE) +set(BUILD_PYTHON_MODULE OFF CACHE BOOL "Ouster SDK: python" FORCE) + +add_subdirectory("${_nebula_ouster_sdk}" "${CMAKE_CURRENT_BINARY_DIR}/ouster-sdk-build") + +if(TARGET ouster_client) + target_compile_options(ouster_client PRIVATE -Wno-pedantic) + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + target_compile_options(ouster_client PRIVATE -Wno-error=stringop-overflow) # cspell:ignore stringop + endif() +endif() +if(TARGET nmea) + target_compile_options(nmea PRIVATE -Wno-pedantic) +endif() + +find_package(Eigen3 REQUIRED) +find_package(OpenSSL REQUIRED) +find_package(Threads REQUIRED) +find_package(PkgConfig REQUIRED) + +pkg_check_modules(LIBZIP REQUIRED IMPORTED_TARGET libzip) + +# vcpkg / distro CMake config names differ; normalize to IMPORTED target @c libzip::zip . +if(NOT TARGET libzip::zip) + if(TARGET libzip::libzip) + add_library(libzip::zip ALIAS libzip::libzip) + elseif(TARGET LibZip::libzip) + add_library(libzip::zip ALIAS LibZip::libzip) + elseif(TARGET PkgConfig::LIBZIP) + add_library(libzip::zip ALIAS PkgConfig::LIBZIP) + endif() +endif() +if(NOT TARGET libzip::zip) + message(FATAL_ERROR "nebula_ouster_decoders: libzip CMake target not found (expected libzip::zip)") +endif() + +add_library( + nebula_ouster_decoders SHARED + src/ouster_decoder.cpp + src/ouster_lidar_scan_conversions.cpp) + +set(_ouster_gen_includes "${CMAKE_CURRENT_BINARY_DIR}/ouster-sdk-build/generated") +target_include_directories( + nebula_ouster_decoders + PUBLIC $ + $ + $ + $ + $) + +target_compile_definitions(nebula_ouster_decoders PRIVATE EIGEN_MPL2_ONLY) + +target_link_libraries( + nebula_ouster_decoders + PUBLIC nebula_core_common::nebula_core_common + nebula_ouster_common::nebula_ouster_common + nebula_core_decoders::nebula_core_decoders + Eigen3::Eigen + PRIVATE ouster_client + OpenSSL::SSL + OpenSSL::Crypto + libzip::zip + Threads::Threads) + +install( + DIRECTORY "${_nebula_ouster_sdk}/ouster_client/include/" + DESTINATION include/nebula_vendor) +install( + DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/ouster-sdk-build/generated/" + DESTINATION include/nebula_vendor) + +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( + Eigen3 + nebula_core_common + nebula_ouster_common + nebula_core_decoders + OpenSSL) + +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..ba13c41ab --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_decoder.hpp @@ -0,0 +1,111 @@ +// 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_DECODER_HPP +#define NEBULA_OUSTER_DECODER_HPP + +#include +#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 content did not match the expected format + 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{}; ///< Timestamp of the packet in nanoseconds + bool did_scan_complete{false}; ///< True if this packet completed 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 +/// @details Contains either successful metadata or an error, plus performance counters +struct PacketDecodeResult +{ + /// Timing measurements for the decode and callback path. + PerformanceCounters performance_counters; + /// Successful metadata or a decode error. + util::expected metadata_or_error; + + PacketDecodeResult() : performance_counters{}, metadata_or_error(DecodeError::CALLBACK_NOT_SET) {} +}; + +/// @brief Decoder that batches Ouster UDP packets via ouster-sdk (@c ScanBatcher) and publishes +/// Nebula point clouds. +class OusterDecoder +{ +public: + /// @brief Callback type for publishing complete point clouds + /// @param pointcloud The decoded point cloud + /// @param timestamp_s Timestamp of the scan in seconds + using pointcloud_callback_t = + std::function; + + /// @brief Constructor + /// @param fov Field of view to crop the point cloud to + /// @param sensor_info Ouster sensor info object. + /// @param apply_sensor_extrinsics If true, pass-through extrinsics when building the XYZ LUT. + /// @param pointcloud_cb Callback invoked when a full scan is assembled + OusterDecoder( + FieldOfView fov, std::shared_ptr & sensor_info, + bool apply_sensor_extrinsics, 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); + +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_lidar_scan_conversions.hpp b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_lidar_scan_conversions.hpp new file mode 100644 index 000000000..e78e8747c --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/include/nebula_ouster_decoders/ouster_lidar_scan_conversions.hpp @@ -0,0 +1,41 @@ +// 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_LIDAR_SCAN_CONVERSIONS_HPP +#define NEBULA_OUSTER_LIDAR_SCAN_CONVERSIONS_HPP + +#include +#include + +#include +#include +#include + +#include + +namespace nebula::drivers +{ + +/// @brief Convert a completed Ouster SDK @c LidarScan into Nebula's dense point cloud type. +/// @param scan Staggered lidar scan (full frame) from @c ouster::sdk::core::ScanBatcher. +/// @param lut XYZ lookup generated with @c ouster::sdk::core::make_xyz_lut. +/// @param fov Angular crop in sensor spherical coordinates (degrees). +/// @return Shared point cloud; empty if @p scan has no RANGE field or zero size. +NebulaPointCloudPtr nebula_point_cloud_from_lidar_scan( + const ouster::sdk::core::LidarScan & scan, const ouster::sdk::core::XYZLut & lut, + const FieldOfView & fov); + +} // namespace nebula::drivers + +#endif 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..29c20c469 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/package.xml @@ -0,0 +1,26 @@ + + + + nebula_ouster_decoders + 1.0.0 + Nebula Ouster Decoders Library + Ussama Naal + + Apache 2 + Ouster + + autoware_cmake + ros_environment + eigen3_cmake_module + curl + eigen + libssl-dev + libzip-dev + nebula_core_common + nebula_core_decoders + nebula_ouster_common + + + 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..cf2f9cf7d --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/src/ouster_decoder.cpp @@ -0,0 +1,172 @@ +// 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. + +#include "nebula_ouster_decoders/ouster_decoder.hpp" + +#include "nebula_ouster_decoders/ouster_lidar_scan_conversions.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers +{ + +using ouster::sdk::core::SensorInfo; + +struct OusterDecoder::Impl +{ + FieldOfView fov; + pointcloud_callback_t pointcloud_callback; + ouster::sdk::core::ScanBatcher batcher; + ouster::sdk::core::LidarScan lidar_scan; + ouster::sdk::core::XYZLut xyz_lut; + std::shared_ptr packet_format; + + Impl( + FieldOfView fov_in, std::shared_ptr & info, + bool apply_sensor_extrinsics, pointcloud_callback_t cb) + : fov(fov_in), + pointcloud_callback(std::move(cb)), + batcher(info), + lidar_scan(info), + xyz_lut(ouster::sdk::core::make_xyz_lut(*info, apply_sensor_extrinsics)), + packet_format(std::make_shared(*info)) + { + } +}; + +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"; + default: + return "unknown decode error"; + } +} + +OusterDecoder::OusterDecoder( + FieldOfView fov, std::shared_ptr & sensor_info, + bool apply_sensor_extrinsics, pointcloud_callback_t pointcloud_cb) +: impl_(std::make_unique(fov, sensor_info, apply_sensor_extrinsics, 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 = + static_cast(std::chrono::duration_cast( + std::chrono::steady_clock::now() - decode_begin) + .count()); + return result; + } + + if (packet.empty()) { + result.metadata_or_error = DecodeError::EMPTY_PACKET; + result.performance_counters.decode_time_ns = + static_cast(std::chrono::duration_cast( + std::chrono::steady_clock::now() - decode_begin) + .count()); + return result; + } + + const auto & pf = impl_->batcher.pf; + if ( + packet.size() != pf.lidar_packet_size && packet.size() != pf.imu_packet_size && + packet.size() != pf.zone_packet_size) { + result.metadata_or_error = DecodeError::PACKET_FORMAT_INVALID; + result.performance_counters.decode_time_ns = + static_cast(std::chrono::duration_cast( + std::chrono::steady_clock::now() - decode_begin) + .count()); + return result; + } + + bool complete = false; + // TODO(unaal) refactor the code so we don't have to copy packet data. + if (packet.size() == pf.lidar_packet_size) { + ouster::sdk::core::LidarPacket lidar_pkt(static_cast(packet.size())); + std::memcpy(lidar_pkt.buf.data(), packet.data(), packet.size()); + lidar_pkt.format = impl_->packet_format; + complete = impl_->batcher(lidar_pkt, impl_->lidar_scan); + } else if (packet.size() == pf.imu_packet_size) { + ouster::sdk::core::ImuPacket imu_pkt(static_cast(packet.size())); + std::memcpy(imu_pkt.buf.data(), packet.data(), packet.size()); + imu_pkt.format = impl_->packet_format; + complete = impl_->batcher(imu_pkt, impl_->lidar_scan); + } else if (packet.size() == pf.zone_packet_size) { + ouster::sdk::core::ZonePacket zone_pkt(static_cast(packet.size())); + std::memcpy(zone_pkt.buf.data(), packet.data(), packet.size()); + zone_pkt.format = impl_->packet_format; + complete = impl_->batcher(zone_pkt, impl_->lidar_scan); + } + + PacketMetadata metadata{}; + metadata.packet_timestamp_ns = + static_cast(std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count()); + metadata.did_scan_complete = complete; + + if (complete) { + const auto callback_begin = std::chrono::steady_clock::now(); + NebulaPointCloudPtr cloud = + nebula_point_cloud_from_lidar_scan(impl_->lidar_scan, impl_->xyz_lut, impl_->fov); + const uint64_t scan_ts = impl_->lidar_scan.get_first_valid_column_timestamp(); + const double timestamp_s = scan_ts != 0U ? static_cast(scan_ts) * 1e-9 : 0.0; + impl_->pointcloud_callback(cloud, timestamp_s); + result.performance_counters.callback_time_ns = + static_cast(std::chrono::duration_cast( + std::chrono::steady_clock::now() - callback_begin) + .count()); + } + + result.metadata_or_error = metadata; + result.performance_counters.decode_time_ns = + static_cast(std::chrono::duration_cast( + std::chrono::steady_clock::now() - decode_begin) + .count()); + return result; +} + +void OusterDecoder::set_pointcloud_callback(pointcloud_callback_t pointcloud_cb) +{ + impl_->pointcloud_callback = std::move(pointcloud_cb); +} + +} // namespace nebula::drivers diff --git a/src/nebula_ouster/nebula_ouster_decoders/src/ouster_lidar_scan_conversions.cpp b/src/nebula_ouster/nebula_ouster_decoders/src/ouster_lidar_scan_conversions.cpp new file mode 100644 index 000000000..f5c2b88f8 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_decoders/src/ouster_lidar_scan_conversions.cpp @@ -0,0 +1,136 @@ +// 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_lidar_scan_conversions.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace nebula::drivers +{ + +namespace +{ +float normalize_azimuth_deg(float az_deg) +{ + while (az_deg < 0.0f) { + az_deg += 360.0F; + } + while (az_deg >= 360.0f) { + az_deg -= 360.0F; + } + return az_deg; +} + +} // namespace + +using ouster::sdk::core::LidarScan; +using ouster::sdk::core::RANGE_UNIT; +using ouster::sdk::core::SensorInfo; +using ouster::sdk::core::XYZLut; +namespace ChanField = ouster::sdk::core::ChanField; + +NebulaPointCloudPtr nebula_point_cloud_from_lidar_scan( + const LidarScan & scan, const XYZLut & lut, const FieldOfView & fov) +{ + if (!scan.has_field(ChanField::RANGE) || scan.w == 0 || scan.h == 0) { + return std::make_shared(); + } + + const auto range_img = scan.field(ChanField::RANGE); + const ouster::sdk::core::PointCloudXYZd xyz = ouster::sdk::core::cartesian(range_img, lut); + + const bool dual = scan.sensor_info->num_returns() > 1; + const Eigen::Index h = static_cast(scan.h); + const Eigen::Index w = static_cast(scan.w); + + uint64_t base_ts = 0U; + const int first_col = scan.get_first_valid_column(); + if (first_col >= 0 && first_col < static_cast(scan.w)) { + base_ts = scan.timestamp()(first_col); + } + + auto cloud = std::make_shared(); + cloud->reserve(static_cast(scan.h * scan.w)); + + Eigen::ArrayX status = scan.status(); + + for (Eigen::Index row = 0; row < h; ++row) { + for (Eigen::Index col = 0; col < w; ++col) { + // Skip invalid columns. + if ((status(col) & 0x01) == 0) { + continue; + } + + const uint32_t r_mm = range_img(row, col); + if (r_mm == 0U) { + continue; + } + + const size_t i = + static_cast(row) * static_cast(scan.w) + static_cast(col); + + Eigen::Vector3d pt = xyz.row(i); + + const float altitude_deg = scan.sensor_info->beam_altitude_angles[row]; + if (altitude_deg < fov.elevation.start || altitude_deg > fov.elevation.end) { + continue; + } + + // TODO(unaal) could we read this directly from beam_azimuth_angles? + const float azimuth_deg = + normalize_azimuth_deg(static_cast(rad2deg(std::atan2(pt.y(), pt.x())))); + if (azimuth_deg < fov.azimuth.start || azimuth_deg > fov.azimuth.end) { + continue; + } + + NebulaPoint tgt_pt{}; + tgt_pt.x = static_cast(pt.x()); + tgt_pt.y = static_cast(pt.y()); + tgt_pt.z = static_cast(pt.z()); + if (scan.has_field(ChanField::REFLECTIVITY)) { + tgt_pt.intensity = scan.field(ChanField::REFLECTIVITY)(row, col); + } else if (scan.has_field(ChanField::SIGNAL)) { + const auto v = scan.field(ChanField::SIGNAL)(row, col); + tgt_pt.intensity = static_cast(std::min(255U, v >> 4)); + } + // TODO(unaal) retrieve return type from the sensor_info config + tgt_pt.return_type = + static_cast(dual ? ReturnType::LAST : ReturnType::STRONGEST); + tgt_pt.channel = static_cast(row); + tgt_pt.azimuth = deg2rad(azimuth_deg); + tgt_pt.elevation = deg2rad(altitude_deg); + tgt_pt.distance = static_cast(r_mm) * static_cast(RANGE_UNIT); + + const uint64_t col_ts = scan.timestamp()(col); + if (base_ts > 0U && col_ts >= base_ts) { + tgt_pt.time_stamp = static_cast((col_ts - base_ts) & 0xFFFFFFFFU); + } else { + tgt_pt.time_stamp = static_cast(col_ts & 0xFFFFFFFFU); + } + + cloud->push_back(tgt_pt); + } + } + + return cloud; +} + +} // 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..d7694191d --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_hw_interfaces/CMakeLists.txt @@ -0,0 +1,52 @@ +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() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest( + test_ouster_hw_interface + test/test_ouster_hw_interface.cpp + ) + target_include_directories( + test_ouster_hw_interface + PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/include" + ) + target_link_libraries( + test_ouster_hw_interface + nebula_ouster_hw_interfaces + ) + ament_target_dependencies( + test_ouster_hw_interface + nebula_core_common + nebula_ouster_common + nebula_core_hw_interfaces + ) +endif() 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..dc429ccde --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_hw_interfaces/include/nebula_ouster_hw_interfaces/ouster_hw_interface.hpp @@ -0,0 +1,83 @@ +// 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 or replace the callback invoked for each incoming packet. + /// @param scan_callback Callback receiving packet bytes and transport metadata. + /// @return std::monostate if callback is accepted, Error otherwise. + util::expected register_scan_callback( + connections::UdpSocket::callback_t scan_callback); + +private: + ConnectionConfiguration connection_configuration_; + std::optional udp_socket_; + std::shared_ptr packet_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..64ed79194 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_hw_interfaces/package.xml @@ -0,0 +1,21 @@ + + + + nebula_ouster_hw_interfaces + 1.0.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..33ae8c5fa --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_hw_interfaces/src/ouster_hw_interface.cpp @@ -0,0 +1,136 @@ +// 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); + } + }); + } 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 { + 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{}; +} + +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 diff --git a/src/nebula_ouster/nebula_ouster_hw_interfaces/test/test_ouster_hw_interface.cpp b/src/nebula_ouster/nebula_ouster_hw_interfaces/test/test_ouster_hw_interface.cpp new file mode 100644 index 000000000..73bee09b9 --- /dev/null +++ b/src/nebula_ouster/nebula_ouster_hw_interfaces/test/test_ouster_hw_interface.cpp @@ -0,0 +1,221 @@ +// Copyright 2026 TIER IV, Inc. + +#include "nebula_ouster_hw_interfaces/ouster_hw_interface.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using std::chrono_literals::operator""ms; + +class TestOusterHwInterface : public ::testing::Test +{ +protected: + void SetUp() override + { + config_.host_ip = "127.0.0.1"; + config_.sensor_ip = "127.0.0.2"; + config_.data_port = 7502; + config_.receiver_mtu_bytes = 65527; + config_.filter_sender_ip = true; + } + + void TearDown() override + { + if (mock_fd_ != -1) { + close(mock_fd_); + mock_fd_ = -1; + } + } + + void setup_mock_sender(uint16_t src_port, uint16_t dst_port) + { + mock_fd_ = socket(AF_INET, SOCK_DGRAM, 0); + if (mock_fd_ < 0) { + throw std::runtime_error("socket() failed"); + } + int opt = 1; + if (setsockopt(mock_fd_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) { + throw std::runtime_error("setsockopt failed"); + } +#ifdef SO_REUSEPORT + if (setsockopt(mock_fd_, SOL_SOCKET, SO_REUSEPORT, &opt, sizeof(opt)) < 0) { + throw std::runtime_error("setsockopt failed"); + } +#endif + + sockaddr_in src_addr{}; + src_addr.sin_family = AF_INET; + src_addr.sin_port = htons(src_port); + inet_pton(AF_INET, config_.sensor_ip.c_str(), &src_addr.sin_addr); + + if (bind(mock_fd_, reinterpret_cast(&src_addr), sizeof(src_addr)) < 0) { + throw std::runtime_error("bind mock fd failed"); + } + + dst_addr_.sin_family = AF_INET; + dst_addr_.sin_port = htons(dst_port); + inet_pton(AF_INET, config_.host_ip.c_str(), &dst_addr_.sin_addr); + } + + void send_mock_payload() + { + std::vector dummy_payload(512, 0xAB); + ssize_t sent = sendto( + mock_fd_, dummy_payload.data(), dummy_payload.size(), 0, + reinterpret_cast(&dst_addr_), sizeof(dst_addr_)); + if (sent < 0) { + throw std::runtime_error("sendto failed"); + } + } + + nebula::drivers::ConnectionConfiguration config_; + int mock_fd_{-1}; + sockaddr_in dst_addr_{}; +}; + +TEST_F(TestOusterHwInterface, TestStartWithoutCallback) +{ + nebula::drivers::OusterHwInterface hw(config_); + auto result = hw.sensor_interface_start(); + ASSERT_FALSE(result.has_value()); + EXPECT_EQ( + result.error().code, nebula::drivers::OusterHwInterface::Error::Code::CALLBACK_NOT_REGISTERED); +} + +TEST_F(TestOusterHwInterface, TestRegisterEmptyCallback) +{ + nebula::drivers::OusterHwInterface hw(config_); + auto result = hw.register_scan_callback(nullptr); + ASSERT_FALSE(result.has_value()); + EXPECT_EQ(result.error().code, nebula::drivers::OusterHwInterface::Error::Code::INVALID_CALLBACK); +} + +TEST_F(TestOusterHwInterface, TestLifecycle) +{ + nebula::drivers::OusterHwInterface hw(config_); + + auto reg_result = hw.register_scan_callback( + [](std::vector &, const nebula::drivers::connections::UdpSocket::RxMetadata &) {}); + ASSERT_TRUE(reg_result.has_value()); + + auto start_result = hw.sensor_interface_start(); + ASSERT_TRUE(start_result.has_value()); + + auto stop_result = hw.sensor_interface_stop(); + ASSERT_TRUE(stop_result.has_value()); +} + +TEST_F(TestOusterHwInterface, TestDoubleStartIsIdempotent) +{ + nebula::drivers::OusterHwInterface hw(config_); + + auto reg_result = hw.register_scan_callback( + [](std::vector &, const nebula::drivers::connections::UdpSocket::RxMetadata &) {}); + ASSERT_TRUE(reg_result.has_value()); + + auto start1 = hw.sensor_interface_start(); + ASSERT_TRUE(start1.has_value()); + + auto start2 = hw.sensor_interface_start(); + ASSERT_TRUE(start2.has_value()); + + auto stop_result = hw.sensor_interface_stop(); + ASSERT_TRUE(stop_result.has_value()); +} + +TEST_F(TestOusterHwInterface, TestStopWithoutStart) +{ + nebula::drivers::OusterHwInterface hw(config_); + auto result = hw.sensor_interface_stop(); + ASSERT_TRUE(result.has_value()); +} + +TEST_F(TestOusterHwInterface, TestRegisterCallbackWhileRunning) +{ + nebula::drivers::OusterHwInterface hw(config_); + + auto reg_result = hw.register_scan_callback( + [](std::vector &, const nebula::drivers::connections::UdpSocket::RxMetadata &) {}); + ASSERT_TRUE(reg_result.has_value()); + + auto start_result = hw.sensor_interface_start(); + ASSERT_TRUE(start_result.has_value()); + + auto replace_result = hw.register_scan_callback( + [](std::vector &, const nebula::drivers::connections::UdpSocket::RxMetadata &) {}); + ASSERT_FALSE(replace_result.has_value()); + EXPECT_EQ( + replace_result.error().code, + nebula::drivers::OusterHwInterface::Error::Code::INVALID_OPERATION); + + auto stop_result = hw.sensor_interface_stop(); + ASSERT_TRUE(stop_result.has_value()); +} + +TEST_F(TestOusterHwInterface, TestScanPacketCallback) +{ + nebula::drivers::OusterHwInterface hw(config_); + + std::atomic_bool callback_triggered{false}; + auto reg_result = hw.register_scan_callback( + [&]( + std::vector & buffer, const nebula::drivers::connections::UdpSocket::RxMetadata &) { + if (buffer.size() > 0) callback_triggered = true; + }); + ASSERT_TRUE(reg_result.has_value()); + + auto start_result = hw.sensor_interface_start(); + ASSERT_TRUE(start_result.has_value()); + + setup_mock_sender(config_.data_port, config_.data_port); + + for (int i = 0; i < 5; ++i) { + send_mock_payload(); + std::this_thread::sleep_for(10ms); + if (callback_triggered) break; + } + + EXPECT_TRUE(callback_triggered); + + auto stop_result = hw.sensor_interface_stop(); + ASSERT_TRUE(stop_result.has_value()); +} + +TEST_F(TestOusterHwInterface, TestErrorCodeToString) +{ + EXPECT_STREQ( + nebula::drivers::OusterHwInterface::to_cstr( + nebula::drivers::OusterHwInterface::Error::Code::CALLBACK_NOT_REGISTERED), + "callback not registered"); + EXPECT_STREQ( + nebula::drivers::OusterHwInterface::to_cstr( + nebula::drivers::OusterHwInterface::Error::Code::INVALID_CALLBACK), + "invalid callback"); + EXPECT_STREQ( + nebula::drivers::OusterHwInterface::to_cstr( + nebula::drivers::OusterHwInterface::Error::Code::INVALID_OPERATION), + "invalid operation"); + EXPECT_STREQ( + nebula::drivers::OusterHwInterface::to_cstr( + nebula::drivers::OusterHwInterface::Error::Code::SOCKET_OPEN_FAILED), + "failed to open UDP socket"); + EXPECT_STREQ( + nebula::drivers::OusterHwInterface::to_cstr( + nebula::drivers::OusterHwInterface::Error::Code::SOCKET_CLOSE_FAILED), + "failed to close UDP socket"); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}