From 9a0fc5c6a71c714c3dda83dd19e5e108a4a93f28 Mon Sep 17 00:00:00 2001 From: David Wong Date: Wed, 3 Dec 2025 22:10:46 +0900 Subject: [PATCH 01/65] feat(nebula_sample): add sample sensor package as template Signed-off-by: David Wong --- src/nebula_sample/README.md | 79 +++++++++++++++ .../nebula_sample/CMakeLists.txt | 61 ++++++++++++ .../nebula_sample/sample_ros_wrapper.hpp | 61 ++++++++++++ .../launch/nebula_sample.launch.xml | 9 ++ src/nebula_sample/nebula_sample/package.xml | 27 ++++++ .../nebula_sample/src/sample_ros_wrapper.cpp | 95 +++++++++++++++++++ .../nebula_sample_common/CMakeLists.txt | 22 +++++ .../nebula_sample_common/sample_common.hpp | 62 ++++++++++++ .../nebula_sample_common/package.xml | 20 ++++ .../nebula_sample_decoders/CMakeLists.txt | 30 ++++++ .../decoders/sample_decoder.hpp | 57 +++++++++++ .../decoders/sample_scan_decoder.hpp | 72 ++++++++++++++ .../nebula_sample_decoders/sample_driver.hpp | 48 ++++++++++ .../nebula_sample_decoders/package.xml | 22 +++++ .../src/sample_driver.cpp | 48 ++++++++++ .../CMakeLists.txt | 27 ++++++ .../sample_hw_interface.hpp | 48 ++++++++++ .../nebula_sample_hw_interfaces/package.xml | 22 +++++ .../src/sample_hw_interface.cpp | 49 ++++++++++ 19 files changed, 859 insertions(+) create mode 100644 src/nebula_sample/README.md create mode 100644 src/nebula_sample/nebula_sample/CMakeLists.txt create mode 100644 src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp create mode 100644 src/nebula_sample/nebula_sample/launch/nebula_sample.launch.xml create mode 100644 src/nebula_sample/nebula_sample/package.xml create mode 100644 src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp create mode 100644 src/nebula_sample/nebula_sample_common/CMakeLists.txt create mode 100644 src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp create mode 100644 src/nebula_sample/nebula_sample_common/package.xml create mode 100644 src/nebula_sample/nebula_sample_decoders/CMakeLists.txt create mode 100644 src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp create mode 100644 src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp create mode 100644 src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp create mode 100644 src/nebula_sample/nebula_sample_decoders/package.xml create mode 100644 src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp create mode 100644 src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt create mode 100644 src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp create mode 100644 src/nebula_sample/nebula_sample_hw_interfaces/package.xml create mode 100644 src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp diff --git a/src/nebula_sample/README.md b/src/nebula_sample/README.md new file mode 100644 index 000000000..decf8b827 --- /dev/null +++ b/src/nebula_sample/README.md @@ -0,0 +1,79 @@ +# Nebula Sample Sensor Package + +A template sensor package for the Nebula LiDAR driver framework. This package provides a minimal, working example that developers can copy and modify to add support for new sensors. + +## Purpose + +This package serves as a starting point for adding new sensor support to Nebula. It includes all necessary components with empty/stub implementations that compile and run without errors. + +## Package Structure + +The sample sensor consists of four packages: + +- **nebula_sample_common** - Common definitions and configuration structures +- **nebula_sample_decoders** - Packet decoder and driver implementation +- **nebula_sample_hw_interfaces** - Hardware interface for sensor communication +- **nebula_sample** - ROS 2 wrapper and launch files + +## Building + +```bash +colcon build --packages-up-to nebula_sample +``` + +## Running + +```bash +ros2 launch nebula_sample nebula_sample.launch.xml +``` + +## Using as a Template + +To create support for a new sensor: + +1. Copy this package structure: + + ```bash + cp -r src/nebula_sample src/nebula_ + ``` + +2. Rename all occurrences: + + - Files: `sample_*` → `_*` + - Classes: `Sample*` → `*` + - Packages: `nebula_sample*` → `nebula_*` + +3. Implement sensor-specific logic: + + - **Common**: Add sensor configuration parameters + - **Decoders**: Implement packet parsing and point cloud generation + - **HW Interfaces**: Implement UDP/TCP communication with sensor + - **ROS Wrapper**: Add ROS parameters and topics + +4. Update dependencies in `package.xml` and `CMakeLists.txt` + +## Key Components + +### Configuration (`*_common`) + +- `SampleSensorConfiguration` - Sensor-specific settings +- `SampleCalibrationConfiguration` - Calibration data structure + +### Decoder (`*_decoders`) + +- `SampleScanDecoder` - Base decoder interface +- `SampleDecoder` - Packet decoder implementation +- `SampleDriver` - Main driver class + +### Hardware Interface (`*_hw_interfaces`) + +- `SampleHwInterface` - Sensor communication interface + +### ROS Wrapper + +- `SampleRosWrapper` - ROS 2 node wrapping the driver +- Point cloud publisher on `/points_raw` + +## Reference Implementation + +This package is based on the Hesai implementation structure but with all vendor-specific code removed. For a complete example, refer to `nebula_hesai`. diff --git a/src/nebula_sample/nebula_sample/CMakeLists.txt b/src/nebula_sample/nebula_sample/CMakeLists.txt new file mode 100644 index 000000000..eed86dfdf --- /dev/null +++ b/src/nebula_sample/nebula_sample/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.20) +project(nebula_sample) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +add_library(nebula_sample_ros_wrapper SHARED src/sample_ros_wrapper.cpp) +target_include_directories(nebula_sample_ros_wrapper PUBLIC + $ + $ +) +ament_target_dependencies(nebula_sample_ros_wrapper + nebula_core_common + nebula_core_ros + nebula_sample_common + nebula_sample_decoders + nebula_sample_hw_interfaces + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs +) + +rclcpp_components_register_node(nebula_sample_ros_wrapper + PLUGIN "nebula::ros::SampleRosWrapper" + EXECUTABLE nebula_sample_ros_wrapper_node +) + +install(TARGETS nebula_sample_ros_wrapper + EXPORT export_nebula_sample_ros_wrapper + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + +ament_export_include_directories(include) +ament_export_targets(export_nebula_sample_ros_wrapper) +ament_export_dependencies( + nebula_core_common + nebula_core_ros + nebula_sample_common + nebula_sample_decoders + nebula_sample_hw_interfaces + rclcpp + rclcpp_components + sensor_msgs +) + +ament_package() diff --git a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp new file mode 100644 index 000000000..5b26c9786 --- /dev/null +++ b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp @@ -0,0 +1,61 @@ +// Copyright 2024 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_SAMPLE_ROS_WRAPPER_HPP +#define NEBULA_SAMPLE_ROS_WRAPPER_HPP + +#include "nebula_core_common/nebula_common.hpp" +#include "nebula_core_common/nebula_status.hpp" +#include "nebula_sample_common/sample_common.hpp" +#include "nebula_sample_decoders/sample_driver.hpp" +#include "nebula_sample_hw_interfaces/sample_hw_interface.hpp" + +#include + +#include + +#include +#include + +namespace nebula::ros +{ + +class SampleRosWrapper : public rclcpp::Node +{ +public: + explicit SampleRosWrapper(const rclcpp::NodeOptions & options); + ~SampleRosWrapper() override; + + Status get_status(); + Status stream_start(); + +private: + void receive_cloud_packet_callback( + const std::vector & packet, + const drivers::connections::UdpSocket::RxMetadata & metadata); + + std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr calibration_cfg_ptr_; + + std::shared_ptr driver_ptr_; + std::shared_ptr hw_interface_ptr_; + + rclcpp::Publisher::SharedPtr points_pub_; + + bool launch_hw_; +}; + +} // namespace nebula::ros + +#endif // NEBULA_SAMPLE_ROS_WRAPPER_HPP diff --git a/src/nebula_sample/nebula_sample/launch/nebula_sample.launch.xml b/src/nebula_sample/nebula_sample/launch/nebula_sample.launch.xml new file mode 100644 index 000000000..d0281adec --- /dev/null +++ b/src/nebula_sample/nebula_sample/launch/nebula_sample.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/src/nebula_sample/nebula_sample/package.xml b/src/nebula_sample/nebula_sample/package.xml new file mode 100644 index 000000000..1989fa404 --- /dev/null +++ b/src/nebula_sample/nebula_sample/package.xml @@ -0,0 +1,27 @@ + + + + nebula_sample + 0.0.1 + Nebula Sample ROS 2 Package + MAP IV + + Apache 2 + TIER IV + + autoware_cmake + + nebula_core_common + nebula_core_ros + nebula_sample_common + nebula_sample_decoders + nebula_sample_hw_interfaces + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + + + ament_cmake + + diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp new file mode 100644 index 000000000..83d369820 --- /dev/null +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -0,0 +1,95 @@ +// Copyright 2024 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_sample/sample_ros_wrapper.hpp" + +#include + +#include +#include +#include + +namespace nebula::ros +{ + +SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) +: Node("nebula_sample_ros_wrapper", options) +{ + // Parameter declaration + declare_parameter("launch_hw", true); + launch_hw_ = get_parameter("launch_hw").as_bool(); + + // Initialize config + sensor_cfg_ptr_ = std::make_shared(); + calibration_cfg_ptr_ = std::make_shared(); + + // Initialize Driver + driver_ptr_ = std::make_shared(sensor_cfg_ptr_, calibration_cfg_ptr_); + + driver_ptr_->set_pointcloud_callback( + [this](const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { + (void)timestamp_s; + if (points_pub_->get_subscription_count() > 0 && pointcloud) { + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = this->now(); + points_pub_->publish(std::move(ros_pc_msg_ptr)); + } + }); + + // Initialize HW Interface + if (launch_hw_) { + hw_interface_ptr_ = std::make_shared(); + hw_interface_ptr_->set_sensor_configuration(sensor_cfg_ptr_); + hw_interface_ptr_->register_scan_callback( + std::bind( + &SampleRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1, + std::placeholders::_2)); + stream_start(); + } + + points_pub_ = create_publisher("points_raw", 10); +} + +SampleRosWrapper::~SampleRosWrapper() +{ + if (hw_interface_ptr_) { + hw_interface_ptr_->sensor_interface_stop(); + } +} + +Status SampleRosWrapper::get_status() +{ + return Status::OK; +} + +Status SampleRosWrapper::stream_start() +{ + if (hw_interface_ptr_) { + return hw_interface_ptr_->sensor_interface_start(); + } + return Status::NOT_INITIALIZED; +} + +void SampleRosWrapper::receive_cloud_packet_callback( + const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata) +{ + (void)metadata; + driver_ptr_->parse_cloud_packet(packet); +} + +} // namespace nebula::ros + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(nebula::ros::SampleRosWrapper) diff --git a/src/nebula_sample/nebula_sample_common/CMakeLists.txt b/src/nebula_sample/nebula_sample_common/CMakeLists.txt new file mode 100644 index 000000000..010cdfd69 --- /dev/null +++ b/src/nebula_sample/nebula_sample_common/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.20) +project(nebula_sample_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +add_library(nebula_sample_common INTERFACE) + +target_include_directories( + nebula_sample_common + INTERFACE $ + $) +target_link_libraries(nebula_sample_common + INTERFACE nebula_core_common::nebula_core_common) + +install(TARGETS nebula_sample_common EXPORT export_nebula_sample_common) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) + +ament_export_targets(export_nebula_sample_common) +ament_export_dependencies(nebula_core_common) + +ament_package() diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp new file mode 100644 index 000000000..09b0a112d --- /dev/null +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp @@ -0,0 +1,62 @@ +// Copyright 2024 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_SAMPLE_COMMON_H +#define NEBULA_SAMPLE_COMMON_H + +#include "nebula_core_common/nebula_common.hpp" +#include "nebula_core_common/nebula_status.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ + +struct SampleSensorConfiguration : public LidarConfigurationBase +{ + // Add any dummy specific configuration here if needed + std::string sample_field; +}; + +inline std::ostream & operator<<(std::ostream & os, SampleSensorConfiguration const & arg) +{ + os << "Sample Sensor Configuration:" << '\n'; + os << static_cast(arg) << '\n'; + os << "Sample Field: " << arg.sample_field << '\n'; + return os; +} + +struct SampleCalibrationConfiguration : public CalibrationConfigurationBase +{ + nebula::Status load_from_file(const std::string & calibration_file) + { + // Sample implementation + return Status::OK; + } + + nebula::Status save_to_file(const std::string & calibration_file) + { + // Sample implementation + return Status::OK; + } +}; + +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_SAMPLE_COMMON_H diff --git a/src/nebula_sample/nebula_sample_common/package.xml b/src/nebula_sample/nebula_sample_common/package.xml new file mode 100644 index 000000000..f19561930 --- /dev/null +++ b/src/nebula_sample/nebula_sample_common/package.xml @@ -0,0 +1,20 @@ + + + + nebula_sample_common + 0.0.1 + Nebula Common Sample Libraries and headers + MAP IV + + Apache 2 + TIER IV + + autoware_cmake + ros_environment + + nebula_core_common + + + ament_cmake + + diff --git a/src/nebula_sample/nebula_sample_decoders/CMakeLists.txt b/src/nebula_sample/nebula_sample_decoders/CMakeLists.txt new file mode 100644 index 000000000..07d36e427 --- /dev/null +++ b/src/nebula_sample/nebula_sample_decoders/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.20) +project(nebula_sample_decoders) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Sample Decoder +add_library(nebula_sample_decoders SHARED src/sample_driver.cpp) + +target_include_directories( + nebula_sample_decoders + PUBLIC $ + $) + +target_link_libraries( + nebula_sample_decoders + PUBLIC nebula_core_common::nebula_core_common + nebula_sample_common::nebula_sample_common + nebula_core_decoders::nebula_core_decoders rclcpp::rclcpp) + +install(TARGETS nebula_sample_decoders EXPORT export_nebula_sample_decoders) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_nebula_sample_decoders) + +ament_export_dependencies(nebula_core_common nebula_sample_common + nebula_core_decoders rclcpp) + +ament_package() diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp new file mode 100644 index 000000000..899cfc6f0 --- /dev/null +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 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_SAMPLE_DECODER_HPP +#define NEBULA_SAMPLE_DECODER_HPP + +#include "nebula_sample_common/sample_common.hpp" +#include "nebula_sample_decoders/decoders/sample_scan_decoder.hpp" + +#include +#include + +namespace nebula::drivers +{ + +class SampleDecoder : public SampleScanDecoder +{ +public: + SampleDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration) + { + // Initialize + (void)sensor_configuration; + (void)calibration_configuration; + } + + PacketDecodeResult unpack(const std::vector & packet) override + { + // Sample implementation - does nothing, just returns an error + (void)packet; + return {{}, DecodeError::PACKET_PARSE_FAILED}; + } + + void set_pointcloud_callback(pointcloud_callback_t callback) override + { + pointcloud_callback_ = callback; + } + +private: + pointcloud_callback_t pointcloud_callback_; +}; + +} // namespace nebula::drivers + +#endif // NEBULA_SAMPLE_DECODER_HPP diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp new file mode 100644 index 000000000..ddf992fd3 --- /dev/null +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp @@ -0,0 +1,72 @@ +// Copyright 2024 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_SAMPLE_SCAN_DECODER_HPP +#define NEBULA_SAMPLE_SCAN_DECODER_HPP + +#include +#include +#include + +#include +#include +#include + +namespace nebula::drivers +{ +enum class DecodeError : uint8_t { + PACKET_PARSE_FAILED, + DRIVER_NOT_OK, + INVALID_PACKET_SIZE, +}; + +struct PacketMetadata +{ + uint64_t packet_timestamp_ns{}; + bool did_scan_complete{false}; +}; + +struct PerformanceCounters +{ + uint64_t decode_time_ns{0}; + uint64_t callback_time_ns{0}; +}; + +struct PacketDecodeResult +{ + PerformanceCounters performance_counters; + util::expected metadata_or_error; +}; + +class SampleScanDecoder +{ +public: + using pointcloud_callback_t = + std::function; + + SampleScanDecoder(SampleScanDecoder && c) = delete; + SampleScanDecoder & operator=(SampleScanDecoder && c) = delete; + SampleScanDecoder(const SampleScanDecoder & c) = delete; + SampleScanDecoder & operator=(const SampleScanDecoder & c) = delete; + + virtual ~SampleScanDecoder() = default; + SampleScanDecoder() = default; + + virtual PacketDecodeResult unpack(const std::vector & packet) = 0; + + virtual void set_pointcloud_callback(pointcloud_callback_t callback) = 0; +}; +} // namespace nebula::drivers + +#endif // NEBULA_SAMPLE_SCAN_DECODER_HPP diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp new file mode 100644 index 000000000..956ce73ce --- /dev/null +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp @@ -0,0 +1,48 @@ +// Copyright 2024 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_SAMPLE_DRIVER_HPP +#define NEBULA_SAMPLE_DRIVER_HPP + +#include "nebula_core_common/nebula_status.hpp" +#include "nebula_sample_common/sample_common.hpp" +#include "nebula_sample_decoders/decoders/sample_scan_decoder.hpp" + +#include +#include + +namespace nebula::drivers +{ + +class SampleDriver +{ +public: + SampleDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration); + + Status get_status(); + + PacketDecodeResult parse_cloud_packet(const std::vector & packet); + + void set_pointcloud_callback(SampleScanDecoder::pointcloud_callback_t pointcloud_cb); + +private: + std::shared_ptr scan_decoder_; + Status driver_status_; +}; + +} // namespace nebula::drivers + +#endif // NEBULA_SAMPLE_DRIVER_HPP diff --git a/src/nebula_sample/nebula_sample_decoders/package.xml b/src/nebula_sample/nebula_sample_decoders/package.xml new file mode 100644 index 000000000..5e52fd60e --- /dev/null +++ b/src/nebula_sample/nebula_sample_decoders/package.xml @@ -0,0 +1,22 @@ + + + + nebula_sample_decoders + 0.0.1 + Nebula Sample Decoders Library + MAP IV + + Apache 2 + TIER IV + + autoware_cmake + ros_environment + nebula_core_common + nebula_core_decoders + nebula_sample_common + rclcpp + + + ament_cmake + + diff --git a/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp b/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp new file mode 100644 index 000000000..a9b3b419b --- /dev/null +++ b/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp @@ -0,0 +1,48 @@ +// Copyright 2024 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_sample_decoders/sample_driver.hpp" + +#include "nebula_sample_decoders/decoders/sample_decoder.hpp" + +#include +#include + +namespace nebula::drivers +{ + +SampleDriver::SampleDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration) +{ + driver_status_ = Status::OK; + scan_decoder_ = std::make_shared(sensor_configuration, calibration_configuration); +} + +Status SampleDriver::get_status() +{ + return driver_status_; +} + +PacketDecodeResult SampleDriver::parse_cloud_packet(const std::vector & packet) +{ + return scan_decoder_->unpack(packet); +} + +void SampleDriver::set_pointcloud_callback(SampleScanDecoder::pointcloud_callback_t pointcloud_cb) +{ + scan_decoder_->set_pointcloud_callback(pointcloud_cb); +} + +} // namespace nebula::drivers diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt b/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt new file mode 100644 index 000000000..3172db715 --- /dev/null +++ b/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.20) +project(nebula_sample_hw_interfaces) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +add_library(nebula_sample_hw_interfaces SHARED src/sample_hw_interface.cpp) + +target_include_directories( + nebula_sample_hw_interfaces + PUBLIC $ + $) + +ament_target_dependencies( + nebula_sample_hw_interfaces PUBLIC boost_tcp_driver nebula_core_common + nebula_sample_common nebula_core_hw_interfaces) + +install(TARGETS nebula_sample_hw_interfaces + EXPORT export_nebula_sample_hw_interfaces) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_nebula_sample_hw_interfaces) +ament_export_dependencies(boost_tcp_driver nebula_core_common + nebula_sample_common nebula_core_hw_interfaces) + +ament_package() diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp new file mode 100644 index 000000000..35a8d0faa --- /dev/null +++ b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp @@ -0,0 +1,48 @@ +// Copyright 2024 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_SAMPLE_HW_INTERFACE_HPP +#define NEBULA_SAMPLE_HW_INTERFACE_HPP + +#include +#include +#include + +#include +#include + +namespace nebula::drivers +{ + +class SampleHwInterface +{ +public: + SampleHwInterface(); + + Status sensor_interface_start(); + Status sensor_interface_stop(); + + Status set_sensor_configuration( + std::shared_ptr sensor_configuration); + + Status register_scan_callback(connections::UdpSocket::callback_t scan_callback); + +private: + std::shared_ptr sensor_configuration_; + connections::UdpSocket::callback_t cloud_packet_callback_; +}; + +} // namespace nebula::drivers + +#endif // NEBULA_SAMPLE_HW_INTERFACE_HPP diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/package.xml b/src/nebula_sample/nebula_sample_hw_interfaces/package.xml new file mode 100644 index 000000000..003631183 --- /dev/null +++ b/src/nebula_sample/nebula_sample_hw_interfaces/package.xml @@ -0,0 +1,22 @@ + + + + nebula_sample_hw_interfaces + 0.0.1 + Nebula HW Interfaces Sample + MAP IV + + Apache 2 + TIER IV + + autoware_cmake + + boost_tcp_driver + nebula_core_common + nebula_core_hw_interfaces + nebula_sample_common + + + ament_cmake + + diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp new file mode 100644 index 000000000..38a8b80bb --- /dev/null +++ b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp @@ -0,0 +1,49 @@ +// Copyright 2024 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_sample_hw_interfaces/sample_hw_interface.hpp" + +#include + +namespace nebula::drivers +{ + +SampleHwInterface::SampleHwInterface() +{ +} + +Status SampleHwInterface::sensor_interface_start() +{ + return Status::OK; +} + +Status SampleHwInterface::sensor_interface_stop() +{ + return Status::OK; +} + +Status SampleHwInterface::set_sensor_configuration( + std::shared_ptr sensor_configuration) +{ + sensor_configuration_ = sensor_configuration; + return Status::OK; +} + +Status SampleHwInterface::register_scan_callback(connections::UdpSocket::callback_t scan_callback) +{ + cloud_packet_callback_ = scan_callback; + return Status::OK; +} + +} // namespace nebula::drivers From ce3d1461781bb44ad3f5cd093e1e652f5ec02167 Mon Sep 17 00:00:00 2001 From: David Wong Date: Wed, 3 Dec 2025 22:50:51 +0900 Subject: [PATCH 02/65] feat(nebula_sample): add integration guide Signed-off-by: David Wong --- src/nebula_sample/INTEGRATION_GUIDE.md | 117 +++++++++++++++++++++++++ src/nebula_sample/README.md | 25 ++---- 2 files changed, 122 insertions(+), 20 deletions(-) create mode 100644 src/nebula_sample/INTEGRATION_GUIDE.md diff --git a/src/nebula_sample/INTEGRATION_GUIDE.md b/src/nebula_sample/INTEGRATION_GUIDE.md new file mode 100644 index 000000000..07d58d6e2 --- /dev/null +++ b/src/nebula_sample/INTEGRATION_GUIDE.md @@ -0,0 +1,117 @@ +# New Sensor Integration Guide + +This guide outlines the process of adding support for a new LiDAR sensor to Nebula using the `nebula_sample` package as a template. + +## Workflow Overview + +The integration process involves four main steps: + +1. **Clone**: Copy the sample package structure. +2. **Rename**: Update all file names, class names, and namespaces to match your sensor vendor. +3. **Implement**: Fill in the stub methods with your sensor-specific logic. +4. **Verify**: Build and test with your sensor hardware or PCAP data. + +## 1. Cloning the Template + +Copy the `nebula_sample` directory to a new directory named after your sensor vendor (e.g., `nebula_myvendor`). + +```bash +cp -r src/nebula_sample src/nebula_myvendor +``` + +## 2. Renaming Components + +You must rename all occurrences of "sample" and "Sample" to your vendor name. This includes: + +- **Directory Names**: `nebula_sample_*` -> `nebula_myvendor_*` +- **File Names**: `sample_*` -> `myvendor_*` +- **Class Names**: `SampleDriver` -> `MyVendorDriver` +- **Namespaces**: `nebula::drivers::sample` -> `nebula::drivers::myvendor` +- **CMake & Package Definitions**: Update `CMakeLists.txt` and `package.xml` in all sub-packages. + +## 3. Implementation Details + +The Nebula architecture is modular. You will need to implement specific classes in each sub-package. + +### A. Common Package (`nebula_myvendor_common`) + +**Purpose**: Defines data structures for configuration and calibration. + +- **`MyVendorSensorConfiguration`**: + + - Add fields for sensor-specific settings (e.g., return mode, frequency, IP address). + - This struct is passed to both the driver and hardware interface. + +- **`MyVendorCalibrationConfiguration`**: + - Define how calibration data (e.g., angle corrections) is stored. + - Implement `load_from_file()` to parse your vendor's calibration file format (csv, dat, xml, etc.). + +### B. Decoders Package (`nebula_myvendor_decoders`) + +**Purpose**: Handles packet parsing and point cloud generation. + +- **`MyVendorScanDecoder`** (Interface): + + - Inherits from `ScanDecoder`. + - Defines the contract for parsing packets. + +- **`MyVendorDecoder`** (Implementation): + + - **`unpack(packet)`**: The core method. It takes raw bytes (UDP packet) and returns a `PacketDecodeResult` containing the decoded points or an error. + - **`get_pointcloud()`**: (Optional) If you buffer points, this returns the constructed point cloud. + - **Key Task**: Parse the raw byte stream according to your sensor's user manual. + +- **`MyVendorDriver`**: + - The high-level manager. + - Initializes the decoder and hardware interface. + - **`parse_cloud_packet(packet)`**: Called when a packet is received. Delegates to `decoder_->unpack()`. + +### C. Hardware Interfaces Package (`nebula_myvendor_hw_interfaces`) + +**Purpose**: Handles network communication (UDP/TCP). + +- **`MyVendorHwInterface`**: + - Inherits from `HwInterfaceBase`. + - **`sensor_interface_start()`**: Setup UDP sockets and start receiving data. + - **`sensor_interface_stop()`**: Close sockets. + - **`register_scan_callback()`**: Register the function to call when a packet is received (usually `MyVendorDriver::parse_cloud_packet`). + - **Key Task**: Implement the UDP receiver loop. You can use `boost::asio` or standard sockets. + +### D. ROS Wrapper Package (`nebula_myvendor`) + +**Purpose**: Bridges the C++ driver with ROS 2. + +- **`MyVendorRosWrapper`**: + - Inherits from `rclcpp::Node`. + - **`initialize_driver()`**: Instantiates your `MyVendorDriver` and `MyVendorHwInterface`. + - **`receive_cloud_packet_callback()`**: The bridge callback. Receives data from HW interface, passes to Driver, gets PointCloud, converts to ROS message, and publishes. + - **Parameters**: Declare ROS parameters that map to your `MyVendorSensorConfiguration`. + +## 4. Verification + +1. **Build**: + + ```bash + colcon build --packages-up-to nebula_myvendor + ``` + +2. **Launch**: + + ```bash + ros2 launch nebula_myvendor nebula_myvendor.launch.xml + ``` + +3. **Test**: + - Verify topics: `ros2 topic list` + - Visualize: `rviz2` (add PointCloud2 display) + +## Checklist + +- [ ] Renamed all directories and files. +- [ ] Updated `CMakeLists.txt` and `package.xml` dependencies. +- [ ] Implemented `SensorConfiguration` struct. +- [ ] Implemented `unpack()` in Decoder. +- [ ] Implemented UDP reception in HW Interface. +- [ ] Mapped ROS parameters in Wrapper. +- [ ] Added copyright headers. +- [ ] Verified build. diff --git a/src/nebula_sample/README.md b/src/nebula_sample/README.md index decf8b827..e5a210ecc 100644 --- a/src/nebula_sample/README.md +++ b/src/nebula_sample/README.md @@ -29,28 +29,13 @@ ros2 launch nebula_sample nebula_sample.launch.xml ## Using as a Template -To create support for a new sensor: +For detailed instructions on how to use this package as a template for adding a new sensor, please refer to the [Integration Guide](INTEGRATION_GUIDE.md). -1. Copy this package structure: +The guide covers: - ```bash - cp -r src/nebula_sample src/nebula_ - ``` - -2. Rename all occurrences: - - - Files: `sample_*` → `_*` - - Classes: `Sample*` → `*` - - Packages: `nebula_sample*` → `nebula_*` - -3. Implement sensor-specific logic: - - - **Common**: Add sensor configuration parameters - - **Decoders**: Implement packet parsing and point cloud generation - - **HW Interfaces**: Implement UDP/TCP communication with sensor - - **ROS Wrapper**: Add ROS parameters and topics - -4. Update dependencies in `package.xml` and `CMakeLists.txt` +1. Cloning and renaming the package +2. Implementing sensor-specific logic +3. Verifying the new implementation ## Key Components From e0e62f79d949db0a8c506ef6da3591e02fbbf823 Mon Sep 17 00:00:00 2001 From: David Wong Date: Wed, 3 Dec 2025 23:00:33 +0900 Subject: [PATCH 03/65] feat(nebula_sample): update license year Signed-off-by: David Wong --- .../nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp | 2 +- src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp | 2 +- .../include/nebula_sample_common/sample_common.hpp | 2 +- .../include/nebula_sample_decoders/decoders/sample_decoder.hpp | 2 +- .../nebula_sample_decoders/decoders/sample_scan_decoder.hpp | 2 +- .../include/nebula_sample_decoders/sample_driver.hpp | 2 +- src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp | 2 +- .../include/nebula_sample_hw_interfaces/sample_hw_interface.hpp | 2 +- .../nebula_sample_hw_interfaces/src/sample_hw_interface.cpp | 2 +- 9 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp index 5b26c9786..4abe2f75a 100644 --- a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp +++ b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// 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. diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index 83d369820..739db956c 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// 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. diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp index 09b0a112d..89ed0e536 100644 --- a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// 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. diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp index 899cfc6f0..5d6aacc58 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// 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. diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp index ddf992fd3..e605f972e 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// 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. diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp index 956ce73ce..86afb37dc 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// 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. diff --git a/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp b/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp index a9b3b419b..aaa285c67 100644 --- a/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp +++ b/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// 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. diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp index 35a8d0faa..e5f0c706d 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// 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. diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp index 38a8b80bb..408d4f6b8 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// 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. From 3b3ee6c966120a36ab893c12688c699bf5f48d84 Mon Sep 17 00:00:00 2001 From: David Wong Date: Tue, 9 Dec 2025 12:58:35 +0900 Subject: [PATCH 04/65] chore(nebula_sample): cspell fixes and integration guide clarity Signed-off-by: David Wong --- .cspell.json | 3 +++ src/nebula_sample/INTEGRATION_GUIDE.md | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/.cspell.json b/.cspell.json index f48e528d0..017120188 100644 --- a/.cspell.json +++ b/.cspell.json @@ -7,6 +7,7 @@ "Adctp", "Agnocast", "applicate", + "asio", "AT", "autosar", "block_id", @@ -34,6 +35,7 @@ "millidegrees", "mkdoxy", "Msop", + "myvendor", "nohup", "nproc", "nsec", @@ -50,6 +52,7 @@ "QT", "rclcpp", "RCPPUTILS", + "rviz", "schedutil", "srvs", "STD_COUT", diff --git a/src/nebula_sample/INTEGRATION_GUIDE.md b/src/nebula_sample/INTEGRATION_GUIDE.md index 07d58d6e2..f223e5831 100644 --- a/src/nebula_sample/INTEGRATION_GUIDE.md +++ b/src/nebula_sample/INTEGRATION_GUIDE.md @@ -44,7 +44,7 @@ The Nebula architecture is modular. You will need to implement specific classes - **`MyVendorCalibrationConfiguration`**: - Define how calibration data (e.g., angle corrections) is stored. - - Implement `load_from_file()` to parse your vendor's calibration file format (csv, dat, xml, etc.). + - Implement `load_from_file()` to parse your vendor's calibration file format (csv, dat, xml, etc.) if required. ### B. Decoders Package (`nebula_myvendor_decoders`) @@ -75,7 +75,7 @@ The Nebula architecture is modular. You will need to implement specific classes - **`sensor_interface_start()`**: Setup UDP sockets and start receiving data. - **`sensor_interface_stop()`**: Close sockets. - **`register_scan_callback()`**: Register the function to call when a packet is received (usually `MyVendorDriver::parse_cloud_packet`). - - **Key Task**: Implement the UDP receiver loop. You can use `boost::asio` or standard sockets. + - **UDP Receiver Example**: Refer to `src/nebula_core/include/nebula_core/hw/hw_interface_base.hpp` for the base interface definition. Concrete implementations are found in sensor-specific packages. ### D. ROS Wrapper Package (`nebula_myvendor`) From 589a49f77dd8fcd5ab06125df062c5fb19d43321 Mon Sep 17 00:00:00 2001 From: David Wong Date: Tue, 9 Dec 2025 13:49:50 +0900 Subject: [PATCH 05/65] feat(sample_sensor): remove calibration configuration Signed-off-by: David Wong --- src/nebula_sample/INTEGRATION_GUIDE.md | 14 +++++++------- src/nebula_sample/README.md | 1 - .../include/nebula_sample/sample_ros_wrapper.hpp | 1 - .../nebula_sample/src/sample_ros_wrapper.cpp | 3 +-- .../decoders/sample_decoder.hpp | 6 ++---- .../nebula_sample_decoders/sample_driver.hpp | 5 ++--- .../nebula_sample_decoders/src/sample_driver.cpp | 5 ++--- 7 files changed, 14 insertions(+), 21 deletions(-) diff --git a/src/nebula_sample/INTEGRATION_GUIDE.md b/src/nebula_sample/INTEGRATION_GUIDE.md index f223e5831..ff828da6a 100644 --- a/src/nebula_sample/INTEGRATION_GUIDE.md +++ b/src/nebula_sample/INTEGRATION_GUIDE.md @@ -42,18 +42,18 @@ The Nebula architecture is modular. You will need to implement specific classes - Add fields for sensor-specific settings (e.g., return mode, frequency, IP address). - This struct is passed to both the driver and hardware interface. -- **`MyVendorCalibrationConfiguration`**: - - Define how calibration data (e.g., angle corrections) is stored. - - Implement `load_from_file()` to parse your vendor's calibration file format (csv, dat, xml, etc.) if required. +- **`MyVendorCalibrationConfiguration`** (Optional): + - Only needed if your sensor requires calibration data (e.g., angle corrections). + - Define how calibration data is stored. + - Implement `load_from_file()` to parse your vendor's calibration file format (csv, dat, xml, etc.). + - If using calibration, you'll need to add it as a parameter to your driver and decoder constructors. ### B. Decoders Package (`nebula_myvendor_decoders`) **Purpose**: Handles packet parsing and point cloud generation. -- **`MyVendorScanDecoder`** (Interface): - - - Inherits from `ScanDecoder`. - - Defines the contract for parsing packets. +- Inherits from `ScanDecoder`. +- Defines the contract for parsing packets. - **`MyVendorDecoder`** (Implementation): diff --git a/src/nebula_sample/README.md b/src/nebula_sample/README.md index e5a210ecc..3bf0f1d53 100644 --- a/src/nebula_sample/README.md +++ b/src/nebula_sample/README.md @@ -42,7 +42,6 @@ The guide covers: ### Configuration (`*_common`) - `SampleSensorConfiguration` - Sensor-specific settings -- `SampleCalibrationConfiguration` - Calibration data structure ### Decoder (`*_decoders`) diff --git a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp index 4abe2f75a..1306b0029 100644 --- a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp +++ b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp @@ -46,7 +46,6 @@ class SampleRosWrapper : public rclcpp::Node const drivers::connections::UdpSocket::RxMetadata & metadata); std::shared_ptr sensor_cfg_ptr_; - std::shared_ptr calibration_cfg_ptr_; std::shared_ptr driver_ptr_; std::shared_ptr hw_interface_ptr_; diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index 739db956c..5fc5807b3 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -32,10 +32,9 @@ SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) // Initialize config sensor_cfg_ptr_ = std::make_shared(); - calibration_cfg_ptr_ = std::make_shared(); // Initialize Driver - driver_ptr_ = std::make_shared(sensor_cfg_ptr_, calibration_cfg_ptr_); + driver_ptr_ = std::make_shared(sensor_cfg_ptr_); driver_ptr_->set_pointcloud_callback( [this](const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp index 5d6aacc58..d717df77e 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp @@ -27,13 +27,11 @@ namespace nebula::drivers class SampleDecoder : public SampleScanDecoder { public: - SampleDecoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + explicit SampleDecoder( + const std::shared_ptr & sensor_configuration) { // Initialize (void)sensor_configuration; - (void)calibration_configuration; } PacketDecodeResult unpack(const std::vector & packet) override diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp index 86afb37dc..a52e225e0 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp @@ -28,9 +28,8 @@ namespace nebula::drivers class SampleDriver { public: - SampleDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + explicit SampleDriver( + const std::shared_ptr & sensor_configuration); Status get_status(); diff --git a/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp b/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp index aaa285c67..be98f804c 100644 --- a/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp +++ b/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp @@ -23,11 +23,10 @@ namespace nebula::drivers { SampleDriver::SampleDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration) { driver_status_ = Status::OK; - scan_decoder_ = std::make_shared(sensor_configuration, calibration_configuration); + scan_decoder_ = std::make_shared(sensor_configuration); } Status SampleDriver::get_status() From d61f78774629c67713f17ad6ca5bbbd9fc664c4d Mon Sep 17 00:00:00 2001 From: David Wong Date: Tue, 9 Dec 2025 14:28:01 +0900 Subject: [PATCH 06/65] feat(sample_sensor): package and CMakeLists tidy up Signed-off-by: David Wong --- .../nebula_sample/CMakeLists.txt | 91 ++++++++++--------- src/nebula_sample/nebula_sample/package.xml | 5 +- 2 files changed, 49 insertions(+), 47 deletions(-) diff --git a/src/nebula_sample/nebula_sample/CMakeLists.txt b/src/nebula_sample/nebula_sample/CMakeLists.txt index eed86dfdf..8f7a93e3b 100644 --- a/src/nebula_sample/nebula_sample/CMakeLists.txt +++ b/src/nebula_sample/nebula_sample/CMakeLists.txt @@ -4,58 +4,59 @@ project(nebula_sample) find_package(autoware_cmake REQUIRED) autoware_package() -add_library(nebula_sample_ros_wrapper SHARED src/sample_ros_wrapper.cpp) -target_include_directories(nebula_sample_ros_wrapper PUBLIC - $ - $ -) -ament_target_dependencies(nebula_sample_ros_wrapper - nebula_core_common - nebula_core_ros - nebula_sample_common - nebula_sample_decoders - nebula_sample_hw_interfaces - pcl_conversions - rclcpp - rclcpp_components +if(BUILD_TESTING OR BUILD_EXAMPLES) + find_package(PCL REQUIRED components common io) +else() + find_package(PCL REQUIRED components common) +endif() + +# Sample +add_library( + sample_ros_wrapper SHARED + src/sample_ros_wrapper.cpp) + +target_include_directories( + sample_ros_wrapper + PUBLIC $ + $) + +target_link_libraries( + sample_ros_wrapper + PUBLIC nebula_sample_decoders::nebula_sample_decoders + nebula_sample_hw_interfaces::nebula_sample_hw_interfaces + nebula_core_ros::nebula_core_ros + pcl_common) +ament_target_dependencies( + sample_ros_wrapper + PUBLIC sensor_msgs -) - -rclcpp_components_register_node(nebula_sample_ros_wrapper - PLUGIN "nebula::ros::SampleRosWrapper" - EXECUTABLE nebula_sample_ros_wrapper_node -) - -install(TARGETS nebula_sample_ros_wrapper - EXPORT export_nebula_sample_ros_wrapper - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} -) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_export_targets(export_nebula_sample_ros_wrapper) + pcl_conversions) + +rclcpp_components_register_node(sample_ros_wrapper PLUGIN "nebula::ros::SampleRosWrapper" + EXECUTABLE sample_ros_wrapper_node) + +install( + TARGETS sample_ros_wrapper + EXPORT export_sample_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_sample_ros_wrapper) + ament_export_dependencies( + PCL nebula_core_common - nebula_core_ros nebula_sample_common + nebula_core_decoders nebula_sample_decoders + nebula_core_hw_interfaces nebula_sample_hw_interfaces - rclcpp + nebula_core_ros rclcpp_components sensor_msgs -) + pcl_conversions) ament_package() diff --git a/src/nebula_sample/nebula_sample/package.xml b/src/nebula_sample/nebula_sample/package.xml index 1989fa404..4b667c483 100644 --- a/src/nebula_sample/nebula_sample/package.xml +++ b/src/nebula_sample/nebula_sample/package.xml @@ -2,9 +2,10 @@ nebula_sample - 0.0.1 + 0.3.0 Nebula Sample ROS 2 Package - MAP IV + David Wong + Max Schmeller Apache 2 TIER IV From ba6e0cfc8601faed7b8b89a7d2d7cadfe8d18d5d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 9 Dec 2025 05:30:44 +0000 Subject: [PATCH 07/65] ci(pre-commit): autofix --- src/nebula_sample/INTEGRATION_GUIDE.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/nebula_sample/INTEGRATION_GUIDE.md b/src/nebula_sample/INTEGRATION_GUIDE.md index ff828da6a..e5026330d 100644 --- a/src/nebula_sample/INTEGRATION_GUIDE.md +++ b/src/nebula_sample/INTEGRATION_GUIDE.md @@ -38,7 +38,6 @@ The Nebula architecture is modular. You will need to implement specific classes **Purpose**: Defines data structures for configuration and calibration. - **`MyVendorSensorConfiguration`**: - - Add fields for sensor-specific settings (e.g., return mode, frequency, IP address). - This struct is passed to both the driver and hardware interface. @@ -56,7 +55,6 @@ The Nebula architecture is modular. You will need to implement specific classes - Defines the contract for parsing packets. - **`MyVendorDecoder`** (Implementation): - - **`unpack(packet)`**: The core method. It takes raw bytes (UDP packet) and returns a `PacketDecodeResult` containing the decoded points or an error. - **`get_pointcloud()`**: (Optional) If you buffer points, this returns the constructed point cloud. - **Key Task**: Parse the raw byte stream according to your sensor's user manual. From 73d76844f0231ee9d37f301614e0f7b01e396515 Mon Sep 17 00:00:00 2001 From: David Wong Date: Tue, 9 Dec 2025 15:16:34 +0900 Subject: [PATCH 08/65] docs(nebula_sample): document implementation requirements and add vendor-neutral guidance Signed-off-by: David Wong --- src/nebula_sample/README.md | 2 +- .../nebula_sample/sample_ros_wrapper.hpp | 36 ++++++++++++--- .../nebula_sample/src/sample_ros_wrapper.cpp | 29 +++++++++--- .../nebula_sample_common/sample_common.hpp | 35 +++++++++++++-- .../decoders/sample_decoder.hpp | 40 +++++++++++++++-- .../decoders/sample_scan_decoder.hpp | 41 +++++++++++++---- .../nebula_sample_decoders/sample_driver.hpp | 26 ++++++++++- .../src/sample_driver.cpp | 7 +++ .../sample_hw_interface.hpp | 44 ++++++++++++++++++- .../src/sample_hw_interface.cpp | 26 +++++++++++ 10 files changed, 256 insertions(+), 30 deletions(-) diff --git a/src/nebula_sample/README.md b/src/nebula_sample/README.md index 3bf0f1d53..28eee641d 100644 --- a/src/nebula_sample/README.md +++ b/src/nebula_sample/README.md @@ -60,4 +60,4 @@ The guide covers: ## Reference Implementation -This package is based on the Hesai implementation structure but with all vendor-specific code removed. For a complete example, refer to `nebula_hesai`. +This package provides a template structure for adding new sensor support. For complete examples, refer to existing sensor packages like `nebula_hesai` or `nebula_velodyne`. diff --git a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp index 1306b0029..83cd61320 100644 --- a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp +++ b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp @@ -31,28 +31,54 @@ namespace nebula::ros { +/// @brief ROS 2 wrapper for the Sample LiDAR driver +/// @details This node bridges the C++ driver with ROS 2. +/// Responsibilities: +/// - Declare and read ROS parameters for sensor configuration +/// - Initialize the driver and hardware interface +/// - Receive packets from HW interface and pass to driver +/// - Convert decoded point clouds to ROS messages +/// - Publish point clouds on ROS topics +/// - Optionally: provide services for runtime configuration class SampleRosWrapper : public rclcpp::Node { public: + /// @brief Constructor + /// @param options ROS node options + /// @details Initializes the driver, HW interface, and ROS publishers explicit SampleRosWrapper(const rclcpp::NodeOptions & options); + + /// @brief Destructor + /// @details Stops the hardware interface cleanly ~SampleRosWrapper() override; + /// @brief Get the current driver status + /// @return Status indicating if the driver is operational Status get_status(); + + /// @brief Start the sensor data stream + /// @return Status::OK on success, error status otherwise Status stream_start(); private: + /// @brief Callback for incoming UDP packets + /// @param packet Raw packet data from the sensor + /// @param metadata Packet metadata (timestamp, source IP, etc.) + /// @details This is called by the HW interface when a packet arrives. + /// Passes the packet to the driver for decoding. void receive_cloud_packet_callback( const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata); - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; ///< Sensor config - std::shared_ptr driver_ptr_; - std::shared_ptr hw_interface_ptr_; + std::shared_ptr driver_ptr_; ///< Driver instance + std::shared_ptr hw_interface_ptr_; ///< HW interface instance - rclcpp::Publisher::SharedPtr points_pub_; + rclcpp::Publisher::SharedPtr + points_pub_; ///< Point cloud publisher - bool launch_hw_; + bool launch_hw_; ///< Whether to launch hardware interface (false for offline playback) }; } // namespace nebula::ros diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index 5fc5807b3..b43a8bc44 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -26,20 +26,31 @@ namespace nebula::ros SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) : Node("nebula_sample_ros_wrapper", options) { - // Parameter declaration + // ========== ROS Parameter Declaration ========== + // Implementation Items: Add more parameters for sensor configuration (IP, port, return mode, + // etc.) declare_parameter("launch_hw", true); launch_hw_ = get_parameter("launch_hw").as_bool(); - // Initialize config + // ========== Initialize Sensor Configuration ========== + // Implementation Items: Read ROS parameters and populate sensor_cfg_ptr_ fields + // Example: + // sensor_cfg_ptr_->sensor_ip = get_parameter("sensor_ip").as_string(); + // sensor_cfg_ptr_->host_ip = get_parameter("host_ip").as_string(); + // sensor_cfg_ptr_->data_port = get_parameter("data_port").as_int(); sensor_cfg_ptr_ = std::make_shared(); - // Initialize Driver + // ========== Initialize Driver ========== driver_ptr_ = std::make_shared(sensor_cfg_ptr_); + // Register callback to receive decoded point clouds from the driver driver_ptr_->set_pointcloud_callback( [this](const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { + // This callback is called when the decoder completes a full scan (void)timestamp_s; + // Only publish if there are subscribers and the pointcloud is valid if (points_pub_->get_subscription_count() > 0 && pointcloud) { + // Convert PCL point cloud to ROS message auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = this->now(); @@ -47,17 +58,23 @@ SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) } }); - // Initialize HW Interface + // ========== Initialize Hardware Interface ========== + // Only create HW interface if launch_hw is true (false for offline bag playback) if (launch_hw_) { hw_interface_ptr_ = std::make_shared(); hw_interface_ptr_->set_sensor_configuration(sensor_cfg_ptr_); + + // Register callback to receive raw packets from the HW interface hw_interface_ptr_->register_scan_callback( std::bind( &SampleRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1, std::placeholders::_2)); + + // Start receiving packets stream_start(); } + // ========== Create ROS Publishers ========== points_pub_ = create_publisher("points_raw", 10); } @@ -84,7 +101,9 @@ Status SampleRosWrapper::stream_start() void SampleRosWrapper::receive_cloud_packet_callback( const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata) { - (void)metadata; + // This callback is called by the HW interface when a UDP packet arrives + // Pass the packet to the driver for decoding + (void)metadata; // Metadata (timestamp, source IP) not used in this simple example driver_ptr_->parse_cloud_packet(packet); } diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp index 89ed0e536..c1d298d23 100644 --- a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp @@ -27,9 +27,18 @@ namespace nebula namespace drivers { +/// @brief Sensor-specific configuration for the Sample LiDAR +/// @details This struct extends LidarConfigurationBase with any sensor-specific settings. +/// When implementing for a real sensor, add fields for: +/// - Return mode (single, dual, strongest, last, etc.) +/// - Rotation speed / scan frequency +/// - IP addresses (sensor, host) +/// - Port numbers +/// - FOV settings +/// - Any vendor-specific parameters struct SampleSensorConfiguration : public LidarConfigurationBase { - // Add any dummy specific configuration here if needed + // Example field - replace with actual sensor parameters std::string sample_field; }; @@ -41,17 +50,37 @@ inline std::ostream & operator<<(std::ostream & os, SampleSensorConfiguration co return os; } +/// @brief Calibration data for the Sample LiDAR (optional) +/// @details This struct is only needed if your sensor requires calibration data. +/// Common calibration data includes: +/// - Vertical/horizontal angle corrections per laser +/// - Distance corrections +/// - Intensity corrections +/// - Timing offsets +/// If your sensor doesn't need calibration, you can remove this struct entirely. struct SampleCalibrationConfiguration : public CalibrationConfigurationBase { + /// @brief Load calibration data from a file + /// @param calibration_file Path to the calibration file + /// @return Status::OK on success, error status otherwise + /// @details Implement parsing logic for your sensor's calibration file format (CSV, XML, binary, + /// etc.) nebula::Status load_from_file(const std::string & calibration_file) { - // Sample implementation + // Implementation Items: Implement calibration file parsing + // Example: Parse CSV/XML/binary file containing angle corrections + (void)calibration_file; return Status::OK; } + /// @brief Save calibration data to a file + /// @param calibration_file Path to save the calibration file + /// @return Status::OK on success, error status otherwise + /// @details Implement serialization logic for your sensor's calibration format nebula::Status save_to_file(const std::string & calibration_file) { - // Sample implementation + // Implementation Items: Implement calibration file writing + (void)calibration_file; return Status::OK; } }; diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp index d717df77e..e977825d6 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp @@ -24,30 +24,64 @@ namespace nebula::drivers { +/// @brief Concrete implementation of the Sample packet decoder +/// @details This class implements the SampleScanDecoder interface. +/// Implement the following in this class: +/// - Parse raw UDP packets according to your sensor's protocol specification +/// - Extract point data (x, y, z, intensity, timestamp, etc.) +/// - Accumulate points until a full scan is complete +/// - Call the pointcloud callback when a scan is ready class SampleDecoder : public SampleScanDecoder { public: + /// @brief Constructor + /// @param sensor_configuration Sensor settings (IP, ports, return mode, etc.) + /// @details Initialize any internal state needed for decoding (e.g., point accumulation buffers) explicit SampleDecoder( const std::shared_ptr & sensor_configuration) { - // Initialize + // Implementation Items: Initialize decoder state + // - Allocate point cloud buffer + // - Set up any lookup tables based on sensor configuration + // - Initialize scan tracking variables (void)sensor_configuration; } + /// @brief Decode a single UDP packet + /// @param packet Raw packet bytes from the sensor + /// @return PacketDecodeResult with metadata or error + /// @details Implement your sensor's packet parsing logic here: + /// 1. Validate packet size and checksum + /// 2. Parse packet header (timestamp, azimuth, etc.) + /// 3. Extract point data blocks + /// 4. Convert raw data to 3D points (apply calibration if needed) + /// 5. Accumulate points in the current scan + /// 6. Detect scan completion + /// 7. If scan complete, call pointcloud_callback_ and set did_scan_complete=true PacketDecodeResult unpack(const std::vector & packet) override { - // Sample implementation - does nothing, just returns an error + // Implementation Items: Implement packet decoding + // This is a stub that does nothing - replace with actual parsing logic (void)packet; return {{}, DecodeError::PACKET_PARSE_FAILED}; } + /// @brief Register callback for complete point clouds + /// @param callback Function to call when a scan is complete void set_pointcloud_callback(pointcloud_callback_t callback) override { pointcloud_callback_ = callback; } private: - pointcloud_callback_t pointcloud_callback_; + pointcloud_callback_t pointcloud_callback_; ///< Callback for publishing scans + // Implementation Items: Add member variables for: + // - Point cloud accumulation buffer + // - Previous azimuth for scan completion detection + // - Scan timestamp + // - Optionally: Double-buffering (two point cloud buffers) for handling scan overlap + // (advanced optimization for sensors with configurable FOV) + // - Any sensor-specific state }; } // namespace nebula::drivers diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp index e605f972e..df91b0117 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp @@ -25,33 +25,47 @@ namespace nebula::drivers { +/// @brief Error codes returned during packet decoding enum class DecodeError : uint8_t { - PACKET_PARSE_FAILED, - DRIVER_NOT_OK, - INVALID_PACKET_SIZE, + PACKET_PARSE_FAILED, ///< Failed to parse packet data (invalid format, checksum error, etc.) + DRIVER_NOT_OK, ///< Driver is not in a valid state to decode packets + INVALID_PACKET_SIZE, ///< Packet size doesn't match expected size for this sensor }; +/// @brief Metadata extracted from a decoded packet struct PacketMetadata { - uint64_t packet_timestamp_ns{}; - bool did_scan_complete{false}; + 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}; - uint64_t callback_time_ns{0}; + 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 { - PerformanceCounters performance_counters; - util::expected metadata_or_error; + PerformanceCounters performance_counters; ///< Timing information + util::expected metadata_or_error; ///< Decode result or error }; +/// @brief Base interface for packet decoders +/// @details Implement this interface to decode raw UDP packets into point clouds. +/// The decoder is responsible for: +/// - Parsing binary packet data according to the sensor's protocol +/// - Accumulating points until a full scan is complete +/// - Calling the pointcloud callback when a scan is ready class SampleScanDecoder { 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; @@ -63,8 +77,17 @@ class SampleScanDecoder virtual ~SampleScanDecoder() = default; SampleScanDecoder() = default; + /// @brief Decode a single UDP packet + /// @param packet Raw packet data received from the sensor + /// @return PacketDecodeResult containing metadata or error, plus performance counters + /// @details This is the main decoding function. Implement sensor-specific parsing logic here. + /// Parse the packet, extract points, and accumulate them. When a full scan is complete, + /// call the pointcloud callback and set did_scan_complete = true. virtual PacketDecodeResult unpack(const std::vector & packet) = 0; + /// @brief Register a callback to receive complete point clouds + /// @param callback Function to call when a full scan is decoded + /// @details The decoder calls this callback when a complete scan is ready virtual void set_pointcloud_callback(pointcloud_callback_t callback) = 0; }; } // namespace nebula::drivers diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp index a52e225e0..25cf88427 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp @@ -25,21 +25,43 @@ namespace nebula::drivers { +/// @brief Main driver class for the Sample LiDAR +/// @details This is the high-level interface for the sensor driver. +/// It manages the decoder and provides the main API for: +/// - Initializing the driver with sensor configuration +/// - Processing incoming packets +/// - Registering callbacks for point clouds +/// - Querying driver status class SampleDriver { public: + /// @brief Constructor + /// @param sensor_configuration Sensor settings + /// @details Initializes the decoder with the given configuration explicit SampleDriver( const std::shared_ptr & sensor_configuration); + /// @brief Get the current driver status + /// @return Status indicating if the driver is ready to process packets + /// @details Returns OK if initialized properly, error status otherwise Status get_status(); + /// @brief Process a single cloud packet + /// @param packet Raw UDP packet data + /// @return PacketDecodeResult with metadata or error + /// @details This is the main entry point for packet processing. + /// Called by the HW interface when a packet is received. + /// Delegates to the decoder's unpack() method. PacketDecodeResult parse_cloud_packet(const std::vector & packet); + /// @brief Register callback for complete point clouds + /// @param pointcloud_cb Function to call when a scan is complete + /// @details The callback receives the decoded point cloud and timestamp void set_pointcloud_callback(SampleScanDecoder::pointcloud_callback_t pointcloud_cb); private: - std::shared_ptr scan_decoder_; - Status driver_status_; + std::shared_ptr scan_decoder_; ///< Decoder instance + Status driver_status_; ///< Current driver status }; } // namespace nebula::drivers diff --git a/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp b/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp index be98f804c..a39c46c9b 100644 --- a/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp +++ b/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp @@ -25,22 +25,29 @@ namespace nebula::drivers SampleDriver::SampleDriver( const std::shared_ptr & sensor_configuration) { + // Initialize driver status driver_status_ = Status::OK; + + // Create decoder instance with sensor configuration scan_decoder_ = std::make_shared(sensor_configuration); } Status SampleDriver::get_status() { + // Return current driver status return driver_status_; } PacketDecodeResult SampleDriver::parse_cloud_packet(const std::vector & packet) { + // Delegate packet parsing to the decoder + // The decoder will accumulate points and call the pointcloud callback when ready return scan_decoder_->unpack(packet); } void SampleDriver::set_pointcloud_callback(SampleScanDecoder::pointcloud_callback_t pointcloud_cb) { + // Forward the callback to the decoder scan_decoder_->set_pointcloud_callback(pointcloud_cb); } diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp index e5f0c706d..c3b0ffec4 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp @@ -25,22 +25,62 @@ namespace nebula::drivers { +/// @brief Hardware interface for Sample LiDAR communication +/// @details This class manages network communication with the sensor. +/// Responsibilities: +/// - Setting up UDP sockets for receiving data packets +/// - Starting/stopping packet reception +/// - Calling registered callbacks when packets arrive +/// - Optionally: sending commands to the sensor (start/stop scan, change settings, etc.) +/// +/// @note Advanced implementations may also include: +/// - TCP connections for sensor configuration commands +/// - HTTP API support for newer sensor models +/// - These are optional and can be added as needed for your specific sensor class SampleHwInterface { public: SampleHwInterface(); + /// @brief Start receiving packets from the sensor + /// @return Status::OK on success, error status otherwise + /// @details Implement the following: + /// 1. Create UDP socket(s) for data reception + /// 2. Bind to the configured port(s) + /// 3. Start async receive loop + /// 4. Optionally: send start command to sensor Status sensor_interface_start(); + + /// @brief Stop receiving packets from the sensor + /// @return Status::OK on success, error status otherwise + /// @details Implement the following: + /// 1. Stop the receive loop + /// 2. Close UDP socket(s) + /// 3. Optionally: send stop command to sensor Status sensor_interface_stop(); + /// @brief Set the sensor configuration + /// @param sensor_configuration Configuration containing IP addresses, ports, etc. + /// @return Status::OK on success, error status otherwise + /// @details Store the configuration for use when starting the interface Status set_sensor_configuration( std::shared_ptr sensor_configuration); + /// @brief Register callback for incoming packets + /// @param scan_callback Function to call when a packet is received + /// @return Status::OK on success, error status otherwise + /// @details The callback receives raw packet data and metadata (timestamp, source IP, etc.) Status register_scan_callback(connections::UdpSocket::callback_t scan_callback); private: - std::shared_ptr sensor_configuration_; - connections::UdpSocket::callback_t cloud_packet_callback_; + std::shared_ptr sensor_configuration_; ///< Sensor config + connections::UdpSocket::callback_t cloud_packet_callback_; ///< Packet callback + // Implementation Items: Add member variables for: + // - UDP socket instance(s) for data reception + // - IO context for async operations + // - Optionally: TCP driver for sensor configuration commands + // - Optionally: HTTP client for newer sensor models + // - Any sensor-specific state }; } // namespace nebula::drivers diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp index 408d4f6b8..030cf178d 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp @@ -21,27 +21,53 @@ namespace nebula::drivers SampleHwInterface::SampleHwInterface() { + // Constructor - initialize any member variables if needed } Status SampleHwInterface::sensor_interface_start() { + // Implementation Items: Implement sensor interface startup + // 1. Create UDP socket using connections::UdpSocket + // 2. Bind to the port specified in sensor_configuration_ + // 3. Start async receive loop + // 4. When packets arrive, call cloud_packet_callback_ with the packet data + // 5. Optionally: send HTTP/TCP command to sensor to start scanning + + // Example (pseudo-code): + // udp_socket_ = std::make_shared(); + // udp_socket_->open(); + // udp_socket_->bind(sensor_configuration_->host_ip, sensor_configuration_->data_port); + // udp_socket_->asyncReceive(cloud_packet_callback_); + return Status::OK; } Status SampleHwInterface::sensor_interface_stop() { + // Implementation Items: Implement sensor interface shutdown + // 1. Stop the receive loop + // 2. Close UDP socket(s) + // 3. Optionally: send command to sensor to stop scanning + + // Example (pseudo-code): + // if (udp_socket_) { + // udp_socket_->close(); + // } + return Status::OK; } Status SampleHwInterface::set_sensor_configuration( std::shared_ptr sensor_configuration) { + // Store the sensor configuration for later use sensor_configuration_ = sensor_configuration; return Status::OK; } Status SampleHwInterface::register_scan_callback(connections::UdpSocket::callback_t scan_callback) { + // Store the callback to be called when packets arrive cloud_packet_callback_ = scan_callback; return Status::OK; } From b84d377246713efe86b527db58b5630975ceab1e Mon Sep 17 00:00:00 2001 From: David Wong Date: Tue, 9 Dec 2025 20:55:22 +0900 Subject: [PATCH 09/65] docs(nebula_sample): add integration guide to mkdocs Signed-off-by: David Wong --- docs/integration_guide.md | 800 ++++++++++++++++++ docs/tutorials.md | 2 +- mkdocs.yml | 14 +- src/nebula_sample/README.md | 2 + .../config/sample_sensor.param.yaml | 27 + 5 files changed, 842 insertions(+), 3 deletions(-) create mode 100644 docs/integration_guide.md create mode 100644 src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml diff --git a/docs/integration_guide.md b/docs/integration_guide.md new file mode 100644 index 000000000..9d77cbce0 --- /dev/null +++ b/docs/integration_guide.md @@ -0,0 +1,800 @@ +# New Sensor Integration Guide + +This guide provides instructions for adding support for a new LiDAR sensor to Nebula using the `nebula_sample` package as a template. + +## Table of Contents + +1. [Architecture Overview](#architecture-overview) +2. [Provided Components](#provided-components) +3. [Integration Workflow](#integration-workflow) +4. [Implementation Details](#implementation-details) +5. [Required Behaviors](#required-behaviors) +6. [Verification](#verification) + +--- + +## Architecture Overview + +### Overall Package Structure + +Nebula is organized into common (reusable) packages and vendor-specific packages: + +```mermaid +graph TB + %% --- STYLE DEFINITIONS --- + %% Blue theme for Common + classDef common fill:#e3f2fd,stroke:#1565c0,stroke-width:2px,rx:5,ry:5,color:#0d47a1; + + %% Teal/Green theme for Vendors + classDef vendor fill:#e0f2f1,stroke:#00695c,stroke-width:2px,rx:5,ry:5,color:#004d40; + + %% --- GRAPH STRUCTURE --- + subgraph Nebula["Nebula Framework"] + direction TB + + subgraph Common["Common Packages"] + direction LR + C1["nebula_core_common
Base types, status codes"]:::common + C2["nebula_core_decoders
Decoder interfaces"]:::common + C3["nebula_core_hw_interfaces
UDP/TCP sockets"]:::common + C4["nebula_core_ros
ROS 2 utils"]:::common + end + + subgraph Vendors["Vendor Packages"] + direction LR + Hesai["nebula_hesai
hesai_common
hesai_decoders
hesai_hw_interfaces
ROS wrapper"]:::vendor + + Velodyne["nebula_velodyne
velodyne_common
velodyne_decoders
velodyne_hw_interfaces
ROS wrapper"]:::vendor + + Sample["nebula_sample
sample_common
sample_decoders
sample_hw_interfaces
ROS wrapper"]:::vendor + end + end + + %% --- SUBGRAPH STYLING --- + %% Style the containers to have dashed borders and light backgrounds + style Nebula fill:#ffffff,stroke:#333,stroke-width:3px + style Common fill:#f5faff,stroke:#2196f3,stroke-dasharray: 5 5 + style Vendors fill:#f0fdfa,stroke:#009688,stroke-dasharray: 5 5 +``` + +**Key Principles**: + +- **Common packages** provide reusable functionality (UDP sockets, point types, etc.) +- **Vendor packages** implement vendor-specific logic (packet parsing, calibration) +- All vendor packages follow the **same 4-layer structure** for consistency +- Vendor packages **depend on** common packages but not on each other + +### Layered Architecture (Per Vendor) + +Each vendor package uses a layered architecture to separate functional blocks and promote code reuse: + +```mermaid +graph TD + %% --- STYLE DEFINITIONS --- + %% Using a hierarchical palette to show data flow depth + classDef wrapper fill:#e3f2fd,stroke:#1565c0,stroke-width:2px,rx:5,ry:5,color:#0d47a1; + classDef logic fill:#f3e5f5,stroke:#7b1fa2,stroke-width:2px,rx:5,ry:5,color:#4a148c; + classDef worker fill:#e0f2f1,stroke:#00695c,stroke-width:2px,rx:5,ry:5,color:#004d40; + + %% --- NODES --- + %% Apply classes using the triple colon syntax (:::className) + Wrapper[ROS 2 Wrapper Layer
Parameter handling, ROS message conversion, publishing]:::wrapper + + Driver[Driver Layer
High-level API, manages decoder and HW interface]:::logic + + Decoder[Decoder Layer
Packet parsing
point cloud generation]:::worker + + HW[HW Interface
UDP/TCP communication
socket management]:::worker + + %% --- RELATIONSHIPS --- + Wrapper --> Driver + Driver --> Decoder + Driver --> HW +``` + +### Data Flow + +1. **Hardware Interface** receives raw UDP packets from the sensor and defines TCP protocols for communication and configuration +2. **Driver** receives packets and delegates to **Decoder** +3. **Decoder** parses packets, accumulates points, detects scan completion, and calls callback with complete point cloud +4. **ROS Wrapper** converts to ROS message and publishes + +--- + +## Provided Components + +Nebula provides reusable components to simplify sensor integration. You should use these instead of implementing from scratch. + +### 1. UDP Socket Handling + +**Location**: `nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp` + +**What it provides**: + +- Asynchronous UDP socket +- Automatic packet reception loop +- Callback-based packet delivery +- Metadata (timestamp, source IP) for each packet + +**Usage**: + +```cpp +#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" + +// In your HW interface: +connections::UdpSocket udp_socket_; + +// Start receiving: +udp_socket_.open(host_ip, port); +udp_socket_.bind(); +udp_socket_.asyncReceive(callback); +``` + +### 2. Status Codes + +**Location**: `nebula_core_common/include/nebula_core_common/nebula_status.hpp` + +**What it provides**: + +- Standardized error/success codes +- `Status` enum with values like `OK`, `INVALID_CALIBRATION_FILE`, `SENSOR_CONFIG_ERROR`, etc. + +**Usage**: + +```cpp +#include "nebula_core_common/nebula_status.hpp" + +nebula::Status validate_config() { + if (!config_valid) { + return Status::SENSOR_CONFIG_ERROR; + } + return Status::OK; +} +``` + +### 3. Point Cloud Types + +**Location**: `nebula_core_common/include/nebula_core_common/point_types.hpp` + +**What it provides**: + +- `NebulaPoint` - Standard point type with x, y, z, intensity, timestamp, return_type, channel, azimuth, elevation, distance +- `NebulaPointCloud` - PCL point cloud of NebulaPoints +- Conversion utilities to ROS/Autoware point types + +**Usage**: + +```cpp +#include "nebula_core_common/point_types.hpp" + +NebulaPointCloudPtr cloud = std::make_shared(); +NebulaPoint point; +point.x = distance * sin_azimuth * cos_elevation; +point.y = distance * cos_azimuth * cos_elevation; +point.z = distance * sin_elevation; +cloud->push_back(point); +``` + +### 4. Angle Utilities + +**Location**: `nebula_core_decoders/include/nebula_core_decoders/angles.hpp` + +**What it provides**: + +- `deg2rad()`, `rad2deg()` - Angle conversions +- `angle_is_between()` - Check if angle is within FOV +- Angle normalization functions + +**Usage**: + +```cpp +#include "nebula_core_decoders/angles.hpp" + +float azimuth_rad = deg2rad(azimuth_deg); +bool in_fov = angle_is_between(fov_min, fov_max, azimuth_rad); +``` + +### 5. Configuration Base Classes + +**Location**: `nebula_core_common/include/nebula_core_common/nebula_common.hpp` + +**What it provides**: + +- `LidarConfigurationBase` - Base for sensor configuration (frame_id, sensor_model, return_mode, etc.) +- `CalibrationConfigurationBase` - Base for calibration data + +**Usage**: + +```cpp +struct MySensorConfiguration : public LidarConfigurationBase { + std::string sensor_ip; + uint16_t data_port; + // ... sensor-specific fields +}; +``` + +### 6. Logger Integration + +**Location**: `nebula_core_common/include/nebula_core_common/loggers/logger.hpp` + +**What it provides**: + +- Unified logging interface +- ROS 2 logger wrapper (`RclcppLogger`) +- Macros: `NEBULA_LOG_STREAM(logger->info, "message")` + +**Usage**: + +```cpp +#include "nebula_core_common/loggers/logger.hpp" + +std::shared_ptr logger_; +NEBULA_LOG_STREAM(logger_->error, "Failed to parse packet"); +``` + +### 7. Diagnostic Integration + +**Location**: ROS 2 `diagnostic_updater` package (used in ROS wrapper) + +**What it provides**: + +- Automatic diagnostic publishing +- Status monitoring (OK, WARN, ERROR) +- Rate monitoring for scan frequency + +**Usage**: + +```cpp +#include + +diagnostic_updater::Updater diagnostic_updater_; +// Add diagnostics in your ROS wrapper +``` + +--- + +## Integration Workflow + +### Step 1: Clone the Template + +Copy the `nebula_sample` directory: + +```bash +cd src +cp -r nebula_sample nebula_myvendor +``` + +### Step 2: Rename Components + +Rename all occurrences of "sample"/"Sample" to your vendor name: + +- **Directories**: `nebula_sample_*` → `nebula_myvendor_*` +- **Files**: `sample_*.{cpp,hpp}` → `myvendor_*.{cpp,hpp}` +- **Classes**: `SampleDriver` → `MyVendorDriver` +- **Namespaces**: Update in all files +- **CMake/Package**: Update `CMakeLists.txt` and `package.xml` + +**Tip**: Use find-and-replace tools: + +```bash +find nebula_myvendor -type f -exec sed -i 's/sample/myvendor/g' {} + +find nebula_myvendor -type f -exec sed -i 's/Sample/MyVendor/g' {} + +``` + +### Step 3: Implement Components + +See [Implementation Details](#implementation-details) below. + +### Step 4: Verify + +See [Verification](#verification) below. + +--- + +## Implementation Details + +### A. Common Package (`nebula_myvendor_common`) + +**Purpose**: Define configuration and calibration structures. + +#### 1. Sensor Configuration + +Edit `include/nebula_myvendor_common/myvendor_common.hpp`: + +```cpp +struct MyVendorSensorConfiguration : public LidarConfigurationBase { + // Network settings + std::string sensor_ip{"192.168.1.201"}; + std::string host_ip{"192.168.1.100"}; + uint16_t data_port{2368}; + + // Sensor settings + uint16_t rotation_speed{600}; // RPM + uint16_t cloud_min_angle{0}; + uint16_t cloud_max_angle{360}; + + // Add your sensor-specific fields +}; +``` + +#### 2. Calibration Configuration (Optional) + +Only implement if your sensor needs calibration data: + +```cpp +struct MyVendorCalibrationConfiguration : public CalibrationConfigurationBase { + std::vector elevation_angles; + std::vector azimuth_offsets; + + Status load_from_file(const std::string & calibration_file) override { + // Parse your calibration file format (CSV, XML, binary, etc.) + // Populate elevation_angles and azimuth_offsets + return Status::OK; + } +}; +``` + +### B. Decoders Package (`nebula_myvendor_decoders`) + +**Purpose**: Parse packets and generate point clouds. + +#### 1. Decoder Interface + +The interface is already defined in `myvendor_scan_decoder.hpp`. You don't need to modify it. + +#### 2. Decoder Implementation + +Edit `include/nebula_myvendor_decoders/decoders/myvendor_decoder.hpp`: + +```cpp +class MyVendorDecoder : public MyVendorScanDecoder { +public: + explicit MyVendorDecoder( + const std::shared_ptr & config) + : config_(config) { + current_cloud_ = std::make_shared(); + current_cloud_->reserve(100000); // Pre-allocate + } + + PacketDecodeResult unpack(const std::vector & packet) override { + // 1. Validate packet size + if (packet.size() != EXPECTED_PACKET_SIZE) { + return {% raw %}{{}, DecodeError::INVALID_PACKET_SIZE}{% endraw %}; + } + + // 2. Parse packet header + uint64_t timestamp_ns = parse_timestamp(packet); + uint16_t azimuth = parse_azimuth(packet); + + // 3. Extract and convert points + for (int block = 0; block < NUM_BLOCKS; ++block) { + for (int channel = 0; channel < NUM_CHANNELS; ++channel) { + uint16_t distance_raw = parse_distance(packet, block, channel); + uint8_t intensity = parse_intensity(packet, block, channel); + + // Convert to 3D point + NebulaPoint point = convert_to_point( + distance_raw, intensity, azimuth, channel, timestamp_ns); + current_cloud_->push_back(point); + } + } + + // 4. Detect scan completion (azimuth wrap) + bool scan_complete = (azimuth < last_azimuth_); + last_azimuth_ = azimuth; + + // 5. If scan complete, call callback + if (scan_complete && pointcloud_callback_) { + pointcloud_callback_(current_cloud_, timestamp_ns * 1e-9); + current_cloud_ = std::make_shared(); + current_cloud_->reserve(100000); + } + + // 6. Return result + PacketMetadata metadata; + metadata.packet_timestamp_ns = timestamp_ns; + metadata.did_scan_complete = scan_complete; + return {% raw %}{{}, metadata}{% endraw %}; + } + +private: + std::shared_ptr config_; + NebulaPointCloudPtr current_cloud_; + uint16_t last_azimuth_{0}; + pointcloud_callback_t pointcloud_callback_; +}; +``` + +#### 3. Driver Implementation + +The driver is a thin wrapper. Edit `src/myvendor_driver.cpp`: + +```cpp +MyVendorDriver::MyVendorDriver( + const std::shared_ptr & config) +: config_(config) { + scan_decoder_ = std::make_shared(config); + driver_status_ = Status::OK; +} + +PacketDecodeResult MyVendorDriver::parse_cloud_packet( + const std::vector & packet) { + return scan_decoder_->unpack(packet); +} +``` + +### C. Hardware Interfaces Package (`nebula_myvendor_hw_interfaces`) + +**Purpose**: Manage UDP communication with the sensor. + +Edit `src/myvendor_hw_interface.cpp`: + +```cpp +Status MyVendorHwInterface::sensor_interface_start() { + if (!sensor_configuration_) { + return Status::SENSOR_CONFIG_ERROR; + } + + // Create UDP socket using Nebula's UdpSocket + udp_socket_ = std::make_unique(); + + // Open and bind + udp_socket_->open( + sensor_configuration_->host_ip, + sensor_configuration_->data_port); + udp_socket_->bind(); + + // Start async receive with callback + udp_socket_->asyncReceive( + [this](const std::vector & buffer, + const connections::UdpSocket::RxMetadata & metadata) { + if (cloud_packet_callback_) { + cloud_packet_callback_(buffer, metadata); + } + }); + + return Status::OK; +} + +Status MyVendorHwInterface::sensor_interface_stop() { + if (udp_socket_) { + udp_socket_->close(); + udp_socket_.reset(); + } + return Status::OK; +} +``` + +### D. ROS Wrapper Package (`nebula_myvendor`) + +**Purpose**: Bridge C++ driver with ROS 2. + +Edit `src/myvendor_ros_wrapper.cpp`: + +```cpp +MyVendorRosWrapper::MyVendorRosWrapper(const rclcpp::NodeOptions & options) +: Node("nebula_myvendor_ros_wrapper", options) { + + // 1. Declare ROS parameters + declare_parameter("sensor_ip", "192.168.1.201"); + declare_parameter("host_ip", "192.168.1.100"); + declare_parameter("data_port", 2368); + declare_parameter("frame_id", "myvendor_lidar"); + declare_parameter("launch_hw", true); + + // 2. Create sensor configuration + sensor_cfg_ptr_ = std::make_shared(); + sensor_cfg_ptr_->sensor_ip = get_parameter("sensor_ip").as_string(); + sensor_cfg_ptr_->host_ip = get_parameter("host_ip").as_string(); + sensor_cfg_ptr_->data_port = get_parameter("data_port").as_int(); + sensor_cfg_ptr_->frame_id = get_parameter("frame_id").as_string(); + launch_hw_ = get_parameter("launch_hw").as_bool(); + + // 3. Initialize driver + driver_ptr_ = std::make_shared(sensor_cfg_ptr_); + + // 4. Register pointcloud callback + driver_ptr_->set_pointcloud_callback( + [this](const NebulaPointCloudPtr & cloud, double timestamp_s) { + auto ros_msg = std::make_unique(); + pcl::toROSMsg(*cloud, *ros_msg); + ros_msg->header.stamp = rclcpp::Time(timestamp_s * 1e9); + ros_msg->header.frame_id = sensor_cfg_ptr_->frame_id; + points_pub_->publish(std::move(ros_msg)); + }); + + // 5. Initialize HW interface + if (launch_hw_) { + hw_interface_ptr_ = std::make_shared(); + hw_interface_ptr_->set_sensor_configuration(sensor_cfg_ptr_); + hw_interface_ptr_->register_scan_callback( + [this](const std::vector & packet, const auto & metadata) { + (void)metadata; + driver_ptr_->parse_cloud_packet(packet); + }); + } + + // 6. Create publisher + points_pub_ = create_publisher( + "points", rclcpp::SensorDataQoS()); +} + +Status MyVendorRosWrapper::stream_start() { + if (hw_interface_ptr_) { + return hw_interface_ptr_->sensor_interface_start(); + } + return Status::OK; +} +``` + +--- + +## Required Behaviors + +Your sensor integration must implement these behaviors correctly. + +### 1. Startup Sequence + +**Order of operations**: + +1. **Parameter Loading**: Declare and read all ROS parameters +2. **Configuration Validation**: Validate IP addresses, ports, ranges +3. **Driver Initialization**: Create driver with validated config +4. **Callback Registration**: Register pointcloud callback +5. **HW Interface Initialization**: Create and configure HW interface +6. **Publisher Creation**: Create ROS publishers +7. **Stream Start**: Call `sensor_interface_start()` to begin receiving data + +**Error Handling**: + +- If any step fails, log error and throw exception +- Do not proceed to next step if previous failed +- Clean up resources on failure + +**Example**: + +```cpp +// In constructor +try { + load_parameters(); + validate_configuration(); + initialize_driver(); + register_callbacks(); + initialize_hw_interface(); + create_publishers(); +} catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "Initialization failed: %s", e.what()); + throw; +} + +// In separate method or after construction +stream_start(); +``` + +### 2. Reconfiguration + +**When parameters change at runtime**: + +1. **Validate New Parameters**: Check if new values are valid +2. **Stop Stream**: Call `sensor_interface_stop()` +3. **Update Configuration**: Apply new parameter values +4. **Reinitialize Driver**: Create new driver with updated config +5. **Restart Stream**: Call `sensor_interface_start()` + +**Example**: + +```cpp +rcl_interfaces::msg::SetParametersResult on_parameter_change( + const std::vector & parameters) { + + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + // Validate parameters + for (const auto & param : parameters) { + if (param.get_name() == "rotation_speed") { + if (param.as_int() < 300 || param.as_int() > 1200) { + result.successful = false; + result.reason = "Invalid rotation speed"; + return result; + } + } + } + + // Apply changes + sensor_interface_stop(); + update_configuration(parameters); + reinitialize_driver(); + sensor_interface_start(); + + return result; +} +``` + +### 3. Connection Loss Handling + +**Detect and handle sensor disconnection**: + +1. **Timeout Detection**: Monitor time since last packet +2. **Diagnostic Update**: Set diagnostic status to ERROR +3. **Logging**: Log connection loss with timestamp +4. **Recovery**: Optionally attempt reconnection + +**Example**: + +```cpp +// In packet callback +void receive_packet_callback(const std::vector & packet) { + last_packet_time_ = now(); + + // Process packet... +} + +// In timer callback (e.g., 1 Hz) +void check_connection() { + auto time_since_last_packet = now() - last_packet_time_; + + if (time_since_last_packet > timeout_threshold_) { + RCLCPP_ERROR(get_logger(), "Connection lost - no packets for %.1fs", + time_since_last_packet.seconds()); + + // Update diagnostics + diagnostic_updater_.broadcast( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "Connection lost"); + + // Optionally attempt reconnection + attempt_reconnection(); + } +} +``` + +### 4. Shutdown Sequence + +**Order of operations**: + +1. **Stop Stream**: Call `sensor_interface_stop()` +2. **Close Sockets**: Ensure all network resources are closed +3. **Clear Buffers**: Release point cloud buffers +4. **Reset Pointers**: Reset shared_ptr members + +**Example**: + +```cpp +~MyVendorRosWrapper() { + if (hw_interface_ptr_) { + hw_interface_ptr_->sensor_interface_stop(); + } + + driver_ptr_.reset(); + hw_interface_ptr_.reset(); +} +``` + +### 5. Diagnostic Reporting + +**Required diagnostic information**: + +- **Packet Rate**: Packets received per second +- **Scan Rate**: Complete scans per second (should match rotation speed) +- **Connection Status**: OK / WARN / ERROR +- **Decode Errors**: Count of failed packet parses + +**Example**: + +```cpp +diagnostic_updater::Updater diagnostic_updater_; + +// In constructor +diagnostic_updater_.setHardwareID("MyVendor LiDAR"); +diagnostic_updater_.add("Scan Rate", this, &MyVendorRosWrapper::check_scan_rate); + +// Diagnostic function +void check_scan_rate(diagnostic_updater::DiagnosticStatusWrapper & stat) { + double expected_rate = sensor_cfg_ptr_->rotation_speed / 60.0; // Hz + double actual_rate = measured_scan_rate_; + + if (std::abs(actual_rate - expected_rate) < 0.5) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Scan rate OK"); + } else { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Scan rate mismatch"); + } + + stat.add("Expected (Hz)", expected_rate); + stat.add("Actual (Hz)", actual_rate); +} +``` + +--- + +## Verification + +### 1. Build + +```bash +colcon build --packages-up-to nebula_myvendor +source install/setup.bash +``` + +### 2. Launch + +```bash +ros2 launch nebula_myvendor nebula_myvendor.launch.xml +``` + +### 3. Verify Topics + +```bash +# List topics +ros2 topic list + +# Check point cloud +ros2 topic echo /points --field header + +# Monitor frequency +ros2 topic hz /points +``` + +### 4. Visualize + +```bash +rviz2 +# Add PointCloud2 display +# Set topic to /points +# Set Fixed Frame to your frame_id +``` + +### 5. Check Diagnostics + +```bash +ros2 topic echo /diagnostics +``` + +### 6. Test with PCAP (Offline) + +```bash +# Set launch_hw:=false to use PCAP playback +ros2 launch nebula_myvendor nebula_myvendor.launch.xml launch_hw:=false + +# In another terminal, play PCAP +ros2 bag play your_sensor_data.bag +``` + +--- + +## Integration Checklist + +- [ ] Cloned and renamed all files and directories +- [ ] Updated `CMakeLists.txt` in all packages +- [ ] Updated `package.xml` in all packages +- [ ] Implemented `SensorConfiguration` struct +- [ ] Implemented `unpack()` method in Decoder +- [ ] Implemented scan completion detection +- [ ] Implemented UDP reception in HW Interface +- [ ] Mapped ROS parameters in Wrapper +- [ ] Implemented startup sequence +- [ ] Implemented shutdown sequence +- [ ] Added diagnostic reporting +- [ ] Added connection loss handling +- [ ] Added copyright headers +- [ ] Verified build succeeds +- [ ] Verified point cloud publishes +- [ ] Verified scan rate matches expected +- [ ] Tested with real sensor or PCAP data + +--- + +## Additional Resources + +- **Hesai Implementation**: `src/nebula_hesai` - Full reference implementation +- **Velodyne Implementation**: `src/nebula_velodyne` - Alternative reference +- **Core Components**: `src/nebula_core` - Reusable utilities +- **Point Types**: See `docs/point_types.md` +- **Parameters**: See `docs/parameters.md` + +## Getting Help + +- Check existing sensor implementations for examples +- Review inline code documentation (Doxygen comments) +- Consult the API reference documentation +- Ask questions in [GitHub Issues](https://github.com/YOUR_ORG/YOUR_REPO/issues) diff --git a/docs/tutorials.md b/docs/tutorials.md index f526027ea..78f6cad90 100644 --- a/docs/tutorials.md +++ b/docs/tutorials.md @@ -1,4 +1,4 @@ # Nebula tutorials WIP - we are currently working on making tutorials for Nebula development, so please check back soon! -In the meantime, check out the [tutorial branch](https://github.com/tier4/nebula/tree/tutorial). +In the meantime, check out the [integration guide](integration_guide.md). diff --git a/mkdocs.yml b/mkdocs.yml index cf6b6f4cc..66e877188 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -32,6 +32,10 @@ theme: name: Switch to light mode use_directory_urls: true +### Extra JavaScript for Mermaid ### +extra_javascript: + - https://unpkg.com/mermaid/dist/mermaid.min.js + ### Navigation ### nav: - Home: index.md @@ -66,7 +70,9 @@ nav: - SRR520: parameters/vendors/continental/srr520.md - Point cloud types: point_types.md - Design: design.md - - Tutorials: tutorials.md + - Tutorials: + - tutorials.md + - Integration Guide: integration_guide.md - Contributing: contribute.md - API reference: - api_reference.md @@ -275,4 +281,8 @@ markdown_extensions: - admonition - pymdownx.details - pymdownx.highlight - - pymdownx.superfences + - pymdownx.superfences: + custom_fences: + - name: mermaid + class: mermaid + format: !!python/name:pymdownx.superfences.fence_div_format diff --git a/src/nebula_sample/README.md b/src/nebula_sample/README.md index 28eee641d..389e59e59 100644 --- a/src/nebula_sample/README.md +++ b/src/nebula_sample/README.md @@ -61,3 +61,5 @@ The guide covers: ## Reference Implementation This package provides a template structure for adding new sensor support. For complete examples, refer to existing sensor packages like `nebula_hesai` or `nebula_velodyne`. + +**For detailed integration instructions, see the [Integration Guide](../../docs/integration_guide.md) in the documentation.** diff --git a/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml b/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml new file mode 100644 index 000000000..4371e6a7b --- /dev/null +++ b/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml @@ -0,0 +1,27 @@ +# Sample Sensor ROS 2 Parameters +# This file contains example parameters for the Sample LiDAR sensor. +# Copy and modify this file for your specific sensor model. + +/**: + ros__parameters: + # Sensor configuration + sensor_model: SampleSensor + return_mode: Dual + frame_id: sample_lidar + + # Network configuration + sensor_ip: 192.168.1.201 + host_ip: 192.168.1.100 + data_port: 2368 + + # Scan configuration + scan_phase: 0 + rotation_speed: 600 # RPM + cloud_min_angle: 0 # degrees + cloud_max_angle: 360 # degrees + + # Optional: Calibration file path + # calibration_file: "$(find-pkg-share nebula_sample)/config/calibration.dat" + + # Hardware interface + launch_hw: true From bd15958cf0a25498b4332c5acebc2e9f5ebde857 Mon Sep 17 00:00:00 2001 From: David Wong Date: Tue, 9 Dec 2025 21:08:55 +0900 Subject: [PATCH 10/65] ci(pre-commit): allow unsafe yaml tags for mkdocs Signed-off-by: David Wong --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b3a145a0e..af3295ae0 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -11,7 +11,7 @@ repos: - id: check-toml - id: check-xml - id: check-yaml - args: [--allow-multiple-documents] + args: [--allow-multiple-documents, --unsafe] - id: detect-private-key - id: end-of-file-fixer - id: mixed-line-ending From aac72dd551a9c41439fee74b07fe35d895f59aa9 Mon Sep 17 00:00:00 2001 From: David Wong Date: Tue, 9 Dec 2025 21:21:11 +0900 Subject: [PATCH 11/65] docs(sample_sensor): remove duplicate integration guide, modify links and titles Signed-off-by: David Wong --- docs/integration_guide.md | 90 +++++++++---------- src/nebula_sample/INTEGRATION_GUIDE.md | 115 ------------------------- src/nebula_sample/README.md | 18 ++-- 3 files changed, 54 insertions(+), 169 deletions(-) delete mode 100644 src/nebula_sample/INTEGRATION_GUIDE.md diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 9d77cbce0..3898652c8 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -1,21 +1,21 @@ -# New Sensor Integration Guide +# New sensor integration guide This guide provides instructions for adding support for a new LiDAR sensor to Nebula using the `nebula_sample` package as a template. -## Table of Contents +## Table of contents -1. [Architecture Overview](#architecture-overview) -2. [Provided Components](#provided-components) -3. [Integration Workflow](#integration-workflow) -4. [Implementation Details](#implementation-details) -5. [Required Behaviors](#required-behaviors) +1. [Architecture overview](#architecture-overview) +2. [Provided components](#provided-components) +3. [Integration workflow](#integration-workflow) +4. [Implementation details](#implementation-details) +5. [Required behaviors](#required-behaviors) 6. [Verification](#verification) --- -## Architecture Overview +## Architecture overview -### Overall Package Structure +### Overall package structure Nebula is organized into common (reusable) packages and vendor-specific packages: @@ -64,7 +64,7 @@ graph TB - All vendor packages follow the **same 4-layer structure** for consistency - Vendor packages **depend on** common packages but not on each other -### Layered Architecture (Per Vendor) +### Layered architecture (per vendor) Each vendor package uses a layered architecture to separate functional blocks and promote code reuse: @@ -92,7 +92,7 @@ graph TD Driver --> HW ``` -### Data Flow +### Data flow 1. **Hardware Interface** receives raw UDP packets from the sensor and defines TCP protocols for communication and configuration 2. **Driver** receives packets and delegates to **Decoder** @@ -101,11 +101,11 @@ graph TD --- -## Provided Components +## Provided components Nebula provides reusable components to simplify sensor integration. You should use these instead of implementing from scratch. -### 1. UDP Socket Handling +### 1. UDP socket handling **Location**: `nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp` @@ -130,7 +130,7 @@ udp_socket_.bind(); udp_socket_.asyncReceive(callback); ``` -### 2. Status Codes +### 2. Status codes **Location**: `nebula_core_common/include/nebula_core_common/nebula_status.hpp` @@ -152,7 +152,7 @@ nebula::Status validate_config() { } ``` -### 3. Point Cloud Types +### 3. Point cloud types **Location**: `nebula_core_common/include/nebula_core_common/point_types.hpp` @@ -175,7 +175,7 @@ point.z = distance * sin_elevation; cloud->push_back(point); ``` -### 4. Angle Utilities +### 4. Angle utilities **Location**: `nebula_core_decoders/include/nebula_core_decoders/angles.hpp` @@ -194,7 +194,7 @@ float azimuth_rad = deg2rad(azimuth_deg); bool in_fov = angle_is_between(fov_min, fov_max, azimuth_rad); ``` -### 5. Configuration Base Classes +### 5. Configuration base classes **Location**: `nebula_core_common/include/nebula_core_common/nebula_common.hpp` @@ -213,7 +213,7 @@ struct MySensorConfiguration : public LidarConfigurationBase { }; ``` -### 6. Logger Integration +### 6. Logger integration **Location**: `nebula_core_common/include/nebula_core_common/loggers/logger.hpp` @@ -232,7 +232,7 @@ std::shared_ptr logger_; NEBULA_LOG_STREAM(logger_->error, "Failed to parse packet"); ``` -### 7. Diagnostic Integration +### 7. Diagnostic integration **Location**: ROS 2 `diagnostic_updater` package (used in ROS wrapper) @@ -253,9 +253,9 @@ diagnostic_updater::Updater diagnostic_updater_; --- -## Integration Workflow +## Integration workflow -### Step 1: Clone the Template +### Step 1: Clone and rename template Copy the `nebula_sample` directory: @@ -264,7 +264,7 @@ cd src cp -r nebula_sample nebula_myvendor ``` -### Step 2: Rename Components +### Step 2: Rename components Rename all occurrences of "sample"/"Sample" to your vendor name: @@ -281,7 +281,7 @@ find nebula_myvendor -type f -exec sed -i 's/sample/myvendor/g' {} + find nebula_myvendor -type f -exec sed -i 's/Sample/MyVendor/g' {} + ``` -### Step 3: Implement Components +### Step 3: Implement components See [Implementation Details](#implementation-details) below. @@ -291,13 +291,13 @@ See [Verification](#verification) below. --- -## Implementation Details +## Implementation details -### A. Common Package (`nebula_myvendor_common`) +### A. Common package (`nebula_myvendor_common`) **Purpose**: Define configuration and calibration structures. -#### 1. Sensor Configuration +#### 1. Sensor configuration Edit `include/nebula_myvendor_common/myvendor_common.hpp`: @@ -317,7 +317,7 @@ struct MyVendorSensorConfiguration : public LidarConfigurationBase { }; ``` -#### 2. Calibration Configuration (Optional) +#### 2. Calibration configuration (Optional) Only implement if your sensor needs calibration data: @@ -334,15 +334,15 @@ struct MyVendorCalibrationConfiguration : public CalibrationConfigurationBase { }; ``` -### B. Decoders Package (`nebula_myvendor_decoders`) +### B. Decoders package (`nebula_myvendor_decoders`) **Purpose**: Parse packets and generate point clouds. -#### 1. Decoder Interface +#### 1. Decoder interface The interface is already defined in `myvendor_scan_decoder.hpp`. You don't need to modify it. -#### 2. Decoder Implementation +#### 2. Decoder implementation Edit `include/nebula_myvendor_decoders/decoders/myvendor_decoder.hpp`: @@ -405,7 +405,7 @@ private: }; ``` -#### 3. Driver Implementation +#### 3. Driver implementation The driver is a thin wrapper. Edit `src/myvendor_driver.cpp`: @@ -423,7 +423,7 @@ PacketDecodeResult MyVendorDriver::parse_cloud_packet( } ``` -### C. Hardware Interfaces Package (`nebula_myvendor_hw_interfaces`) +### C. HW interfaces package (`nebula_myvendor_hw_interfaces`) **Purpose**: Manage UDP communication with the sensor. @@ -465,7 +465,7 @@ Status MyVendorHwInterface::sensor_interface_stop() { } ``` -### D. ROS Wrapper Package (`nebula_myvendor`) +### D. ROS wrapper package (`nebula_myvendor`) **Purpose**: Bridge C++ driver with ROS 2. @@ -529,11 +529,11 @@ Status MyVendorRosWrapper::stream_start() { --- -## Required Behaviors +## Required behaviors Your sensor integration must implement these behaviors correctly. -### 1. Startup Sequence +### 1. Startup sequence **Order of operations**: @@ -611,7 +611,7 @@ rcl_interfaces::msg::SetParametersResult on_parameter_change( } ``` -### 3. Connection Loss Handling +### 3. Connection loss handling **Detect and handle sensor disconnection**: @@ -649,7 +649,7 @@ void check_connection() { } ``` -### 4. Shutdown Sequence +### 4. Shutdown sequence **Order of operations**: @@ -671,7 +671,7 @@ void check_connection() { } ``` -### 5. Diagnostic Reporting +### 5. Diagnostic reporting **Required diagnostic information**: @@ -722,7 +722,7 @@ source install/setup.bash ros2 launch nebula_myvendor nebula_myvendor.launch.xml ``` -### 3. Verify Topics +### 3. Verify topics ```bash # List topics @@ -744,13 +744,13 @@ rviz2 # Set Fixed Frame to your frame_id ``` -### 5. Check Diagnostics +### 5. Check diagnostics ```bash ros2 topic echo /diagnostics ``` -### 6. Test with PCAP (Offline) +### 6. Test with PCAP (offline) ```bash # Set launch_hw:=false to use PCAP playback @@ -762,7 +762,7 @@ ros2 bag play your_sensor_data.bag --- -## Integration Checklist +## Integration checklist - [ ] Cloned and renamed all files and directories - [ ] Updated `CMakeLists.txt` in all packages @@ -784,7 +784,7 @@ ros2 bag play your_sensor_data.bag --- -## Additional Resources +## Additional resources - **Hesai Implementation**: `src/nebula_hesai` - Full reference implementation - **Velodyne Implementation**: `src/nebula_velodyne` - Alternative reference @@ -792,9 +792,9 @@ ros2 bag play your_sensor_data.bag - **Point Types**: See `docs/point_types.md` - **Parameters**: See `docs/parameters.md` -## Getting Help +## Getting help - Check existing sensor implementations for examples - Review inline code documentation (Doxygen comments) - Consult the API reference documentation -- Ask questions in [GitHub Issues](https://github.com/YOUR_ORG/YOUR_REPO/issues) +- Ask questions in [GitHub Issues](https://github.com/tier4/nebula/issues) diff --git a/src/nebula_sample/INTEGRATION_GUIDE.md b/src/nebula_sample/INTEGRATION_GUIDE.md deleted file mode 100644 index e5026330d..000000000 --- a/src/nebula_sample/INTEGRATION_GUIDE.md +++ /dev/null @@ -1,115 +0,0 @@ -# New Sensor Integration Guide - -This guide outlines the process of adding support for a new LiDAR sensor to Nebula using the `nebula_sample` package as a template. - -## Workflow Overview - -The integration process involves four main steps: - -1. **Clone**: Copy the sample package structure. -2. **Rename**: Update all file names, class names, and namespaces to match your sensor vendor. -3. **Implement**: Fill in the stub methods with your sensor-specific logic. -4. **Verify**: Build and test with your sensor hardware or PCAP data. - -## 1. Cloning the Template - -Copy the `nebula_sample` directory to a new directory named after your sensor vendor (e.g., `nebula_myvendor`). - -```bash -cp -r src/nebula_sample src/nebula_myvendor -``` - -## 2. Renaming Components - -You must rename all occurrences of "sample" and "Sample" to your vendor name. This includes: - -- **Directory Names**: `nebula_sample_*` -> `nebula_myvendor_*` -- **File Names**: `sample_*` -> `myvendor_*` -- **Class Names**: `SampleDriver` -> `MyVendorDriver` -- **Namespaces**: `nebula::drivers::sample` -> `nebula::drivers::myvendor` -- **CMake & Package Definitions**: Update `CMakeLists.txt` and `package.xml` in all sub-packages. - -## 3. Implementation Details - -The Nebula architecture is modular. You will need to implement specific classes in each sub-package. - -### A. Common Package (`nebula_myvendor_common`) - -**Purpose**: Defines data structures for configuration and calibration. - -- **`MyVendorSensorConfiguration`**: - - Add fields for sensor-specific settings (e.g., return mode, frequency, IP address). - - This struct is passed to both the driver and hardware interface. - -- **`MyVendorCalibrationConfiguration`** (Optional): - - Only needed if your sensor requires calibration data (e.g., angle corrections). - - Define how calibration data is stored. - - Implement `load_from_file()` to parse your vendor's calibration file format (csv, dat, xml, etc.). - - If using calibration, you'll need to add it as a parameter to your driver and decoder constructors. - -### B. Decoders Package (`nebula_myvendor_decoders`) - -**Purpose**: Handles packet parsing and point cloud generation. - -- Inherits from `ScanDecoder`. -- Defines the contract for parsing packets. - -- **`MyVendorDecoder`** (Implementation): - - **`unpack(packet)`**: The core method. It takes raw bytes (UDP packet) and returns a `PacketDecodeResult` containing the decoded points or an error. - - **`get_pointcloud()`**: (Optional) If you buffer points, this returns the constructed point cloud. - - **Key Task**: Parse the raw byte stream according to your sensor's user manual. - -- **`MyVendorDriver`**: - - The high-level manager. - - Initializes the decoder and hardware interface. - - **`parse_cloud_packet(packet)`**: Called when a packet is received. Delegates to `decoder_->unpack()`. - -### C. Hardware Interfaces Package (`nebula_myvendor_hw_interfaces`) - -**Purpose**: Handles network communication (UDP/TCP). - -- **`MyVendorHwInterface`**: - - Inherits from `HwInterfaceBase`. - - **`sensor_interface_start()`**: Setup UDP sockets and start receiving data. - - **`sensor_interface_stop()`**: Close sockets. - - **`register_scan_callback()`**: Register the function to call when a packet is received (usually `MyVendorDriver::parse_cloud_packet`). - - **UDP Receiver Example**: Refer to `src/nebula_core/include/nebula_core/hw/hw_interface_base.hpp` for the base interface definition. Concrete implementations are found in sensor-specific packages. - -### D. ROS Wrapper Package (`nebula_myvendor`) - -**Purpose**: Bridges the C++ driver with ROS 2. - -- **`MyVendorRosWrapper`**: - - Inherits from `rclcpp::Node`. - - **`initialize_driver()`**: Instantiates your `MyVendorDriver` and `MyVendorHwInterface`. - - **`receive_cloud_packet_callback()`**: The bridge callback. Receives data from HW interface, passes to Driver, gets PointCloud, converts to ROS message, and publishes. - - **Parameters**: Declare ROS parameters that map to your `MyVendorSensorConfiguration`. - -## 4. Verification - -1. **Build**: - - ```bash - colcon build --packages-up-to nebula_myvendor - ``` - -2. **Launch**: - - ```bash - ros2 launch nebula_myvendor nebula_myvendor.launch.xml - ``` - -3. **Test**: - - Verify topics: `ros2 topic list` - - Visualize: `rviz2` (add PointCloud2 display) - -## Checklist - -- [ ] Renamed all directories and files. -- [ ] Updated `CMakeLists.txt` and `package.xml` dependencies. -- [ ] Implemented `SensorConfiguration` struct. -- [ ] Implemented `unpack()` in Decoder. -- [ ] Implemented UDP reception in HW Interface. -- [ ] Mapped ROS parameters in Wrapper. -- [ ] Added copyright headers. -- [ ] Verified build. diff --git a/src/nebula_sample/README.md b/src/nebula_sample/README.md index 389e59e59..5952572f9 100644 --- a/src/nebula_sample/README.md +++ b/src/nebula_sample/README.md @@ -1,4 +1,4 @@ -# Nebula Sample Sensor Package +# Nebula sample sensor package A template sensor package for the Nebula LiDAR driver framework. This package provides a minimal, working example that developers can copy and modify to add support for new sensors. @@ -6,7 +6,7 @@ A template sensor package for the Nebula LiDAR driver framework. This package pr This package serves as a starting point for adding new sensor support to Nebula. It includes all necessary components with empty/stub implementations that compile and run without errors. -## Package Structure +## Package structure The sample sensor consists of four packages: @@ -27,9 +27,9 @@ colcon build --packages-up-to nebula_sample ros2 launch nebula_sample nebula_sample.launch.xml ``` -## Using as a Template +## Using as a template -For detailed instructions on how to use this package as a template for adding a new sensor, please refer to the [Integration Guide](INTEGRATION_GUIDE.md). +For detailed instructions on how to use this package as a template for adding a new sensor, please refer to the [Integration guide](../../docs/integration_guide.md). The guide covers: @@ -37,7 +37,7 @@ The guide covers: 2. Implementing sensor-specific logic 3. Verifying the new implementation -## Key Components +## Key components ### Configuration (`*_common`) @@ -49,17 +49,17 @@ The guide covers: - `SampleDecoder` - Packet decoder implementation - `SampleDriver` - Main driver class -### Hardware Interface (`*_hw_interfaces`) +### Hardware interface (`*_hw_interfaces`) - `SampleHwInterface` - Sensor communication interface -### ROS Wrapper +### ROS wrapper - `SampleRosWrapper` - ROS 2 node wrapping the driver - Point cloud publisher on `/points_raw` -## Reference Implementation +## Reference implementation This package provides a template structure for adding new sensor support. For complete examples, refer to existing sensor packages like `nebula_hesai` or `nebula_velodyne`. -**For detailed integration instructions, see the [Integration Guide](../../docs/integration_guide.md) in the documentation.** +**For detailed integration instructions, see the [Integration guide](../../docs/integration_guide.md) in the documentation.** From 3f235db56c121c587c7dec41f272ae7001fbdc05 Mon Sep 17 00:00:00 2001 From: David Wong Date: Tue, 9 Dec 2025 21:56:41 +0900 Subject: [PATCH 12/65] feat(sample_sensor): package and integration guide minor fixes Signed-off-by: David Wong --- docs/integration_guide.md | 80 +++++++++---------- .../nebula_sample_common/package.xml | 7 +- .../nebula_sample_decoders/package.xml | 5 +- .../nebula_sample_hw_interfaces/package.xml | 6 +- 4 files changed, 50 insertions(+), 48 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 3898652c8..c347e4a30 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -29,10 +29,10 @@ graph TB classDef vendor fill:#e0f2f1,stroke:#00695c,stroke-width:2px,rx:5,ry:5,color:#004d40; %% --- GRAPH STRUCTURE --- - subgraph Nebula["Nebula Framework"] + subgraph Nebula["Nebula framework"] direction TB - subgraph Common["Common Packages"] + subgraph Common["Common packages"] direction LR C1["nebula_core_common
Base types, status codes"]:::common C2["nebula_core_decoders
Decoder interfaces"]:::common @@ -40,7 +40,7 @@ graph TB C4["nebula_core_ros
ROS 2 utils"]:::common end - subgraph Vendors["Vendor Packages"] + subgraph Vendors["Vendor packages"] direction LR Hesai["nebula_hesai
hesai_common
hesai_decoders
hesai_hw_interfaces
ROS wrapper"]:::vendor @@ -57,7 +57,7 @@ graph TB style Vendors fill:#f0fdfa,stroke:#009688,stroke-dasharray: 5 5 ``` -**Key Principles**: +**Key principles**: - **Common packages** provide reusable functionality (UDP sockets, point types, etc.) - **Vendor packages** implement vendor-specific logic (packet parsing, calibration) @@ -78,13 +78,13 @@ graph TD %% --- NODES --- %% Apply classes using the triple colon syntax (:::className) - Wrapper[ROS 2 Wrapper Layer
Parameter handling, ROS message conversion, publishing]:::wrapper + Wrapper[ROS 2 wrapper layer
Parameter handling, ROS message conversion, publishing]:::wrapper - Driver[Driver Layer
High-level API, manages decoder and HW interface]:::logic + Driver[Driver layer
High-level API, manages decoder and HW interface]:::logic - Decoder[Decoder Layer
Packet parsing
point cloud generation]:::worker + Decoder[Decoder layer
Packet parsing
point cloud generation]:::worker - HW[HW Interface
UDP/TCP communication
socket management]:::worker + HW[HW interface
UDP/TCP communication
socket management]:::worker %% --- RELATIONSHIPS --- Wrapper --> Driver @@ -94,10 +94,10 @@ graph TD ### Data flow -1. **Hardware Interface** receives raw UDP packets from the sensor and defines TCP protocols for communication and configuration +1. **Hardware interface** receives raw UDP packets from the sensor and defines TCP protocols for communication and configuration 2. **Driver** receives packets and delegates to **Decoder** 3. **Decoder** parses packets, accumulates points, detects scan completion, and calls callback with complete point cloud -4. **ROS Wrapper** converts to ROS message and publishes +4. **ROS wrapper** converts to ROS message and publishes --- @@ -272,7 +272,7 @@ Rename all occurrences of "sample"/"Sample" to your vendor name: - **Files**: `sample_*.{cpp,hpp}` → `myvendor_*.{cpp,hpp}` - **Classes**: `SampleDriver` → `MyVendorDriver` - **Namespaces**: Update in all files -- **CMake/Package**: Update `CMakeLists.txt` and `package.xml` +- **CMake/package**: Update `CMakeLists.txt` and `package.xml` **Tip**: Use find-and-replace tools: @@ -283,7 +283,7 @@ find nebula_myvendor -type f -exec sed -i 's/Sample/MyVendor/g' {} + ### Step 3: Implement components -See [Implementation Details](#implementation-details) below. +See [Implementation details](#implementation-details) below. ### Step 4: Verify @@ -317,7 +317,7 @@ struct MyVendorSensorConfiguration : public LidarConfigurationBase { }; ``` -#### 2. Calibration configuration (Optional) +#### 2. Calibration configuration (optional) Only implement if your sensor needs calibration data: @@ -537,15 +537,15 @@ Your sensor integration must implement these behaviors correctly. **Order of operations**: -1. **Parameter Loading**: Declare and read all ROS parameters -2. **Configuration Validation**: Validate IP addresses, ports, ranges -3. **Driver Initialization**: Create driver with validated config -4. **Callback Registration**: Register pointcloud callback -5. **HW Interface Initialization**: Create and configure HW interface -6. **Publisher Creation**: Create ROS publishers -7. **Stream Start**: Call `sensor_interface_start()` to begin receiving data +1. **Parameter loading**: Declare and read all ROS parameters +2. **Configuration validation**: Validate IP addresses, ports, ranges +3. **Driver initialization**: Create driver with validated config +4. **Callback registration**: Register pointcloud callback +5. **HW interface initialization**: Create and configure HW interface +6. **Publisher creation**: Create ROS publishers +7. **Stream start**: Call `sensor_interface_start()` to begin receiving data -**Error Handling**: +**Error handling**: - If any step fails, log error and throw exception - Do not proceed to next step if previous failed @@ -575,11 +575,11 @@ stream_start(); **When parameters change at runtime**: -1. **Validate New Parameters**: Check if new values are valid -2. **Stop Stream**: Call `sensor_interface_stop()` -3. **Update Configuration**: Apply new parameter values -4. **Reinitialize Driver**: Create new driver with updated config -5. **Restart Stream**: Call `sensor_interface_start()` +1. **Validate new parameters**: Check if new values are valid +2. **Stop stream**: Call `sensor_interface_stop()` +3. **Update configuration**: Apply new parameter values +4. **Reinitialize driver**: Create new driver with updated config +5. **Restart stream**: Call `sensor_interface_start()` **Example**: @@ -615,8 +615,8 @@ rcl_interfaces::msg::SetParametersResult on_parameter_change( **Detect and handle sensor disconnection**: -1. **Timeout Detection**: Monitor time since last packet -2. **Diagnostic Update**: Set diagnostic status to ERROR +1. **Timeout detection**: Monitor time since last packet +2. **Diagnostic update**: Set diagnostic status to ERROR 3. **Logging**: Log connection loss with timestamp 4. **Recovery**: Optionally attempt reconnection @@ -653,10 +653,10 @@ void check_connection() { **Order of operations**: -1. **Stop Stream**: Call `sensor_interface_stop()` -2. **Close Sockets**: Ensure all network resources are closed -3. **Clear Buffers**: Release point cloud buffers -4. **Reset Pointers**: Reset shared_ptr members +1. **Stop stream**: Call `sensor_interface_stop()` +2. **Close sockets**: Ensure all network resources are closed +3. **Clear buffers**: Release point cloud buffers +4. **Reset pointers**: Reset shared_ptr members **Example**: @@ -675,10 +675,10 @@ void check_connection() { **Required diagnostic information**: -- **Packet Rate**: Packets received per second -- **Scan Rate**: Complete scans per second (should match rotation speed) -- **Connection Status**: OK / WARN / ERROR -- **Decode Errors**: Count of failed packet parses +- **Packet rate**: Packets received per second +- **Scan rate**: Complete scans per second (should match rotation speed) +- **Connection status**: OK / WARN / ERROR +- **Decode errors**: Count of failed packet parses **Example**: @@ -786,10 +786,10 @@ ros2 bag play your_sensor_data.bag ## Additional resources -- **Hesai Implementation**: `src/nebula_hesai` - Full reference implementation -- **Velodyne Implementation**: `src/nebula_velodyne` - Alternative reference -- **Core Components**: `src/nebula_core` - Reusable utilities -- **Point Types**: See `docs/point_types.md` +- **Hesai implementation**: `src/nebula_hesai` - Full reference implementation +- **Velodyne implementation**: `src/nebula_velodyne` - Alternative reference +- **Core components**: `src/nebula_core` - Reusable utilities +- **Point types**: See `docs/point_types.md` - **Parameters**: See `docs/parameters.md` ## Getting help diff --git a/src/nebula_sample/nebula_sample_common/package.xml b/src/nebula_sample/nebula_sample_common/package.xml index f19561930..6023612b4 100644 --- a/src/nebula_sample/nebula_sample_common/package.xml +++ b/src/nebula_sample/nebula_sample_common/package.xml @@ -2,9 +2,10 @@ nebula_sample_common - 0.0.1 - Nebula Common Sample Libraries and headers - MAP IV + 0.3.0 + Nebula Sample Common Libraries and Headers + David Wong + Max Schmeller Apache 2 TIER IV diff --git a/src/nebula_sample/nebula_sample_decoders/package.xml b/src/nebula_sample/nebula_sample_decoders/package.xml index 5e52fd60e..055999a77 100644 --- a/src/nebula_sample/nebula_sample_decoders/package.xml +++ b/src/nebula_sample/nebula_sample_decoders/package.xml @@ -2,9 +2,10 @@ nebula_sample_decoders - 0.0.1 + 0.3.0 Nebula Sample Decoders Library - MAP IV + David Wong + Max Schmeller Apache 2 TIER IV diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/package.xml b/src/nebula_sample/nebula_sample_hw_interfaces/package.xml index 003631183..a0af340cb 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/package.xml +++ b/src/nebula_sample/nebula_sample_hw_interfaces/package.xml @@ -2,9 +2,9 @@ nebula_sample_hw_interfaces - 0.0.1 - Nebula HW Interfaces Sample - MAP IV + 0.3.0 + Nebula Sample HW Interfaces + David Wong Apache 2 TIER IV From f7a4ce6aa4ceb3a70cf199f3737c549d99dda5b5 Mon Sep 17 00:00:00 2001 From: David Wong <33114676+drwnz@users.noreply.github.com> Date: Wed, 10 Dec 2025 17:42:53 +0900 Subject: [PATCH 13/65] chore(nebula_sample): fix capitalization in integration guide entry --- mkdocs.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mkdocs.yml b/mkdocs.yml index 66e877188..fc01be4c9 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -72,7 +72,7 @@ nav: - Design: design.md - Tutorials: - tutorials.md - - Integration Guide: integration_guide.md + - Integration guide: integration_guide.md - Contributing: contribute.md - API reference: - api_reference.md From 443358155e349ab809161fe4bf0d9d38c35d0405 Mon Sep 17 00:00:00 2001 From: David Wong Date: Thu, 11 Dec 2025 17:56:41 +0900 Subject: [PATCH 14/65] docs(sample_sensor): make integration guide diagrams visible in dark mode Signed-off-by: David Wong --- docs/integration_guide.md | 12 ++- mkdocs.yml | 154 +++++++++++++++++++------------------- 2 files changed, 85 insertions(+), 81 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index c347e4a30..c1ea9f2f3 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -51,10 +51,10 @@ graph TB end %% --- SUBGRAPH STYLING --- - %% Style the containers to have dashed borders and light backgrounds - style Nebula fill:#ffffff,stroke:#333,stroke-width:3px - style Common fill:#f5faff,stroke:#2196f3,stroke-dasharray: 5 5 - style Vendors fill:#f0fdfa,stroke:#009688,stroke-dasharray: 5 5 + %% Style the containers to have dashed borders and transparent backgrounds + style Nebula fill:none,stroke:#666,stroke-width:3px + style Common fill:none,stroke:#2196f3,stroke-dasharray: 5 5 + style Vendors fill:none,stroke:#009688,stroke-dasharray: 5 5 ``` **Key principles**: @@ -90,6 +90,10 @@ graph TD Wrapper --> Driver Driver --> Decoder Driver --> HW + + %% --- LINK STYLING --- + %% Make arrows visible in dark mode (light grey-blue) + linkStyle default stroke:#b0bec5,stroke-width:2px,fill:none ``` ### Data flow diff --git a/mkdocs.yml b/mkdocs.yml index fc01be4c9..0c628911e 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -196,83 +196,83 @@ plugins: * !*_source.md !class*.md - - mkdoxy: - projects: - core: - src-dirs: > - src/nebula_core/nebula_core_common/include - src/nebula_core/nebula_core_decoders/include - src/nebula_core/nebula_core_hw_interfaces/include - src/nebula_core/nebula_core_ros/include - full-doc: true - doxy-cfg: - FILE_PATTERNS: "*.hpp *.h" - RECURSIVE: true - EXTRACT_ALL: true - INLINE_SOURCES: true - ENABLE_PREPROCESSING: true - MACRO_EXPANSION: true - hesai: - src-dirs: > - src/nebula_hesai/nebula_hesai_common/include - src/nebula_hesai/nebula_hesai_decoders/include - src/nebula_hesai/nebula_hesai_hw_interfaces/include - src/nebula_hesai/nebula_hesai/include - full-doc: true - doxy-cfg: - FILE_PATTERNS: "*.hpp *.h" - RECURSIVE: true - EXTRACT_ALL: true - INLINE_SOURCES: true - ENABLE_PREPROCESSING: true - MACRO_EXPANSION: true - velodyne: - src-dirs: > - src/nebula_velodyne/nebula_velodyne_common/include - src/nebula_velodyne/nebula_velodyne_decoders/include - src/nebula_velodyne/nebula_velodyne_hw_interfaces/include - src/nebula_velodyne/nebula_velodyne/include - full-doc: true - doxy-cfg: - FILE_PATTERNS: "*.hpp *.h" - RECURSIVE: true - EXTRACT_ALL: true - INLINE_SOURCES: true - ENABLE_PREPROCESSING: true - MACRO_EXPANSION: true - robosense: - src-dirs: > - src/nebula_robosense/nebula_robosense_common/include - src/nebula_robosense/nebula_robosense_decoders/include - src/nebula_robosense/nebula_robosense_hw_interfaces/include - src/nebula_robosense/nebula_robosense/include - full-doc: true - doxy-cfg: - FILE_PATTERNS: "*.hpp *.h" - RECURSIVE: true - EXTRACT_ALL: true - INLINE_SOURCES: true - ENABLE_PREPROCESSING: true - MACRO_EXPANSION: true - continental: - src-dirs: > - src/nebula_continental/nebula_continental_common/include - src/nebula_continental/nebula_continental_decoders/include - src/nebula_continental/nebula_continental_hw_interfaces/include - src/nebula_continental/nebula_continental/include - full-doc: true - doxy-cfg: - FILE_PATTERNS: "*.hpp *.h" - RECURSIVE: true - EXTRACT_ALL: true - INLINE_SOURCES: true - ENABLE_PREPROCESSING: true - MACRO_EXPANSION: true - - save-api: .mkdoxy - full-doc: true - debug: false - ignore-errors: false + # - mkdoxy: + # projects: + # core: + # src-dirs: > + # src/nebula_core/nebula_core_common/include + # src/nebula_core/nebula_core_decoders/include + # src/nebula_core/nebula_core_hw_interfaces/include + # src/nebula_core/nebula_core_ros/include + # full-doc: true + # doxy-cfg: + # FILE_PATTERNS: "*.hpp *.h" + # RECURSIVE: true + # EXTRACT_ALL: true + # INLINE_SOURCES: true + # ENABLE_PREPROCESSING: true + # MACRO_EXPANSION: true + # hesai: + # src-dirs: > + # src/nebula_hesai/nebula_hesai_common/include + # src/nebula_hesai/nebula_hesai_decoders/include + # src/nebula_hesai/nebula_hesai_hw_interfaces/include + # src/nebula_hesai/nebula_hesai/include + # full-doc: true + # doxy-cfg: + # FILE_PATTERNS: "*.hpp *.h" + # RECURSIVE: true + # EXTRACT_ALL: true + # INLINE_SOURCES: true + # ENABLE_PREPROCESSING: true + # MACRO_EXPANSION: true + # velodyne: + # src-dirs: > + # src/nebula_velodyne/nebula_velodyne_common/include + # src/nebula_velodyne/nebula_velodyne_decoders/include + # src/nebula_velodyne/nebula_velodyne_hw_interfaces/include + # src/nebula_velodyne/nebula_velodyne/include + # full-doc: true + # doxy-cfg: + # FILE_PATTERNS: "*.hpp *.h" + # RECURSIVE: true + # EXTRACT_ALL: true + # INLINE_SOURCES: true + # ENABLE_PREPROCESSING: true + # MACRO_EXPANSION: true + # robosense: + # src-dirs: > + # src/nebula_robosense/nebula_robosense_common/include + # src/nebula_robosense/nebula_robosense_decoders/include + # src/nebula_robosense/nebula_robosense_hw_interfaces/include + # src/nebula_robosense/nebula_robosense/include + # full-doc: true + # doxy-cfg: + # FILE_PATTERNS: "*.hpp *.h" + # RECURSIVE: true + # EXTRACT_ALL: true + # INLINE_SOURCES: true + # ENABLE_PREPROCESSING: true + # MACRO_EXPANSION: true + # continental: + # src-dirs: > + # src/nebula_continental/nebula_continental_common/include + # src/nebula_continental/nebula_continental_decoders/include + # src/nebula_continental/nebula_continental_hw_interfaces/include + # src/nebula_continental/nebula_continental/include + # full-doc: true + # doxy-cfg: + # FILE_PATTERNS: "*.hpp *.h" + # RECURSIVE: true + # EXTRACT_ALL: true + # INLINE_SOURCES: true + # ENABLE_PREPROCESSING: true + # MACRO_EXPANSION: true + # + # save-api: .mkdoxy + # full-doc: true + # debug: false + # ignore-errors: false - search: lang: en From d2a4f18ff08df8e882947babb2af71f6938f6fc1 Mon Sep 17 00:00:00 2001 From: David Wong Date: Thu, 11 Dec 2025 20:42:19 +0900 Subject: [PATCH 15/65] docs(sample_sensor): remove commented code Signed-off-by: David Wong --- mkdocs.yml | 77 ------------------------------------------------------ 1 file changed, 77 deletions(-) diff --git a/mkdocs.yml b/mkdocs.yml index 0c628911e..26dbc584d 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -196,83 +196,6 @@ plugins: * !*_source.md !class*.md - # - mkdoxy: - # projects: - # core: - # src-dirs: > - # src/nebula_core/nebula_core_common/include - # src/nebula_core/nebula_core_decoders/include - # src/nebula_core/nebula_core_hw_interfaces/include - # src/nebula_core/nebula_core_ros/include - # full-doc: true - # doxy-cfg: - # FILE_PATTERNS: "*.hpp *.h" - # RECURSIVE: true - # EXTRACT_ALL: true - # INLINE_SOURCES: true - # ENABLE_PREPROCESSING: true - # MACRO_EXPANSION: true - # hesai: - # src-dirs: > - # src/nebula_hesai/nebula_hesai_common/include - # src/nebula_hesai/nebula_hesai_decoders/include - # src/nebula_hesai/nebula_hesai_hw_interfaces/include - # src/nebula_hesai/nebula_hesai/include - # full-doc: true - # doxy-cfg: - # FILE_PATTERNS: "*.hpp *.h" - # RECURSIVE: true - # EXTRACT_ALL: true - # INLINE_SOURCES: true - # ENABLE_PREPROCESSING: true - # MACRO_EXPANSION: true - # velodyne: - # src-dirs: > - # src/nebula_velodyne/nebula_velodyne_common/include - # src/nebula_velodyne/nebula_velodyne_decoders/include - # src/nebula_velodyne/nebula_velodyne_hw_interfaces/include - # src/nebula_velodyne/nebula_velodyne/include - # full-doc: true - # doxy-cfg: - # FILE_PATTERNS: "*.hpp *.h" - # RECURSIVE: true - # EXTRACT_ALL: true - # INLINE_SOURCES: true - # ENABLE_PREPROCESSING: true - # MACRO_EXPANSION: true - # robosense: - # src-dirs: > - # src/nebula_robosense/nebula_robosense_common/include - # src/nebula_robosense/nebula_robosense_decoders/include - # src/nebula_robosense/nebula_robosense_hw_interfaces/include - # src/nebula_robosense/nebula_robosense/include - # full-doc: true - # doxy-cfg: - # FILE_PATTERNS: "*.hpp *.h" - # RECURSIVE: true - # EXTRACT_ALL: true - # INLINE_SOURCES: true - # ENABLE_PREPROCESSING: true - # MACRO_EXPANSION: true - # continental: - # src-dirs: > - # src/nebula_continental/nebula_continental_common/include - # src/nebula_continental/nebula_continental_decoders/include - # src/nebula_continental/nebula_continental_hw_interfaces/include - # src/nebula_continental/nebula_continental/include - # full-doc: true - # doxy-cfg: - # FILE_PATTERNS: "*.hpp *.h" - # RECURSIVE: true - # EXTRACT_ALL: true - # INLINE_SOURCES: true - # ENABLE_PREPROCESSING: true - # MACRO_EXPANSION: true - # - # save-api: .mkdoxy - # full-doc: true - # debug: false - # ignore-errors: false - search: lang: en From e1d89892d46c572fedf555d8a575f54dcd568c08 Mon Sep 17 00:00:00 2001 From: David Wong Date: Fri, 12 Dec 2025 18:16:57 +0900 Subject: [PATCH 16/65] docs(sample_sensor): simplify diagrams Signed-off-by: David Wong --- docs/integration_guide.md | 43 +++----- docs/stylesheets/extra.css | 221 +++++++++++++++++++++++++++++++++++++ mkdocs.yml | 4 +- 3 files changed, 237 insertions(+), 31 deletions(-) create mode 100644 docs/stylesheets/extra.css diff --git a/docs/integration_guide.md b/docs/integration_guide.md index c1ea9f2f3..1392c4ff4 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -21,12 +21,6 @@ Nebula is organized into common (reusable) packages and vendor-specific packages ```mermaid graph TB - %% --- STYLE DEFINITIONS --- - %% Blue theme for Common - classDef common fill:#e3f2fd,stroke:#1565c0,stroke-width:2px,rx:5,ry:5,color:#0d47a1; - - %% Teal/Green theme for Vendors - classDef vendor fill:#e0f2f1,stroke:#00695c,stroke-width:2px,rx:5,ry:5,color:#004d40; %% --- GRAPH STRUCTURE --- subgraph Nebula["Nebula framework"] @@ -34,27 +28,23 @@ graph TB subgraph Common["Common packages"] direction LR - C1["nebula_core_common
Base types, status codes"]:::common - C2["nebula_core_decoders
Decoder interfaces"]:::common - C3["nebula_core_hw_interfaces
UDP/TCP sockets"]:::common - C4["nebula_core_ros
ROS 2 utils"]:::common + C1["nebula_core_common
Base types, status codes"] + C2["nebula_core_decoders
Decoder interfaces"] + C3["nebula_core_hw_interfaces
UDP/TCP sockets"] + C4["nebula_core_ros
ROS 2 utils"] end subgraph Vendors["Vendor packages"] direction LR - Hesai["nebula_hesai
hesai_common
hesai_decoders
hesai_hw_interfaces
ROS wrapper"]:::vendor + Hesai["nebula_hesai
hesai_common
hesai_decoders
hesai_hw_interfaces
ROS wrapper"] - Velodyne["nebula_velodyne
velodyne_common
velodyne_decoders
velodyne_hw_interfaces
ROS wrapper"]:::vendor + Velodyne["nebula_velodyne
velodyne_common
velodyne_decoders
velodyne_hw_interfaces
ROS wrapper"] - Sample["nebula_sample
sample_common
sample_decoders
sample_hw_interfaces
ROS wrapper"]:::vendor + Sample["nebula_sample
sample_common
sample_decoders
sample_hw_interfaces
ROS wrapper"] end end - %% --- SUBGRAPH STYLING --- - %% Style the containers to have dashed borders and transparent backgrounds - style Nebula fill:none,stroke:#666,stroke-width:3px - style Common fill:none,stroke:#2196f3,stroke-dasharray: 5 5 - style Vendors fill:none,stroke:#009688,stroke-dasharray: 5 5 + ``` **Key principles**: @@ -70,30 +60,23 @@ Each vendor package uses a layered architecture to separate functional blocks an ```mermaid graph TD - %% --- STYLE DEFINITIONS --- - %% Using a hierarchical palette to show data flow depth - classDef wrapper fill:#e3f2fd,stroke:#1565c0,stroke-width:2px,rx:5,ry:5,color:#0d47a1; - classDef logic fill:#f3e5f5,stroke:#7b1fa2,stroke-width:2px,rx:5,ry:5,color:#4a148c; - classDef worker fill:#e0f2f1,stroke:#00695c,stroke-width:2px,rx:5,ry:5,color:#004d40; %% --- NODES --- %% Apply classes using the triple colon syntax (:::className) - Wrapper[ROS 2 wrapper layer
Parameter handling, ROS message conversion, publishing]:::wrapper + Wrapper[ROS 2 wrapper layer
Parameter handling, ROS message conversion, publishing] - Driver[Driver layer
High-level API, manages decoder and HW interface]:::logic + Driver[Driver layer
High-level API, manages decoder and HW interface] - Decoder[Decoder layer
Packet parsing
point cloud generation]:::worker + Decoder[Decoder layer
Packet parsing
point cloud generation] - HW[HW interface
UDP/TCP communication
socket management]:::worker + HW[HW interface
UDP/TCP communication
socket management] %% --- RELATIONSHIPS --- Wrapper --> Driver Driver --> Decoder Driver --> HW - %% --- LINK STYLING --- - %% Make arrows visible in dark mode (light grey-blue) - linkStyle default stroke:#b0bec5,stroke-width:2px,fill:none + ``` ### Data flow diff --git a/docs/stylesheets/extra.css b/docs/stylesheets/extra.css new file mode 100644 index 000000000..3edd18bd3 --- /dev/null +++ b/docs/stylesheets/extra.css @@ -0,0 +1,221 @@ +:root { + /* Common packages - Light Mode */ + --mermaid-common-fill: #e3f2fd; + --mermaid-common-stroke: #1565c0; + --mermaid-common-text: #0d47a1; + + /* Vendor packages - Light Mode */ + --mermaid-vendor-fill: #e0f2f1; + --mermaid-vendor-stroke: #00695c; + --mermaid-vendor-text: #004d40; + + /* Wrapper layer - Light Mode */ + --mermaid-wrapper-fill: #e3f2fd; + --mermaid-wrapper-stroke: #1565c0; + --mermaid-wrapper-text: #0d47a1; + + /* Logic layer - Light Mode */ + --mermaid-logic-fill: #f3e5f5; + --mermaid-logic-stroke: #7b1fa2; + --mermaid-logic-text: #4a148c; + + /* Worker layer - Light Mode */ + --mermaid-worker-fill: #e0f2f1; + --mermaid-worker-stroke: #00695c; + --mermaid-worker-text: #004d40; + + /* Subgraph strokes - Light Mode */ + --mermaid-subgraph-nebula-stroke: #666; + --mermaid-subgraph-common-stroke: #2196f3; + --mermaid-subgraph-vendor-stroke: #009688; + + /* Link stroke - Light Mode */ + --mermaid-link-stroke: #546e7a; + + /* Text Color - Light Mode */ + --mermaid-text-color: #000000; +} + +[data-md-color-scheme="slate"] { + /* Common packages - Dark Mode */ + --mermaid-common-fill: #0d47a1; + --mermaid-common-stroke: #64b5f6; + --mermaid-common-text: #e3f2fd; + + /* Vendor packages - Dark Mode */ + --mermaid-vendor-fill: #004d40; + --mermaid-vendor-stroke: #4db6ac; + --mermaid-vendor-text: #e0f2f1; + + /* Wrapper layer - Dark Mode */ + --mermaid-wrapper-fill: #0d47a1; + --mermaid-wrapper-stroke: #64b5f6; + --mermaid-wrapper-text: #e3f2fd; + + /* Logic layer - Dark Mode */ + --mermaid-logic-fill: #4a148c; + --mermaid-logic-stroke: #e1bee7; + --mermaid-logic-text: #f3e5f5; + + /* Worker layer - Dark Mode */ + --mermaid-worker-fill: #004d40; + --mermaid-worker-stroke: #4db6ac; + --mermaid-worker-text: #e0f2f1; + + /* Subgraph strokes - Dark Mode */ + --mermaid-subgraph-nebula-stroke: #bbb; + --mermaid-subgraph-common-stroke: #64b5f6; + --mermaid-subgraph-vendor-stroke: #4db6ac; + + /* Link stroke - Dark Mode */ + --mermaid-link-stroke: #b0bec5; + + /* Text Color - Dark Mode */ + --mermaid-text-color: #e3f2fd; +} + +/* --- Nodes --- */ + +/* Common Nodes */ +.mermaid .common rect, +.mermaid .common circle, +.mermaid .common polygon { + fill: var(--mermaid-common-fill) !important; + stroke: var(--mermaid-common-stroke) !important; +} + +.mermaid .common text, +.mermaid .common tspan, +.mermaid .common .label, +.mermaid .common span, +.mermaid .common div { + fill: var(--mermaid-common-text) !important; + color: var(--mermaid-common-text) !important; +} + +/* Vendor Nodes */ +.mermaid .vendor rect, +.mermaid .vendor circle, +.mermaid .vendor polygon { + fill: var(--mermaid-vendor-fill) !important; + stroke: var(--mermaid-vendor-stroke) !important; +} + +.mermaid .vendor text, +.mermaid .vendor tspan, +.mermaid .vendor .label, +.mermaid .vendor span, +.mermaid .vendor div { + fill: var(--mermaid-vendor-text) !important; + color: var(--mermaid-vendor-text) !important; +} + +/* Wrapper Nodes */ +.mermaid .wrapper rect, +.mermaid .wrapper circle, +.mermaid .wrapper polygon { + fill: var(--mermaid-wrapper-fill) !important; + stroke: var(--mermaid-wrapper-stroke) !important; +} + +.mermaid .wrapper text, +.mermaid .wrapper tspan, +.mermaid .wrapper .label, +.mermaid .wrapper span, +.mermaid .wrapper div { + fill: var(--mermaid-wrapper-text) !important; + color: var(--mermaid-wrapper-text) !important; +} + +/* Logic Nodes */ +.mermaid .logic rect, +.mermaid .logic circle, +.mermaid .logic polygon { + fill: var(--mermaid-logic-fill) !important; + stroke: var(--mermaid-logic-stroke) !important; +} + +.mermaid .logic text, +.mermaid .logic tspan, +.mermaid .logic .label, +.mermaid .logic span, +.mermaid .logic div { + fill: var(--mermaid-logic-text) !important; + color: var(--mermaid-logic-text) !important; +} + +/* Worker Nodes */ +.mermaid .worker rect, +.mermaid .worker circle, +.mermaid .worker polygon { + fill: var(--mermaid-worker-fill) !important; + stroke: var(--mermaid-worker-stroke) !important; +} + +.mermaid .worker text, +.mermaid .worker tspan, +.mermaid .worker .label, +.mermaid .worker span, +.mermaid .worker div { + fill: var(--mermaid-worker-text) !important; + color: var(--mermaid-worker-text) !important; +} + +/* --- Subgraphs --- */ + +/* Subgraph Borders */ +.mermaid .nebula-subgraph>rect { + fill: none !important; + stroke: var(--mermaid-subgraph-nebula-stroke) !important; + stroke-width: 3px !important; +} + +.mermaid .common-subgraph>rect { + fill: none !important; + stroke: var(--mermaid-subgraph-common-stroke) !important; + stroke-dasharray: 5 5 !important; +} + +.mermaid .vendor-subgraph>rect { + fill: none !important; + stroke: var(--mermaid-subgraph-vendor-stroke) !important; + stroke-dasharray: 5 5 !important; +} + +/* Subgraph Titles / Cluster Labels */ +.mermaid .cluster text, +.mermaid .cluster .label, +.mermaid .cluster span, +.mermaid .cluster div, +.mermaid g.cluster foreignObject div { + fill: var(--mermaid-text-color) !important; + color: var(--mermaid-text-color) !important; +} + +/* --- Links / Edges --- */ + +/* Link Paths */ +.mermaid .edgePath path, +.mermaid path.edgePath, +.mermaid .flowchart-link { + stroke: var(--mermaid-link-stroke) !important; + stroke-width: 2px !important; + fill: none !important; +} + +/* Arrowheads (Markers) */ +/* Note: Mermaid often puts markers in defs, but we try to catch them here */ +.mermaid marker path, +.mermaid marker circle, +.mermaid marker { + fill: var(--mermaid-link-stroke) !important; + stroke: var(--mermaid-link-stroke) !important; +} + +/* General Text Fallback for Dark Mode */ +[data-md-color-scheme="slate"] .mermaid text, +[data-md-color-scheme="slate"] .mermaid tspan, +[data-md-color-scheme="slate"] .mermaid span { + fill: var(--mermaid-text-color) !important; + color: var(--mermaid-text-color) !important; +} \ No newline at end of file diff --git a/mkdocs.yml b/mkdocs.yml index 26dbc584d..03f6b2f27 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -36,6 +36,8 @@ use_directory_urls: true extra_javascript: - https://unpkg.com/mermaid/dist/mermaid.min.js + + ### Navigation ### nav: - Home: index.md @@ -208,4 +210,4 @@ markdown_extensions: custom_fences: - name: mermaid class: mermaid - format: !!python/name:pymdownx.superfences.fence_div_format + format: !!python/name:pymdownx.superfences.fence_code_format From 8b0f8103836e258f4749284ef57a83380e5a550a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 12 Dec 2025 09:17:37 +0000 Subject: [PATCH 17/65] ci(pre-commit): autofix --- docs/stylesheets/extra.css | 196 ++++++++++++++++++------------------- mkdocs.yml | 2 - 2 files changed, 98 insertions(+), 100 deletions(-) diff --git a/docs/stylesheets/extra.css b/docs/stylesheets/extra.css index 3edd18bd3..aade93a82 100644 --- a/docs/stylesheets/extra.css +++ b/docs/stylesheets/extra.css @@ -1,77 +1,77 @@ :root { - /* Common packages - Light Mode */ - --mermaid-common-fill: #e3f2fd; - --mermaid-common-stroke: #1565c0; - --mermaid-common-text: #0d47a1; + /* Common packages - Light Mode */ + --mermaid-common-fill: #e3f2fd; + --mermaid-common-stroke: #1565c0; + --mermaid-common-text: #0d47a1; - /* Vendor packages - Light Mode */ - --mermaid-vendor-fill: #e0f2f1; - --mermaid-vendor-stroke: #00695c; - --mermaid-vendor-text: #004d40; + /* Vendor packages - Light Mode */ + --mermaid-vendor-fill: #e0f2f1; + --mermaid-vendor-stroke: #00695c; + --mermaid-vendor-text: #004d40; - /* Wrapper layer - Light Mode */ - --mermaid-wrapper-fill: #e3f2fd; - --mermaid-wrapper-stroke: #1565c0; - --mermaid-wrapper-text: #0d47a1; + /* Wrapper layer - Light Mode */ + --mermaid-wrapper-fill: #e3f2fd; + --mermaid-wrapper-stroke: #1565c0; + --mermaid-wrapper-text: #0d47a1; - /* Logic layer - Light Mode */ - --mermaid-logic-fill: #f3e5f5; - --mermaid-logic-stroke: #7b1fa2; - --mermaid-logic-text: #4a148c; + /* Logic layer - Light Mode */ + --mermaid-logic-fill: #f3e5f5; + --mermaid-logic-stroke: #7b1fa2; + --mermaid-logic-text: #4a148c; - /* Worker layer - Light Mode */ - --mermaid-worker-fill: #e0f2f1; - --mermaid-worker-stroke: #00695c; - --mermaid-worker-text: #004d40; + /* Worker layer - Light Mode */ + --mermaid-worker-fill: #e0f2f1; + --mermaid-worker-stroke: #00695c; + --mermaid-worker-text: #004d40; - /* Subgraph strokes - Light Mode */ - --mermaid-subgraph-nebula-stroke: #666; - --mermaid-subgraph-common-stroke: #2196f3; - --mermaid-subgraph-vendor-stroke: #009688; + /* Subgraph strokes - Light Mode */ + --mermaid-subgraph-nebula-stroke: #666; + --mermaid-subgraph-common-stroke: #2196f3; + --mermaid-subgraph-vendor-stroke: #009688; - /* Link stroke - Light Mode */ - --mermaid-link-stroke: #546e7a; + /* Link stroke - Light Mode */ + --mermaid-link-stroke: #546e7a; - /* Text Color - Light Mode */ - --mermaid-text-color: #000000; + /* Text Color - Light Mode */ + --mermaid-text-color: #000000; } [data-md-color-scheme="slate"] { - /* Common packages - Dark Mode */ - --mermaid-common-fill: #0d47a1; - --mermaid-common-stroke: #64b5f6; - --mermaid-common-text: #e3f2fd; + /* Common packages - Dark Mode */ + --mermaid-common-fill: #0d47a1; + --mermaid-common-stroke: #64b5f6; + --mermaid-common-text: #e3f2fd; - /* Vendor packages - Dark Mode */ - --mermaid-vendor-fill: #004d40; - --mermaid-vendor-stroke: #4db6ac; - --mermaid-vendor-text: #e0f2f1; + /* Vendor packages - Dark Mode */ + --mermaid-vendor-fill: #004d40; + --mermaid-vendor-stroke: #4db6ac; + --mermaid-vendor-text: #e0f2f1; - /* Wrapper layer - Dark Mode */ - --mermaid-wrapper-fill: #0d47a1; - --mermaid-wrapper-stroke: #64b5f6; - --mermaid-wrapper-text: #e3f2fd; + /* Wrapper layer - Dark Mode */ + --mermaid-wrapper-fill: #0d47a1; + --mermaid-wrapper-stroke: #64b5f6; + --mermaid-wrapper-text: #e3f2fd; - /* Logic layer - Dark Mode */ - --mermaid-logic-fill: #4a148c; - --mermaid-logic-stroke: #e1bee7; - --mermaid-logic-text: #f3e5f5; + /* Logic layer - Dark Mode */ + --mermaid-logic-fill: #4a148c; + --mermaid-logic-stroke: #e1bee7; + --mermaid-logic-text: #f3e5f5; - /* Worker layer - Dark Mode */ - --mermaid-worker-fill: #004d40; - --mermaid-worker-stroke: #4db6ac; - --mermaid-worker-text: #e0f2f1; + /* Worker layer - Dark Mode */ + --mermaid-worker-fill: #004d40; + --mermaid-worker-stroke: #4db6ac; + --mermaid-worker-text: #e0f2f1; - /* Subgraph strokes - Dark Mode */ - --mermaid-subgraph-nebula-stroke: #bbb; - --mermaid-subgraph-common-stroke: #64b5f6; - --mermaid-subgraph-vendor-stroke: #4db6ac; + /* Subgraph strokes - Dark Mode */ + --mermaid-subgraph-nebula-stroke: #bbb; + --mermaid-subgraph-common-stroke: #64b5f6; + --mermaid-subgraph-vendor-stroke: #4db6ac; - /* Link stroke - Dark Mode */ - --mermaid-link-stroke: #b0bec5; + /* Link stroke - Dark Mode */ + --mermaid-link-stroke: #b0bec5; - /* Text Color - Dark Mode */ - --mermaid-text-color: #e3f2fd; + /* Text Color - Dark Mode */ + --mermaid-text-color: #e3f2fd; } /* --- Nodes --- */ @@ -80,8 +80,8 @@ .mermaid .common rect, .mermaid .common circle, .mermaid .common polygon { - fill: var(--mermaid-common-fill) !important; - stroke: var(--mermaid-common-stroke) !important; + fill: var(--mermaid-common-fill) !important; + stroke: var(--mermaid-common-stroke) !important; } .mermaid .common text, @@ -89,16 +89,16 @@ .mermaid .common .label, .mermaid .common span, .mermaid .common div { - fill: var(--mermaid-common-text) !important; - color: var(--mermaid-common-text) !important; + fill: var(--mermaid-common-text) !important; + color: var(--mermaid-common-text) !important; } /* Vendor Nodes */ .mermaid .vendor rect, .mermaid .vendor circle, .mermaid .vendor polygon { - fill: var(--mermaid-vendor-fill) !important; - stroke: var(--mermaid-vendor-stroke) !important; + fill: var(--mermaid-vendor-fill) !important; + stroke: var(--mermaid-vendor-stroke) !important; } .mermaid .vendor text, @@ -106,16 +106,16 @@ .mermaid .vendor .label, .mermaid .vendor span, .mermaid .vendor div { - fill: var(--mermaid-vendor-text) !important; - color: var(--mermaid-vendor-text) !important; + fill: var(--mermaid-vendor-text) !important; + color: var(--mermaid-vendor-text) !important; } /* Wrapper Nodes */ .mermaid .wrapper rect, .mermaid .wrapper circle, .mermaid .wrapper polygon { - fill: var(--mermaid-wrapper-fill) !important; - stroke: var(--mermaid-wrapper-stroke) !important; + fill: var(--mermaid-wrapper-fill) !important; + stroke: var(--mermaid-wrapper-stroke) !important; } .mermaid .wrapper text, @@ -123,16 +123,16 @@ .mermaid .wrapper .label, .mermaid .wrapper span, .mermaid .wrapper div { - fill: var(--mermaid-wrapper-text) !important; - color: var(--mermaid-wrapper-text) !important; + fill: var(--mermaid-wrapper-text) !important; + color: var(--mermaid-wrapper-text) !important; } /* Logic Nodes */ .mermaid .logic rect, .mermaid .logic circle, .mermaid .logic polygon { - fill: var(--mermaid-logic-fill) !important; - stroke: var(--mermaid-logic-stroke) !important; + fill: var(--mermaid-logic-fill) !important; + stroke: var(--mermaid-logic-stroke) !important; } .mermaid .logic text, @@ -140,16 +140,16 @@ .mermaid .logic .label, .mermaid .logic span, .mermaid .logic div { - fill: var(--mermaid-logic-text) !important; - color: var(--mermaid-logic-text) !important; + fill: var(--mermaid-logic-text) !important; + color: var(--mermaid-logic-text) !important; } /* Worker Nodes */ .mermaid .worker rect, .mermaid .worker circle, .mermaid .worker polygon { - fill: var(--mermaid-worker-fill) !important; - stroke: var(--mermaid-worker-stroke) !important; + fill: var(--mermaid-worker-fill) !important; + stroke: var(--mermaid-worker-stroke) !important; } .mermaid .worker text, @@ -157,29 +157,29 @@ .mermaid .worker .label, .mermaid .worker span, .mermaid .worker div { - fill: var(--mermaid-worker-text) !important; - color: var(--mermaid-worker-text) !important; + fill: var(--mermaid-worker-text) !important; + color: var(--mermaid-worker-text) !important; } /* --- Subgraphs --- */ /* Subgraph Borders */ -.mermaid .nebula-subgraph>rect { - fill: none !important; - stroke: var(--mermaid-subgraph-nebula-stroke) !important; - stroke-width: 3px !important; +.mermaid .nebula-subgraph > rect { + fill: none !important; + stroke: var(--mermaid-subgraph-nebula-stroke) !important; + stroke-width: 3px !important; } -.mermaid .common-subgraph>rect { - fill: none !important; - stroke: var(--mermaid-subgraph-common-stroke) !important; - stroke-dasharray: 5 5 !important; +.mermaid .common-subgraph > rect { + fill: none !important; + stroke: var(--mermaid-subgraph-common-stroke) !important; + stroke-dasharray: 5 5 !important; } -.mermaid .vendor-subgraph>rect { - fill: none !important; - stroke: var(--mermaid-subgraph-vendor-stroke) !important; - stroke-dasharray: 5 5 !important; +.mermaid .vendor-subgraph > rect { + fill: none !important; + stroke: var(--mermaid-subgraph-vendor-stroke) !important; + stroke-dasharray: 5 5 !important; } /* Subgraph Titles / Cluster Labels */ @@ -188,8 +188,8 @@ .mermaid .cluster span, .mermaid .cluster div, .mermaid g.cluster foreignObject div { - fill: var(--mermaid-text-color) !important; - color: var(--mermaid-text-color) !important; + fill: var(--mermaid-text-color) !important; + color: var(--mermaid-text-color) !important; } /* --- Links / Edges --- */ @@ -198,9 +198,9 @@ .mermaid .edgePath path, .mermaid path.edgePath, .mermaid .flowchart-link { - stroke: var(--mermaid-link-stroke) !important; - stroke-width: 2px !important; - fill: none !important; + stroke: var(--mermaid-link-stroke) !important; + stroke-width: 2px !important; + fill: none !important; } /* Arrowheads (Markers) */ @@ -208,14 +208,14 @@ .mermaid marker path, .mermaid marker circle, .mermaid marker { - fill: var(--mermaid-link-stroke) !important; - stroke: var(--mermaid-link-stroke) !important; + fill: var(--mermaid-link-stroke) !important; + stroke: var(--mermaid-link-stroke) !important; } /* General Text Fallback for Dark Mode */ [data-md-color-scheme="slate"] .mermaid text, [data-md-color-scheme="slate"] .mermaid tspan, [data-md-color-scheme="slate"] .mermaid span { - fill: var(--mermaid-text-color) !important; - color: var(--mermaid-text-color) !important; -} \ No newline at end of file + fill: var(--mermaid-text-color) !important; + color: var(--mermaid-text-color) !important; +} diff --git a/mkdocs.yml b/mkdocs.yml index 03f6b2f27..9de13c179 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -36,8 +36,6 @@ use_directory_urls: true extra_javascript: - https://unpkg.com/mermaid/dist/mermaid.min.js - - ### Navigation ### nav: - Home: index.md From ae8871d728f7b87c2283e4016f42fc92a5635ee6 Mon Sep 17 00:00:00 2001 From: David Wong Date: Sun, 14 Dec 2025 23:00:40 +0900 Subject: [PATCH 18/65] docs(sample_sensor): simplify integration guide Signed-off-by: David Wong --- docs/integration_guide.md | 6 +++--- mkdocs.yml | 4 ---- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 1392c4ff4..aed4d2d92 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -400,7 +400,7 @@ The driver is a thin wrapper. Edit `src/myvendor_driver.cpp`: MyVendorDriver::MyVendorDriver( const std::shared_ptr & config) : config_(config) { - scan_decoder_ = std::make_shared(config); + scan_decoder_ = std::make_unique(config); driver_status_ = Status::OK; } @@ -478,7 +478,7 @@ MyVendorRosWrapper::MyVendorRosWrapper(const rclcpp::NodeOptions & options) launch_hw_ = get_parameter("launch_hw").as_bool(); // 3. Initialize driver - driver_ptr_ = std::make_shared(sensor_cfg_ptr_); + driver_ptr_ = std::make_unique(sensor_cfg_ptr_); // 4. Register pointcloud callback driver_ptr_->set_pointcloud_callback( @@ -492,7 +492,7 @@ MyVendorRosWrapper::MyVendorRosWrapper(const rclcpp::NodeOptions & options) // 5. Initialize HW interface if (launch_hw_) { - hw_interface_ptr_ = std::make_shared(); + hw_interface_ptr_ = std::make_unique(); hw_interface_ptr_->set_sensor_configuration(sensor_cfg_ptr_); hw_interface_ptr_->register_scan_callback( [this](const std::vector & packet, const auto & metadata) { diff --git a/mkdocs.yml b/mkdocs.yml index 9de13c179..9f2146a86 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -32,10 +32,6 @@ theme: name: Switch to light mode use_directory_urls: true -### Extra JavaScript for Mermaid ### -extra_javascript: - - https://unpkg.com/mermaid/dist/mermaid.min.js - ### Navigation ### nav: - Home: index.md From 41c7cb03185a2da285f594ec21391f88a0e230df Mon Sep 17 00:00:00 2001 From: David Wong <33114676+drwnz@users.noreply.github.com> Date: Tue, 16 Dec 2025 15:06:35 +0900 Subject: [PATCH 19/65] docs(integration_guide): delete unnecessary stylesheets --- docs/stylesheets/extra.css | 221 ------------------------------------- 1 file changed, 221 deletions(-) delete mode 100644 docs/stylesheets/extra.css diff --git a/docs/stylesheets/extra.css b/docs/stylesheets/extra.css deleted file mode 100644 index aade93a82..000000000 --- a/docs/stylesheets/extra.css +++ /dev/null @@ -1,221 +0,0 @@ -:root { - /* Common packages - Light Mode */ - --mermaid-common-fill: #e3f2fd; - --mermaid-common-stroke: #1565c0; - --mermaid-common-text: #0d47a1; - - /* Vendor packages - Light Mode */ - --mermaid-vendor-fill: #e0f2f1; - --mermaid-vendor-stroke: #00695c; - --mermaid-vendor-text: #004d40; - - /* Wrapper layer - Light Mode */ - --mermaid-wrapper-fill: #e3f2fd; - --mermaid-wrapper-stroke: #1565c0; - --mermaid-wrapper-text: #0d47a1; - - /* Logic layer - Light Mode */ - --mermaid-logic-fill: #f3e5f5; - --mermaid-logic-stroke: #7b1fa2; - --mermaid-logic-text: #4a148c; - - /* Worker layer - Light Mode */ - --mermaid-worker-fill: #e0f2f1; - --mermaid-worker-stroke: #00695c; - --mermaid-worker-text: #004d40; - - /* Subgraph strokes - Light Mode */ - --mermaid-subgraph-nebula-stroke: #666; - --mermaid-subgraph-common-stroke: #2196f3; - --mermaid-subgraph-vendor-stroke: #009688; - - /* Link stroke - Light Mode */ - --mermaid-link-stroke: #546e7a; - - /* Text Color - Light Mode */ - --mermaid-text-color: #000000; -} - -[data-md-color-scheme="slate"] { - /* Common packages - Dark Mode */ - --mermaid-common-fill: #0d47a1; - --mermaid-common-stroke: #64b5f6; - --mermaid-common-text: #e3f2fd; - - /* Vendor packages - Dark Mode */ - --mermaid-vendor-fill: #004d40; - --mermaid-vendor-stroke: #4db6ac; - --mermaid-vendor-text: #e0f2f1; - - /* Wrapper layer - Dark Mode */ - --mermaid-wrapper-fill: #0d47a1; - --mermaid-wrapper-stroke: #64b5f6; - --mermaid-wrapper-text: #e3f2fd; - - /* Logic layer - Dark Mode */ - --mermaid-logic-fill: #4a148c; - --mermaid-logic-stroke: #e1bee7; - --mermaid-logic-text: #f3e5f5; - - /* Worker layer - Dark Mode */ - --mermaid-worker-fill: #004d40; - --mermaid-worker-stroke: #4db6ac; - --mermaid-worker-text: #e0f2f1; - - /* Subgraph strokes - Dark Mode */ - --mermaid-subgraph-nebula-stroke: #bbb; - --mermaid-subgraph-common-stroke: #64b5f6; - --mermaid-subgraph-vendor-stroke: #4db6ac; - - /* Link stroke - Dark Mode */ - --mermaid-link-stroke: #b0bec5; - - /* Text Color - Dark Mode */ - --mermaid-text-color: #e3f2fd; -} - -/* --- Nodes --- */ - -/* Common Nodes */ -.mermaid .common rect, -.mermaid .common circle, -.mermaid .common polygon { - fill: var(--mermaid-common-fill) !important; - stroke: var(--mermaid-common-stroke) !important; -} - -.mermaid .common text, -.mermaid .common tspan, -.mermaid .common .label, -.mermaid .common span, -.mermaid .common div { - fill: var(--mermaid-common-text) !important; - color: var(--mermaid-common-text) !important; -} - -/* Vendor Nodes */ -.mermaid .vendor rect, -.mermaid .vendor circle, -.mermaid .vendor polygon { - fill: var(--mermaid-vendor-fill) !important; - stroke: var(--mermaid-vendor-stroke) !important; -} - -.mermaid .vendor text, -.mermaid .vendor tspan, -.mermaid .vendor .label, -.mermaid .vendor span, -.mermaid .vendor div { - fill: var(--mermaid-vendor-text) !important; - color: var(--mermaid-vendor-text) !important; -} - -/* Wrapper Nodes */ -.mermaid .wrapper rect, -.mermaid .wrapper circle, -.mermaid .wrapper polygon { - fill: var(--mermaid-wrapper-fill) !important; - stroke: var(--mermaid-wrapper-stroke) !important; -} - -.mermaid .wrapper text, -.mermaid .wrapper tspan, -.mermaid .wrapper .label, -.mermaid .wrapper span, -.mermaid .wrapper div { - fill: var(--mermaid-wrapper-text) !important; - color: var(--mermaid-wrapper-text) !important; -} - -/* Logic Nodes */ -.mermaid .logic rect, -.mermaid .logic circle, -.mermaid .logic polygon { - fill: var(--mermaid-logic-fill) !important; - stroke: var(--mermaid-logic-stroke) !important; -} - -.mermaid .logic text, -.mermaid .logic tspan, -.mermaid .logic .label, -.mermaid .logic span, -.mermaid .logic div { - fill: var(--mermaid-logic-text) !important; - color: var(--mermaid-logic-text) !important; -} - -/* Worker Nodes */ -.mermaid .worker rect, -.mermaid .worker circle, -.mermaid .worker polygon { - fill: var(--mermaid-worker-fill) !important; - stroke: var(--mermaid-worker-stroke) !important; -} - -.mermaid .worker text, -.mermaid .worker tspan, -.mermaid .worker .label, -.mermaid .worker span, -.mermaid .worker div { - fill: var(--mermaid-worker-text) !important; - color: var(--mermaid-worker-text) !important; -} - -/* --- Subgraphs --- */ - -/* Subgraph Borders */ -.mermaid .nebula-subgraph > rect { - fill: none !important; - stroke: var(--mermaid-subgraph-nebula-stroke) !important; - stroke-width: 3px !important; -} - -.mermaid .common-subgraph > rect { - fill: none !important; - stroke: var(--mermaid-subgraph-common-stroke) !important; - stroke-dasharray: 5 5 !important; -} - -.mermaid .vendor-subgraph > rect { - fill: none !important; - stroke: var(--mermaid-subgraph-vendor-stroke) !important; - stroke-dasharray: 5 5 !important; -} - -/* Subgraph Titles / Cluster Labels */ -.mermaid .cluster text, -.mermaid .cluster .label, -.mermaid .cluster span, -.mermaid .cluster div, -.mermaid g.cluster foreignObject div { - fill: var(--mermaid-text-color) !important; - color: var(--mermaid-text-color) !important; -} - -/* --- Links / Edges --- */ - -/* Link Paths */ -.mermaid .edgePath path, -.mermaid path.edgePath, -.mermaid .flowchart-link { - stroke: var(--mermaid-link-stroke) !important; - stroke-width: 2px !important; - fill: none !important; -} - -/* Arrowheads (Markers) */ -/* Note: Mermaid often puts markers in defs, but we try to catch them here */ -.mermaid marker path, -.mermaid marker circle, -.mermaid marker { - fill: var(--mermaid-link-stroke) !important; - stroke: var(--mermaid-link-stroke) !important; -} - -/* General Text Fallback for Dark Mode */ -[data-md-color-scheme="slate"] .mermaid text, -[data-md-color-scheme="slate"] .mermaid tspan, -[data-md-color-scheme="slate"] .mermaid span { - fill: var(--mermaid-text-color) !important; - color: var(--mermaid-text-color) !important; -} From 7bdd22075a13e245fce6eaf80cad4fc5dab8f2dc Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 15:22:00 +0900 Subject: [PATCH 20/65] docs: fix mermaid and drawio errors Signed-off-by: Max SCHMELLER --- docs/requirements.txt | 8 ++++---- zensical.toml | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/requirements.txt b/docs/requirements.txt index 3e3dab3a5..f6afb57ee 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,6 +1,6 @@ -mkdocs==1.6.1 -mkdocs-drawio>=1.8.2 -mkdoxy==1.2.8 -zensical==0.0.15 +mkdocs>=1.6.1,<2.0.0 +mkdocs-drawio>=1.13.0,<2.0.0 +mkdoxy>=1.2.8,<2.0.0 +zensical>=0.0.15,<1.0.0 tabulate lxml diff --git a/zensical.toml b/zensical.toml index 2523e0aa1..5ee3e4df3 100644 --- a/zensical.toml +++ b/zensical.toml @@ -208,7 +208,7 @@ extra_css = [ [project.markdown_extensions.pymdownx.highlight] [project.markdown_extensions.pymdownx.superfences] custom_fences = [ - { "name" = "mermaid", "class" = "mermaid", "format" = "!!python/name:pymdownx.superfences.fence_code_format" }, + { "name" = "mermaid", "class" = "mermaid", "format" = "pymdownx.superfences.fence_code_format" }, ] [project.markdown_extensions.pymdownx.arithmatex] From cfe6e8c3e0e7c5f84103d62897662f0aef8d91b2 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 15:25:58 +0900 Subject: [PATCH 21/65] docs: fix checklist Signed-off-by: Max SCHMELLER --- zensical.toml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/zensical.toml b/zensical.toml index 5ee3e4df3..f3b4b381a 100644 --- a/zensical.toml +++ b/zensical.toml @@ -214,6 +214,9 @@ custom_fences = [ [project.markdown_extensions.pymdownx.arithmatex] generic = true +[project.markdown_extensions.pymdownx.tasklist] +custom_checkbox = true + [project.theme] language = "en" features = [ From ac2a0dcdb8922ca7eb310ecc2dd32ce3d353e43b Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 15:29:21 +0900 Subject: [PATCH 22/65] chore: remove unsafe argument from check-yaml hook Signed-off-by: Max SCHMELLER --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index aa61a6ce5..09a364580 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -11,7 +11,7 @@ repos: - id: check-toml - id: check-xml - id: check-yaml - args: [--allow-multiple-documents, --unsafe] + args: [--allow-multiple-documents] - id: detect-private-key - id: end-of-file-fixer - id: mixed-line-ending From 38d18704b7229d321dc97999242060caf667de6e Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 15:30:12 +0900 Subject: [PATCH 23/65] docs: remove manual table of contents Signed-off-by: Max SCHMELLER --- docs/integration_guide.md | 9 --------- 1 file changed, 9 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index aed4d2d92..43d177ec7 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -2,15 +2,6 @@ This guide provides instructions for adding support for a new LiDAR sensor to Nebula using the `nebula_sample` package as a template. -## Table of contents - -1. [Architecture overview](#architecture-overview) -2. [Provided components](#provided-components) -3. [Integration workflow](#integration-workflow) -4. [Implementation details](#implementation-details) -5. [Required behaviors](#required-behaviors) -6. [Verification](#verification) - --- ## Architecture overview From 9c0759a9820ac397cb9d7c2ed776ed91b5312e83 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:22:58 +0900 Subject: [PATCH 24/65] docs(api): restore C++ API docs for sample --- scripts/mkdoxy_gen.yml | 16 ++++++++++++++++ zensical.toml | 26 ++++++++++++++++++++++++++ 2 files changed, 42 insertions(+) diff --git a/scripts/mkdoxy_gen.yml b/scripts/mkdoxy_gen.yml index cbfe0abed..09749885e 100644 --- a/scripts/mkdoxy_gen.yml +++ b/scripts/mkdoxy_gen.yml @@ -79,6 +79,22 @@ plugins: ENABLE_PREPROCESSING: true MACRO_EXPANSION: true + sample: + src-dirs: > + src/nebula_sample/nebula_sample_common/include + src/nebula_sample/nebula_sample_decoders/include + src/nebula_sample/nebula_sample_hw_interfaces/include + src/nebula_sample/nebula_sample/include + api-path: . + full-doc: true + doxy-cfg: + FILE_PATTERNS: "*.hpp *.h" + RECURSIVE: true + EXTRACT_ALL: true + INLINE_SOURCES: true + ENABLE_PREPROCESSING: true + MACRO_EXPANSION: true + save-api: .mkdoxy full-doc: true debug: false diff --git a/zensical.toml b/zensical.toml index f3b4b381a..8df0ceb8d 100644 --- a/zensical.toml +++ b/zensical.toml @@ -186,6 +186,32 @@ nav = [ { "Macros" = "continental/macros.md" }, { "Files" = "continental/files.md" }, ] }, + + { "Sample" = [ + { "Links" = "sample/links.md" }, + { "Classes" = [ + { "Class List" = "sample/annotated.md" }, + { "Class Index" = "sample/classes.md" }, + { "Class Hierarchy" = "sample/hierarchy.md" }, + { "Class Members" = "sample/class_members.md" }, + { "Class Member Functions" = "sample/class_member_functions.md" }, + { "Class Member Variables" = "sample/class_member_variables.md" }, + { "Class Member Typedefs" = "sample/class_member_typedefs.md" }, + { "Class Member Enumerations" = "sample/class_member_enums.md" }, + ] }, + { "Namespaces" = [ + { "Namespace List" = "sample/namespaces.md" }, + { "Namespace Members" = "sample/namespace_members.md" }, + { "Namespace Member Functions" = "sample/namespace_member_functions.md" }, + { "Namespace Member Variables" = "sample/namespace_member_variables.md" }, + { "Namespace Member Typedefs" = "sample/namespace_member_typedefs.md" }, + { "Namespace Member Enumerations" = "sample/namespace_member_enums.md" }, + ] }, + { "Functions" = "sample/functions.md" }, + { "Variables" = "sample/variables.md" }, + { "Macros" = "sample/macros.md" }, + { "Files" = "sample/files.md" }, + ] }, ] }, ] From 799920ee6a71f1f9bb2ae9d12c87a587402a20fb Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:23:25 +0900 Subject: [PATCH 25/65] docs(integration): drop redundant divider --- docs/integration_guide.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 43d177ec7..b7041090f 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -2,8 +2,6 @@ This guide provides instructions for adding support for a new LiDAR sensor to Nebula using the `nebula_sample` package as a template. ---- - ## Architecture overview ### Overall package structure From 243a88ba31f209de769feb53a0e2dade8974f548 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:24:45 +0900 Subject: [PATCH 26/65] docs(integration): align architecture with current driver/wrapper structure --- docs/integration_guide.md | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index b7041090f..2bdb4a66f 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -40,39 +40,38 @@ graph TB - **Common packages** provide reusable functionality (UDP sockets, point types, etc.) - **Vendor packages** implement vendor-specific logic (packet parsing, calibration) -- All vendor packages follow the **same 4-layer structure** for consistency +- Vendor packages are typically split into **common / decoders / hw_interfaces / ROS wrapper** - Vendor packages **depend on** common packages but not on each other ### Layered architecture (per vendor) -Each vendor package uses a layered architecture to separate functional blocks and promote code reuse: +Each vendor package uses a layered architecture to separate functional blocks and promote code reuse. +In the current Nebula codebase, most orchestration ("driver" responsibilities) is implemented in the +ROS wrapper / decoder wrapper, while the `*_decoders` library focuses on packet decoding: ```mermaid graph TD %% --- NODES --- %% Apply classes using the triple colon syntax (:::className) - Wrapper[ROS 2 wrapper layer
Parameter handling, ROS message conversion, publishing] - - Driver[Driver layer
High-level API, manages decoder and HW interface] + Wrapper[ROS 2 wrapper / decoder wrapper
Parameters, lifecycle, publishing, orchestration] Decoder[Decoder layer
Packet parsing
point cloud generation] HW[HW interface
UDP/TCP communication
socket management] %% --- RELATIONSHIPS --- - Wrapper --> Driver - Driver --> Decoder - Driver --> HW + Wrapper --> Decoder + Wrapper --> HW ``` ### Data flow -1. **Hardware interface** receives raw UDP packets from the sensor and defines TCP protocols for communication and configuration -2. **Driver** receives packets and delegates to **Decoder** -3. **Decoder** parses packets, accumulates points, detects scan completion, and calls callback with complete point cloud +1. **HW interface** receives raw UDP packets from the sensor +2. **ROS wrapper / decoder wrapper** receives packets and delegates to the **Decoder** +3. **Decoder** parses packets, accumulates points, detects scan completion, and calls a callback with a complete point cloud 4. **ROS wrapper** converts to ROS message and publishes --- From 59c85c448e470302ef3009e44d5346c05b33f08e Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:25:14 +0900 Subject: [PATCH 27/65] docs(integration): adjust rename checklist to match current patterns --- docs/integration_guide.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 2bdb4a66f..076c4f7fd 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -245,7 +245,7 @@ Rename all occurrences of "sample"/"Sample" to your vendor name: - **Directories**: `nebula_sample_*` → `nebula_myvendor_*` - **Files**: `sample_*.{cpp,hpp}` → `myvendor_*.{cpp,hpp}` -- **Classes**: `SampleDriver` → `MyVendorDriver` +- **Classes**: Rename wrapper / decoder / HW interface classes (e.g., `SampleRosWrapper`, `SampleDecoder`, `SampleHwInterface`) - **Namespaces**: Update in all files - **CMake/package**: Update `CMakeLists.txt` and `package.xml` From b0bf9bace844d420ea46d9d421683e376a95b3c0 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:34:56 +0900 Subject: [PATCH 28/65] docs(integration): use real UdpSocket API via snippets --- docs/integration_guide.md | 10 +---- .../CMakeLists.txt | 9 +++++ .../examples/udp_socket_usage_example.cpp | 38 +++++++++++++++++++ zensical.toml | 2 + 4 files changed, 50 insertions(+), 9 deletions(-) create mode 100644 src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 076c4f7fd..286a634b6 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -94,15 +94,7 @@ Nebula provides reusable components to simplify sensor integration. You should u **Usage**: ```cpp -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" - -// In your HW interface: -connections::UdpSocket udp_socket_; - -// Start receiving: -udp_socket_.open(host_ip, port); -udp_socket_.bind(); -udp_socket_.asyncReceive(callback); +--8<-- "../../src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp" ``` ### 2. Status codes diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt b/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt index 3172db715..89f483d93 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt +++ b/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt @@ -15,6 +15,15 @@ ament_target_dependencies( nebula_sample_hw_interfaces PUBLIC boost_tcp_driver nebula_core_common nebula_sample_common nebula_core_hw_interfaces) +if(BUILD_TESTING OR BUILD_EXAMPLES) + add_library( + nebula_sample_hw_interfaces_examples OBJECT + examples/udp_socket_usage_example.cpp) + target_link_libraries( + nebula_sample_hw_interfaces_examples + PRIVATE nebula_core_hw_interfaces::nebula_core_hw_interfaces) +endif() + install(TARGETS nebula_sample_hw_interfaces EXPORT export_nebula_sample_hw_interfaces) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp b/src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp new file mode 100644 index 000000000..2219ab405 --- /dev/null +++ b/src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp @@ -0,0 +1,38 @@ +// 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_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" + +#include +#include + +namespace nebula::drivers::examples +{ + +void udp_socket_usage_example() +{ + // This file is intended to be compiled (via CMake) to keep documentation snippets up-to-date. + // It does not need to be executed. + auto socket = connections::UdpSocket::Builder("0.0.0.0", static_cast(0)).bind(); + + socket.subscribe( + [](const std::vector & data, const connections::UdpSocket::RxMetadata & metadata) { + (void)data; + (void)metadata; + }); + + socket.unsubscribe(); +} + +} // namespace nebula::drivers::examples diff --git a/zensical.toml b/zensical.toml index 8df0ceb8d..56b137683 100644 --- a/zensical.toml +++ b/zensical.toml @@ -237,6 +237,8 @@ custom_fences = [ { "name" = "mermaid", "class" = "mermaid", "format" = "pymdownx.superfences.fence_code_format" }, ] +[project.markdown_extensions.pymdownx.snippets] + [project.markdown_extensions.pymdownx.arithmatex] generic = true From dad61cb87628b9829498459fb11cfeb1a69c3108 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:35:30 +0900 Subject: [PATCH 29/65] docs(integration): recommend util::expected for errors --- docs/integration_guide.md | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 286a634b6..e117ca48b 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -119,6 +119,23 @@ nebula::Status validate_config() { } ``` +For new code, prefer returning values via `nebula::util::expected` (instead of passing error +codes around separately). This keeps APIs explicit about what can fail: + +```cpp +#include "nebula_core_common/nebula_status.hpp" +#include "nebula_core_common/util/expected.hpp" + +#include + +nebula::util::expected validate_config() { + if (!config_valid) { + return Status::SENSOR_CONFIG_ERROR; + } + return std::monostate{}; +} +``` + ### 3. Point cloud types **Location**: `nebula_core_common/include/nebula_core_common/point_types.hpp` From b954c10631a5ee78049a804d21c18e8716079376 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:35:56 +0900 Subject: [PATCH 30/65] docs(integration): clarify azimuth wraparound handling --- docs/integration_guide.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index e117ca48b..c14d8752a 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -364,6 +364,7 @@ public: } // 4. Detect scan completion (azimuth wrap) + // Note: comparing to the previous azimuth handles wrap-around at the 0/360 boundary. bool scan_complete = (azimuth < last_azimuth_); last_azimuth_ = azimuth; From 60e27d44c3c98126b6644c421c7991b69218f292 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:36:08 +0900 Subject: [PATCH 31/65] docs(integration): document angle normalization utility --- docs/integration_guide.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index c14d8752a..9b0d7762a 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -176,6 +176,10 @@ cloud->push_back(point); float azimuth_rad = deg2rad(azimuth_deg); bool in_fov = angle_is_between(fov_min, fov_max, azimuth_rad); + +// Normalize any angle to [0, 360) or [0, 2pi) (depending on the chosen max_angle): +float azimuth_deg_norm = normalize_angle(azimuth_deg, 360.0f); +float azimuth_rad_norm = normalize_angle(azimuth_rad, static_cast(Radians::circle_modulus)); ``` ### 5. Configuration base classes From 46599ba07951c98da756f4547ab625f7e2a89fad Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:36:31 +0900 Subject: [PATCH 32/65] docs(integration): explain logger dependency injection --- docs/integration_guide.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 9b0d7762a..47c2dd0eb 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -211,6 +211,10 @@ struct MySensorConfiguration : public LidarConfigurationBase { - ROS 2 logger wrapper (`RclcppLogger`) - Macros: `NEBULA_LOG_STREAM(logger->info, "message")` +This is a dependency-injection pattern: non-ROS (ROS-independent) modules log through the generic +`loggers::Logger` interface, while the ROS wrapper can provide a `RclcppLogger` implementation so +those modules still log into ROS 2. + **Usage**: ```cpp From 46356af1f25192dd67885a4d612af08253345dbf Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:37:08 +0900 Subject: [PATCH 33/65] docs(integration): showcase RateBoundStatus and LivenessMonitor --- docs/integration_guide.md | 35 ++++++++++++++++++++++++++++++----- 1 file changed, 30 insertions(+), 5 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 47c2dd0eb..0a1947050 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -226,21 +226,46 @@ NEBULA_LOG_STREAM(logger_->error, "Failed to parse packet"); ### 7. Diagnostic integration -**Location**: ROS 2 `diagnostic_updater` package (used in ROS wrapper) +**Location**: + +- `nebula_core_ros/include/nebula_core_ros/diagnostics/*` +- ROS 2 `diagnostic_updater` package (used in ROS wrapper) **What it provides**: -- Automatic diagnostic publishing -- Status monitoring (OK, WARN, ERROR) -- Rate monitoring for scan frequency +- Diagnostic task helpers used across Nebula (e.g., `RateBoundStatus`, `LivenessMonitor`) +- Consistent patterns for publish-rate and liveness monitoring **Usage**: ```cpp #include +#include "nebula_core_ros/diagnostics/liveness_monitor.hpp" +#include "nebula_core_ros/diagnostics/rate_bound_status.hpp" + diagnostic_updater::Updater diagnostic_updater_; -// Add diagnostics in your ROS wrapper + +// Monitor publish rate (tick this on publish) +custom_diagnostic_tasks::RateBoundStatus publish_rate_diag_{ + this, + custom_diagnostic_tasks::RateBoundStatusParam{9.0, 11.0}, // OK range + custom_diagnostic_tasks::RateBoundStatusParam{8.0, 12.0}, // WARN range + 5, // hysteresis frames + false // immediate error report +}; + +// Monitor liveness (tick this on packet receive / decode loop) +nebula::ros::LivenessMonitor liveness_diag_{ + "packet receive", this, rclcpp::Duration::from_seconds(1.0)}; + +// Register tasks +diagnostic_updater_.add(publish_rate_diag_); +diagnostic_updater_.add(liveness_diag_); + +// In your code paths: +// publish_rate_diag_.tick(); +// liveness_diag_.tick(); ``` --- From 3f9ab20097c63bf85fc8870fe1519660fa0bea39 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:37:38 +0900 Subject: [PATCH 34/65] docs(integration): drop heading numbering --- docs/integration_guide.md | 54 +++++++++++++++++++-------------------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 0a1947050..8b3672232 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -80,7 +80,7 @@ graph TD Nebula provides reusable components to simplify sensor integration. You should use these instead of implementing from scratch. -### 1. UDP socket handling +### UDP socket handling **Location**: `nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp` @@ -97,7 +97,7 @@ Nebula provides reusable components to simplify sensor integration. You should u --8<-- "../../src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp" ``` -### 2. Status codes +### Status codes **Location**: `nebula_core_common/include/nebula_core_common/nebula_status.hpp` @@ -136,7 +136,7 @@ nebula::util::expected validate_config() { } ``` -### 3. Point cloud types +### Point cloud types **Location**: `nebula_core_common/include/nebula_core_common/point_types.hpp` @@ -159,7 +159,7 @@ point.z = distance * sin_elevation; cloud->push_back(point); ``` -### 4. Angle utilities +### Angle utilities **Location**: `nebula_core_decoders/include/nebula_core_decoders/angles.hpp` @@ -182,7 +182,7 @@ float azimuth_deg_norm = normalize_angle(azimuth_deg, 360.0f); float azimuth_rad_norm = normalize_angle(azimuth_rad, static_cast(Radians::circle_modulus)); ``` -### 5. Configuration base classes +### Configuration base classes **Location**: `nebula_core_common/include/nebula_core_common/nebula_common.hpp` @@ -201,7 +201,7 @@ struct MySensorConfiguration : public LidarConfigurationBase { }; ``` -### 6. Logger integration +### Logger integration **Location**: `nebula_core_common/include/nebula_core_common/loggers/logger.hpp` @@ -224,7 +224,7 @@ std::shared_ptr logger_; NEBULA_LOG_STREAM(logger_->error, "Failed to parse packet"); ``` -### 7. Diagnostic integration +### Diagnostic integration **Location**: @@ -310,11 +310,11 @@ See [Verification](#verification) below. ## Implementation details -### A. Common package (`nebula_myvendor_common`) +### Common package (`nebula_myvendor_common`) **Purpose**: Define configuration and calibration structures. -#### 1. Sensor configuration +#### Sensor configuration Edit `include/nebula_myvendor_common/myvendor_common.hpp`: @@ -334,7 +334,7 @@ struct MyVendorSensorConfiguration : public LidarConfigurationBase { }; ``` -#### 2. Calibration configuration (optional) +#### Calibration configuration (optional) Only implement if your sensor needs calibration data: @@ -351,15 +351,15 @@ struct MyVendorCalibrationConfiguration : public CalibrationConfigurationBase { }; ``` -### B. Decoders package (`nebula_myvendor_decoders`) +### Decoders package (`nebula_myvendor_decoders`) **Purpose**: Parse packets and generate point clouds. -#### 1. Decoder interface +#### Decoder interface The interface is already defined in `myvendor_scan_decoder.hpp`. You don't need to modify it. -#### 2. Decoder implementation +#### Decoder implementation Edit `include/nebula_myvendor_decoders/decoders/myvendor_decoder.hpp`: @@ -423,7 +423,7 @@ private: }; ``` -#### 3. Driver implementation +#### Driver implementation The driver is a thin wrapper. Edit `src/myvendor_driver.cpp`: @@ -441,7 +441,7 @@ PacketDecodeResult MyVendorDriver::parse_cloud_packet( } ``` -### C. HW interfaces package (`nebula_myvendor_hw_interfaces`) +### HW interfaces package (`nebula_myvendor_hw_interfaces`) **Purpose**: Manage UDP communication with the sensor. @@ -483,7 +483,7 @@ Status MyVendorHwInterface::sensor_interface_stop() { } ``` -### D. ROS wrapper package (`nebula_myvendor`) +### ROS wrapper package (`nebula_myvendor`) **Purpose**: Bridge C++ driver with ROS 2. @@ -551,7 +551,7 @@ Status MyVendorRosWrapper::stream_start() { Your sensor integration must implement these behaviors correctly. -### 1. Startup sequence +### Startup sequence **Order of operations**: @@ -589,7 +589,7 @@ try { stream_start(); ``` -### 2. Reconfiguration +### Reconfiguration **When parameters change at runtime**: @@ -629,7 +629,7 @@ rcl_interfaces::msg::SetParametersResult on_parameter_change( } ``` -### 3. Connection loss handling +### Connection loss handling **Detect and handle sensor disconnection**: @@ -667,7 +667,7 @@ void check_connection() { } ``` -### 4. Shutdown sequence +### Shutdown sequence **Order of operations**: @@ -689,7 +689,7 @@ void check_connection() { } ``` -### 5. Diagnostic reporting +### Diagnostic reporting **Required diagnostic information**: @@ -727,20 +727,20 @@ void check_scan_rate(diagnostic_updater::DiagnosticStatusWrapper & stat) { ## Verification -### 1. Build +### Build ```bash colcon build --packages-up-to nebula_myvendor source install/setup.bash ``` -### 2. Launch +### Launch ```bash ros2 launch nebula_myvendor nebula_myvendor.launch.xml ``` -### 3. Verify topics +### Verify topics ```bash # List topics @@ -753,7 +753,7 @@ ros2 topic echo /points --field header ros2 topic hz /points ``` -### 4. Visualize +### Visualize ```bash rviz2 @@ -762,13 +762,13 @@ rviz2 # Set Fixed Frame to your frame_id ``` -### 5. Check diagnostics +### Check diagnostics ```bash ros2 topic echo /diagnostics ``` -### 6. Test with PCAP (offline) +### Test with PCAP (offline) ```bash # Set launch_hw:=false to use PCAP playback From 1b5e5ee275d981c95e69159c552a9885f21112b3 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:38:27 +0900 Subject: [PATCH 35/65] docs(integration): render wrapper example via snippets --- docs/integration_guide.md | 54 +-------------------------------------- 1 file changed, 1 insertion(+), 53 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 8b3672232..a08499657 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -490,59 +490,7 @@ Status MyVendorHwInterface::sensor_interface_stop() { Edit `src/myvendor_ros_wrapper.cpp`: ```cpp -MyVendorRosWrapper::MyVendorRosWrapper(const rclcpp::NodeOptions & options) -: Node("nebula_myvendor_ros_wrapper", options) { - - // 1. Declare ROS parameters - declare_parameter("sensor_ip", "192.168.1.201"); - declare_parameter("host_ip", "192.168.1.100"); - declare_parameter("data_port", 2368); - declare_parameter("frame_id", "myvendor_lidar"); - declare_parameter("launch_hw", true); - - // 2. Create sensor configuration - sensor_cfg_ptr_ = std::make_shared(); - sensor_cfg_ptr_->sensor_ip = get_parameter("sensor_ip").as_string(); - sensor_cfg_ptr_->host_ip = get_parameter("host_ip").as_string(); - sensor_cfg_ptr_->data_port = get_parameter("data_port").as_int(); - sensor_cfg_ptr_->frame_id = get_parameter("frame_id").as_string(); - launch_hw_ = get_parameter("launch_hw").as_bool(); - - // 3. Initialize driver - driver_ptr_ = std::make_unique(sensor_cfg_ptr_); - - // 4. Register pointcloud callback - driver_ptr_->set_pointcloud_callback( - [this](const NebulaPointCloudPtr & cloud, double timestamp_s) { - auto ros_msg = std::make_unique(); - pcl::toROSMsg(*cloud, *ros_msg); - ros_msg->header.stamp = rclcpp::Time(timestamp_s * 1e9); - ros_msg->header.frame_id = sensor_cfg_ptr_->frame_id; - points_pub_->publish(std::move(ros_msg)); - }); - - // 5. Initialize HW interface - if (launch_hw_) { - hw_interface_ptr_ = std::make_unique(); - hw_interface_ptr_->set_sensor_configuration(sensor_cfg_ptr_); - hw_interface_ptr_->register_scan_callback( - [this](const std::vector & packet, const auto & metadata) { - (void)metadata; - driver_ptr_->parse_cloud_packet(packet); - }); - } - - // 6. Create publisher - points_pub_ = create_publisher( - "points", rclcpp::SensorDataQoS()); -} - -Status MyVendorRosWrapper::stream_start() { - if (hw_interface_ptr_) { - return hw_interface_ptr_->sensor_interface_start(); - } - return Status::OK; -} +--8<-- "../../src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp" ``` --- From d012d5dddb6ab252aa78dc94fe64275f339280d1 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:38:44 +0900 Subject: [PATCH 36/65] docs(nav): move integration guide under contributing --- zensical.toml | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/zensical.toml b/zensical.toml index 56b137683..2cd1c972d 100644 --- a/zensical.toml +++ b/zensical.toml @@ -51,12 +51,9 @@ nav = [ { "Design" = "design.md" }, { "Scan cutting" = "design/scan_cutting.md" }, ] }, - { "Tutorials" = [ - { "Tutorials" = "tutorials.md" }, - { "Integration Guide" = "integration_guide.md" }, - ] }, { "Contributing" = [ { "Contributing to Nebula" = "contributing/index.md" }, + { "Integration Guide" = "integration_guide.md" }, { "Benchmarking" = "contributing/benchmarking.md" }, ] }, { "API Reference" = [ From 0c611a23f64eb64df1342e35ee46b3d1c9b3b0c4 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:40:13 +0900 Subject: [PATCH 37/65] docs(integration): avoid connection-loss log spam --- docs/integration_guide.md | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index a08499657..ea9c2a942 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -583,7 +583,7 @@ rcl_interfaces::msg::SetParametersResult on_parameter_change( 1. **Timeout detection**: Monitor time since last packet 2. **Diagnostic update**: Set diagnostic status to ERROR -3. **Logging**: Log connection loss with timestamp +3. **Logging**: Log connection loss only on state changes (avoid log spam) 4. **Recovery**: Optionally attempt reconnection **Example**: @@ -601,8 +601,11 @@ void check_connection() { auto time_since_last_packet = now() - last_packet_time_; if (time_since_last_packet > timeout_threshold_) { - RCLCPP_ERROR(get_logger(), "Connection lost - no packets for %.1fs", - time_since_last_packet.seconds()); + if (!connection_lost_) { + connection_lost_ = true; + RCLCPP_WARN( + get_logger(), "Connection lost - no packets for %.1fs", time_since_last_packet.seconds()); + } // Update diagnostics diagnostic_updater_.broadcast( From 463ef1934ebead24f005e36867f09c9fa1ddace4 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:40:26 +0900 Subject: [PATCH 38/65] docs(integration): require transparent recovery on disconnect --- docs/integration_guide.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index ea9c2a942..fbbc8d09d 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -584,7 +584,7 @@ rcl_interfaces::msg::SetParametersResult on_parameter_change( 1. **Timeout detection**: Monitor time since last packet 2. **Diagnostic update**: Set diagnostic status to ERROR 3. **Logging**: Log connection loss only on state changes (avoid log spam) -4. **Recovery**: Optionally attempt reconnection +4. **Recovery**: Attempt reconnection transparently (with backoff) **Example**: @@ -612,7 +612,7 @@ void check_connection() { diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Connection lost"); - // Optionally attempt reconnection + // Attempt reconnection (ideally transparent to the user) attempt_reconnection(); } } From 45a21b83902e8d8d2200bd8393fdc8768075ca19 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:40:44 +0900 Subject: [PATCH 39/65] docs(integration): emphasize RAII for shutdown --- docs/integration_guide.md | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index fbbc8d09d..bf8aeb48c 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -622,10 +622,13 @@ void check_connection() { **Order of operations**: -1. **Stop stream**: Call `sensor_interface_stop()` +Prefer RAII-based shutdown: sockets/threads/buffers should be owned by objects whose destructors +stop/join/close automatically, so the wrapper does not require sensor-specific shutdown logic. + +1. **Stop stream**: Ensure receiver threads stop and join 2. **Close sockets**: Ensure all network resources are closed -3. **Clear buffers**: Release point cloud buffers -4. **Reset pointers**: Reset shared_ptr members +3. **Release buffers**: Release point cloud buffers +4. **Destroy owners**: Destroy the owning objects (RAII) **Example**: From d10dc5a4d5990b3421178fc8818b13fd681d7f98 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:40:57 +0900 Subject: [PATCH 40/65] docs(integration): focus diagnostics on core helpers --- docs/integration_guide.md | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index bf8aeb48c..2fda286dd 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -647,10 +647,9 @@ stop/join/close automatically, so the wrapper does not require sensor-specific s **Required diagnostic information**: -- **Packet rate**: Packets received per second -- **Scan rate**: Complete scans per second (should match rotation speed) -- **Connection status**: OK / WARN / ERROR -- **Decode errors**: Count of failed packet parses +- **Publish rate**: Use `custom_diagnostic_tasks::RateBoundStatus` +- **Liveness**: Use `nebula::ros::LivenessMonitor` +- **Debug timings**: Publish receive/decode/publish durations for profiling **Example**: From 3355f58704d69e1892871614faba07a45cb9a146 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:41:13 +0900 Subject: [PATCH 41/65] docs(integration): correct offline test to rosbag2 --- docs/integration_guide.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 2fda286dd..ff7b9c110 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -721,13 +721,13 @@ rviz2 ros2 topic echo /diagnostics ``` -### Test with PCAP (offline) +### Test with Rosbag (offline) ```bash -# Set launch_hw:=false to use PCAP playback +# Set launch_hw:=false to use rosbag replay ros2 launch nebula_myvendor nebula_myvendor.launch.xml launch_hw:=false -# In another terminal, play PCAP +# In another terminal, play a ROS 2 bag ros2 bag play your_sensor_data.bag ``` From 80a49bbfe1c11c9f335bed351632417cc703f61e Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:41:39 +0900 Subject: [PATCH 42/65] docs: add pymdown-extensions for tasklists --- docs/requirements.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/requirements.txt b/docs/requirements.txt index f6afb57ee..83e54680d 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,6 +1,7 @@ mkdocs>=1.6.1,<2.0.0 mkdocs-drawio>=1.13.0,<2.0.0 mkdoxy>=1.2.8,<2.0.0 +pymdown-extensions zensical>=0.0.15,<1.0.0 tabulate lxml From 112271381783e370837b369741424ccabb0d6463 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:47:08 +0900 Subject: [PATCH 43/65] docs(integration): remove horizontal rules --- docs/integration_guide.md | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index ff7b9c110..e10301766 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -74,8 +74,6 @@ graph TD 3. **Decoder** parses packets, accumulates points, detects scan completion, and calls a callback with a complete point cloud 4. **ROS wrapper** converts to ROS message and publishes ---- - ## Provided components Nebula provides reusable components to simplify sensor integration. You should use these instead of implementing from scratch. @@ -268,8 +266,6 @@ diagnostic_updater_.add(liveness_diag_); // liveness_diag_.tick(); ``` ---- - ## Integration workflow ### Step 1: Clone and rename template @@ -306,8 +302,6 @@ See [Implementation details](#implementation-details) below. See [Verification](#verification) below. ---- - ## Implementation details ### Common package (`nebula_myvendor_common`) @@ -493,8 +487,6 @@ Edit `src/myvendor_ros_wrapper.cpp`: --8<-- "../../src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp" ``` ---- - ## Required behaviors Your sensor integration must implement these behaviors correctly. @@ -676,8 +668,6 @@ void check_scan_rate(diagnostic_updater::DiagnosticStatusWrapper & stat) { } ``` ---- - ## Verification ### Build @@ -731,8 +721,6 @@ ros2 launch nebula_myvendor nebula_myvendor.launch.xml launch_hw:=false ros2 bag play your_sensor_data.bag ``` ---- - ## Integration checklist - [ ] Cloned and renamed all files and directories @@ -753,8 +741,6 @@ ros2 bag play your_sensor_data.bag - [ ] Verified scan rate matches expected - [ ] Tested with real sensor or PCAP data ---- - ## Additional resources - **Hesai implementation**: `src/nebula_hesai` - Full reference implementation From 646a96c3a98ad073c77acbb95b615418cc37b7a5 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 4 Feb 2026 16:48:07 +0900 Subject: [PATCH 44/65] docs(integration): make startup exceptions optional --- docs/integration_guide.md | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index e10301766..5eda75549 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -505,27 +505,18 @@ Your sensor integration must implement these behaviors correctly. **Error handling**: -- If any step fails, log error and throw exception +- If any step fails, log the error and stop initialization (throwing exceptions is optional) - Do not proceed to next step if previous failed - Clean up resources on failure **Example**: ```cpp -// In constructor -try { - load_parameters(); - validate_configuration(); - initialize_driver(); - register_callbacks(); - initialize_hw_interface(); - create_publishers(); -} catch (const std::exception & e) { - RCLCPP_ERROR(get_logger(), "Initialization failed: %s", e.what()); - throw; +if (!initialize()) { + RCLCPP_ERROR(get_logger(), "Initialization failed"); + return; } -// In separate method or after construction stream_start(); ``` From b57820b6ca16dc661ea1252eaae3ef31bd8fa62f Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 5 Feb 2026 09:56:33 +0900 Subject: [PATCH 45/65] chore: remove nebula_common directory Signed-off-by: Max SCHMELLER --- docs/integration_guide.md | 29 ++++++++++----- .../continental_ars548_hw_interface.hpp | 2 +- .../continental_srr520_hw_interface.hpp | 2 +- .../src/continental_ars548_hw_interface.cpp | 2 +- .../nebula_core_hw_interfaces/CMakeLists.txt | 7 ++++ .../examples/udp_socket_usage_example.cpp | 37 +++++++++++++++++++ .../connections/udp.hpp | 0 .../nebula_hw_interface_base.hpp | 0 .../test/common/test_udp.cpp | 2 +- .../test/common/test_udp/utils.hpp | 2 +- .../nebula_hesai/hesai_ros_wrapper.hpp | 2 +- .../hesai_hw_interface.hpp | 2 +- .../src/hesai_hw_interface.cpp | 2 +- .../examples/udp_socket_usage_example.cpp | 2 +- .../sample_hw_interface.hpp | 3 +- .../velodyne_hw_interface.hpp | 4 +- zensical.toml | 2 + 17 files changed, 78 insertions(+), 22 deletions(-) create mode 100644 src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp rename src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/{nebula_hw_interfaces_common => }/connections/udp.hpp (100%) rename src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/{nebula_hw_interfaces_common => }/nebula_hw_interface_base.hpp (100%) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 5eda75549..af79c11bf 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -1,6 +1,7 @@ # New sensor integration guide -This guide provides instructions for adding support for a new LiDAR sensor to Nebula using the `nebula_sample` package as a template. +This guide provides instructions for adding support for a new LiDAR sensor to Nebula using the +`nebula_sample` package as a template. ## Architecture overview @@ -80,7 +81,7 @@ Nebula provides reusable components to simplify sensor integration. You should u ### UDP socket handling -**Location**: `nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp` +**Location**: `src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp` **What it provides**: @@ -92,7 +93,7 @@ Nebula provides reusable components to simplify sensor integration. You should u **Usage**: ```cpp ---8<-- "../../src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp" +--8<-- "src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp:udp_socket_usage" ``` ### Status codes @@ -121,15 +122,25 @@ For new code, prefer returning values via `nebula::util::expected` (instea codes around separately). This keeps APIs explicit about what can fail: ```cpp -#include "nebula_core_common/nebula_status.hpp" #include "nebula_core_common/util/expected.hpp" -#include +enum class ValidateConfigError { + MissingField, + OutOfRange, + // ... +}; -nebula::util::expected validate_config() { - if (!config_valid) { - return Status::SENSOR_CONFIG_ERROR; +nebula::util::expected validate_config( + const Config & config + ) { + if (config.required_field.empty()) { + return ValidateConfigError::MissingField; } + + if (config.percentage_field < 0 || config.percentage_field > 100) { + return ValidateConfigError::OutOfRange; + } + return std::monostate{}; } ``` @@ -484,7 +495,7 @@ Status MyVendorHwInterface::sensor_interface_stop() { Edit `src/myvendor_ros_wrapper.cpp`: ```cpp ---8<-- "../../src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp" +--8<-- "src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp" ``` ## Required behaviors diff --git a/src/nebula_continental/nebula_continental_hw_interfaces/include/nebula_continental_hw_interfaces/continental_ars548_hw_interface.hpp b/src/nebula_continental/nebula_continental_hw_interfaces/include/nebula_continental_hw_interfaces/continental_ars548_hw_interface.hpp index 76168280d..39231ae06 100644 --- a/src/nebula_continental/nebula_continental_hw_interfaces/include/nebula_continental_hw_interfaces/continental_ars548_hw_interface.hpp +++ b/src/nebula_continental/nebula_continental_hw_interfaces/include/nebula_continental_hw_interfaces/continental_ars548_hw_interface.hpp @@ -16,7 +16,7 @@ #define NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H #include "nebula_core_common/nebula_status.hpp" -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" +#include "nebula_core_hw_interfaces/connections/udp.hpp" #include #include diff --git a/src/nebula_continental/nebula_continental_hw_interfaces/include/nebula_continental_hw_interfaces/continental_srr520_hw_interface.hpp b/src/nebula_continental/nebula_continental_hw_interfaces/include/nebula_continental_hw_interfaces/continental_srr520_hw_interface.hpp index 29c4d46dd..65d7c4df5 100644 --- a/src/nebula_continental/nebula_continental_hw_interfaces/include/nebula_continental_hw_interfaces/continental_srr520_hw_interface.hpp +++ b/src/nebula_continental/nebula_continental_hw_interfaces/include/nebula_continental_hw_interfaces/continental_srr520_hw_interface.hpp @@ -15,7 +15,7 @@ #ifndef NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H #define NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" +#include "nebula_core_hw_interfaces/nebula_hw_interface_base.hpp" #include #include diff --git a/src/nebula_continental/nebula_continental_hw_interfaces/src/continental_ars548_hw_interface.cpp b/src/nebula_continental/nebula_continental_hw_interfaces/src/continental_ars548_hw_interface.cpp index ff3362dbf..846c2eae3 100644 --- a/src/nebula_continental/nebula_continental_hw_interfaces/src/continental_ars548_hw_interface.cpp +++ b/src/nebula_continental/nebula_continental_hw_interfaces/src/continental_ars548_hw_interface.cpp @@ -15,7 +15,7 @@ #include "nebula_continental_hw_interfaces/continental_ars548_hw_interface.hpp" -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" +#include "nebula_core_hw_interfaces/connections/udp.hpp" #include diff --git a/src/nebula_core/nebula_core_hw_interfaces/CMakeLists.txt b/src/nebula_core/nebula_core_hw_interfaces/CMakeLists.txt index 32f4c5a4a..95f87a323 100644 --- a/src/nebula_core/nebula_core_hw_interfaces/CMakeLists.txt +++ b/src/nebula_core/nebula_core_hw_interfaces/CMakeLists.txt @@ -30,6 +30,13 @@ if(BUILD_TESTING) target_link_libraries(test_udp nebula_core_common::nebula_core_common) endif() +if(BUILD_TESTING OR BUILD_EXAMPLES) + add_library(nebula_core_hw_interfaces_examples OBJECT + examples/udp_socket_usage_example.cpp) + target_link_libraries(nebula_core_hw_interfaces_examples + PRIVATE nebula_core_hw_interfaces) +endif() + ament_export_targets(export_nebula_core_hw_interfaces) ament_export_dependencies(nebula_core_common) diff --git a/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp b/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp new file mode 100644 index 000000000..5a38f83a1 --- /dev/null +++ b/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp @@ -0,0 +1,37 @@ +// 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_core_hw_interfaces/connections/udp.hpp" + +#include +#include + +int main() +{ + using nebula::drivers::connections::UdpSocket; + // # --8<-- [start:udp_socket_usage] + auto socket = UdpSocket::Builder("192.168.1.10", 9000) + .set_socket_buffer_size(10'000'000) + .limit_to_sender("192.168.10.20", 7000) + .bind(); + + socket.subscribe([](const std::vector & data, const UdpSocket::RxMetadata & metadata) { + (void)data; + (void)metadata; + }); + + socket.unsubscribe(); + // # --8<-- [end:udp_socket_usage] + return 0; +} diff --git a/src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp b/src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/connections/udp.hpp similarity index 100% rename from src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp rename to src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/connections/udp.hpp diff --git a/src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp b/src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interface_base.hpp similarity index 100% rename from src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp rename to src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interface_base.hpp diff --git a/src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp.cpp b/src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp.cpp index ebdd7eeb2..db7eb58a7 100644 --- a/src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp.cpp +++ b/src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp.cpp @@ -2,7 +2,7 @@ #include "common/test_udp/utils.hpp" #include "nebula_core_common/util/expected.hpp" -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" +#include "nebula_core_hw_interfaces/connections/udp.hpp" #include #include diff --git a/src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp/utils.hpp b/src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp/utils.hpp index 9ae90de76..244e4a25a 100644 --- a/src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp/utils.hpp +++ b/src/nebula_core/nebula_core_hw_interfaces/test/common/test_udp/utils.hpp @@ -14,7 +14,7 @@ #pragma once -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" +#include "nebula_core_hw_interfaces/connections/udp.hpp" #include #include diff --git a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hesai_ros_wrapper.hpp b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hesai_ros_wrapper.hpp index 482eba2d8..fc13208a4 100644 --- a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hesai_ros_wrapper.hpp +++ b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hesai_ros_wrapper.hpp @@ -16,7 +16,7 @@ #include "nebula_core_common/nebula_common.hpp" #include "nebula_core_common/nebula_status.hpp" -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" +#include "nebula_core_hw_interfaces/connections/udp.hpp" #include "nebula_core_ros/sync_tooling/sync_tooling_worker.hpp" #include "nebula_hesai/decoder_wrapper.hpp" #include "nebula_hesai/hw_interface_wrapper.hpp" diff --git a/src/nebula_hesai/nebula_hesai_hw_interfaces/include/nebula_hesai_hw_interfaces/hesai_hw_interface.hpp b/src/nebula_hesai/nebula_hesai_hw_interfaces/include/nebula_hesai_hw_interfaces/hesai_hw_interface.hpp index 3bbfeb355..0c75e9c6c 100644 --- a/src/nebula_hesai/nebula_hesai_hw_interfaces/include/nebula_hesai_hw_interfaces/hesai_hw_interface.hpp +++ b/src/nebula_hesai/nebula_hesai_hw_interfaces/include/nebula_hesai_hw_interfaces/hesai_hw_interface.hpp @@ -17,7 +17,7 @@ // Have to define macros to silence warnings about deprecated headers being used by // boost/property_tree/ in some versions of boost. // See: https://github.com/boostorg/property_tree/issues/51 -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" +#include "nebula_core_hw_interfaces/connections/udp.hpp" #include diff --git a/src/nebula_hesai/nebula_hesai_hw_interfaces/src/hesai_hw_interface.cpp b/src/nebula_hesai/nebula_hesai_hw_interfaces/src/hesai_hw_interface.cpp index a80c0b07b..cd588759e 100644 --- a/src/nebula_hesai/nebula_hesai_hw_interfaces/src/hesai_hw_interface.cpp +++ b/src/nebula_hesai/nebula_hesai_hw_interfaces/src/hesai_hw_interface.cpp @@ -5,7 +5,7 @@ #include "nebula_core_common/loggers/logger.hpp" #include "nebula_core_common/nebula_common.hpp" #include "nebula_core_common/nebula_status.hpp" -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" +#include "nebula_core_hw_interfaces/connections/udp.hpp" #include "nebula_hesai_common/hesai_common.hpp" #include "nebula_hesai_common/hesai_status.hpp" #include "nebula_hesai_hw_interfaces/hesai_cmd_response.hpp" diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp b/src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp index 2219ab405..0f76282fd 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" +#include "nebula_core_hw_interfaces/connections/udp.hpp" #include #include diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp index c3b0ffec4..6779f578d 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp @@ -16,11 +16,10 @@ #define NEBULA_SAMPLE_HW_INTERFACE_HPP #include -#include +#include #include #include -#include namespace nebula::drivers { diff --git a/src/nebula_velodyne/nebula_velodyne_hw_interfaces/include/nebula_velodyne_hw_interfaces/velodyne_hw_interface.hpp b/src/nebula_velodyne/nebula_velodyne_hw_interfaces/include/nebula_velodyne_hw_interfaces/velodyne_hw_interface.hpp index 2f37645c3..9f93e4c51 100644 --- a/src/nebula_velodyne/nebula_velodyne_hw_interfaces/include/nebula_velodyne_hw_interfaces/velodyne_hw_interface.hpp +++ b/src/nebula_velodyne/nebula_velodyne_hw_interfaces/include/nebula_velodyne_hw_interfaces/velodyne_hw_interface.hpp @@ -29,12 +29,12 @@ #endif #include "nebula_core_common/util/expected.hpp" -#include "nebula_core_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" +#include "nebula_core_hw_interfaces/nebula_hw_interface_base.hpp" #include #include #include -#include +#include #include #include diff --git a/zensical.toml b/zensical.toml index 2cd1c972d..1040cc5c2 100644 --- a/zensical.toml +++ b/zensical.toml @@ -235,6 +235,8 @@ custom_fences = [ ] [project.markdown_extensions.pymdownx.snippets] +check_paths = true +dedent_subsections = true [project.markdown_extensions.pymdownx.arithmatex] generic = true From 141cf1ea9c47b3838b1f1e8a5c62710c2988b287 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 6 Feb 2026 13:46:09 +0900 Subject: [PATCH 46/65] docs: make integration guide more concise and informative Signed-off-by: Max SCHMELLER --- docs/integration_guide.md | 697 +++++++++----------------------------- 1 file changed, 168 insertions(+), 529 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index af79c11bf..f7c9cdc10 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -1,13 +1,13 @@ -# New sensor integration guide +# Integrating a new Sensor -This guide provides instructions for adding support for a new LiDAR sensor to Nebula using the -`nebula_sample` package as a template. +This guide provides instructions for adding support for a new sensor to Nebula. We provide a +template implementation (`nebula_sample`) and reusable components to simplify the process. ## Architecture overview ### Overall package structure -Nebula is organized into common (reusable) packages and vendor-specific packages: +Nebula is organized into corepackages and vendor-specific packages: ```mermaid graph TB @@ -37,186 +37,128 @@ graph TB ``` -**Key principles**: +- Core packages provide reusable functionality (UDP sockets, point types, etc.) +- Vendor packages implement vendor-specific logic (packet parsing, calibration) +- Vendor packages are typically split into `common`, `decoders`, `hw_interfaces` and + `ROS wrapper`: + - `common`: Vendor-specific types, internal interfaces and utilities + - `decoders`: Packet parsing and conversion to point clouds + - `hw_interfaces`: Network communication + - `ROS wrapper`: ROS 2 node, parameters, publishers, orchestration +- Vendor packages depend on core packages but not on each other -- **Common packages** provide reusable functionality (UDP sockets, point types, etc.) -- **Vendor packages** implement vendor-specific logic (packet parsing, calibration) -- Vendor packages are typically split into **common / decoders / hw_interfaces / ROS wrapper** -- Vendor packages **depend on** common packages but not on each other +Except for the ROS wrappers, no package should depend on ROS 2. This allows users to run parts +of Nebula as a library e.g. in ML pipelines without ROS 2. -### Layered architecture (per vendor) +!!! warning +Existing vendor implementations do not strictly follow this architecture as the project +evolved over time. New implementations should follow the architecture described here. -Each vendor package uses a layered architecture to separate functional blocks and promote code reuse. -In the current Nebula codebase, most orchestration ("driver" responsibilities) is implemented in the -ROS wrapper / decoder wrapper, while the `*_decoders` library focuses on packet decoding: - -```mermaid -graph TD - - %% --- NODES --- - %% Apply classes using the triple colon syntax (:::className) - Wrapper[ROS 2 wrapper / decoder wrapper
Parameters, lifecycle, publishing, orchestration] - - Decoder[Decoder layer
Packet parsing
point cloud generation] - - HW[HW interface
UDP/TCP communication
socket management] - - %% --- RELATIONSHIPS --- - Wrapper --> Decoder - Wrapper --> HW +### Data Flow +Packets flow from the hardware interface to the decoder to the ROS wrapper as follows: +```mermaid +flowchart LR + HW[Hardware Interface] -->| Raw packets | DC[Decoder] + DC -->| Nebula PointCloud | RW[ROS Wrapper] + RW -->| ROS 2 PointCloud2 | ROS[ROS 2] ``` -### Data flow +Since decoder and HW interface are separate libraries, the ROS wrapper sets up the data flow +between them on startup by registering callbacks. -1. **HW interface** receives raw UDP packets from the sensor -2. **ROS wrapper / decoder wrapper** receives packets and delegates to the **Decoder** -3. **Decoder** parses packets, accumulates points, detects scan completion, and calls a callback with a complete point cloud -4. **ROS wrapper** converts to ROS message and publishes +## Component Library -## Provided components - -Nebula provides reusable components to simplify sensor integration. You should use these instead of implementing from scratch. +Nebula provides reusable components to simplify sensor integration. Use these components to +reduce boilerplate code and ensure consistent behavior across sensors and vendors. ### UDP socket handling -**Location**: `src/nebula_core/nebula_core_hw_interfaces/include/nebula_core_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp` +```cpp +--8<-- "src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp:include" +``` **What it provides**: -- Asynchronous UDP socket -- Automatic packet reception loop -- Callback-based packet delivery -- Metadata (timestamp, source IP) for each packet +- UDP socket with easy builder-style configuration +- Threaded packet reception with user-defined callback +- Metadata (socket timestamp, buffer overflow count) for each packet **Usage**: ```cpp ---8<-- "src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp:udp_socket_usage" +--8<-- "src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp:usage" ``` -### Status codes +### Expected -**Location**: `nebula_core_common/include/nebula_core_common/nebula_status.hpp` +```c++ +--8<-- "src/nebula_core/nebula_core_common/examples/expected_usage_example.cpp:include" +``` **What it provides**: -- Standardized error/success codes -- `Status` enum with values like `OK`, `INVALID_CALIBRATION_FILE`, `SENSOR_CONFIG_ERROR`, etc. +For operations that can fail, but should not crash the program, return values via +`nebula::util::expected`. Avoid sentinel return values (e.g., `nullptr`, `-1`) and +exceptions. This keeps APIs explicit about what can fail: **Usage**: ```cpp -#include "nebula_core_common/nebula_status.hpp" - -nebula::Status validate_config() { - if (!config_valid) { - return Status::SENSOR_CONFIG_ERROR; - } - return Status::OK; -} -``` - -For new code, prefer returning values via `nebula::util::expected` (instead of passing error -codes around separately). This keeps APIs explicit about what can fail: - -```cpp -#include "nebula_core_common/util/expected.hpp" - -enum class ValidateConfigError { - MissingField, - OutOfRange, - // ... -}; - -nebula::util::expected validate_config( - const Config & config - ) { - if (config.required_field.empty()) { - return ValidateConfigError::MissingField; - } - - if (config.percentage_field < 0 || config.percentage_field > 100) { - return ValidateConfigError::OutOfRange; - } - - return std::monostate{}; -} +--8<-- "src/nebula_core/nebula_core_common/examples/expected_usage_example.cpp:usage" ``` ### Point cloud types -**Location**: `nebula_core_common/include/nebula_core_common/point_types.hpp` +```c++ +--8<-- "src/nebula_core/nebula_core_ros/examples/point_types_usage_example.cpp:include" +``` **What it provides**: -- `NebulaPoint` - Standard point type with x, y, z, intensity, timestamp, return_type, channel, azimuth, elevation, distance -- `NebulaPointCloud` - PCL point cloud of NebulaPoints +- `NebulaPoint` - Standard point type with x, y, z, intensity, timestamp, return_type, channel, + azimuth, elevation, distance +- `NebulaPointCloud` - Point cloud of NebulaPoints - Conversion utilities to ROS/Autoware point types +Nebula point types are designed for compatibility with and efficiency of downstream tasks. +Since Nebula is the official sensor driver framework for Autoware, using Nebula point types +ensures seamless integration with Autoware components. + **Usage**: ```cpp -#include "nebula_core_common/point_types.hpp" - -NebulaPointCloudPtr cloud = std::make_shared(); -NebulaPoint point; -point.x = distance * sin_azimuth * cos_elevation; -point.y = distance * cos_azimuth * cos_elevation; -point.z = distance * sin_elevation; -cloud->push_back(point); +--8<-- "src/nebula_core/nebula_core_ros/examples/point_types_usage_example.cpp:usage" ``` ### Angle utilities -**Location**: `nebula_core_decoders/include/nebula_core_decoders/angles.hpp` +```c++ +--8<-- "src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp:include" +``` **What it provides**: - `deg2rad()`, `rad2deg()` - Angle conversions -- `angle_is_between()` - Check if angle is within FOV +- `angle_is_between()` - Angle checks with wrap-around support - Angle normalization functions **Usage**: -```cpp -#include "nebula_core_decoders/angles.hpp" - -float azimuth_rad = deg2rad(azimuth_deg); -bool in_fov = angle_is_between(fov_min, fov_max, azimuth_rad); - -// Normalize any angle to [0, 360) or [0, 2pi) (depending on the chosen max_angle): -float azimuth_deg_norm = normalize_angle(azimuth_deg, 360.0f); -float azimuth_rad_norm = normalize_angle(azimuth_rad, static_cast(Radians::circle_modulus)); -``` - -### Configuration base classes - -**Location**: `nebula_core_common/include/nebula_core_common/nebula_common.hpp` - -**What it provides**: - -- `LidarConfigurationBase` - Base for sensor configuration (frame_id, sensor_model, return_mode, etc.) -- `CalibrationConfigurationBase` - Base for calibration data - -**Usage**: - -```cpp -struct MySensorConfiguration : public LidarConfigurationBase { - std::string sensor_ip; - uint16_t data_port; - // ... sensor-specific fields -}; +```c++ +--8<-- "src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp:usage" ``` ### Logger integration -**Location**: `nebula_core_common/include/nebula_core_common/loggers/logger.hpp` +```c++ +--8<-- "src/nebula_core/nebula_core_common/examples/logger_integration_usage_example.cpp:include" +``` **What it provides**: -- Unified logging interface +- Unified logging interface via `Logger` - ROS 2 logger wrapper (`RclcppLogger`) - Macros: `NEBULA_LOG_STREAM(logger->info, "message")` @@ -227,18 +169,14 @@ those modules still log into ROS 2. **Usage**: ```cpp -#include "nebula_core_common/loggers/logger.hpp" - -std::shared_ptr logger_; -NEBULA_LOG_STREAM(logger_->error, "Failed to parse packet"); +--8<-- "src/nebula_core/nebula_core_common/examples/logger_integration_usage_example.cpp:usage" ``` ### Diagnostic integration -**Location**: - -- `nebula_core_ros/include/nebula_core_ros/diagnostics/*` -- ROS 2 `diagnostic_updater` package (used in ROS wrapper) +```c++ +--8<-- "src/nebula_core/nebula_core_ros/examples/diagnostic_integration_usage_example.cpp:include" +``` **What it provides**: @@ -248,33 +186,7 @@ NEBULA_LOG_STREAM(logger_->error, "Failed to parse packet"); **Usage**: ```cpp -#include - -#include "nebula_core_ros/diagnostics/liveness_monitor.hpp" -#include "nebula_core_ros/diagnostics/rate_bound_status.hpp" - -diagnostic_updater::Updater diagnostic_updater_; - -// Monitor publish rate (tick this on publish) -custom_diagnostic_tasks::RateBoundStatus publish_rate_diag_{ - this, - custom_diagnostic_tasks::RateBoundStatusParam{9.0, 11.0}, // OK range - custom_diagnostic_tasks::RateBoundStatusParam{8.0, 12.0}, // WARN range - 5, // hysteresis frames - false // immediate error report -}; - -// Monitor liveness (tick this on packet receive / decode loop) -nebula::ros::LivenessMonitor liveness_diag_{ - "packet receive", this, rclcpp::Duration::from_seconds(1.0)}; - -// Register tasks -diagnostic_updater_.add(publish_rate_diag_); -diagnostic_updater_.add(liveness_diag_); - -// In your code paths: -// publish_rate_diag_.tick(); -// liveness_diag_.tick(); +--8<-- "src/nebula_core/nebula_core_ros/examples/diagnostic_integration_usage_example.cpp:usage" ``` ## Integration workflow @@ -292,13 +204,14 @@ cp -r nebula_sample nebula_myvendor Rename all occurrences of "sample"/"Sample" to your vendor name: -- **Directories**: `nebula_sample_*` → `nebula_myvendor_*` -- **Files**: `sample_*.{cpp,hpp}` → `myvendor_*.{cpp,hpp}` -- **Classes**: Rename wrapper / decoder / HW interface classes (e.g., `SampleRosWrapper`, `SampleDecoder`, `SampleHwInterface`) -- **Namespaces**: Update in all files -- **CMake/package**: Update `CMakeLists.txt` and `package.xml` +- Directories: `nebula_sample_*` → `nebula_myvendor_*` +- Files: `sample_*.{cpp,hpp}` → `myvendor_*.{cpp,hpp}` +- Classes: Rename wrapper / decoder / HW interface classes (e.g., `SampleRosWrapper`, + `SampleDecoder`, `SampleHwInterface`) +- Namespaces: Update in all files +- CMake/package: Update `CMakeLists.txt` and `package.xml` -**Tip**: Use find-and-replace tools: +Tip: Use find-and-replace tools: ```bash find nebula_myvendor -type f -exec sed -i 's/sample/myvendor/g' {} + @@ -315,188 +228,32 @@ See [Verification](#verification) below. ## Implementation details -### Common package (`nebula_myvendor_common`) +The `nebula_sample` package provides a template implementation. Its source files contain +comments and TODOs throughout to guide you through the implementation. -**Purpose**: Define configuration and calibration structures. +Some examples might not be relevant for your sensor, e.g. calibration handling. Implement only +what is necessary for your sensor. -#### Sensor configuration +### About SDK integration -Edit `include/nebula_myvendor_common/myvendor_common.hpp`: +If you are a sensor vendor with your own SDK, you might be able to replace parts of the decoder +and HW interface with calls to the SDK. Integrate your SDK either through Git submodules, or +by adding it to `build_depends.repos`. -```cpp -struct MyVendorSensorConfiguration : public LidarConfigurationBase { - // Network settings - std::string sensor_ip{"192.168.1.201"}; - std::string host_ip{"192.168.1.100"}; - uint16_t data_port{2368}; - - // Sensor settings - uint16_t rotation_speed{600}; // RPM - uint16_t cloud_min_angle{0}; - uint16_t cloud_max_angle{360}; - - // Add your sensor-specific fields -}; -``` - -#### Calibration configuration (optional) - -Only implement if your sensor needs calibration data: - -```cpp -struct MyVendorCalibrationConfiguration : public CalibrationConfigurationBase { - std::vector elevation_angles; - std::vector azimuth_offsets; - - Status load_from_file(const std::string & calibration_file) override { - // Parse your calibration file format (CSV, XML, binary, etc.) - // Populate elevation_angles and azimuth_offsets - return Status::OK; - } -}; -``` - -### Decoders package (`nebula_myvendor_decoders`) +!!! warning +Nebula is licensed under the Apache 2.0 license and has a strict no-binaries policy. Ensure +that your SDK _source code_ is public, and ensure that your SDK license allows shipping as +part of an Apache 2.0 project. -**Purpose**: Parse packets and generate point clouds. +Please ensure that the SDK is fit for automotive use cases (real-time, safety, reliability). +Nebula interfaces like -#### Decoder interface - -The interface is already defined in `myvendor_scan_decoder.hpp`. You don't need to modify it. - -#### Decoder implementation - -Edit `include/nebula_myvendor_decoders/decoders/myvendor_decoder.hpp`: - -```cpp -class MyVendorDecoder : public MyVendorScanDecoder { -public: - explicit MyVendorDecoder( - const std::shared_ptr & config) - : config_(config) { - current_cloud_ = std::make_shared(); - current_cloud_->reserve(100000); // Pre-allocate - } - - PacketDecodeResult unpack(const std::vector & packet) override { - // 1. Validate packet size - if (packet.size() != EXPECTED_PACKET_SIZE) { - return {% raw %}{{}, DecodeError::INVALID_PACKET_SIZE}{% endraw %}; - } - - // 2. Parse packet header - uint64_t timestamp_ns = parse_timestamp(packet); - uint16_t azimuth = parse_azimuth(packet); - - // 3. Extract and convert points - for (int block = 0; block < NUM_BLOCKS; ++block) { - for (int channel = 0; channel < NUM_CHANNELS; ++channel) { - uint16_t distance_raw = parse_distance(packet, block, channel); - uint8_t intensity = parse_intensity(packet, block, channel); - - // Convert to 3D point - NebulaPoint point = convert_to_point( - distance_raw, intensity, azimuth, channel, timestamp_ns); - current_cloud_->push_back(point); - } - } - - // 4. Detect scan completion (azimuth wrap) - // Note: comparing to the previous azimuth handles wrap-around at the 0/360 boundary. - bool scan_complete = (azimuth < last_azimuth_); - last_azimuth_ = azimuth; - - // 5. If scan complete, call callback - if (scan_complete && pointcloud_callback_) { - pointcloud_callback_(current_cloud_, timestamp_ns * 1e-9); - current_cloud_ = std::make_shared(); - current_cloud_->reserve(100000); - } - - // 6. Return result - PacketMetadata metadata; - metadata.packet_timestamp_ns = timestamp_ns; - metadata.did_scan_complete = scan_complete; - return {% raw %}{{}, metadata}{% endraw %}; - } - -private: - std::shared_ptr config_; - NebulaPointCloudPtr current_cloud_; - uint16_t last_azimuth_{0}; - pointcloud_callback_t pointcloud_callback_; -}; -``` +- `/nebula_points` (including correct point types) +- `/nebula_packets` (publish and replay) +- `/diagnostics` +- launch patterns -#### Driver implementation - -The driver is a thin wrapper. Edit `src/myvendor_driver.cpp`: - -```cpp -MyVendorDriver::MyVendorDriver( - const std::shared_ptr & config) -: config_(config) { - scan_decoder_ = std::make_unique(config); - driver_status_ = Status::OK; -} - -PacketDecodeResult MyVendorDriver::parse_cloud_packet( - const std::vector & packet) { - return scan_decoder_->unpack(packet); -} -``` - -### HW interfaces package (`nebula_myvendor_hw_interfaces`) - -**Purpose**: Manage UDP communication with the sensor. - -Edit `src/myvendor_hw_interface.cpp`: - -```cpp -Status MyVendorHwInterface::sensor_interface_start() { - if (!sensor_configuration_) { - return Status::SENSOR_CONFIG_ERROR; - } - - // Create UDP socket using Nebula's UdpSocket - udp_socket_ = std::make_unique(); - - // Open and bind - udp_socket_->open( - sensor_configuration_->host_ip, - sensor_configuration_->data_port); - udp_socket_->bind(); - - // Start async receive with callback - udp_socket_->asyncReceive( - [this](const std::vector & buffer, - const connections::UdpSocket::RxMetadata & metadata) { - if (cloud_packet_callback_) { - cloud_packet_callback_(buffer, metadata); - } - }); - - return Status::OK; -} - -Status MyVendorHwInterface::sensor_interface_stop() { - if (udp_socket_) { - udp_socket_->close(); - udp_socket_.reset(); - } - return Status::OK; -} -``` - -### ROS wrapper package (`nebula_myvendor`) - -**Purpose**: Bridge C++ driver with ROS 2. - -Edit `src/myvendor_ros_wrapper.cpp`: - -```cpp ---8<-- "src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp" -``` +must be implemented correctly. ## Required behaviors @@ -504,256 +261,138 @@ Your sensor integration must implement these behaviors correctly. ### Startup sequence -**Order of operations**: - -1. **Parameter loading**: Declare and read all ROS parameters -2. **Configuration validation**: Validate IP addresses, ports, ranges -3. **Driver initialization**: Create driver with validated config -4. **Callback registration**: Register pointcloud callback -5. **HW interface initialization**: Create and configure HW interface -6. **Publisher creation**: Create ROS publishers -7. **Stream start**: Call `sensor_interface_start()` to begin receiving data - -**Error handling**: +Order of operations: -- If any step fails, log the error and stop initialization (throwing exceptions is optional) -- Do not proceed to next step if previous failed -- Clean up resources on failure +1. Parameter loading: Declare and read ROS parameters +2. Configuration validation: Validate IP addresses, ports, ranges +3. Decoder initialization: Create decoder with validated config +4. Callback registration: Register pointcloud callback +5. HW interface initialization: Create and configure HW interface +6. Publisher creation: Create ROS publishers +7. Stream start: Call `sensor_interface_start()` to begin receiving data -**Example**: +### Reconfiguration (optional) -```cpp -if (!initialize()) { - RCLCPP_ERROR(get_logger(), "Initialization failed"); - return; -} - -stream_start(); -``` +When parameters change at runtime: -### Reconfiguration - -**When parameters change at runtime**: - -1. **Validate new parameters**: Check if new values are valid -2. **Stop stream**: Call `sensor_interface_stop()` -3. **Update configuration**: Apply new parameter values -4. **Reinitialize driver**: Create new driver with updated config -5. **Restart stream**: Call `sensor_interface_start()` - -**Example**: - -```cpp -rcl_interfaces::msg::SetParametersResult on_parameter_change( - const std::vector & parameters) { - - auto result = rcl_interfaces::msg::SetParametersResult(); - result.successful = true; - - // Validate parameters - for (const auto & param : parameters) { - if (param.get_name() == "rotation_speed") { - if (param.as_int() < 300 || param.as_int() > 1200) { - result.successful = false; - result.reason = "Invalid rotation speed"; - return result; - } - } - } - - // Apply changes - sensor_interface_stop(); - update_configuration(parameters); - reinitialize_driver(); - sensor_interface_start(); - - return result; -} -``` +1. Validate new parameters: Check if new values are valid +2. Update configuration: Apply new parameter values +3. Reinitialize driver: Create new driver with updated config ### Connection loss handling -**Detect and handle sensor disconnection**: +Detect and handle sensor disconnection: -1. **Timeout detection**: Monitor time since last packet -2. **Diagnostic update**: Set diagnostic status to ERROR -3. **Logging**: Log connection loss only on state changes (avoid log spam) -4. **Recovery**: Attempt reconnection transparently (with backoff) - -**Example**: - -```cpp -// In packet callback -void receive_packet_callback(const std::vector & packet) { - last_packet_time_ = now(); - - // Process packet... -} - -// In timer callback (e.g., 1 Hz) -void check_connection() { - auto time_since_last_packet = now() - last_packet_time_; - - if (time_since_last_packet > timeout_threshold_) { - if (!connection_lost_) { - connection_lost_ = true; - RCLCPP_WARN( - get_logger(), "Connection lost - no packets for %.1fs", time_since_last_packet.seconds()); - } - - // Update diagnostics - diagnostic_updater_.broadcast( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, - "Connection lost"); - - // Attempt reconnection (ideally transparent to the user) - attempt_reconnection(); - } -} -``` +1. Timeout detection: Monitor time since last packet +2. Diagnostic update: Set diagnostic status to `ERROR` +3. Logging: Log connection loss only on state changes (avoid log spam) +4. Recovery: Attempt reconnection transparently ### Shutdown sequence -**Order of operations**: +Order of operations: Prefer RAII-based shutdown: sockets/threads/buffers should be owned by objects whose destructors stop/join/close automatically, so the wrapper does not require sensor-specific shutdown logic. -1. **Stop stream**: Ensure receiver threads stop and join -2. **Close sockets**: Ensure all network resources are closed -3. **Release buffers**: Release point cloud buffers -4. **Destroy owners**: Destroy the owning objects (RAII) - -**Example**: - -```cpp -~MyVendorRosWrapper() { - if (hw_interface_ptr_) { - hw_interface_ptr_->sensor_interface_stop(); - } - - driver_ptr_.reset(); - hw_interface_ptr_.reset(); -} -``` +1. Stop stream: Ensure receiver threads stop and join +2. Close sockets: Ensure all network resources are closed +3. Release buffers: Release point cloud buffers +4. Destroy owners: Destroy the owning objects (RAII) ### Diagnostic reporting -**Required diagnostic information**: +Required diagnostic information: -- **Publish rate**: Use `custom_diagnostic_tasks::RateBoundStatus` -- **Liveness**: Use `nebula::ros::LivenessMonitor` -- **Debug timings**: Publish receive/decode/publish durations for profiling +- Publish rate: Use `custom_diagnostic_tasks::RateBoundStatus` +- Liveness: Use `nebula::ros::LivenessMonitor` +- Debug timings: Publish receive/decode/publish durations for profiling -**Example**: +## Verification -```cpp -diagnostic_updater::Updater diagnostic_updater_; - -// In constructor -diagnostic_updater_.setHardwareID("MyVendor LiDAR"); -diagnostic_updater_.add("Scan Rate", this, &MyVendorRosWrapper::check_scan_rate); - -// Diagnostic function -void check_scan_rate(diagnostic_updater::DiagnosticStatusWrapper & stat) { - double expected_rate = sensor_cfg_ptr_->rotation_speed / 60.0; // Hz - double actual_rate = measured_scan_rate_; - - if (std::abs(actual_rate - expected_rate) < 0.5) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Scan rate OK"); - } else { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Scan rate mismatch"); - } - - stat.add("Expected (Hz)", expected_rate); - stat.add("Actual (Hz)", actual_rate); -} -``` +This section contains basic commands you can use to verify your implementation. -## Verification +### Build and test -### Build +Build and test both have to succeed without warnings or errors. ```bash colcon build --packages-up-to nebula_myvendor -source install/setup.bash +# This tests all myvendor packages +colcon test --packages-select-regex nebula_myvendor +colcon test-result --verbose ``` -### Launch +### Live testing -```bash -ros2 launch nebula_myvendor nebula_myvendor.launch.xml -``` - -### Verify topics +For testing with a real sensor, launch Nebula with your sensor model: ```bash -# List topics -ros2 topic list - -# Check point cloud -ros2 topic echo /points --field header - -# Monitor frequency -ros2 topic hz /points +# From your package +ros2 launch nebula_myvendor nebula_myvendor.launch.xml sensor_model:=mysensor +# From the `nebula` package +ros2 launch nebula nebula_launch.py sensor_model:=mysensor ``` -### Visualize +To record a rosbag of Nebula packet output for later replay: ```bash -rviz2 -# Add PointCloud2 display -# Set topic to /points -# Set Fixed Frame to your frame_id +ros2 bag record /nebula_packets -o mysensor_packets ``` -### Check diagnostics +### Replay testing -```bash -ros2 topic echo /diagnostics -``` - -### Test with Rosbag (offline) +You can test Nebula offline with the rosbag recorded above: ```bash # Set launch_hw:=false to use rosbag replay ros2 launch nebula_myvendor nebula_myvendor.launch.xml launch_hw:=false -# In another terminal, play a ROS 2 bag -ros2 bag play your_sensor_data.bag +# In another terminal, play the recorded ROS 2 bag +ros2 bag play mysensor_packets ``` ## Integration checklist -- [ ] Cloned and renamed all files and directories -- [ ] Updated `CMakeLists.txt` in all packages -- [ ] Updated `package.xml` in all packages +### Basic setup + +- [ ] Copied and renamed `nebula_sample` to `nebula_myvendor` +- [ ] Renamed all occurrences of "sample"/"Sample" to your vendor name +- [ ] Updated `package.xml`s with author and package info +- [ ] Updated copyright headers + +### Implementation tasks + - [ ] Implemented `SensorConfiguration` struct +- [ ] Mapped ROS parameters in Wrapper - [ ] Implemented `unpack()` method in Decoder - [ ] Implemented scan completion detection -- [ ] Implemented UDP reception in HW Interface -- [ ] Mapped ROS parameters in Wrapper +- [ ] Implemented communication in HW Interface - [ ] Implemented startup sequence - [ ] Implemented shutdown sequence - [ ] Added diagnostic reporting - [ ] Added connection loss handling -- [ ] Added copyright headers -- [ ] Verified build succeeds + +### Verification tasks + +- [ ] Verified build succeeds without warnings - [ ] Verified point cloud publishes - [ ] Verified scan rate matches expected -- [ ] Tested with real sensor or PCAP data +- [ ] Verified diagnostics report correctly +- [ ] Tested with real sensor +- [ ] Tested rosbag recording of Nebula packet output +- [ ] Tested rosbag replay with `launch_hw:=false` ## Additional resources -- **Hesai implementation**: `src/nebula_hesai` - Full reference implementation -- **Velodyne implementation**: `src/nebula_velodyne` - Alternative reference -- **Core components**: `src/nebula_core` - Reusable utilities -- **Point types**: See `docs/point_types.md` -- **Parameters**: See `docs/parameters.md` +- Hesai implementation: `src/nebula_hesai` - Full reference implementation +- Velodyne implementation: `src/nebula_velodyne` - Alternative reference +- Core components: `src/nebula_core` - Reusable building blocks +- Point types: See `docs/point_types.md` +- Parameters: See `docs/parameters.md` ## Getting help - Check existing sensor implementations for examples -- Review inline code documentation (Doxygen comments) -- Consult the API reference documentation +- Consult the [API reference](https://tier4.github.io/nebula/api_reference/) - Ask questions in [GitHub Issues](https://github.com/tier4/nebula/issues) From c61b21f2b009a633391676355383a7f5d5c6c4ca Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 6 Feb 2026 13:46:39 +0900 Subject: [PATCH 47/65] chore: add example cpps to use as snippets in integ guide Signed-off-by: Max SCHMELLER --- .../nebula_core_common/CMakeLists.txt | 8 +++ .../examples/expected_usage_example.cpp | 47 ++++++++++++++ .../logger_integration_usage_example.cpp | 48 ++++++++++++++ .../nebula_core_decoders/CMakeLists.txt | 9 +++ .../angle_utilities_usage_example.cpp | 49 ++++++++++++++ .../examples/udp_socket_usage_example.cpp | 6 +- .../nebula_core_ros/CMakeLists.txt | 10 +++ .../diagnostic_integration_usage_example.cpp | 64 +++++++++++++++++++ .../examples/point_types_usage_example.cpp | 63 ++++++++++++++++++ src/nebula_core/nebula_core_ros/package.xml | 1 + 10 files changed, 303 insertions(+), 2 deletions(-) create mode 100644 src/nebula_core/nebula_core_common/examples/expected_usage_example.cpp create mode 100644 src/nebula_core/nebula_core_common/examples/logger_integration_usage_example.cpp create mode 100644 src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp create mode 100644 src/nebula_core/nebula_core_ros/examples/diagnostic_integration_usage_example.cpp create mode 100644 src/nebula_core/nebula_core_ros/examples/point_types_usage_example.cpp diff --git a/src/nebula_core/nebula_core_common/CMakeLists.txt b/src/nebula_core/nebula_core_common/CMakeLists.txt index c512267d4..1862acce1 100644 --- a/src/nebula_core/nebula_core_common/CMakeLists.txt +++ b/src/nebula_core/nebula_core_common/CMakeLists.txt @@ -28,6 +28,14 @@ endif() install(TARGETS nebula_core_common EXPORT export_nebula_core_common) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) +if(BUILD_TESTING OR BUILD_EXAMPLES) + add_library( + nebula_core_common_examples OBJECT + examples/expected_usage_example.cpp + examples/logger_integration_usage_example.cpp) + target_link_libraries(nebula_core_common_examples PRIVATE nebula_core_common) +endif() + ament_export_include_directories("include/${PROJECT_NAME}") ament_export_targets(export_nebula_core_common) diff --git a/src/nebula_core/nebula_core_common/examples/expected_usage_example.cpp b/src/nebula_core/nebula_core_common/examples/expected_usage_example.cpp new file mode 100644 index 000000000..4b7a26039 --- /dev/null +++ b/src/nebula_core/nebula_core_common/examples/expected_usage_example.cpp @@ -0,0 +1,47 @@ +// 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. + +// # --8<-- [start:include] +#include "nebula_core_common/util/expected.hpp" +// # --8<-- [end:include] + +#include +#include +#include + +struct Config +{ + std::string required_field; + int percentage_field{0}; +}; + +// # --8<-- [start:usage] +enum class ValidationError : uint8_t { + MissingField, + OutOfRange, +}; + +nebula::util::expected validate(const Config & config) +{ + if (config.required_field.empty()) { + return ValidationError::MissingField; + } + + if (config.percentage_field < 0 || config.percentage_field > 100) { + return ValidationError::OutOfRange; + } + + return std::monostate{}; +} +// # --8<-- [end:usage] diff --git a/src/nebula_core/nebula_core_common/examples/logger_integration_usage_example.cpp b/src/nebula_core/nebula_core_common/examples/logger_integration_usage_example.cpp new file mode 100644 index 000000000..cc6181384 --- /dev/null +++ b/src/nebula_core/nebula_core_common/examples/logger_integration_usage_example.cpp @@ -0,0 +1,48 @@ +// 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. + +// # --8<-- [start:include] +#include "nebula_core_common/loggers/console_logger.hpp" +#include "nebula_core_common/loggers/logger.hpp" +// # --8<-- [end:include] + +#include +#include + +using nebula::drivers::loggers::ConsoleLogger; +using nebula::drivers::loggers::Logger; + +// # --8<-- [start:usage] +class SomeModule +{ +private: + std::shared_ptr logger_; + +public: + explicit SomeModule(const std::shared_ptr & logger) : logger_(logger) {} + + void do_something() { logger_->info("Doing something important"); } +}; + +int main() +{ + auto logger = std::make_shared("MyApp"); + SomeModule module(logger->child("SomeModule")); + module.do_something(); + return 0; +} + +// Output: +// [MyApp.SomeModule][INFO] Doing something important +// # --8<-- [end:usage] diff --git a/src/nebula_core/nebula_core_decoders/CMakeLists.txt b/src/nebula_core/nebula_core_decoders/CMakeLists.txt index a3eb4de58..af318bf97 100644 --- a/src/nebula_core/nebula_core_decoders/CMakeLists.txt +++ b/src/nebula_core/nebula_core_decoders/CMakeLists.txt @@ -17,6 +17,15 @@ ament_target_dependencies(nebula_core_decoders INTERFACE nebula_core_common install(TARGETS nebula_core_decoders EXPORT export_nebula_core_decoders) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) +if(BUILD_TESTING OR BUILD_EXAMPLES) + add_library( + nebula_core_decoders_examples OBJECT + examples/angle_utilities_usage_example.cpp) + target_link_libraries( + nebula_core_decoders_examples + PRIVATE nebula_core_decoders nebula_core_common::nebula_core_common) +endif() + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp b/src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp new file mode 100644 index 000000000..e2fb079a6 --- /dev/null +++ b/src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp @@ -0,0 +1,49 @@ +// 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. + +// # --8<-- [start:include] +#include "nebula_core_common/nebula_common.hpp" +#include "nebula_core_decoders/angles.hpp" +// # --8<-- [end:include] + +#include +#include + +namespace nebula::drivers::examples +{ + +void angle_utilities_usage_example() +{ + // # --8<-- [start:usage] + const float azimuth_deg = 45.0F; + + const auto azimuth_rad = deg2rad(azimuth_deg); + const float fov_min_rad = 0.0F; + const auto fov_max_rad = deg2rad(90.0F); + assert(angle_is_between(fov_min_rad, fov_max_rad, azimuth_rad)); + + // Normalize any angle to [0, 360) or [0, 2pi) (depending on the chosen max_angle): + const float azimuth_deg_norm = normalize_angle(azimuth_deg, 360.0F); + assert(azimuth_deg_norm >= 0.0F && azimuth_deg_norm < 360.0F); + const float azimuth_rad_norm = normalize_angle(azimuth_rad, 2 * M_PI); + assert(azimuth_rad_norm >= 0.0F && azimuth_rad_norm < 2 * M_PI); + // # --8<-- [end:usage] + + (void)fov_min_rad; + (void)fov_max_rad; + (void)azimuth_deg_norm; + (void)azimuth_rad_norm; +} + +} // namespace nebula::drivers::examples diff --git a/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp b/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp index 5a38f83a1..46d4bf287 100644 --- a/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp +++ b/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp @@ -12,15 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. +// # --8<-- [start:include] #include "nebula_core_hw_interfaces/connections/udp.hpp" +// # --8<-- [end:include] #include #include int main() { + // # --8<-- [start:usage] using nebula::drivers::connections::UdpSocket; - // # --8<-- [start:udp_socket_usage] auto socket = UdpSocket::Builder("192.168.1.10", 9000) .set_socket_buffer_size(10'000'000) .limit_to_sender("192.168.10.20", 7000) @@ -32,6 +34,6 @@ int main() }); socket.unsubscribe(); - // # --8<-- [end:udp_socket_usage] + // # --8<-- [end:usage] return 0; } diff --git a/src/nebula_core/nebula_core_ros/CMakeLists.txt b/src/nebula_core/nebula_core_ros/CMakeLists.txt index b64376482..fb8a89860 100644 --- a/src/nebula_core/nebula_core_ros/CMakeLists.txt +++ b/src/nebula_core/nebula_core_ros/CMakeLists.txt @@ -20,6 +20,16 @@ install( LIBRARY DESTINATION lib) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) +if(BUILD_TESTING OR BUILD_EXAMPLES) + add_library( + nebula_core_ros_examples OBJECT + examples/diagnostic_integration_usage_example.cpp + examples/point_types_usage_example.cpp) + target_link_libraries(nebula_core_ros_examples PRIVATE nebula_core_ros) + ament_target_dependencies(nebula_core_ros_examples PUBLIC + diagnostic_msgs diagnostic_updater rclcpp sensor_msgs pcl_conversions) +endif() + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) diff --git a/src/nebula_core/nebula_core_ros/examples/diagnostic_integration_usage_example.cpp b/src/nebula_core/nebula_core_ros/examples/diagnostic_integration_usage_example.cpp new file mode 100644 index 000000000..d377f74a3 --- /dev/null +++ b/src/nebula_core/nebula_core_ros/examples/diagnostic_integration_usage_example.cpp @@ -0,0 +1,64 @@ +// 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. + +// # --8<-- [start:include] +#include "nebula_core_ros/diagnostics/liveness_monitor.hpp" +#include "nebula_core_ros/diagnostics/rate_bound_status.hpp" +// # --8<-- [end:include] + +#include +#include +#include +#include + +#include +#include +#include + +namespace nebula::ros::examples +{ + +using custom_diagnostic_tasks::RateBoundStatus; +using custom_diagnostic_tasks::RateBoundStatusParam; +using std::chrono_literals::operator""ms; + +// # --8<-- [start:usage] +class MyNode : public rclcpp::Node +{ +public: + MyNode() : Node("diagnostic_integration_usage_example") + { + updater_.add(publish_rate_diag_); + updater_.add(liveness_diag_); + } + + void on_packet_received(const std::array & packet) + { + liveness_diag_.tick(); + + if (packet[0] == 1 /* some condition indicating we should publish */) { + // publish something here + publish_rate_diag_.tick(); + } + } + +private: + diagnostic_updater::Updater updater_{this, 0.1}; + RateBoundStatus publish_rate_diag_{ + this, RateBoundStatusParam{9, 11}, RateBoundStatusParam{8, 12}}; + LivenessMonitor liveness_diag_{"packet_receive", this, 10ms}; +}; +// # --8<-- [end:usage] + +} // namespace nebula::ros::examples diff --git a/src/nebula_core/nebula_core_ros/examples/point_types_usage_example.cpp b/src/nebula_core/nebula_core_ros/examples/point_types_usage_example.cpp new file mode 100644 index 000000000..97372ddb9 --- /dev/null +++ b/src/nebula_core/nebula_core_ros/examples/point_types_usage_example.cpp @@ -0,0 +1,63 @@ +// 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_core_common/nebula_common.hpp" +// # --8<-- [start:include] +#include "nebula_core_common/point_types.hpp" +// # --8<-- [end:include] +#include + +#include +#include + +#include +#include +#include + +namespace nebula::ros::examples +{ + +void point_types_usage_example() +{ + // # --8<-- [start:usage] + auto cloud = std::make_shared(); + cloud->reserve(2); + + nebula::drivers::NebulaPoint point{}; + point.x = 1.0F; + point.y = 2.0F; + point.z = 3.0F; + point.intensity = 10U; + point.return_type = static_cast(nebula::drivers::ReturnType::STRONGEST); + point.channel = 5U; + point.azimuth = 0.0F; + point.elevation = 0.1F; + point.distance = 3.74F; + point.time_stamp = 42U; + cloud->push_back(point); + + const auto cloud_xyzir = nebula::drivers::convert_point_xyzircaedt_to_point_xyzir(cloud); + assert(cloud_xyzir != nullptr); + + sensor_msgs::msg::PointCloud2 cloud_ros{}; + pcl::toROSMsg(*cloud_xyzir, cloud_ros); + + // my_publisher->publish(cloud_ros); + // # --8<-- [end:usage] + + (void)cloud; + (void)cloud_xyzir; +} + +} // namespace nebula::ros::examples diff --git a/src/nebula_core/nebula_core_ros/package.xml b/src/nebula_core/nebula_core_ros/package.xml index 0ab508f83..59e7b5edb 100644 --- a/src/nebula_core/nebula_core_ros/package.xml +++ b/src/nebula_core/nebula_core_ros/package.xml @@ -16,6 +16,7 @@ diagnostic_msgs diagnostic_updater nebula_core_common + pcl_conversions rclcpp rosbag2_storage From 65eb4b1b8886ac6ef3cd64ebdb78ac9b8d4911be Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 6 Feb 2026 14:34:14 +0900 Subject: [PATCH 48/65] chore: better expected docs Signed-off-by: Max SCHMELLER --- docs/integration_guide.md | 8 ++++ .../nebula_sample_common/sample_common.hpp | 41 +++++++++++++++---- 2 files changed, 40 insertions(+), 9 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index f7c9cdc10..af2d09080 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -109,6 +109,14 @@ exceptions. This keeps APIs explicit about what can fail: --8<-- "src/nebula_core/nebula_core_common/examples/expected_usage_example.cpp:usage" ``` +!!! tip +If the happy path has no meaningful return value, use `std::monostate` (a type with no data) as +`T`: + + ```cpp + nebula::util::expected do_something(); + ``` + ### Point cloud types ```c++ diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp index c1d298d23..9d27812da 100644 --- a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp @@ -18,8 +18,12 @@ #include "nebula_core_common/nebula_common.hpp" #include "nebula_core_common/nebula_status.hpp" +#include + +#include #include #include +#include #include namespace nebula @@ -58,30 +62,49 @@ inline std::ostream & operator<<(std::ostream & os, SampleSensorConfiguration co /// - Intensity corrections /// - Timing offsets /// If your sensor doesn't need calibration, you can remove this struct entirely. -struct SampleCalibrationConfiguration : public CalibrationConfigurationBase +struct SampleCalibrationData { /// @brief Load calibration data from a file /// @param calibration_file Path to the calibration file /// @return Status::OK on success, error status otherwise /// @details Implement parsing logic for your sensor's calibration file format (CSV, XML, binary, /// etc.) - nebula::Status load_from_file(const std::string & calibration_file) + static util::expected load_from_file( + const std::string & calibration_file) { - // Implementation Items: Implement calibration file parsing - // Example: Parse CSV/XML/binary file containing angle corrections - (void)calibration_file; - return Status::OK; + std::ifstream file; + file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + + try { + file.open(calibration_file); + } catch (const std::ifstream::failure & e) { + return "Failed to open calibration file: " + calibration_file + " Error: " + e.what(); + } + + // Parse the file and populate calibration data fields here + + return SampleCalibrationData{}; } /// @brief Save calibration data to a file /// @param calibration_file Path to save the calibration file /// @return Status::OK on success, error status otherwise /// @details Implement serialization logic for your sensor's calibration format - nebula::Status save_to_file(const std::string & calibration_file) + 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 "Failed to open calibration file for writing: " + calibration_file + + " Error: " + e.what(); + } + // Implementation Items: Implement calibration file writing - (void)calibration_file; - return Status::OK; + + return std::monostate{}; } }; From 25d7e1c2a133f33cfd5afef7e7eb215dad808261 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 6 Feb 2026 19:09:10 +0900 Subject: [PATCH 49/65] chore: nicer sample_common Signed-off-by: Max SCHMELLER --- .../nebula_sample_common/sample_common.hpp | 32 +++++++------------ 1 file changed, 11 insertions(+), 21 deletions(-) diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp index 9d27812da..69e5eb9bb 100644 --- a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp @@ -15,42 +15,36 @@ #ifndef NEBULA_SAMPLE_COMMON_H #define NEBULA_SAMPLE_COMMON_H -#include "nebula_core_common/nebula_common.hpp" -#include "nebula_core_common/nebula_status.hpp" - #include #include #include #include #include -#include -namespace nebula -{ -namespace drivers +namespace nebula::drivers { /// @brief Sensor-specific configuration for the Sample LiDAR -/// @details This struct extends LidarConfigurationBase with any sensor-specific settings. +/// /// When implementing for a real sensor, add fields for: -/// - Return mode (single, dual, strongest, last, etc.) +/// - Return mode (single, strongest, forst_strongest, etc.) /// - Rotation speed / scan frequency /// - IP addresses (sensor, host) /// - Port numbers /// - FOV settings /// - Any vendor-specific parameters -struct SampleSensorConfiguration : public LidarConfigurationBase +struct SampleSensorConfiguration { - // Example field - replace with actual sensor parameters - std::string sample_field; + std::string host_ip; + std::string sensor_ip; }; inline std::ostream & operator<<(std::ostream & os, SampleSensorConfiguration const & arg) { os << "Sample Sensor Configuration:" << '\n'; - os << static_cast(arg) << '\n'; - os << "Sample Field: " << arg.sample_field << '\n'; + os << "Host IP: " << arg.host_ip << '\n'; + os << "Sensor IP: " << arg.sensor_ip << '\n'; return os; } @@ -66,9 +60,7 @@ struct SampleCalibrationData { /// @brief Load calibration data from a file /// @param calibration_file Path to the calibration file - /// @return Status::OK on success, error status otherwise - /// @details Implement parsing logic for your sensor's calibration file format (CSV, XML, binary, - /// etc.) + /// @return Nothing on success, error message otherwise static util::expected load_from_file( const std::string & calibration_file) { @@ -88,8 +80,7 @@ struct SampleCalibrationData /// @brief Save calibration data to a file /// @param calibration_file Path to save the calibration file - /// @return Status::OK on success, error status otherwise - /// @details Implement serialization logic for your sensor's calibration format + /// @return Nothing on success, error message otherwise util::expected save_to_file(const std::string & calibration_file) { std::ofstream file; @@ -108,7 +99,6 @@ struct SampleCalibrationData } }; -} // namespace drivers -} // namespace nebula +} // namespace nebula::drivers #endif // NEBULA_SAMPLE_COMMON_H From 61bb22707086122411f717e13b3b8d6503276fae Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 10 Feb 2026 14:57:29 +0900 Subject: [PATCH 50/65] chore: streamlining changes Signed-off-by: Max SCHMELLER --- .../examples/udp_socket_usage_example.cpp | 2 + .../config/sample_sensor.param.yaml | 29 +++--- ...common.hpp => sample_calibration_data.hpp} | 30 +----- .../sample_configuration.hpp | 87 +++++++++++++++++ .../decoders/sample_scan_decoder.hpp | 95 ------------------- .../{decoders => }/sample_decoder.hpp | 73 ++++++++------ .../nebula_sample_decoders/sample_driver.hpp | 69 -------------- .../src/sample_decoder.cpp | 38 ++++++++ .../src/sample_driver.cpp | 54 ----------- .../examples/udp_socket_usage_example.cpp | 38 -------- .../sample_hw_interface.hpp | 11 +-- .../src/sample_hw_interface.cpp | 12 --- 12 files changed, 190 insertions(+), 348 deletions(-) rename src/nebula_sample/nebula_sample_common/include/nebula_sample_common/{sample_common.hpp => sample_calibration_data.hpp} (77%) create mode 100644 src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp delete mode 100644 src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp rename src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/{decoders => }/sample_decoder.hpp (55%) delete mode 100644 src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp create mode 100644 src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp delete mode 100644 src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp delete mode 100644 src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp diff --git a/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp b/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp index 46d4bf287..e2d684157 100644 --- a/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp +++ b/src/nebula_core/nebula_core_hw_interfaces/examples/udp_socket_usage_example.cpp @@ -31,6 +31,8 @@ int main() socket.subscribe([](const std::vector & data, const UdpSocket::RxMetadata & metadata) { (void)data; (void)metadata; + // Process received data and metadata here. This callback will be executed in the socket's + // receive thread. }); socket.unsubscribe(); diff --git a/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml b/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml index 4371e6a7b..488b97506 100644 --- a/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml +++ b/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml @@ -4,24 +4,21 @@ /**: ros__parameters: - # Sensor configuration + # ROS wrapper configuration sensor_model: SampleSensor - return_mode: Dual + launch_hw: true frame_id: sample_lidar # Network configuration - sensor_ip: 192.168.1.201 - host_ip: 192.168.1.100 - data_port: 2368 - - # Scan configuration - scan_phase: 0 - rotation_speed: 600 # RPM - cloud_min_angle: 0 # degrees - cloud_max_angle: 360 # degrees + connection: + sensor_ip: 192.168.1.201 + host_ip: 192.168.1.100 - # Optional: Calibration file path - # calibration_file: "$(find-pkg-share nebula_sample)/config/calibration.dat" - - # Hardware interface - launch_hw: true + # Decoder configuration + fov: + azimuth: + min_deg: 0.0 + max_deg: 360.0 + elevation: + min_deg: -15.0 + max_deg: 15.0 diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp similarity index 77% rename from src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp rename to src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp index 69e5eb9bb..9fc7b6011 100644 --- a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_common.hpp +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NEBULA_SAMPLE_COMMON_H -#define NEBULA_SAMPLE_COMMON_H +#pragma once #include +#include +#include #include #include @@ -25,29 +26,6 @@ namespace nebula::drivers { -/// @brief Sensor-specific configuration for the Sample LiDAR -/// -/// When implementing for a real sensor, add fields for: -/// - Return mode (single, strongest, forst_strongest, etc.) -/// - Rotation speed / scan frequency -/// - IP addresses (sensor, host) -/// - Port numbers -/// - FOV settings -/// - Any vendor-specific parameters -struct SampleSensorConfiguration -{ - std::string host_ip; - std::string sensor_ip; -}; - -inline std::ostream & operator<<(std::ostream & os, SampleSensorConfiguration const & arg) -{ - os << "Sample Sensor Configuration:" << '\n'; - os << "Host IP: " << arg.host_ip << '\n'; - os << "Sensor IP: " << arg.sensor_ip << '\n'; - return os; -} - /// @brief Calibration data for the Sample LiDAR (optional) /// @details This struct is only needed if your sensor requires calibration data. /// Common calibration data includes: @@ -100,5 +78,3 @@ struct SampleCalibrationData }; } // namespace nebula::drivers - -#endif // NEBULA_SAMPLE_COMMON_H diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp new file mode 100644 index 000000000..5d0f5c6a0 --- /dev/null +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp @@ -0,0 +1,87 @@ +// 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 +{ + +struct ConnectionConfiguration +{ + std::string host_ip; + std::string sensor_ip; + uint16_t data_port; +}; + +// Allow automatic JSON serialization/deserialization +NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(ConnectionConfiguration, host_ip, sensor_ip, data_port); + +/// @brief Sensor-specific configuration for the Sample LiDAR +/// +/// When implementing for a real sensor, add fields for: +/// - Return mode (single, strongest, forst_strongest, etc.) +/// - Rotation speed / scan frequency +/// - IP addresses (sensor, host) +/// - Port numbers +/// - FOV settings +/// - Any vendor-specific parameters +struct SampleSensorConfiguration +{ + ConnectionConfiguration connection; + FieldOfView fov{}; +}; + +// These allow automatic serialization/deserialization: +// ```c++ +// nlohmann::ordered_json j = config; // to_json +// SampleSensorConfiguration config = j.get<...>(); // from_json +// ``` +inline void to_json(nlohmann::json & j, const SampleSensorConfiguration & 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}}; +} + +inline void from_json(const nlohmann::json & j, SampleSensorConfiguration & 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_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp deleted file mode 100644 index df91b0117..000000000 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_scan_decoder.hpp +++ /dev/null @@ -1,95 +0,0 @@ -// 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_SAMPLE_SCAN_DECODER_HPP -#define NEBULA_SAMPLE_SCAN_DECODER_HPP - -#include -#include -#include - -#include -#include -#include - -namespace nebula::drivers -{ -/// @brief Error codes returned during packet decoding -enum class DecodeError : uint8_t { - PACKET_PARSE_FAILED, ///< Failed to parse packet data (invalid format, checksum error, etc.) - DRIVER_NOT_OK, ///< Driver is not in a valid state to decode packets - INVALID_PACKET_SIZE, ///< Packet size doesn't match expected size for this sensor -}; - -/// @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 -{ - PerformanceCounters performance_counters; ///< Timing information - util::expected metadata_or_error; ///< Decode result or error -}; - -/// @brief Base interface for packet decoders -/// @details Implement this interface to decode raw UDP packets into point clouds. -/// The decoder is responsible for: -/// - Parsing binary packet data according to the sensor's protocol -/// - Accumulating points until a full scan is complete -/// - Calling the pointcloud callback when a scan is ready -class SampleScanDecoder -{ -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; - - SampleScanDecoder(SampleScanDecoder && c) = delete; - SampleScanDecoder & operator=(SampleScanDecoder && c) = delete; - SampleScanDecoder(const SampleScanDecoder & c) = delete; - SampleScanDecoder & operator=(const SampleScanDecoder & c) = delete; - - virtual ~SampleScanDecoder() = default; - SampleScanDecoder() = default; - - /// @brief Decode a single UDP packet - /// @param packet Raw packet data received from the sensor - /// @return PacketDecodeResult containing metadata or error, plus performance counters - /// @details This is the main decoding function. Implement sensor-specific parsing logic here. - /// Parse the packet, extract points, and accumulate them. When a full scan is complete, - /// call the pointcloud callback and set did_scan_complete = true. - virtual PacketDecodeResult unpack(const std::vector & packet) = 0; - - /// @brief Register a callback to receive complete point clouds - /// @param callback Function to call when a full scan is decoded - /// @details The decoder calls this callback when a complete scan is ready - virtual void set_pointcloud_callback(pointcloud_callback_t callback) = 0; -}; -} // namespace nebula::drivers - -#endif // NEBULA_SAMPLE_SCAN_DECODER_HPP diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp similarity index 55% rename from src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp rename to src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp index e977825d6..cee6e6f3a 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/decoders/sample_decoder.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp @@ -15,37 +15,67 @@ #ifndef NEBULA_SAMPLE_DECODER_HPP #define NEBULA_SAMPLE_DECODER_HPP -#include "nebula_sample_common/sample_common.hpp" -#include "nebula_sample_decoders/decoders/sample_scan_decoder.hpp" +#include +#include +#include -#include +#include #include namespace nebula::drivers { +/// @brief Error codes returned during packet decoding +enum class DecodeError : uint8_t { + PACKET_PARSE_FAILED, ///< Failed to parse packet data (invalid format, checksum error, etc.) + DRIVER_NOT_OK, ///< Driver is not in a valid state to decode packets + INVALID_PACKET_SIZE, ///< Packet size doesn't match expected size for this sensor +}; + +/// @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 +{ + PerformanceCounters performance_counters; ///< Timing information + util::expected metadata_or_error; ///< Decode result or error +}; + /// @brief Concrete implementation of the Sample packet decoder /// @details This class implements the SampleScanDecoder interface. /// Implement the following in this class: /// - Parse raw UDP packets according to your sensor's protocol specification +/// - Perform validation checks /// - Extract point data (x, y, z, intensity, timestamp, etc.) /// - Accumulate points until a full scan is complete /// - Call the pointcloud callback when a scan is ready -class SampleDecoder : public SampleScanDecoder +class SampleDecoder { 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 sensor_configuration Sensor settings (IP, ports, return mode, etc.) + /// @param fov Field of view to crop the point cloud to /// @details Initialize any internal state needed for decoding (e.g., point accumulation buffers) explicit SampleDecoder( - const std::shared_ptr & sensor_configuration) - { - // Implementation Items: Initialize decoder state - // - Allocate point cloud buffer - // - Set up any lookup tables based on sensor configuration - // - Initialize scan tracking variables - (void)sensor_configuration; - } + FieldOfView fov /*, other decoder args */, pointcloud_callback_t pointcloud_cb); /// @brief Decode a single UDP packet /// @param packet Raw packet bytes from the sensor @@ -58,20 +88,7 @@ class SampleDecoder : public SampleScanDecoder /// 5. Accumulate points in the current scan /// 6. Detect scan completion /// 7. If scan complete, call pointcloud_callback_ and set did_scan_complete=true - PacketDecodeResult unpack(const std::vector & packet) override - { - // Implementation Items: Implement packet decoding - // This is a stub that does nothing - replace with actual parsing logic - (void)packet; - return {{}, DecodeError::PACKET_PARSE_FAILED}; - } - - /// @brief Register callback for complete point clouds - /// @param callback Function to call when a scan is complete - void set_pointcloud_callback(pointcloud_callback_t callback) override - { - pointcloud_callback_ = callback; - } + PacketDecodeResult unpack(const std::vector & packet); private: pointcloud_callback_t pointcloud_callback_; ///< Callback for publishing scans @@ -80,7 +97,7 @@ class SampleDecoder : public SampleScanDecoder // - Previous azimuth for scan completion detection // - Scan timestamp // - Optionally: Double-buffering (two point cloud buffers) for handling scan overlap - // (advanced optimization for sensors with configurable FOV) + // (required if azimuth correction is done in Nebula) // - Any sensor-specific state }; diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp deleted file mode 100644 index 25cf88427..000000000 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_driver.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// 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_SAMPLE_DRIVER_HPP -#define NEBULA_SAMPLE_DRIVER_HPP - -#include "nebula_core_common/nebula_status.hpp" -#include "nebula_sample_common/sample_common.hpp" -#include "nebula_sample_decoders/decoders/sample_scan_decoder.hpp" - -#include -#include - -namespace nebula::drivers -{ - -/// @brief Main driver class for the Sample LiDAR -/// @details This is the high-level interface for the sensor driver. -/// It manages the decoder and provides the main API for: -/// - Initializing the driver with sensor configuration -/// - Processing incoming packets -/// - Registering callbacks for point clouds -/// - Querying driver status -class SampleDriver -{ -public: - /// @brief Constructor - /// @param sensor_configuration Sensor settings - /// @details Initializes the decoder with the given configuration - explicit SampleDriver( - const std::shared_ptr & sensor_configuration); - - /// @brief Get the current driver status - /// @return Status indicating if the driver is ready to process packets - /// @details Returns OK if initialized properly, error status otherwise - Status get_status(); - - /// @brief Process a single cloud packet - /// @param packet Raw UDP packet data - /// @return PacketDecodeResult with metadata or error - /// @details This is the main entry point for packet processing. - /// Called by the HW interface when a packet is received. - /// Delegates to the decoder's unpack() method. - PacketDecodeResult parse_cloud_packet(const std::vector & packet); - - /// @brief Register callback for complete point clouds - /// @param pointcloud_cb Function to call when a scan is complete - /// @details The callback receives the decoded point cloud and timestamp - void set_pointcloud_callback(SampleScanDecoder::pointcloud_callback_t pointcloud_cb); - -private: - std::shared_ptr scan_decoder_; ///< Decoder instance - Status driver_status_; ///< Current driver status -}; - -} // namespace nebula::drivers - -#endif // NEBULA_SAMPLE_DRIVER_HPP diff --git a/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp b/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp new file mode 100644 index 000000000..6dd800e25 --- /dev/null +++ b/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp @@ -0,0 +1,38 @@ +// 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_sample_decoders/sample_decoder.hpp" + +#include +#include + +namespace nebula::drivers +{ + +SampleDecoder::SampleDecoder( + FieldOfView fov /*, other decoder args */, pointcloud_callback_t pointcloud_cb) +: pointcloud_callback_(std::move(pointcloud_cb)) +{ + (void)fov; +} + +PacketDecodeResult SampleDecoder::unpack(const std::vector & packet) +{ + // Implementation Items: Implement packet decoding + // This is a stub that does nothing - replace with actual parsing logic + (void)packet; + return {{}, DecodeError::PACKET_PARSE_FAILED}; +} + +} // namespace nebula::drivers diff --git a/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp b/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp deleted file mode 100644 index a39c46c9b..000000000 --- a/src/nebula_sample/nebula_sample_decoders/src/sample_driver.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// 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_sample_decoders/sample_driver.hpp" - -#include "nebula_sample_decoders/decoders/sample_decoder.hpp" - -#include -#include - -namespace nebula::drivers -{ - -SampleDriver::SampleDriver( - const std::shared_ptr & sensor_configuration) -{ - // Initialize driver status - driver_status_ = Status::OK; - - // Create decoder instance with sensor configuration - scan_decoder_ = std::make_shared(sensor_configuration); -} - -Status SampleDriver::get_status() -{ - // Return current driver status - return driver_status_; -} - -PacketDecodeResult SampleDriver::parse_cloud_packet(const std::vector & packet) -{ - // Delegate packet parsing to the decoder - // The decoder will accumulate points and call the pointcloud callback when ready - return scan_decoder_->unpack(packet); -} - -void SampleDriver::set_pointcloud_callback(SampleScanDecoder::pointcloud_callback_t pointcloud_cb) -{ - // Forward the callback to the decoder - scan_decoder_->set_pointcloud_callback(pointcloud_cb); -} - -} // namespace nebula::drivers diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp b/src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp deleted file mode 100644 index 0f76282fd..000000000 --- a/src/nebula_sample/nebula_sample_hw_interfaces/examples/udp_socket_usage_example.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// 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_core_hw_interfaces/connections/udp.hpp" - -#include -#include - -namespace nebula::drivers::examples -{ - -void udp_socket_usage_example() -{ - // This file is intended to be compiled (via CMake) to keep documentation snippets up-to-date. - // It does not need to be executed. - auto socket = connections::UdpSocket::Builder("0.0.0.0", static_cast(0)).bind(); - - socket.subscribe( - [](const std::vector & data, const connections::UdpSocket::RxMetadata & metadata) { - (void)data; - (void)metadata; - }); - - socket.unsubscribe(); -} - -} // namespace nebula::drivers::examples diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp index 6779f578d..8375bb847 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include @@ -39,7 +39,7 @@ namespace nebula::drivers class SampleHwInterface { public: - SampleHwInterface(); + explicit SampleHwInterface(ConnectionConfiguration connection_config /*, other args */); /// @brief Start receiving packets from the sensor /// @return Status::OK on success, error status otherwise @@ -58,13 +58,6 @@ class SampleHwInterface /// 3. Optionally: send stop command to sensor Status sensor_interface_stop(); - /// @brief Set the sensor configuration - /// @param sensor_configuration Configuration containing IP addresses, ports, etc. - /// @return Status::OK on success, error status otherwise - /// @details Store the configuration for use when starting the interface - Status set_sensor_configuration( - std::shared_ptr sensor_configuration); - /// @brief Register callback for incoming packets /// @param scan_callback Function to call when a packet is received /// @return Status::OK on success, error status otherwise diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp index 030cf178d..aa91ca19d 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp @@ -33,12 +33,6 @@ Status SampleHwInterface::sensor_interface_start() // 4. When packets arrive, call cloud_packet_callback_ with the packet data // 5. Optionally: send HTTP/TCP command to sensor to start scanning - // Example (pseudo-code): - // udp_socket_ = std::make_shared(); - // udp_socket_->open(); - // udp_socket_->bind(sensor_configuration_->host_ip, sensor_configuration_->data_port); - // udp_socket_->asyncReceive(cloud_packet_callback_); - return Status::OK; } @@ -46,14 +40,8 @@ Status SampleHwInterface::sensor_interface_stop() { // Implementation Items: Implement sensor interface shutdown // 1. Stop the receive loop - // 2. Close UDP socket(s) // 3. Optionally: send command to sensor to stop scanning - // Example (pseudo-code): - // if (udp_socket_) { - // udp_socket_->close(); - // } - return Status::OK; } From a1900f749ee336bc92e92684b5ccddfa3c5e7cd6 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 17 Feb 2026 11:14:53 +0900 Subject: [PATCH 51/65] refactor(nebula_sample): align wrapper wiring and typed error flow --- .../nebula_sample/CMakeLists.txt | 2 + .../config/sample_sensor.param.yaml | 1 + .../nebula_sample/sample_ros_wrapper.hpp | 45 ++-- .../launch/nebula_sample.launch.xml | 2 + .../nebula_sample/src/sample_ros_wrapper.cpp | 218 +++++++++++++----- .../sample_configuration.hpp | 3 +- .../nebula_sample_decoders/CMakeLists.txt | 7 +- .../nebula_sample_decoders/package.xml | 1 - .../CMakeLists.txt | 16 +- .../sample_hw_interface.hpp | 30 ++- .../nebula_sample_hw_interfaces/package.xml | 1 - .../src/sample_hw_interface.cpp | 38 +-- 12 files changed, 243 insertions(+), 121 deletions(-) diff --git a/src/nebula_sample/nebula_sample/CMakeLists.txt b/src/nebula_sample/nebula_sample/CMakeLists.txt index 8f7a93e3b..8bad521ae 100644 --- a/src/nebula_sample/nebula_sample/CMakeLists.txt +++ b/src/nebula_sample/nebula_sample/CMakeLists.txt @@ -29,6 +29,8 @@ target_link_libraries( ament_target_dependencies( sample_ros_wrapper PUBLIC + rclcpp + rclcpp_components sensor_msgs pcl_conversions) diff --git a/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml b/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml index 488b97506..c6108a527 100644 --- a/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml +++ b/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml @@ -13,6 +13,7 @@ connection: sensor_ip: 192.168.1.201 host_ip: 192.168.1.100 + data_port: 2368 # Decoder configuration fov: diff --git a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp index 83cd61320..07980fb30 100644 --- a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp +++ b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp @@ -15,22 +15,33 @@ #ifndef NEBULA_SAMPLE_ROS_WRAPPER_HPP #define NEBULA_SAMPLE_ROS_WRAPPER_HPP -#include "nebula_core_common/nebula_common.hpp" -#include "nebula_core_common/nebula_status.hpp" -#include "nebula_sample_common/sample_common.hpp" -#include "nebula_sample_decoders/sample_driver.hpp" +#include "nebula_sample_decoders/sample_decoder.hpp" #include "nebula_sample_hw_interfaces/sample_hw_interface.hpp" +#include +#include #include #include +#include #include +#include +#include +#include #include namespace nebula::ros { +struct ConfigError +{ + std::string message; +}; + +util::expected load_config_from_ros_parameters( + rclcpp::Node & node); + /// @brief ROS 2 wrapper for the Sample LiDAR driver /// @details This node bridges the C++ driver with ROS 2. /// Responsibilities: @@ -43,6 +54,11 @@ namespace nebula::ros class SampleRosWrapper : public rclcpp::Node { public: + enum class Error : uint8_t { + HW_INTERFACE_NOT_INITIALIZED, + HW_STREAM_START_FAILED, + }; + /// @brief Constructor /// @param options ROS node options /// @details Initializes the driver, HW interface, and ROS publishers @@ -52,13 +68,9 @@ class SampleRosWrapper : public rclcpp::Node /// @details Stops the hardware interface cleanly ~SampleRosWrapper() override; - /// @brief Get the current driver status - /// @return Status indicating if the driver is operational - Status get_status(); - /// @brief Start the sensor data stream - /// @return Status::OK on success, error status otherwise - Status stream_start(); + /// @return Nothing on success, error otherwise + util::expected stream_start(); private: /// @brief Callback for incoming UDP packets @@ -70,15 +82,16 @@ class SampleRosWrapper : public rclcpp::Node const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata); - std::shared_ptr sensor_cfg_ptr_; ///< Sensor config + static const char * to_cstr(Error error); + static const char * to_cstr(drivers::SampleHwInterface::Error error); - std::shared_ptr driver_ptr_; ///< Driver instance - std::shared_ptr hw_interface_ptr_; ///< HW interface instance + drivers::SampleSensorConfiguration config_; + std::string frame_id_; - rclcpp::Publisher::SharedPtr - points_pub_; ///< Point cloud publisher + std::optional decoder_; + std::optional hw_interface_; - bool launch_hw_; ///< Whether to launch hardware interface (false for offline playback) + rclcpp::Publisher::SharedPtr points_pub_; }; } // namespace nebula::ros diff --git a/src/nebula_sample/nebula_sample/launch/nebula_sample.launch.xml b/src/nebula_sample/nebula_sample/launch/nebula_sample.launch.xml index d0281adec..868f36f4e 100644 --- a/src/nebula_sample/nebula_sample/launch/nebula_sample.launch.xml +++ b/src/nebula_sample/nebula_sample/launch/nebula_sample.launch.xml @@ -1,8 +1,10 @@ + + diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index b43a8bc44..4d08dbbd8 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -14,100 +14,210 @@ #include "nebula_sample/sample_ros_wrapper.hpp" +#include +#include +#include +#include +#include + #include +#include +#include #include +#include +#include #include #include namespace nebula::ros { +namespace +{ +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{"Failed to declare/read parameter '" + name + "': " + e.what()}; + } +} +} // namespace + +util::expected load_config_from_ros_parameters( + rclcpp::Node & node) +{ + drivers::SampleSensorConfiguration 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(); + + 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(); + + 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{ + "Parameter 'connection.data_port' must be in [0, 65535], got " + + std::to_string(data_port.value())}; + } + config.connection.data_port = static_cast(data_port.value()); + + 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()); + + return config; +} + SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) -: Node("nebula_sample_ros_wrapper", options) +: Node("nebula_sample_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)) { - // ========== ROS Parameter Declaration ========== - // Implementation Items: Add more parameters for sensor configuration (IP, port, return mode, - // etc.) - declare_parameter("launch_hw", true); - launch_hw_ = get_parameter("launch_hw").as_bool(); - - // ========== Initialize Sensor Configuration ========== - // Implementation Items: Read ROS parameters and populate sensor_cfg_ptr_ fields - // Example: - // sensor_cfg_ptr_->sensor_ip = get_parameter("sensor_ip").as_string(); - // sensor_cfg_ptr_->host_ip = get_parameter("host_ip").as_string(); - // sensor_cfg_ptr_->data_port = get_parameter("data_port").as_int(); - sensor_cfg_ptr_ = std::make_shared(); - - // ========== Initialize Driver ========== - driver_ptr_ = std::make_shared(sensor_cfg_ptr_); - - // Register callback to receive decoded point clouds from the driver - driver_ptr_->set_pointcloud_callback( - [this](const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { - // This callback is called when the decoder completes a full scan + const bool launch_hw = declare_parameter("launch_hw", true, param_read_only()); + declare_parameter("sensor_model", "SampleSensor", param_read_only()); + frame_id_ = declare_parameter("frame_id", "sample_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 sample sensor configuration: " + config_or_error.error().message); + } + config_ = config_or_error.value(); + + points_pub_ = + create_publisher("points_raw", rclcpp::SensorDataQoS()); + + decoder_.emplace( + config_.fov, [this](const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { (void)timestamp_s; - // Only publish if there are subscribers and the pointcloud is valid if (points_pub_->get_subscription_count() > 0 && pointcloud) { - // Convert PCL point cloud to ROS message auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = this->now(); + ros_pc_msg_ptr->header.stamp = now(); + ros_pc_msg_ptr->header.frame_id = frame_id_; points_pub_->publish(std::move(ros_pc_msg_ptr)); } }); - // ========== Initialize Hardware Interface ========== - // Only create HW interface if launch_hw is true (false for offline bag playback) - if (launch_hw_) { - hw_interface_ptr_ = std::make_shared(); - hw_interface_ptr_->set_sensor_configuration(sensor_cfg_ptr_); - - // Register callback to receive raw packets from the HW interface - hw_interface_ptr_->register_scan_callback( + if (launch_hw) { + hw_interface_.emplace(config_); + const auto callback_result = hw_interface_->register_scan_callback( std::bind( &SampleRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1, std::placeholders::_2)); - - // Start receiving packets - stream_start(); + if (!callback_result.has_value()) { + throw std::runtime_error( + "Failed to register sample sensor packet callback: " + + std::string(to_cstr(callback_result.error()))); + } + + const auto stream_start_result = stream_start(); + if (!stream_start_result.has_value()) { + throw std::runtime_error( + "Failed to start sample sensor stream: " + + std::string(to_cstr(stream_start_result.error()))); + } } - - // ========== Create ROS Publishers ========== - points_pub_ = create_publisher("points_raw", 10); } SampleRosWrapper::~SampleRosWrapper() { - if (hw_interface_ptr_) { - hw_interface_ptr_->sensor_interface_stop(); + if (hw_interface_) { + const auto stop_result = hw_interface_->sensor_interface_stop(); + (void)stop_result; } } -Status SampleRosWrapper::get_status() +util::expected SampleRosWrapper::stream_start() { - return Status::OK; + if (hw_interface_) { + const auto start_result = hw_interface_->sensor_interface_start(); + if (!start_result.has_value()) { + return Error::HW_STREAM_START_FAILED; + } + return std::monostate{}; + } + return Error::HW_INTERFACE_NOT_INITIALIZED; } -Status SampleRosWrapper::stream_start() +void SampleRosWrapper::receive_cloud_packet_callback( + const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata) { - if (hw_interface_ptr_) { - return hw_interface_ptr_->sensor_interface_start(); + (void)metadata; + if (!decoder_) { + return; + } + + const auto decode_result = decoder_->unpack(packet); + if (!decode_result.metadata_or_error.has_value()) { + RCLCPP_DEBUG_THROTTLE( + get_logger(), *get_clock(), 1000, "Packet decode failed with error code %u.", + static_cast(decode_result.metadata_or_error.error())); } - return Status::NOT_INITIALIZED; } -void SampleRosWrapper::receive_cloud_packet_callback( - const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata) +const char * SampleRosWrapper::to_cstr(const Error error) +{ + switch (error) { + case Error::HW_INTERFACE_NOT_INITIALIZED: + return "hardware interface not initialized"; + case Error::HW_STREAM_START_FAILED: + return "hardware stream start failed"; + default: + return "unknown wrapper error"; + } +} + +const char * SampleRosWrapper::to_cstr(const drivers::SampleHwInterface::Error error) { - // This callback is called by the HW interface when a UDP packet arrives - // Pass the packet to the driver for decoding - (void)metadata; // Metadata (timestamp, source IP) not used in this simple example - driver_ptr_->parse_cloud_packet(packet); + switch (error) { + case drivers::SampleHwInterface::Error::CALLBACK_NOT_REGISTERED: + return "callback not registered"; + case drivers::SampleHwInterface::Error::INVALID_CALLBACK: + return "invalid callback"; + default: + return "unknown hardware error"; + } } } // namespace nebula::ros -#include RCLCPP_COMPONENTS_REGISTER_NODE(nebula::ros::SampleRosWrapper) diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp index 5d0f5c6a0..f94a30599 100644 --- a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp @@ -14,9 +14,8 @@ #pragma once +#include #include -#include -#include #include #include diff --git a/src/nebula_sample/nebula_sample_decoders/CMakeLists.txt b/src/nebula_sample/nebula_sample_decoders/CMakeLists.txt index 07d36e427..95f4f84f8 100644 --- a/src/nebula_sample/nebula_sample_decoders/CMakeLists.txt +++ b/src/nebula_sample/nebula_sample_decoders/CMakeLists.txt @@ -5,7 +5,7 @@ find_package(autoware_cmake REQUIRED) autoware_package() # Sample Decoder -add_library(nebula_sample_decoders SHARED src/sample_driver.cpp) +add_library(nebula_sample_decoders SHARED src/sample_decoder.cpp) target_include_directories( nebula_sample_decoders @@ -16,7 +16,7 @@ target_link_libraries( nebula_sample_decoders PUBLIC nebula_core_common::nebula_core_common nebula_sample_common::nebula_sample_common - nebula_core_decoders::nebula_core_decoders rclcpp::rclcpp) + nebula_core_decoders::nebula_core_decoders) install(TARGETS nebula_sample_decoders EXPORT export_nebula_sample_decoders) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) @@ -24,7 +24,6 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) ament_export_include_directories("include/${PROJECT_NAME}") ament_export_targets(export_nebula_sample_decoders) -ament_export_dependencies(nebula_core_common nebula_sample_common - nebula_core_decoders rclcpp) +ament_export_dependencies(nebula_core_common nebula_sample_common nebula_core_decoders) ament_package() diff --git a/src/nebula_sample/nebula_sample_decoders/package.xml b/src/nebula_sample/nebula_sample_decoders/package.xml index 055999a77..b2880ce36 100644 --- a/src/nebula_sample/nebula_sample_decoders/package.xml +++ b/src/nebula_sample/nebula_sample_decoders/package.xml @@ -15,7 +15,6 @@ nebula_core_common nebula_core_decoders nebula_sample_common - rclcpp ament_cmake diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt b/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt index 89f483d93..5b621e020 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt +++ b/src/nebula_sample/nebula_sample_hw_interfaces/CMakeLists.txt @@ -12,17 +12,8 @@ target_include_directories( $) ament_target_dependencies( - nebula_sample_hw_interfaces PUBLIC boost_tcp_driver nebula_core_common - nebula_sample_common nebula_core_hw_interfaces) - -if(BUILD_TESTING OR BUILD_EXAMPLES) - add_library( - nebula_sample_hw_interfaces_examples OBJECT - examples/udp_socket_usage_example.cpp) - target_link_libraries( - nebula_sample_hw_interfaces_examples - PRIVATE nebula_core_hw_interfaces::nebula_core_hw_interfaces) -endif() + nebula_sample_hw_interfaces PUBLIC nebula_core_common nebula_sample_common + nebula_core_hw_interfaces) install(TARGETS nebula_sample_hw_interfaces EXPORT export_nebula_sample_hw_interfaces) @@ -30,7 +21,6 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) ament_export_include_directories("include/${PROJECT_NAME}") ament_export_targets(export_nebula_sample_hw_interfaces) -ament_export_dependencies(boost_tcp_driver nebula_core_common - nebula_sample_common nebula_core_hw_interfaces) +ament_export_dependencies(nebula_core_common nebula_sample_common nebula_core_hw_interfaces) ament_package() diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp index 8375bb847..2610c6cfb 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp @@ -15,11 +15,13 @@ #ifndef NEBULA_SAMPLE_HW_INTERFACE_HPP #define NEBULA_SAMPLE_HW_INTERFACE_HPP -#include +#include #include #include -#include +#include +#include +#include namespace nebula::drivers { @@ -39,34 +41,40 @@ namespace nebula::drivers class SampleHwInterface { public: - explicit SampleHwInterface(ConnectionConfiguration connection_config /*, other args */); + enum class Error : uint8_t { + CALLBACK_NOT_REGISTERED, + INVALID_CALLBACK, + }; + + explicit SampleHwInterface(SampleSensorConfiguration sensor_configuration); /// @brief Start receiving packets from the sensor - /// @return Status::OK on success, error status otherwise + /// @return Nothing on success, error otherwise /// @details Implement the following: /// 1. Create UDP socket(s) for data reception /// 2. Bind to the configured port(s) /// 3. Start async receive loop /// 4. Optionally: send start command to sensor - Status sensor_interface_start(); + util::expected sensor_interface_start(); /// @brief Stop receiving packets from the sensor - /// @return Status::OK on success, error status otherwise + /// @return Nothing on success, error otherwise /// @details Implement the following: /// 1. Stop the receive loop /// 2. Close UDP socket(s) /// 3. Optionally: send stop command to sensor - Status sensor_interface_stop(); + util::expected sensor_interface_stop(); /// @brief Register callback for incoming packets /// @param scan_callback Function to call when a packet is received - /// @return Status::OK on success, error status otherwise + /// @return Nothing on success, error otherwise /// @details The callback receives raw packet data and metadata (timestamp, source IP, etc.) - Status register_scan_callback(connections::UdpSocket::callback_t scan_callback); + util::expected register_scan_callback( + connections::UdpSocket::callback_t scan_callback); private: - std::shared_ptr sensor_configuration_; ///< Sensor config - connections::UdpSocket::callback_t cloud_packet_callback_; ///< Packet callback + SampleSensorConfiguration sensor_configuration_; ///< Sensor configuration + std::optional packet_callback_; ///< Packet callback // Implementation Items: Add member variables for: // - UDP socket instance(s) for data reception // - IO context for async operations diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/package.xml b/src/nebula_sample/nebula_sample_hw_interfaces/package.xml index a0af340cb..e890e48da 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/package.xml +++ b/src/nebula_sample/nebula_sample_hw_interfaces/package.xml @@ -11,7 +11,6 @@ autoware_cmake - boost_tcp_driver nebula_core_common nebula_core_hw_interfaces nebula_sample_common diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp index aa91ca19d..152d4f0da 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp @@ -14,50 +14,50 @@ #include "nebula_sample_hw_interfaces/sample_hw_interface.hpp" -#include +#include namespace nebula::drivers { -SampleHwInterface::SampleHwInterface() +SampleHwInterface::SampleHwInterface(SampleSensorConfiguration sensor_configuration) +: sensor_configuration_(std::move(sensor_configuration)) { // Constructor - initialize any member variables if needed } -Status SampleHwInterface::sensor_interface_start() +util::expected SampleHwInterface::sensor_interface_start() { + if (!packet_callback_) { + return Error::CALLBACK_NOT_REGISTERED; + } + // Implementation Items: Implement sensor interface startup // 1. Create UDP socket using connections::UdpSocket // 2. Bind to the port specified in sensor_configuration_ // 3. Start async receive loop - // 4. When packets arrive, call cloud_packet_callback_ with the packet data + // 4. When packets arrive, call packet_callback_ with the packet data // 5. Optionally: send HTTP/TCP command to sensor to start scanning - return Status::OK; + return std::monostate{}; } -Status SampleHwInterface::sensor_interface_stop() +util::expected SampleHwInterface::sensor_interface_stop() { // Implementation Items: Implement sensor interface shutdown // 1. Stop the receive loop // 3. Optionally: send command to sensor to stop scanning - return Status::OK; -} - -Status SampleHwInterface::set_sensor_configuration( - std::shared_ptr sensor_configuration) -{ - // Store the sensor configuration for later use - sensor_configuration_ = sensor_configuration; - return Status::OK; + return std::monostate{}; } -Status SampleHwInterface::register_scan_callback(connections::UdpSocket::callback_t scan_callback) +util::expected SampleHwInterface::register_scan_callback( + connections::UdpSocket::callback_t scan_callback) { - // Store the callback to be called when packets arrive - cloud_packet_callback_ = scan_callback; - return Status::OK; + if (!scan_callback) { + return Error::INVALID_CALLBACK; + } + packet_callback_.emplace(std::move(scan_callback)); + return std::monostate{}; } } // namespace nebula::drivers From a0e754499b1705629363fc3300071f6222100f4a Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 17 Feb 2026 11:16:03 +0900 Subject: [PATCH 52/65] refactor(nebula_sample_common): type calibration expected errors --- .../sample_calibration_data.hpp | 32 +++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp index 9fc7b6011..872cfea12 100644 --- a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp @@ -15,11 +15,9 @@ #pragma once #include -#include -#include +#include #include -#include #include #include @@ -36,10 +34,21 @@ namespace nebula::drivers /// If your sensor doesn't need calibration, you can remove this struct entirely. struct SampleCalibrationData { + enum class ErrorCode : uint8_t { + OPEN_FOR_READ_FAILED, + OPEN_FOR_WRITE_FAILED, + }; + + struct Error + { + ErrorCode code; + std::string message; + }; + /// @brief Load calibration data from a file /// @param calibration_file Path to the calibration file - /// @return Nothing on success, error message otherwise - static util::expected load_from_file( + /// @return Calibration data on success, error otherwise + static util::expected load_from_file( const std::string & calibration_file) { std::ifstream file; @@ -48,7 +57,9 @@ struct SampleCalibrationData try { file.open(calibration_file); } catch (const std::ifstream::failure & e) { - return "Failed to open calibration file: " + calibration_file + " Error: " + e.what(); + return Error{ + ErrorCode::OPEN_FOR_READ_FAILED, + "Failed to open calibration file: " + calibration_file + " Error: " + e.what()}; } // Parse the file and populate calibration data fields here @@ -58,8 +69,8 @@ struct SampleCalibrationData /// @brief Save calibration data to a file /// @param calibration_file Path to save the calibration file - /// @return Nothing on success, error message otherwise - util::expected save_to_file(const std::string & calibration_file) + /// @return Nothing on success, error otherwise + util::expected save_to_file(const std::string & calibration_file) { std::ofstream file; file.exceptions(std::ofstream::failbit | std::ofstream::badbit); @@ -67,8 +78,9 @@ struct SampleCalibrationData try { file.open(calibration_file); } catch (const std::ofstream::failure & e) { - return "Failed to open calibration file for writing: " + calibration_file + - " Error: " + e.what(); + return Error{ + ErrorCode::OPEN_FOR_WRITE_FAILED, + "Failed to open calibration file for writing: " + calibration_file + " Error: " + e.what()}; } // Implementation Items: Implement calibration file writing From a49cb37ad681f8d5ce3115a1e89dcbdf4052a7a9 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 17 Feb 2026 11:36:25 +0900 Subject: [PATCH 53/65] refactor(nebula_sample_decoders): streamline decoder stub and error reporting --- .../nebula_sample/src/sample_ros_wrapper.cpp | 4 +- .../nebula_sample_decoders/sample_decoder.hpp | 10 +++- .../src/sample_decoder.cpp | 49 ++++++++++++++++--- 3 files changed, 53 insertions(+), 10 deletions(-) diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index 4d08dbbd8..36da345de 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -189,8 +189,8 @@ void SampleRosWrapper::receive_cloud_packet_callback( const auto decode_result = decoder_->unpack(packet); if (!decode_result.metadata_or_error.has_value()) { RCLCPP_DEBUG_THROTTLE( - get_logger(), *get_clock(), 1000, "Packet decode failed with error code %u.", - static_cast(decode_result.metadata_or_error.error())); + get_logger(), *get_clock(), 1000, "Packet decode failed: %s.", + drivers::to_cstr(decode_result.metadata_or_error.error())); } } diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp index cee6e6f3a..86904902e 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp @@ -16,9 +16,10 @@ #define NEBULA_SAMPLE_DECODER_HPP #include +#include #include -#include +#include #include #include @@ -32,6 +33,8 @@ enum class DecodeError : uint8_t { INVALID_PACKET_SIZE, ///< Packet size doesn't match expected size for this sensor }; +const char * to_cstr(DecodeError error); + /// @brief Metadata extracted from a decoded packet struct PacketMetadata { @@ -88,9 +91,12 @@ class SampleDecoder /// 5. Accumulate points in the current scan /// 6. Detect scan completion /// 7. If scan complete, call pointcloud_callback_ and set did_scan_complete=true - PacketDecodeResult unpack(const std::vector & packet); + [[nodiscard]] PacketDecodeResult unpack(const std::vector & packet); + + void set_pointcloud_callback(pointcloud_callback_t pointcloud_cb); private: + FieldOfView fov_; ///< Field of view used for future point filtering pointcloud_callback_t pointcloud_callback_; ///< Callback for publishing scans // Implementation Items: Add member variables for: // - Point cloud accumulation buffer diff --git a/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp b/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp index 6dd800e25..bbc413730 100644 --- a/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp +++ b/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp @@ -14,25 +14,62 @@ #include "nebula_sample_decoders/sample_decoder.hpp" +#include #include #include namespace nebula::drivers { +const char * to_cstr(const DecodeError error) +{ + switch (error) { + case DecodeError::PACKET_PARSE_FAILED: + return "packet parse failed"; + case DecodeError::DRIVER_NOT_OK: + return "decoder is not ready"; + case DecodeError::INVALID_PACKET_SIZE: + return "invalid packet size"; + default: + return "unknown decode error"; + } +} + SampleDecoder::SampleDecoder( FieldOfView fov /*, other decoder args */, pointcloud_callback_t pointcloud_cb) -: pointcloud_callback_(std::move(pointcloud_cb)) +: fov_(std::move(fov)), pointcloud_callback_(std::move(pointcloud_cb)) { - (void)fov; } PacketDecodeResult SampleDecoder::unpack(const std::vector & packet) { - // Implementation Items: Implement packet decoding - // This is a stub that does nothing - replace with actual parsing logic - (void)packet; - return {{}, DecodeError::PACKET_PARSE_FAILED}; + const auto decode_begin = std::chrono::steady_clock::now(); + PacketDecodeResult result{{}, DecodeError::DRIVER_NOT_OK}; + + if (!pointcloud_callback_) { + result.metadata_or_error = DecodeError::DRIVER_NOT_OK; + } else if (packet.empty()) { + result.metadata_or_error = DecodeError::INVALID_PACKET_SIZE; + } else { + 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 = false; + 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 SampleDecoder::set_pointcloud_callback(pointcloud_callback_t pointcloud_cb) +{ + pointcloud_callback_ = std::move(pointcloud_cb); } } // namespace nebula::drivers From 1593dde128bfc0555ca93c09bc3999c0526f6802 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 17 Feb 2026 11:40:53 +0900 Subject: [PATCH 54/65] refactor(nebula_sample_decoders): clarify decode errors and add dummy scan completion --- .../nebula_sample_decoders/sample_decoder.hpp | 10 ++-- .../src/sample_decoder.cpp | 47 +++++++++++++++---- 2 files changed, 44 insertions(+), 13 deletions(-) diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp index 86904902e..09d7a61b1 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp @@ -28,9 +28,9 @@ namespace nebula::drivers /// @brief Error codes returned during packet decoding enum class DecodeError : uint8_t { - PACKET_PARSE_FAILED, ///< Failed to parse packet data (invalid format, checksum error, etc.) - DRIVER_NOT_OK, ///< Driver is not in a valid state to decode packets - INVALID_PACKET_SIZE, ///< Packet size doesn't match expected size for this sensor + 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 }; const char * to_cstr(DecodeError error); @@ -96,8 +96,12 @@ class SampleDecoder void set_pointcloud_callback(pointcloud_callback_t pointcloud_cb); private: + static constexpr uint64_t kPacketsPerDummyScan = 10; + FieldOfView fov_; ///< Field of view used for future point filtering pointcloud_callback_t pointcloud_callback_; ///< Callback for publishing scans + NebulaPointCloudPtr current_scan_cloud_{new NebulaPointCloud()}; + uint64_t packet_count_{0}; // Implementation Items: Add member variables for: // - Point cloud accumulation buffer // - Previous azimuth for scan completion detection diff --git a/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp b/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp index bbc413730..980847f74 100644 --- a/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp +++ b/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp @@ -24,12 +24,12 @@ namespace nebula::drivers const char * to_cstr(const DecodeError error) { switch (error) { - case DecodeError::PACKET_PARSE_FAILED: - return "packet parse failed"; - case DecodeError::DRIVER_NOT_OK: - return "decoder is not ready"; - case DecodeError::INVALID_PACKET_SIZE: - return "invalid packet size"; + 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"; } @@ -44,19 +44,46 @@ SampleDecoder::SampleDecoder( PacketDecodeResult SampleDecoder::unpack(const std::vector & packet) { const auto decode_begin = std::chrono::steady_clock::now(); - PacketDecodeResult result{{}, DecodeError::DRIVER_NOT_OK}; + PacketDecodeResult result{{}, DecodeError::CALLBACK_NOT_SET}; if (!pointcloud_callback_) { - result.metadata_or_error = DecodeError::DRIVER_NOT_OK; + result.metadata_or_error = DecodeError::CALLBACK_NOT_SET; } else if (packet.empty()) { - result.metadata_or_error = DecodeError::INVALID_PACKET_SIZE; + result.metadata_or_error = DecodeError::EMPTY_PACKET; } else { + ++packet_count_; + + NebulaPoint point{}; + point.x = static_cast(packet_count_); + point.y = 0.0F; + point.z = 0.0F; + point.intensity = packet.front(); + point.return_type = 0U; + point.channel = 0U; + point.azimuth = fov_.azimuth.start; + point.elevation = fov_.elevation.start; + point.distance = static_cast(packet.size()); + point.time_stamp = static_cast(packet_count_); + current_scan_cloud_->push_back(point); + 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 = false; + metadata.did_scan_complete = (packet_count_ % kPacketsPerDummyScan) == 0; + + if (metadata.did_scan_complete) { + const auto callback_begin = std::chrono::steady_clock::now(); + pointcloud_callback_( + current_scan_cloud_, static_cast(metadata.packet_timestamp_ns) * 1e-9); + result.performance_counters.callback_time_ns = + static_cast(std::chrono::duration_cast( + std::chrono::steady_clock::now() - callback_begin) + .count()); + current_scan_cloud_.reset(new NebulaPointCloud()); + } + result.metadata_or_error = metadata; } From 31a5bf36700f276d999e99aab82ff8d5a3a1b412 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 17 Feb 2026 12:00:15 +0900 Subject: [PATCH 55/65] refactor(core): move angle utilities to nebula_core_common --- src/nebula_core/nebula_core_common/CMakeLists.txt | 3 ++- .../examples/angle_utilities_usage_example.cpp | 2 +- .../include/nebula_core_common/util}/angles.hpp | 0 src/nebula_core/nebula_core_decoders/CMakeLists.txt | 9 --------- .../point_filters/blockage_mask.hpp | 2 +- .../point_filters/downsample_mask.hpp | 2 +- .../include/nebula_core_decoders/scan_cutter.hpp | 2 +- .../tests/point_filters/test_downsample_mask.cpp | 2 +- .../nebula_core_decoders/tests/test_angles.cpp | 2 +- .../include/nebula_core_ros/parameter_descriptors.hpp | 11 +++++++++++ .../nebula_hesai/src/hesai_ros_wrapper.cpp | 2 +- .../tests/hesai_ros_scan_cutting_test_main.cpp | 2 +- .../decoders/angle_corrector_calibration_based.hpp | 2 +- .../decoders/angle_corrector_correction_based.hpp | 2 +- .../nebula_hesai_decoders/decoders/pandar_40.hpp | 2 +- 15 files changed, 24 insertions(+), 21 deletions(-) rename src/nebula_core/{nebula_core_decoders => nebula_core_common}/examples/angle_utilities_usage_example.cpp (97%) rename src/nebula_core/{nebula_core_decoders/include/nebula_core_decoders => nebula_core_common/include/nebula_core_common/util}/angles.hpp (100%) diff --git a/src/nebula_core/nebula_core_common/CMakeLists.txt b/src/nebula_core/nebula_core_common/CMakeLists.txt index 1862acce1..272c601d6 100644 --- a/src/nebula_core/nebula_core_common/CMakeLists.txt +++ b/src/nebula_core/nebula_core_common/CMakeLists.txt @@ -32,7 +32,8 @@ if(BUILD_TESTING OR BUILD_EXAMPLES) add_library( nebula_core_common_examples OBJECT examples/expected_usage_example.cpp - examples/logger_integration_usage_example.cpp) + examples/logger_integration_usage_example.cpp + examples/angle_utilities_usage_example.cpp) target_link_libraries(nebula_core_common_examples PRIVATE nebula_core_common) endif() diff --git a/src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp b/src/nebula_core/nebula_core_common/examples/angle_utilities_usage_example.cpp similarity index 97% rename from src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp rename to src/nebula_core/nebula_core_common/examples/angle_utilities_usage_example.cpp index e2fb079a6..3d6d4ab87 100644 --- a/src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp +++ b/src/nebula_core/nebula_core_common/examples/angle_utilities_usage_example.cpp @@ -14,7 +14,7 @@ // # --8<-- [start:include] #include "nebula_core_common/nebula_common.hpp" -#include "nebula_core_decoders/angles.hpp" +#include "nebula_core_common/util/angles.hpp" // # --8<-- [end:include] #include diff --git a/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/angles.hpp b/src/nebula_core/nebula_core_common/include/nebula_core_common/util/angles.hpp similarity index 100% rename from src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/angles.hpp rename to src/nebula_core/nebula_core_common/include/nebula_core_common/util/angles.hpp diff --git a/src/nebula_core/nebula_core_decoders/CMakeLists.txt b/src/nebula_core/nebula_core_decoders/CMakeLists.txt index af318bf97..a3eb4de58 100644 --- a/src/nebula_core/nebula_core_decoders/CMakeLists.txt +++ b/src/nebula_core/nebula_core_decoders/CMakeLists.txt @@ -17,15 +17,6 @@ ament_target_dependencies(nebula_core_decoders INTERFACE nebula_core_common install(TARGETS nebula_core_decoders EXPORT export_nebula_core_decoders) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) -if(BUILD_TESTING OR BUILD_EXAMPLES) - add_library( - nebula_core_decoders_examples OBJECT - examples/angle_utilities_usage_example.cpp) - target_link_libraries( - nebula_core_decoders_examples - PRIVATE nebula_core_decoders nebula_core_common::nebula_core_common) -endif() - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/blockage_mask.hpp b/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/blockage_mask.hpp index ce0590716..bac798576 100644 --- a/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/blockage_mask.hpp +++ b/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/blockage_mask.hpp @@ -14,7 +14,7 @@ #pragma once -#include "nebula_core_decoders/angles.hpp" +#include "nebula_core_common/util/angles.hpp" #include #include diff --git a/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/downsample_mask.hpp b/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/downsample_mask.hpp index 55b64d7f8..f6703b35b 100644 --- a/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/downsample_mask.hpp +++ b/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/point_filters/downsample_mask.hpp @@ -14,7 +14,7 @@ #pragma once -#include "nebula_core_decoders/angles.hpp" +#include "nebula_core_common/util/angles.hpp" #include #include diff --git a/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/scan_cutter.hpp b/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/scan_cutter.hpp index 255aabfe3..029e093fa 100644 --- a/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/scan_cutter.hpp +++ b/src/nebula_core/nebula_core_decoders/include/nebula_core_decoders/scan_cutter.hpp @@ -14,7 +14,7 @@ #pragma once -#include "nebula_core_decoders/angles.hpp" +#include "nebula_core_common/util/angles.hpp" #include "nebula_core_decoders/scan_cutter/fsm_cut_at_fov_end.hpp" #include "nebula_core_decoders/scan_cutter/fsm_cut_in_fov.hpp" #include "nebula_core_decoders/scan_cutter/types.hpp" diff --git a/src/nebula_core/nebula_core_decoders/tests/point_filters/test_downsample_mask.cpp b/src/nebula_core/nebula_core_decoders/tests/point_filters/test_downsample_mask.cpp index 62e92ffaa..b2ec7772a 100644 --- a/src/nebula_core/nebula_core_decoders/tests/point_filters/test_downsample_mask.cpp +++ b/src/nebula_core/nebula_core_decoders/tests/point_filters/test_downsample_mask.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nebula_core_decoders/angles.hpp" +#include "nebula_core_common/util/angles.hpp" #include "nebula_core_decoders/point_filters/downsample_mask.hpp" #include diff --git a/src/nebula_core/nebula_core_decoders/tests/test_angles.cpp b/src/nebula_core/nebula_core_decoders/tests/test_angles.cpp index d9c5039ca..bfaa189a3 100644 --- a/src/nebula_core/nebula_core_decoders/tests/test_angles.cpp +++ b/src/nebula_core/nebula_core_decoders/tests/test_angles.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nebula_core_decoders/angles.hpp" +#include "nebula_core_common/util/angles.hpp" #include diff --git a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/parameter_descriptors.hpp b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/parameter_descriptors.hpp index 8d5355c35..9611c38a5 100644 --- a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/parameter_descriptors.hpp +++ b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/parameter_descriptors.hpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -52,4 +53,14 @@ bool get_param(const std::vector & p, const std::string & nam return false; } +template +T ensure_and_get_parameter(rclcpp::Node & node, const std::string & name) +{ + if (!node.has_parameter(name)) { + node.declare_parameter(name); + } + + return node.get_parameter(name); +} + } // namespace nebula::ros diff --git a/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp b/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp index 5f92f2407..c28643b70 100644 --- a/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp +++ b/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp @@ -5,10 +5,10 @@ #include "nebula_core_ros/parameter_descriptors.hpp" #include +#include #include #include #include -#include #include #include diff --git a/src/nebula_hesai/nebula_hesai/tests/hesai_ros_scan_cutting_test_main.cpp b/src/nebula_hesai/nebula_hesai/tests/hesai_ros_scan_cutting_test_main.cpp index acacba1c8..aeeccdc43 100644 --- a/src/nebula_hesai/nebula_hesai/tests/hesai_ros_scan_cutting_test_main.cpp +++ b/src/nebula_hesai/nebula_hesai/tests/hesai_ros_scan_cutting_test_main.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_calibration_based.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_calibration_based.hpp index 86027d3f1..fa9044c42 100644 --- a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_calibration_based.hpp +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_calibration_based.hpp @@ -14,7 +14,7 @@ #pragma once -#include "nebula_core_decoders/angles.hpp" +#include "nebula_core_common/util/angles.hpp" #include "nebula_hesai_common/hesai_common.hpp" #include "nebula_hesai_decoders/decoders/angle_corrector.hpp" diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_correction_based.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_correction_based.hpp index b04e507c7..983a6bea4 100644 --- a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_correction_based.hpp +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_correction_based.hpp @@ -14,7 +14,7 @@ #pragma once -#include "nebula_core_decoders/angles.hpp" +#include "nebula_core_common/util/angles.hpp" #include "nebula_hesai_common/hesai_common.hpp" #include "nebula_hesai_decoders/decoders/angle_corrector.hpp" diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/pandar_40.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/pandar_40.hpp index 60b1fa881..36481b8cf 100644 --- a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/pandar_40.hpp +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/pandar_40.hpp @@ -14,7 +14,7 @@ #pragma once -#include "nebula_core_decoders/angles.hpp" +#include "nebula_core_common/util/angles.hpp" #include "nebula_hesai_decoders/decoders/hesai_packet.hpp" #include "nebula_hesai_decoders/decoders/hesai_sensor.hpp" From e2c3c95d92dbe1348a2ce2863190cf49fdc3c9da Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 17 Feb 2026 14:42:05 +0900 Subject: [PATCH 56/65] docs: update angle utility snippet paths after core move --- docs/integration_guide.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index af2d09080..65ff9b83a 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -143,7 +143,7 @@ ensures seamless integration with Autoware components. ### Angle utilities ```c++ ---8<-- "src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp:include" +--8<-- "src/nebula_core/nebula_core_common/examples/angle_utilities_usage_example.cpp:include" ``` **What it provides**: @@ -155,7 +155,7 @@ ensures seamless integration with Autoware components. **Usage**: ```c++ ---8<-- "src/nebula_core/nebula_core_decoders/examples/angle_utilities_usage_example.cpp:usage" +--8<-- "src/nebula_core/nebula_core_common/examples/angle_utilities_usage_example.cpp:usage" ``` ### Logger integration From bc8ffa44c36cc226d91af1b79a8af4874efbf99c Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 17 Feb 2026 17:12:53 +0900 Subject: [PATCH 57/65] fix: address Codex PR review comments --- .../nebula_core_ros/examples/point_types_usage_example.cpp | 2 +- src/nebula_sample/README.md | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/nebula_core/nebula_core_ros/examples/point_types_usage_example.cpp b/src/nebula_core/nebula_core_ros/examples/point_types_usage_example.cpp index 97372ddb9..bfc956bff 100644 --- a/src/nebula_core/nebula_core_ros/examples/point_types_usage_example.cpp +++ b/src/nebula_core/nebula_core_ros/examples/point_types_usage_example.cpp @@ -18,7 +18,7 @@ // # --8<-- [end:include] #include -#include +#include #include #include diff --git a/src/nebula_sample/README.md b/src/nebula_sample/README.md index 5952572f9..8bb1680f9 100644 --- a/src/nebula_sample/README.md +++ b/src/nebula_sample/README.md @@ -45,9 +45,9 @@ The guide covers: ### Decoder (`*_decoders`) -- `SampleScanDecoder` - Base decoder interface - `SampleDecoder` - Packet decoder implementation -- `SampleDriver` - Main driver class +- `PacketDecodeResult` - Decoder output containing metadata/error and performance counters +- `DecodeError` - Decoder error codes for packet handling failures ### Hardware interface (`*_hw_interfaces`) From 654364d6fd26a6e7853ef7f395daa17789a559c3 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 18 Feb 2026 19:07:58 +0900 Subject: [PATCH 58/65] docs: improve formatting and clarity in integration guide Signed-off-by: Max SCHMELLER --- docs/integration_guide.md | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 65ff9b83a..b72af4f87 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -51,8 +51,9 @@ Except for the ROS wrappers, no package should depend on ROS 2. This allows user of Nebula as a library e.g. in ML pipelines without ROS 2. !!! warning -Existing vendor implementations do not strictly follow this architecture as the project -evolved over time. New implementations should follow the architecture described here. + + Existing vendor implementations do not strictly follow this architecture as the project + evolved over time. New implementations should follow the architecture described here. ### Data Flow @@ -110,13 +111,17 @@ exceptions. This keeps APIs explicit about what can fail: ``` !!! tip -If the happy path has no meaningful return value, use `std::monostate` (a type with no data) as -`T`: + + If the happy path has no meaningful return value, use `std::monostate` (a type with no data) as + `T`: ```cpp nebula::util::expected do_something(); ``` +See [CPP Reference](https://en.cppreference.com/w/cpp/utility/expected.html) for more details on the +`std::expected` type of C++23, which `nebula::util::expected` is based on. + ### Point cloud types ```c++ @@ -249,9 +254,10 @@ and HW interface with calls to the SDK. Integrate your SDK either through Git su by adding it to `build_depends.repos`. !!! warning -Nebula is licensed under the Apache 2.0 license and has a strict no-binaries policy. Ensure -that your SDK _source code_ is public, and ensure that your SDK license allows shipping as -part of an Apache 2.0 project. + + Nebula is licensed under the Apache 2.0 license and has a strict no-binaries policy. Ensure + that your SDK _source code_ is public, and ensure that your SDK license allows shipping as + part of an Apache 2.0 project. Please ensure that the SDK is fit for automotive use cases (real-time, safety, reliability). Nebula interfaces like From 93b6fd696450c16674d0ba6dde3d38028d22a3d1 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 18 Feb 2026 22:04:10 +0900 Subject: [PATCH 59/65] refactor: enhance documentation and structure in sample ROS wrapper and decoder interfaces Signed-off-by: Max SCHMELLER --- .../nebula_sample/sample_ros_wrapper.hpp | 40 ++++++------ .../nebula_sample/src/sample_ros_wrapper.cpp | 63 ++++++------------- .../sample_calibration_data.hpp | 28 ++++----- .../sample_configuration.hpp | 25 ++++---- .../nebula_sample_decoders/sample_decoder.hpp | 46 +++++--------- .../src/sample_decoder.cpp | 8 ++- .../sample_hw_interface.hpp | 59 ++++++----------- .../src/sample_hw_interface.cpp | 34 +++++----- 8 files changed, 123 insertions(+), 180 deletions(-) diff --git a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp index 07980fb30..81c7c60c5 100644 --- a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp +++ b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp @@ -39,15 +39,18 @@ struct ConfigError std::string message; }; +/// @brief Read and validate sample driver configuration from ROS parameters. +/// @param node Node used to declare/read parameters. +/// @return Parsed SampleSensorConfiguration or ConfigError on validation failure. util::expected load_config_from_ros_parameters( rclcpp::Node & node); /// @brief ROS 2 wrapper for the Sample LiDAR driver /// @details This node bridges the C++ driver with ROS 2. /// Responsibilities: -/// - Declare and read ROS parameters for sensor configuration -/// - Initialize the driver and hardware interface -/// - Receive packets from HW interface and pass to driver +/// - 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 @@ -55,29 +58,28 @@ class SampleRosWrapper : public rclcpp::Node { public: enum class Error : uint8_t { - HW_INTERFACE_NOT_INITIALIZED, - HW_STREAM_START_FAILED, + HW_INTERFACE_NOT_INITIALIZED, ///< Stream start requested while HW interface is absent. + HW_STREAM_START_FAILED, ///< Underlying HW interface failed to start. }; - /// @brief Constructor - /// @param options ROS node options - /// @details Initializes the driver, HW interface, and ROS publishers + /// @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 SampleRosWrapper(const rclcpp::NodeOptions & options); - /// @brief Destructor - /// @details Stops the hardware interface cleanly + /// @brief Stop sensor streaming before destruction when initialized. ~SampleRosWrapper() override; - /// @brief Start the sensor data stream - /// @return Nothing on success, error otherwise - util::expected stream_start(); - private: - /// @brief Callback for incoming UDP packets - /// @param packet Raw packet data from the sensor - /// @param metadata Packet metadata (timestamp, source IP, etc.) - /// @details This is called by the HW interface when a packet arrives. - /// Passes the packet to the driver for decoding. + /// @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( const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata); diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index 36da345de..d4e115061 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -127,57 +128,45 @@ SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) decoder_.emplace( config_.fov, [this](const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { - (void)timestamp_s; - if (points_pub_->get_subscription_count() > 0 && pointcloud) { - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = now(); - ros_pc_msg_ptr->header.frame_id = frame_id_; - points_pub_->publish(std::move(ros_pc_msg_ptr)); - } + publish_pointcloud_callback(pointcloud, timestamp_s); }); if (launch_hw) { - hw_interface_.emplace(config_); + hw_interface_.emplace(config_.connection); const auto callback_result = hw_interface_->register_scan_callback( - std::bind( - &SampleRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1, - std::placeholders::_2)); + [this]( + const 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 sample sensor packet callback: " + - std::string(to_cstr(callback_result.error()))); + std::string(drivers::SampleHwInterface::to_cstr(callback_result.error()))); } - const auto stream_start_result = stream_start(); + const auto stream_start_result = hw_interface_->sensor_interface_start(); if (!stream_start_result.has_value()) { throw std::runtime_error( "Failed to start sample sensor stream: " + - std::string(to_cstr(stream_start_result.error()))); + std::string(drivers::SampleHwInterface::to_cstr(stream_start_result.error()))); } } } -SampleRosWrapper::~SampleRosWrapper() +void SampleRosWrapper::publish_pointcloud_callback( + const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { - if (hw_interface_) { - const auto stop_result = hw_interface_->sensor_interface_stop(); - (void)stop_result; + (void)timestamp_s; + if (points_pub_->get_subscription_count() > 0 && pointcloud) { + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = now(); + ros_pc_msg_ptr->header.frame_id = frame_id_; + points_pub_->publish(std::move(ros_pc_msg_ptr)); } } -util::expected SampleRosWrapper::stream_start() -{ - if (hw_interface_) { - const auto start_result = hw_interface_->sensor_interface_start(); - if (!start_result.has_value()) { - return Error::HW_STREAM_START_FAILED; - } - return std::monostate{}; - } - return Error::HW_INTERFACE_NOT_INITIALIZED; -} - void SampleRosWrapper::receive_cloud_packet_callback( const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata) { @@ -206,18 +195,6 @@ const char * SampleRosWrapper::to_cstr(const Error error) } } -const char * SampleRosWrapper::to_cstr(const drivers::SampleHwInterface::Error error) -{ - switch (error) { - case drivers::SampleHwInterface::Error::CALLBACK_NOT_REGISTERED: - return "callback not registered"; - case drivers::SampleHwInterface::Error::INVALID_CALLBACK: - return "invalid callback"; - default: - return "unknown hardware error"; - } -} - } // namespace nebula::ros RCLCPP_COMPONENTS_REGISTER_NODE(nebula::ros::SampleRosWrapper) diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp index 872cfea12..2d6ab6113 100644 --- a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_calibration_data.hpp @@ -24,30 +24,26 @@ namespace nebula::drivers { -/// @brief Calibration data for the Sample LiDAR (optional) -/// @details This struct is only needed if your sensor requires calibration data. -/// Common calibration data includes: -/// - Vertical/horizontal angle corrections per laser -/// - Distance corrections -/// - Intensity corrections -/// - Timing offsets -/// If your sensor doesn't need calibration, you can remove this struct entirely. +/// @brief Calibration data for the Sample 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 SampleCalibrationData { enum class ErrorCode : uint8_t { - OPEN_FOR_READ_FAILED, - OPEN_FOR_WRITE_FAILED, + 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 + /// @brief Load calibration data from a file, e.g. for offline decoding /// @param calibration_file Path to the calibration file - /// @return Calibration data on success, error otherwise + /// @return Parsed calibration data on success, Error on failure. static util::expected load_from_file( const std::string & calibration_file) { @@ -62,14 +58,13 @@ struct SampleCalibrationData "Failed to open calibration file: " + calibration_file + " Error: " + e.what()}; } - // Parse the file and populate calibration data fields here - + // Implement: Parse and validate sensor calibration fields from the opened file. return SampleCalibrationData{}; } /// @brief Save calibration data to a file /// @param calibration_file Path to save the calibration file - /// @return Nothing on success, error otherwise + /// @return std::monostate on success, Error on failure. util::expected save_to_file(const std::string & calibration_file) { std::ofstream file; @@ -83,8 +78,7 @@ struct SampleCalibrationData "Failed to open calibration file for writing: " + calibration_file + " Error: " + e.what()}; } - // Implementation Items: Implement calibration file writing - + // Implement: Serialize sensor calibration fields to the opened file. return std::monostate{}; } }; diff --git a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp index f94a30599..95453821a 100644 --- a/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp +++ b/src/nebula_sample/nebula_sample_common/include/nebula_sample_common/sample_configuration.hpp @@ -23,10 +23,15 @@ namespace nebula::drivers { +/// @brief Network endpoint settings used by the sample 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; }; @@ -34,25 +39,17 @@ struct ConnectionConfiguration NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(ConnectionConfiguration, host_ip, sensor_ip, data_port); /// @brief Sensor-specific configuration for the Sample LiDAR -/// -/// When implementing for a real sensor, add fields for: -/// - Return mode (single, strongest, forst_strongest, etc.) -/// - Rotation speed / scan frequency -/// - IP addresses (sensor, host) -/// - Port numbers -/// - FOV settings -/// - Any vendor-specific parameters +/// @details Minimal tutorial configuration that combines connection settings and angular filtering. struct SampleSensorConfiguration { ConnectionConfiguration connection; FieldOfView fov{}; }; -// These allow automatic serialization/deserialization: -// ```c++ -// nlohmann::ordered_json j = config; // to_json -// SampleSensorConfiguration config = j.get<...>(); // from_json -// ``` +/// @brief Serialize SampleSensorConfiguration to JSON. +/// @details The JSON schema contains: +/// - `connection` (ConnectionConfiguration) +/// - `fov` (FieldOfView) inline void to_json(nlohmann::json & j, const SampleSensorConfiguration & config) { j = nlohmann::json{}; @@ -69,6 +66,8 @@ inline void to_json(nlohmann::json & j, const SampleSensorConfiguration & config j["fov"] = {{"azimuth", azimuth_fov}, {"elevation", elevation_fov}}; } +/// @brief Parse SampleSensorConfiguration from JSON. +/// @throws nlohmann::json::exception if required fields are missing or have incompatible types. inline void from_json(const nlohmann::json & j, SampleSensorConfiguration & config) { j.at("connection").get_to(config.connection); diff --git a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp index 09d7a61b1..2f9d5c21c 100644 --- a/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp +++ b/src/nebula_sample/nebula_sample_decoders/include/nebula_sample_decoders/sample_decoder.hpp @@ -33,6 +33,7 @@ enum class DecodeError : uint8_t { 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 @@ -53,18 +54,14 @@ struct PerformanceCounters /// @details Contains either successful metadata or an error, plus performance counters struct PacketDecodeResult { - PerformanceCounters performance_counters; ///< Timing information - util::expected metadata_or_error; ///< Decode result or error + /// Timing measurements for the decode and callback path. + PerformanceCounters performance_counters; + /// Successful metadata or a decode error. + util::expected metadata_or_error; }; -/// @brief Concrete implementation of the Sample packet decoder -/// @details This class implements the SampleScanDecoder interface. -/// Implement the following in this class: -/// - Parse raw UDP packets according to your sensor's protocol specification -/// - Perform validation checks -/// - Extract point data (x, y, z, intensity, timestamp, etc.) -/// - Accumulate points until a full scan is complete -/// - Call the pointcloud callback when a scan is ready +/// @brief Decoder that accumulates and converts incoming raw packets into point clouds. +/// @details The tutorial implementation emits synthetic points for demonstration purposes. class SampleDecoder { public: @@ -76,39 +73,26 @@ class SampleDecoder /// @brief Constructor /// @param fov Field of view to crop the point cloud to - /// @details Initialize any internal state needed for decoding (e.g., point accumulation buffers) + /// @param pointcloud_cb Callback invoked when a full scan is assembled explicit SampleDecoder( FieldOfView fov /*, other decoder args */, pointcloud_callback_t pointcloud_cb); /// @brief Decode a single UDP packet /// @param packet Raw packet bytes from the sensor - /// @return PacketDecodeResult with metadata or error - /// @details Implement your sensor's packet parsing logic here: - /// 1. Validate packet size and checksum - /// 2. Parse packet header (timestamp, azimuth, etc.) - /// 3. Extract point data blocks - /// 4. Convert raw data to 3D points (apply calibration if needed) - /// 5. Accumulate points in the current scan - /// 6. Detect scan completion - /// 7. If scan complete, call pointcloud_callback_ and set did_scan_complete=true + /// @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: - static constexpr uint64_t kPacketsPerDummyScan = 10; + static constexpr uint64_t k_packets_per_dummy_scan = 10; - FieldOfView fov_; ///< Field of view used for future point filtering - pointcloud_callback_t pointcloud_callback_; ///< Callback for publishing scans - NebulaPointCloudPtr current_scan_cloud_{new NebulaPointCloud()}; + FieldOfView fov_; + pointcloud_callback_t pointcloud_callback_; + NebulaPointCloudPtr current_scan_cloud_{std::make_shared()}; uint64_t packet_count_{0}; - // Implementation Items: Add member variables for: - // - Point cloud accumulation buffer - // - Previous azimuth for scan completion detection - // - Scan timestamp - // - Optionally: Double-buffering (two point cloud buffers) for handling scan overlap - // (required if azimuth correction is done in Nebula) - // - Any sensor-specific state }; } // namespace nebula::drivers diff --git a/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp b/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp index 980847f74..d28cce8b5 100644 --- a/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp +++ b/src/nebula_sample/nebula_sample_decoders/src/sample_decoder.cpp @@ -15,6 +15,7 @@ #include "nebula_sample_decoders/sample_decoder.hpp" #include +#include #include #include @@ -37,8 +38,9 @@ const char * to_cstr(const DecodeError error) SampleDecoder::SampleDecoder( FieldOfView fov /*, other decoder args */, pointcloud_callback_t pointcloud_cb) -: fov_(std::move(fov)), pointcloud_callback_(std::move(pointcloud_cb)) +: fov_(fov), pointcloud_callback_(std::move(pointcloud_cb)) { + // Implement: Initialize sensor-specific decode state (buffers, angle tracking, timestamps). } PacketDecodeResult SampleDecoder::unpack(const std::vector & packet) @@ -71,7 +73,7 @@ PacketDecodeResult SampleDecoder::unpack(const std::vector & packet) static_cast(std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()) .count()); - metadata.did_scan_complete = (packet_count_ % kPacketsPerDummyScan) == 0; + metadata.did_scan_complete = (packet_count_ % k_packets_per_dummy_scan) == 0; if (metadata.did_scan_complete) { const auto callback_begin = std::chrono::steady_clock::now(); @@ -81,7 +83,7 @@ PacketDecodeResult SampleDecoder::unpack(const std::vector & packet) static_cast(std::chrono::duration_cast( std::chrono::steady_clock::now() - callback_begin) .count()); - current_scan_cloud_.reset(new NebulaPointCloud()); + current_scan_cloud_ = std::make_shared(); } result.metadata_or_error = metadata; diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp index 2610c6cfb..e69cb1c46 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp @@ -26,61 +26,40 @@ namespace nebula::drivers { -/// @brief Hardware interface for Sample LiDAR communication -/// @details This class manages network communication with the sensor. -/// Responsibilities: -/// - Setting up UDP sockets for receiving data packets -/// - Starting/stopping packet reception -/// - Calling registered callbacks when packets arrive -/// - Optionally: sending commands to the sensor (start/stop scan, change settings, etc.) -/// -/// @note Advanced implementations may also include: -/// - TCP connections for sensor configuration commands -/// - HTTP API support for newer sensor models -/// - These are optional and can be added as needed for your specific sensor +/// @brief Receives raw sensor packets and forwards them to a registered callback. +/// @details This class owns the transport-facing state used by the sample driver. class SampleHwInterface { public: enum class Error : uint8_t { - CALLBACK_NOT_REGISTERED, - INVALID_CALLBACK, + CALLBACK_NOT_REGISTERED, ///< Start requested before a callback was registered. + INVALID_CALLBACK, ///< Empty callback passed to register_scan_callback. }; - explicit SampleHwInterface(SampleSensorConfiguration sensor_configuration); + static const char * to_cstr(Error error); - /// @brief Start receiving packets from the sensor - /// @return Nothing on success, error otherwise - /// @details Implement the following: - /// 1. Create UDP socket(s) for data reception - /// 2. Bind to the configured port(s) - /// 3. Start async receive loop - /// 4. Optionally: send start command to sensor + /// @brief Construct the hardware interface with connection settings. + /// @param connection_configuration Network addresses and ports used by the sensor stream. + explicit SampleHwInterface(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 receiving packets from the sensor - /// @return Nothing on success, error otherwise - /// @details Implement the following: - /// 1. Stop the receive loop - /// 2. Close UDP socket(s) - /// 3. Optionally: send stop command to sensor + /// @brief Stop packet reception. + /// @return std::monostate on success, Error on failure. util::expected sensor_interface_stop(); - /// @brief Register callback for incoming packets - /// @param scan_callback Function to call when a packet is received - /// @return Nothing on success, error otherwise - /// @details The callback receives raw packet data and metadata (timestamp, source IP, etc.) + /// @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: - SampleSensorConfiguration sensor_configuration_; ///< Sensor configuration - std::optional packet_callback_; ///< Packet callback - // Implementation Items: Add member variables for: - // - UDP socket instance(s) for data reception - // - IO context for async operations - // - Optionally: TCP driver for sensor configuration commands - // - Optionally: HTTP client for newer sensor models - // - Any sensor-specific state + ConnectionConfiguration connection_configuration_; + std::optional packet_callback_; }; } // namespace nebula::drivers diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp index 152d4f0da..d846b19b3 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp @@ -19,10 +19,10 @@ namespace nebula::drivers { -SampleHwInterface::SampleHwInterface(SampleSensorConfiguration sensor_configuration) -: sensor_configuration_(std::move(sensor_configuration)) +SampleHwInterface::SampleHwInterface(ConnectionConfiguration connection_configuration) +: connection_configuration_(std::move(connection_configuration)) { - // Constructor - initialize any member variables if needed + // Implement: Initialize non-pointcloud hardware connections, and sync/check sensor settings } util::expected SampleHwInterface::sensor_interface_start() @@ -31,22 +31,13 @@ util::expected SampleHwInterface::sens return Error::CALLBACK_NOT_REGISTERED; } - // Implementation Items: Implement sensor interface startup - // 1. Create UDP socket using connections::UdpSocket - // 2. Bind to the port specified in sensor_configuration_ - // 3. Start async receive loop - // 4. When packets arrive, call packet_callback_ with the packet data - // 5. Optionally: send HTTP/TCP command to sensor to start scanning - + // Implement: Create/bind UDP receiver and forward incoming packets to packet_callback_. return std::monostate{}; } util::expected SampleHwInterface::sensor_interface_stop() { - // Implementation Items: Implement sensor interface shutdown - // 1. Stop the receive loop - // 3. Optionally: send command to sensor to stop scanning - + // Implement: Stop receiver threads and release network resources (RAII preferred). return std::monostate{}; } @@ -56,8 +47,23 @@ util::expected SampleHwInterface::regi if (!scan_callback) { return Error::INVALID_CALLBACK; } + + // Implement: Depending on threading model, may need synchronization around packet_callback_ + // access. packet_callback_.emplace(std::move(scan_callback)); return std::monostate{}; } +const char * SampleHwInterface::to_cstr(SampleHwInterface::Error error) +{ + switch (error) { + case Error::CALLBACK_NOT_REGISTERED: + return "callback not registered"; + case Error::INVALID_CALLBACK: + return "invalid callback"; + default: + return "unknown hardware error"; + } +} + } // namespace nebula::drivers From b08432e38d3c2885e66f0c13bce194f5dc09300a Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 18 Feb 2026 22:36:10 +0900 Subject: [PATCH 60/65] docs: enhance integration guide with clearer implementation details and order of operations Signed-off-by: Max SCHMELLER --- docs/integration_guide.md | 22 +++++++++---------- .../nebula_sample/sample_ros_wrapper.hpp | 4 ++-- .../nebula_sample/src/sample_ros_wrapper.cpp | 1 - 3 files changed, 13 insertions(+), 14 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index b72af4f87..0a520a6d6 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -242,7 +242,7 @@ See [Verification](#verification) below. ## Implementation details The `nebula_sample` package provides a template implementation. Its source files contain -comments and TODOs throughout to guide you through the implementation. +comments and concise `Implement:` markers in key stubs to guide implementation. Some examples might not be relevant for your sensor, e.g. calibration handling. Implement only what is necessary for your sensor. @@ -279,10 +279,10 @@ Order of operations: 1. Parameter loading: Declare and read ROS parameters 2. Configuration validation: Validate IP addresses, ports, ranges -3. Decoder initialization: Create decoder with validated config -4. Callback registration: Register pointcloud callback +3. Publisher creation: Create ROS publishers +4. Decoder initialization: Create decoder and register pointcloud callback 5. HW interface initialization: Create and configure HW interface -6. Publisher creation: Create ROS publishers +6. Forward packets: Register packet callback to forward packets from HW interface to decoder 7. Stream start: Call `sensor_interface_start()` to begin receiving data ### Reconfiguration (optional) @@ -304,15 +304,15 @@ Detect and handle sensor disconnection: ### Shutdown sequence -Order of operations: - Prefer RAII-based shutdown: sockets/threads/buffers should be owned by objects whose destructors -stop/join/close automatically, so the wrapper does not require sensor-specific shutdown logic. +stop/join/close automatically, so the wrapper does not require sensor-specific resource cleanup. + +Order of operations: -1. Stop stream: Ensure receiver threads stop and join -2. Close sockets: Ensure all network resources are closed -3. Release buffers: Release point cloud buffers -4. Destroy owners: Destroy the owning objects (RAII) +1. Stop stream explicitly (e.g., `sensor_interface_stop()`) +2. Use RAII to clean up resources (sockets, threads, buffers) +3. Make sure that dependent components are still valid during shutdown (e.g., decoder should not be + destroyed before HW interface stops) ### Diagnostic reporting diff --git a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp index 81c7c60c5..0a9da5588 100644 --- a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp +++ b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp @@ -90,10 +90,10 @@ class SampleRosWrapper : public rclcpp::Node drivers::SampleSensorConfiguration config_; std::string frame_id_; + rclcpp::Publisher::SharedPtr points_pub_; + std::optional decoder_; std::optional hw_interface_; - - rclcpp::Publisher::SharedPtr points_pub_; }; } // namespace nebula::ros diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index d4e115061..fdb60d3be 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -24,7 +24,6 @@ #include #include -#include #include #include #include From acea4ec3f1beb3c054f796a5f121e7a0c252531d Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 19 Feb 2026 09:43:04 +0900 Subject: [PATCH 61/65] chore: address codex review feedback Signed-off-by: Max SCHMELLER --- .../nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp | 3 --- src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp index 0a9da5588..24e7514a1 100644 --- a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp +++ b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp @@ -67,9 +67,6 @@ class SampleRosWrapper : public rclcpp::Node /// @throws std::runtime_error on invalid configuration or startup failures. explicit SampleRosWrapper(const rclcpp::NodeOptions & options); - /// @brief Stop sensor streaming before destruction when initialized. - ~SampleRosWrapper() override; - private: /// @brief Publish a decoded pointcloud to ROS. /// @param pointcloud Decoded pointcloud from the decoder. diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index fdb60d3be..b10fa014c 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -160,7 +160,7 @@ void SampleRosWrapper::publish_pointcloud_callback( if (points_pub_->get_subscription_count() > 0 && pointcloud) { auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = now(); + ros_pc_msg_ptr->header.stamp = rclcpp::Time(timestamp_s); ros_pc_msg_ptr->header.frame_id = frame_id_; points_pub_->publish(std::move(ros_pc_msg_ptr)); } From 1d0b24c5e41c8aa6dbc9e8f9a92b2a9f1ead7f37 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 20 Feb 2026 15:11:25 +0900 Subject: [PATCH 62/65] feat: add diagnostics and error handling to SampleRosWrapper and SampleHwInterface Signed-off-by: Max SCHMELLER --- .../nebula_sample/CMakeLists.txt | 4 + .../config/sample_sensor.param.yaml | 13 ++ .../nebula_sample/sample_ros_wrapper.hpp | 59 +++++- src/nebula_sample/nebula_sample/package.xml | 2 + .../nebula_sample/src/sample_ros_wrapper.cpp | 168 +++++++++++++++--- .../sample_hw_interface.hpp | 26 ++- .../src/sample_hw_interface.cpp | 87 +++++++-- 7 files changed, 313 insertions(+), 46 deletions(-) diff --git a/src/nebula_sample/nebula_sample/CMakeLists.txt b/src/nebula_sample/nebula_sample/CMakeLists.txt index 8bad521ae..377920cff 100644 --- a/src/nebula_sample/nebula_sample/CMakeLists.txt +++ b/src/nebula_sample/nebula_sample/CMakeLists.txt @@ -29,9 +29,11 @@ target_link_libraries( ament_target_dependencies( sample_ros_wrapper PUBLIC + diagnostic_updater rclcpp rclcpp_components sensor_msgs + std_msgs pcl_conversions) rclcpp_components_register_node(sample_ros_wrapper PLUGIN "nebula::ros::SampleRosWrapper" @@ -57,8 +59,10 @@ ament_export_dependencies( nebula_core_hw_interfaces nebula_sample_hw_interfaces nebula_core_ros + diagnostic_updater rclcpp_components sensor_msgs + std_msgs pcl_conversions) ament_package() diff --git a/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml b/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml index c6108a527..e0ab6dd04 100644 --- a/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml +++ b/src/nebula_sample/nebula_sample/config/sample_sensor.param.yaml @@ -23,3 +23,16 @@ elevation: min_deg: -15.0 max_deg: 15.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: 10 diff --git a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp index 24e7514a1..4f68cd96b 100644 --- a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp +++ b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp @@ -18,17 +18,19 @@ #include "nebula_sample_decoders/sample_decoder.hpp" #include "nebula_sample_hw_interfaces/sample_hw_interface.hpp" +#include #include +#include +#include #include #include #include +#include #include -#include #include #include -#include #include namespace nebula::ros @@ -36,6 +38,12 @@ 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; }; @@ -57,17 +65,41 @@ util::expected load_config_from class SampleRosWrapper : public rclcpp::Node { public: - enum class Error : uint8_t { - HW_INTERFACE_NOT_INITIALIZED, ///< Stream start requested while HW interface is absent. - HW_STREAM_START_FAILED, ///< Underlying HW interface failed to start. + 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 SampleRosWrapper(const rclcpp::NodeOptions & options); + ~SampleRosWrapper() override; private: + 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. @@ -81,13 +113,22 @@ class SampleRosWrapper : public rclcpp::Node const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata); - static const char * to_cstr(Error error); - static const char * to_cstr(drivers::SampleHwInterface::Error error); + /// @brief Configure common diagnostics used by most sensor integrations. + void initialize_diagnostics(); + + /// @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::SampleSensorConfiguration config_; std::string frame_id_; - - rclcpp::Publisher::SharedPtr points_pub_; + Publishers publishers_; + Diagnostics diagnostics_; std::optional decoder_; std::optional hw_interface_; diff --git a/src/nebula_sample/nebula_sample/package.xml b/src/nebula_sample/nebula_sample/package.xml index 4b667c483..831dfe56a 100644 --- a/src/nebula_sample/nebula_sample/package.xml +++ b/src/nebula_sample/nebula_sample/package.xml @@ -12,6 +12,7 @@ autoware_cmake + diagnostic_updater nebula_core_common nebula_core_ros nebula_sample_common @@ -21,6 +22,7 @@ rclcpp rclcpp_components sensor_msgs + std_msgs ament_cmake diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index b10fa014c..7c5b1563c 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -21,8 +21,12 @@ #include #include +#include + #include +#include +#include #include #include #include @@ -35,6 +39,8 @@ namespace nebula::ros namespace { +constexpr double k_ns_to_ms = 1e-6; + template util::expected declare_required_parameter( rclcpp::Node & node, const std::string & name, @@ -43,7 +49,9 @@ util::expected declare_required_parameter( try { return node.declare_parameter(name, descriptor); } catch (const std::exception & e) { - return ConfigError{"Failed to declare/read parameter '" + name + "': " + e.what()}; + return ConfigError{ + ConfigError::Code::PARAMETER_DECLARATION_FAILED, + "Failed to declare/read parameter '" + name + "': " + e.what()}; } } } // namespace @@ -59,6 +67,12 @@ util::expected load_config_from 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()); @@ -66,16 +80,23 @@ util::expected load_config_from 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) { + if (data_port.value() <= 0 || data_port.value() > 65535) { return ConfigError{ - "Parameter 'connection.data_port' must be in [0, 65535], got " + - std::to_string(data_port.value())}; + 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()); @@ -109,7 +130,8 @@ util::expected load_config_from } SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) -: Node("nebula_sample_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)) +: Node("nebula_sample_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + diagnostics_(this) { const bool launch_hw = declare_parameter("launch_hw", true, param_read_only()); declare_parameter("sensor_model", "SampleSensor", param_read_only()); @@ -122,8 +144,16 @@ SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) } config_ = config_or_error.value(); - points_pub_ = + publishers_.points = create_publisher("points_raw", 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(); decoder_.emplace( config_.fov, [this](const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { @@ -139,42 +169,117 @@ SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) 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 sample sensor packet callback: " + - std::string(drivers::SampleHwInterface::to_cstr(callback_result.error()))); + "Failed to register sample sensor packet callback: " + error.message); } const auto stream_start_result = hw_interface_->sensor_interface_start(); if (!stream_start_result.has_value()) { - throw std::runtime_error( - "Failed to start sample sensor stream: " + - std::string(drivers::SampleHwInterface::to_cstr(stream_start_result.error()))); + const auto error = stream_start_result.error(); + throw std::runtime_error("Failed to start sample sensor stream: " + error.message); } + } else { + RCLCPP_INFO( + get_logger(), + "Hardware connection disabled (launch_hw:=false). Sample wrapper currently expects live UDP " + "packets only."); } } +SampleRosWrapper::~SampleRosWrapper() +{ + if (!hw_interface_) { + return; + } + + const auto stop_result = hw_interface_->sensor_interface_stop(); + if (!stop_result.has_value()) { + const auto error = stop_result.error(); + RCLCPP_WARN( + get_logger(), "Failed to stop sample sensor stream cleanly: %s", error.message.c_str()); + } +} + +void SampleRosWrapper::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 SampleRosWrapper::publish_pointcloud_callback( const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s) { - (void)timestamp_s; - if (points_pub_->get_subscription_count() > 0 && pointcloud) { + if (!pointcloud) { + return; + } + + if (publishers_.points->get_subscription_count() > 0) { auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = rclcpp::Time(timestamp_s); + ros_pc_msg_ptr->header.stamp = rclcpp::Time(static_cast(timestamp_s * 1e9)); ros_pc_msg_ptr->header.frame_id = frame_id_; - points_pub_->publish(std::move(ros_pc_msg_ptr)); + publishers_.points->publish(std::move(ros_pc_msg_ptr)); } + + diagnostics_.publish_rate->tick(); } void SampleRosWrapper::receive_cloud_packet_callback( const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata) { - (void)metadata; + diagnostics_.packet_liveness->tick(); + if (!decoder_) { return; } const auto decode_result = decoder_->unpack(packet); + publish_debug_durations( + metadata.packet_perf_counters.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.", @@ -182,12 +287,35 @@ void SampleRosWrapper::receive_cloud_packet_callback( } } -const char * SampleRosWrapper::to_cstr(const Error error) +void SampleRosWrapper::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 * SampleRosWrapper::to_cstr(const Error::Code code) { - switch (error) { - case Error::HW_INTERFACE_NOT_INITIALIZED: + switch (code) { + case Error::Code::HW_INTERFACE_NOT_INITIALIZED: return "hardware interface not initialized"; - case Error::HW_STREAM_START_FAILED: + case Error::Code::HW_STREAM_START_FAILED: return "hardware stream start failed"; default: return "unknown wrapper error"; diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp index e69cb1c46..005822193 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/include/nebula_sample_hw_interfaces/sample_hw_interface.hpp @@ -20,7 +20,10 @@ #include #include +#include +#include #include +#include #include namespace nebula::drivers @@ -31,12 +34,23 @@ namespace nebula::drivers class SampleHwInterface { public: - enum class Error : uint8_t { - CALLBACK_NOT_REGISTERED, ///< Start requested before a callback was registered. - INVALID_CALLBACK, ///< Empty callback passed to register_scan_callback. + /// @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 error); + 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. @@ -59,7 +73,9 @@ class SampleHwInterface private: ConnectionConfiguration connection_configuration_; - std::optional packet_callback_; + std::optional udp_socket_; + std::shared_ptr packet_callback_; + std::mutex callback_mutex_; }; } // namespace nebula::drivers diff --git a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp index d846b19b3..dd503231c 100644 --- a/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp +++ b/src/nebula_sample/nebula_sample_hw_interfaces/src/sample_hw_interface.cpp @@ -14,7 +14,11 @@ #include "nebula_sample_hw_interfaces/sample_hw_interface.hpp" +#include +#include +#include #include +#include namespace nebula::drivers { @@ -22,22 +26,71 @@ namespace nebula::drivers SampleHwInterface::SampleHwInterface(ConnectionConfiguration connection_configuration) : connection_configuration_(std::move(connection_configuration)) { - // Implement: Initialize non-pointcloud hardware connections, and sync/check sensor settings + // The sample driver is UDP-only. Real integrations can initialize control/sync channels here. } util::expected SampleHwInterface::sensor_interface_start() { + std::lock_guard lock(callback_mutex_); + if (!packet_callback_) { - return Error::CALLBACK_NOT_REGISTERED; + 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 { + udp_socket_.emplace( + connections::UdpSocket::Builder( + connection_configuration_.host_ip, connection_configuration_.data_port) + .bind()); + + // Callback can only be set while udp_socket_ is nullopt, so we don't need locking here + udp_socket_->subscribe( + [this]( + const 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()}; } - // Implement: Create/bind UDP receiver and forward incoming packets to packet_callback_. return std::monostate{}; } util::expected SampleHwInterface::sensor_interface_stop() { - // Implement: Stop receiver threads and release network resources (RAII preferred). + 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{}; } @@ -45,22 +98,32 @@ util::expected SampleHwInterface::regi connections::UdpSocket::callback_t scan_callback) { if (!scan_callback) { - return Error::INVALID_CALLBACK; + return Error{Error::Code::INVALID_CALLBACK, "Cannot register an empty packet callback"}; } - // Implement: Depending on threading model, may need synchronization around packet_callback_ - // access. - packet_callback_.emplace(std::move(scan_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 * SampleHwInterface::to_cstr(SampleHwInterface::Error error) +const char * SampleHwInterface::to_cstr(SampleHwInterface::Error::Code code) { - switch (error) { - case Error::CALLBACK_NOT_REGISTERED: + switch (code) { + case Error::Code::CALLBACK_NOT_REGISTERED: return "callback not registered"; - case Error::INVALID_CALLBACK: + 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"; } From 4edd64c8375386c14f1e566b351b2a5fc871f6f9 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 20 Feb 2026 16:07:26 +0900 Subject: [PATCH 63/65] feat: Make different launch_hw modes mutually explicit Signed-off-by: Max SCHMELLER --- .../nebula_sample/CMakeLists.txt | 2 + .../nebula_sample/sample_ros_wrapper.hpp | 39 +++++++- src/nebula_sample/nebula_sample/package.xml | 1 + .../nebula_sample/src/sample_ros_wrapper.cpp | 94 ++++++++++++++++--- 4 files changed, 124 insertions(+), 12 deletions(-) diff --git a/src/nebula_sample/nebula_sample/CMakeLists.txt b/src/nebula_sample/nebula_sample/CMakeLists.txt index 377920cff..6da37e514 100644 --- a/src/nebula_sample/nebula_sample/CMakeLists.txt +++ b/src/nebula_sample/nebula_sample/CMakeLists.txt @@ -30,6 +30,7 @@ ament_target_dependencies( sample_ros_wrapper PUBLIC diagnostic_updater + nebula_msgs rclcpp rclcpp_components sensor_msgs @@ -60,6 +61,7 @@ ament_export_dependencies( nebula_sample_hw_interfaces nebula_core_ros diagnostic_updater + nebula_msgs rclcpp_components sensor_msgs std_msgs diff --git a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp index 4f68cd96b..b214c543b 100644 --- a/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp +++ b/src/nebula_sample/nebula_sample/include/nebula_sample/sample_ros_wrapper.hpp @@ -25,12 +25,17 @@ #include #include +#include +#include #include #include #include +#include #include #include +#include +#include #include namespace nebula::ros @@ -83,6 +88,26 @@ class SampleRosWrapper : public rclcpp::Node ~SampleRosWrapper() 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::SampleHwInterface 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; @@ -113,9 +138,19 @@ class SampleRosWrapper : public rclcpp::Node const 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. @@ -131,7 +166,9 @@ class SampleRosWrapper : public rclcpp::Node Diagnostics diagnostics_; std::optional decoder_; - std::optional hw_interface_; + /// @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 diff --git a/src/nebula_sample/nebula_sample/package.xml b/src/nebula_sample/nebula_sample/package.xml index 831dfe56a..943bcf576 100644 --- a/src/nebula_sample/nebula_sample/package.xml +++ b/src/nebula_sample/nebula_sample/package.xml @@ -15,6 +15,7 @@ diagnostic_updater nebula_core_common nebula_core_ros + nebula_msgs nebula_sample_common nebula_sample_decoders nebula_sample_hw_interfaces diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index 7c5b1563c..70a57c79f 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -41,6 +42,13 @@ 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, @@ -131,7 +139,8 @@ util::expected load_config_from SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) : Node("nebula_sample_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), - diagnostics_(this) + diagnostics_(this), + runtime_mode_(std::monostate{}) { const bool launch_hw = declare_parameter("launch_hw", true, param_read_only()); declare_parameter("sensor_model", "SampleSensor", param_read_only()); @@ -161,8 +170,11 @@ SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) }); if (launch_hw) { - hw_interface_.emplace(config_.connection); - const auto callback_result = hw_interface_->register_scan_callback( + runtime_mode_.emplace(config_.connection); + auto & online_mode = std::get(runtime_mode_); + online_mode.packets_pub = + create_publisher("packets_raw", rclcpp::SensorDataQoS()); + const auto callback_result = online_mode.hw_interface.register_scan_callback( [this]( const std::vector & raw_packet, const drivers::connections::UdpSocket::RxMetadata & metadata) { @@ -174,26 +186,33 @@ SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) "Failed to register sample sensor packet callback: " + error.message); } - const auto stream_start_result = hw_interface_->sensor_interface_start(); + 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 sample sensor stream: " + error.message); } } else { + runtime_mode_.emplace(); + auto & offline_mode = std::get(runtime_mode_); + offline_mode.packets_sub = create_subscription( + "packets_raw", rclcpp::SensorDataQoS(), + [this](std::unique_ptr packets_msg) { + receive_packets_message_callback(std::move(packets_msg)); + }); RCLCPP_INFO( - get_logger(), - "Hardware connection disabled (launch_hw:=false). Sample wrapper currently expects live UDP " - "packets only."); + get_logger(), "Hardware connection disabled, listening for packets on %s", + offline_mode.packets_sub->get_topic_name()); } } SampleRosWrapper::~SampleRosWrapper() { - if (!hw_interface_) { + auto * online_mode = std::get_if(&runtime_mode_); + if (!online_mode) { return; } - const auto stop_result = hw_interface_->sensor_interface_stop(); + const auto stop_result = online_mode->hw_interface.sensor_interface_stop(); if (!stop_result.has_value()) { const auto error = stop_result.error(); RCLCPP_WARN( @@ -269,21 +288,74 @@ void SampleRosWrapper::receive_cloud_packet_callback( const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata) { diagnostics_.packet_liveness->tick(); + 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 SampleRosWrapper::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 SampleRosWrapper::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( - metadata.packet_perf_counters.receive_duration_ns, - decode_result.performance_counters.decode_time_ns, + 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(); } } From 8a880b57949d5627825cc9358e820f26cb3fce86 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 20 Feb 2026 18:10:58 +0900 Subject: [PATCH 64/65] feat: update topic names for point cloud and packet publishing in SampleRosWrapper Signed-off-by: Max SCHMELLER --- docs/integration_guide.md | 70 ++++++++++++++----- src/nebula_sample/README.md | 7 +- .../nebula_sample/src/sample_ros_wrapper.cpp | 6 +- 3 files changed, 60 insertions(+), 23 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 0a520a6d6..351e0a2f6 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -33,8 +33,6 @@ graph TB Sample["nebula_sample
sample_common
sample_decoders
sample_hw_interfaces
ROS wrapper"] end end - - ``` - Core packages provide reusable functionality (UDP sockets, point types, etc.) @@ -57,17 +55,48 @@ of Nebula as a library e.g. in ML pipelines without ROS 2. ### Data Flow -Packets flow from the hardware interface to the decoder to the ROS wrapper as follows: +`nebula_sample` supports two runtime modes controlled by `launch_hw`: ```mermaid flowchart LR - HW[Hardware Interface] -->| Raw packets | DC[Decoder] - DC -->| Nebula PointCloud | RW[ROS Wrapper] - RW -->| ROS 2 PointCloud2 | ROS[ROS 2] + + subgraph Online["launch_hw:=true (online)"] + direction LR + + S[Sensor] + HW[Hardware Interface] + DC[Decoder] + RW[ROS Wrapper] + PKT[(packets / NebulaPackets)] + + S-->|UDP| HW + HW -->|data packet| DC + HW -->|data packet| RW + DC -->|pointcloud| RW + RW -->|decoded scan's raw packets| PKT[(packets / NebulaPackets)] + end + + subgraph Offline["launch_hw:=false (offline replay)"] + direction LR + + PKT2[(packets / NebulaPackets)] + DC2[Decoder] + RW2[ROS Wrapper] + + PKT2 -->|replayed packets| DC2 + DC2 -->|pointcloud| RW2 + end + + PTS[(points / PointCloud2)] + + RW -->|decoded scan| PTS + RW2 -->|decoded scan| PTS ``` -Since decoder and HW interface are separate libraries, the ROS wrapper sets up the data flow -between them on startup by registering callbacks. +The ROS wrapper owns the runtime mode resources and wiring: + +- Online mode: hardware interface + packet publisher +- Offline mode: packet subscriber ## Component Library @@ -262,8 +291,8 @@ by adding it to `build_depends.repos`. Please ensure that the SDK is fit for automotive use cases (real-time, safety, reliability). Nebula interfaces like -- `/nebula_points` (including correct point types) -- `/nebula_packets` (publish and replay) +- pointcloud output topic (the sample template uses `/points` with `PointCloud2`) +- packet publish/replay topic (the sample template uses `/packets` with `NebulaPackets`) - `/diagnostics` - launch patterns @@ -281,9 +310,13 @@ Order of operations: 2. Configuration validation: Validate IP addresses, ports, ranges 3. Publisher creation: Create ROS publishers 4. Decoder initialization: Create decoder and register pointcloud callback -5. HW interface initialization: Create and configure HW interface -6. Forward packets: Register packet callback to forward packets from HW interface to decoder -7. Stream start: Call `sensor_interface_start()` to begin receiving data +5. Runtime mode setup: + - `launch_hw:=true`: create HW interface + packet publisher + - `launch_hw:=false`: create packet subscriber for replay +6. Packet forwarding: + - online: register HW callback, decode packets, publish accumulated `NebulaPackets` per scan + - offline: decode packets received from `NebulaPackets` subscription +7. Stream start (online only): call `sensor_interface_start()` ### Reconfiguration (optional) @@ -297,10 +330,9 @@ When parameters change at runtime: Detect and handle sensor disconnection: -1. Timeout detection: Monitor time since last packet -2. Diagnostic update: Set diagnostic status to `ERROR` -3. Logging: Log connection loss only on state changes (avoid log spam) -4. Recovery: Attempt reconnection transparently +1. Liveness timeout: Configure `diagnostics.packet_liveness.timeout_ms` +2. Diagnostic update: Tick `LivenessMonitor` on each packet path (online and replay) +3. Optional: Add explicit connection-loss logs/recovery if required by your sensor ### Shutdown sequence @@ -351,7 +383,7 @@ ros2 launch nebula nebula_launch.py sensor_model:=mysensor To record a rosbag of Nebula packet output for later replay: ```bash -ros2 bag record /nebula_packets -o mysensor_packets +ros2 bag record /packets -o mysensor_packets ``` ### Replay testing @@ -385,7 +417,7 @@ ros2 bag play mysensor_packets - [ ] Implemented startup sequence - [ ] Implemented shutdown sequence - [ ] Added diagnostic reporting -- [ ] Added connection loss handling +- [ ] Added packet liveness diagnostics ### Verification tasks diff --git a/src/nebula_sample/README.md b/src/nebula_sample/README.md index 8bb1680f9..3ef3468a8 100644 --- a/src/nebula_sample/README.md +++ b/src/nebula_sample/README.md @@ -24,7 +24,10 @@ colcon build --packages-up-to nebula_sample ## Running ```bash +# Online mode (with hardware) ros2 launch nebula_sample nebula_sample.launch.xml +# Offline mode (replay from rosbag) +ros2 launch nebula_sample nebula_sample.launch.xml launch_hw:=false ``` ## Using as a template @@ -56,7 +59,9 @@ The guide covers: ### ROS wrapper - `SampleRosWrapper` - ROS 2 node wrapping the driver -- Point cloud publisher on `/points_raw` +- Point cloud publisher on `/points` +- Packet publish/replay topic on `/packets` (`nebula_msgs/msg/NebulaPackets`) depending on + runtime mode (`launch_hw` parameter) ## Reference implementation diff --git a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp index 70a57c79f..11e14f91f 100644 --- a/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp +++ b/src/nebula_sample/nebula_sample/src/sample_ros_wrapper.cpp @@ -154,7 +154,7 @@ SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) config_ = config_or_error.value(); publishers_.points = - create_publisher("points_raw", rclcpp::SensorDataQoS()); + create_publisher("points", rclcpp::SensorDataQoS()); publishers_.receive_duration_ms = create_publisher("debug/receive_duration_ms", 10); publishers_.decode_duration_ms = @@ -173,7 +173,7 @@ SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) runtime_mode_.emplace(config_.connection); auto & online_mode = std::get(runtime_mode_); online_mode.packets_pub = - create_publisher("packets_raw", rclcpp::SensorDataQoS()); + create_publisher("packets", rclcpp::SensorDataQoS()); const auto callback_result = online_mode.hw_interface.register_scan_callback( [this]( const std::vector & raw_packet, @@ -195,7 +195,7 @@ SampleRosWrapper::SampleRosWrapper(const rclcpp::NodeOptions & options) runtime_mode_.emplace(); auto & offline_mode = std::get(runtime_mode_); offline_mode.packets_sub = create_subscription( - "packets_raw", rclcpp::SensorDataQoS(), + "packets", rclcpp::SensorDataQoS(), [this](std::unique_ptr packets_msg) { receive_packets_message_callback(std::move(packets_msg)); }); From 02d40bac1af6015da84eba03523a5354ab81688b Mon Sep 17 00:00:00 2001 From: David Wong Date: Mon, 9 Mar 2026 12:01:19 +0900 Subject: [PATCH 65/65] docs(sample_sensor): typo and add markdown links Signed-off-by: David Wong --- docs/integration_guide.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/integration_guide.md b/docs/integration_guide.md index 351e0a2f6..e7be6ece5 100644 --- a/docs/integration_guide.md +++ b/docs/integration_guide.md @@ -7,7 +7,7 @@ template implementation (`nebula_sample`) and reusable components to simplify th ### Overall package structure -Nebula is organized into corepackages and vendor-specific packages: +Nebula is organized into core packages and vendor-specific packages: ```mermaid graph TB @@ -434,8 +434,8 @@ ros2 bag play mysensor_packets - Hesai implementation: `src/nebula_hesai` - Full reference implementation - Velodyne implementation: `src/nebula_velodyne` - Alternative reference - Core components: `src/nebula_core` - Reusable building blocks -- Point types: See `docs/point_types.md` -- Parameters: See `docs/parameters.md` +- Point types: See [`docs/point_types.md`](point_types.md) +- Parameters: See [`docs/parameters.md`](parameters.md) ## Getting help