diff --git a/.clang-format b/.clang-format index b41fae912..82c94e663 100644 --- a/.clang-format +++ b/.clang-format @@ -45,3 +45,6 @@ IncludeCategories: - Regex: '".*"' Priority: 1 CaseSensitive: true +--- +Language: Json +BasedOnStyle: llvm diff --git a/.cspell.json b/.cspell.json index 4f17f7ddd..4c14cb9f3 100644 --- a/.cspell.json +++ b/.cspell.json @@ -5,45 +5,57 @@ "words": [ "adctp", "Adctp", + "applicate", "AT", + "autosar", "block_id", + "Bpearl", + "calib", + "centi", + "ddeg", + "DHAVE", + "Difop", "extrinsics", + "fout", "gprmc", + "gptp", + "Helios", "Hesai", + "horiz", + "Idat", + "ipaddr", + "manc", "memcpy", "mkdoxy", + "Msop", "nohup", "nproc", + "nsec", + "ntoa", "pandar", "PANDAR", "PANDARAT", "PANDARQT", "PANDARXT", "Pdelay", + "Piyush", + "piyushk", "QT", "rclcpp", "schedutil", + "srvs", "STD_COUT", "stds", "struct", "structs", "UDP_SEQ", + "usec", "vccint", "Vccint", + "Vdat", + "Wbitwise", "XT", "XTM", - "DHAVE", - "Bpearl", - "Helios", - "Msop", - "Difop", - "gptp", - "Idat", - "Vdat", - "autosar", - "srvs", - "manc", - "ipaddr", - "ntoa" + "yukkysaito" ] } diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 354a1b3d3..cdd07d2e3 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -54,4 +54,3 @@ jobs: fail_ci_if_error: false verbose: true flags: differential - diff --git a/.github/workflows/documentation.yml b/.github/workflows/documentation.yml index b4acc7f2e..48d8dad35 100644 --- a/.github/workflows/documentation.yml +++ b/.github/workflows/documentation.yml @@ -1,28 +1,44 @@ +# cspell: ignore nojekyll name: documentation on: + push: + branches: + - develop + - main pull_request: types: [closed] permissions: contents: write + id-token: write + pages: write jobs: deploy: runs-on: ubuntu-latest - if: github.event.pull_request.merged == true + if: github.event_name == 'push' || github.event.pull_request.merged == true steps: - - uses: actions/checkout@v3 - - uses: actions/cache@v3 + - uses: actions/checkout@v4 + - uses: actions/cache@v4 with: key: ${{ github.ref }} path: .cache - uses: actions/setup-python@v4 with: python-version: 3.x + - uses: actions/setup-python@v5 + with: + python-version: 3.x - run: | sudo apt update && sudo apt install -y doxygen && \ - pip install mkdocs-material \ - mkdocs \ + pip install mkdocs \ mkdocs-material \ Jinja2 \ ruamel.yaml && \ pip install git+https://github.com/JakubAndrysek/mkdoxy - - run: mkdocs gh-deploy --force + - run: mkdocs build --site-dir _site + - run: touch _site/.nojekyll + - run: | + chmod -c -R +rX "_site/" | while read line; do + echo "::warning title=Invalid file permissions automatically fixed::$line" + done + - uses: actions/upload-pages-artifact@v3 + - uses: actions/deploy-pages@v4 diff --git a/.github/workflows/json-schema-check.yaml b/.github/workflows/json-schema-check.yaml new file mode 100644 index 000000000..bac83a126 --- /dev/null +++ b/.github/workflows/json-schema-check.yaml @@ -0,0 +1,39 @@ +name: json-schema-check + +on: + pull_request: + workflow_dispatch: + +jobs: + check-if-relevant-files-changed: + runs-on: ubuntu-latest + outputs: + run-check: ${{ steps.paths_filter.outputs.json_or_yaml }} + steps: + - uses: actions/checkout@v4 + - uses: dorny/paths-filter@v3 + id: paths_filter + with: + filters: | + json_or_yaml: + - '**/schema/*.schema.json' + - '**/config/**/*.param.yaml' + + json-schema-check: + needs: check-if-relevant-files-changed + if: needs.check-if-relevant-files-changed.outputs.run-check == 'true' + runs-on: ubuntu-latest + steps: + - name: Check out repository + uses: actions/checkout@v4 + + - name: Run json-schema-check + uses: autowarefoundation/autoware-github-actions/json-schema-check@main + + no-relevant-changes: + needs: check-if-relevant-files-changed + if: needs.check-if-relevant-files-changed.outputs.run-check == 'false' + runs-on: ubuntu-latest + steps: + - name: Dummy step + run: echo "No relevant changes, passing check" diff --git a/.gitignore b/.gitignore index e966ffdfb..f068ec8be 100644 --- a/.gitignore +++ b/.gitignore @@ -29,3 +29,6 @@ site/ # qcreator stuff CMakeLists.txt.user + +# pre-commit +node_modules/ diff --git a/.markdown-link-check.json b/.markdown-link-check.json index c71a3e425..03b977748 100644 --- a/.markdown-link-check.json +++ b/.markdown-link-check.json @@ -1,5 +1,9 @@ { - "aliveStatusCodes": [200, 206, 403], + "aliveStatusCodes": [ + 200, + 206, + 403 + ], "ignorePatterns": [ { "pattern": "^http://localhost" diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 13c31ce62..9bc48e55e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,13 +4,14 @@ ci: repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.1.0 + rev: v4.6.0 hooks: - id: check-json - id: check-merge-conflict - id: check-toml - id: check-xml - id: check-yaml + args: [--allow-multiple-documents] - id: detect-private-key - id: end-of-file-fixer - id: mixed-line-ending @@ -18,51 +19,51 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.30.0 + rev: v0.40.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] - repo: https://github.com/pre-commit/mirrors-prettier - rev: v2.5.1 + rev: v3.1.0 hooks: - id: prettier - repo: https://github.com/adrienverge/yamllint - rev: v1.26.3 + rev: v1.35.1 hooks: - id: yamllint - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.4.0 + rev: v0.8.0 hooks: - id: prettier-package-xml - id: sort-package-xml - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.8.0.3 + rev: v0.10.0.1 hooks: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.4.2-1 + rev: v3.8.0-1 hooks: - id: shfmt args: [-w, -s, -i=4] - repo: https://github.com/pycqa/isort - rev: 5.12.0 + rev: 5.13.2 hooks: - id: isort - repo: https://github.com/psf/black - rev: 22.1.0 + rev: 24.4.2 hooks: - id: black args: [--line-length=100] - repo: https://github.com/PyCQA/flake8 - rev: 4.0.1 + rev: 7.0.0 hooks: - id: flake8 additional_dependencies: @@ -78,12 +79,12 @@ repos: ] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v13.0.0 + rev: v18.1.5 hooks: - id: clang-format - repo: https://github.com/cpplint/cpplint - rev: 1.5.5 + rev: 1.6.1 hooks: - id: cpplint args: [--quiet] diff --git a/.prettierignore b/.prettierignore index a3c34d00a..21c2dd9fd 100644 --- a/.prettierignore +++ b/.prettierignore @@ -1,2 +1,3 @@ -*.param.yaml +*.yaml *.rviz +*.json diff --git a/.yamllint.yaml b/.yamllint.yaml index 2c7bd088e..7837bfe0a 100644 --- a/.yamllint.yaml +++ b/.yamllint.yaml @@ -1,8 +1,5 @@ extends: default -ignore: | - *.param.yaml - rules: braces: level: error @@ -13,6 +10,8 @@ rules: document-start: level: error present: false # Don't need document start markers + ignore: + - .clang-format # Needs '---' between languages line-length: disable # Delegate to Prettier truthy: level: error @@ -20,3 +19,7 @@ rules: quoted-strings: level: error required: only-when-needed # To keep consistent style + indentation: + spaces: consistent + indent-sequences: true + check-multi-line-strings: false diff --git a/README.md b/README.md index a509950ca..52b87964a 100644 --- a/README.md +++ b/README.md @@ -1,29 +1,37 @@ -# Nebula Sensor Driver +# Nebula + +## Welcome to Nebula, the universal sensor driver Nebula is a sensor driver platform that is designed to provide a unified framework for as wide a variety of devices as possible. While it primarily targets Ethernet-based LiDAR sensors, it aims to be easily extendable to support new sensors and interfaces. -Nebula provides the following features: +Nebula works with ROS 2 and is the recommended sensor driver for the [Autoware](https://autoware.org/) project. + +## Documentation -- Support for Velodyne and Hesai sensors, with other LiDAR vendor support under development -- ROS 2 interface implementations -- TCP/IP and UDP communication implementations -- Abstraction of sensor decoders and hardware interfaces available as libraries -- Handling of standard LiDAR functionality, including but not limited to: - - Configuration of communication settings such as sensor and host IP addresses and communication ports - - Configuration of scan speed, synchronization settings, scan phase, and field of view - - Receiving and conversion of UDP packet data into point clouds in Cartesian co-ordinates - - Receiving and interpretation of diagnostics information from the sensor - - Support for multiple return modes and labelling of return types for each point +We recommend you get started with the [Nebula Documention](https://tier4.github.io/nebula/). +Here you will find information about the background of the project, how to install and use with ROS 2, and also how to add new sensors to the Nebula driver. -With a rapidly increasing number of sensor types and models becoming available, and varying levels of vendor and third-party driver support, Nebula creates a centralized driver methodology. We hope that this project will be used to facilitate active collaboration and efficiency in development projects by providing a platform that reduces the need to re-implement and maintain many different sensor drivers. Contributions to extend the supported devices and features of Nebula are always welcome. +- [About Nebula](https://tier4.github.io/nebula/about) +- [Design](https://tier4.github.io/nebula/design) +- [Supported Sensors](https://tier4.github.io/nebula/supported_sensors) +- [Installation](https://tier4.github.io/nebula/installation) +- [Launching with ROS 2](https://tier4.github.io/nebula/usage) +- [Parameters](https://tier4.github.io/nebula/parameters) +- [Point cloud types](https://tier4.github.io/nebula/point_types) +- [Contributing](https://tier4.github.io/nebula/contribute) +- [Tutorials](https://tier4.github.io/nebula/tutorials) -## How to build +## Quick start -Nebula builds on ROS Galactic and Humble. +Nebula builds with ROS 2 Galactic and Humble. > **Note** > -> A [TCP enabled version of ROS' Transport Driver](https://github.com/MapIV/transport_drivers/tree/tcp) is required to use Nebula. +> Boost version 1.74.0 or later is required. A manual install may be required in Ubuntu versions earlier than 22.04. +> +> **Note** +> +> A [TCP enabled version of ROS' Transport Driver](https://github.com/mojomex/transport_drivers/tree/mutable-buffer-in-udp-callback) is required to use Nebula. > It is installed automatically into your workspace using the below commands. However, if you already have ROS transport driver binaries installed, you will have to uninstall them to avoid conflicts (replace `humble` with your ROS distribution): > `sudo apt remove ros-humble-udp-driver ros-humble-io-context` @@ -40,242 +48,14 @@ rosdep install --from-paths src --ignore-src -y -r colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release ``` -## How to run tests - -Run tests: - -```bash -colcon test --event-handlers console_cohesion+ --packages-above nebula_common -``` - -Show results: - -```bash -colcon test-result --all -``` - -## Generic launch file - -You can easily run the sensor hardware interface, the sensor hardware monitor and sensor driver using (e.g. Pandar64): - -```bash -ros2 launch nebula_ros nebula_launch.py sensor_model:=Pandar64 -``` - -If you don't want to launch the hardware (i.e. when you are working from a rosbag), set the `launch_hw` flag to false: - -```bash -ros2 launch nebula_ros nebula_launch.py sensor_model:=Pandar64 launch_hw:=false -``` - -If you don't want the hardware driver to perform the sensor configuration communication (i.e. limited number of connections) set the `setup_sensor` flag to false: - -```bash -ros2 launch nebula_ros nebula_launch.py sensor_model:=Pandar64 setup_sensor:=false -``` - -You should ideally provide a config file for your specific sensor, but default ones are provided `nebula_drivers/config`: - -```bash -ros2 launch nebula_ros nebula_launch.py sensor_model:=Pandar64 config_file:=your_sensor.yaml -``` - -## Supported sensors - -Supported models, where sensor_model is the ROS param to be used at launch: - -| Manufacturer | Model | sensor_model | Configuration file | Test status | -| ------------ | ------------- | ------------ | ------------------ | -------------------- | -| HESAI | Pandar 64 | Pandar64 | Pandar64.yaml | :heavy_check_mark: | -| HESAI | Pandar 40P | Pandar40P | Pandar40P.yaml | :heavy_check_mark: | -| HESAI | Pandar XT32 | PandarXT32 | PandarXT32.yaml | :heavy_check_mark: | -| HESAI | Pandar XT32M | PandarXT32M | PandarXT32M.yaml | :warning: | -| HESAI | Pandar QT64 | PandarQT64 | PandarQT64.yaml | :heavy_check_mark: | -| HESAI | Pandar QT128 | PandarQT128 | PandarQT128.yaml | :warning: | -| HESAI | Pandar AT128 | PandarAT128 | PandarAT128.yaml | :heavy_check_mark:\* | -| HESAI | Pandar 128E4X | Pandar128E4X | Pandar128E4X.yaml | :warning: | -| Velodyne | VLP-16 | VLP16 | VLP16.yaml | :warning: | -| Velodyne | VLP-16-HiRes | VLP16 | | :x: | -| Velodyne | VLP-32 | VLP32 | VLP32.yaml | :warning: | -| Velodyne | VLS-128 | VLS128 | VLS128.yaml | :warning: | -| Continental | ARS548 | ARS548 | ARS548.yaml | :warning: | - -Test status:\ -:heavy_check_mark:: complete\ -:warning:: some functionality yet to be tested\ -:x:: untested\ -\*: AT128 needs software version 3.50.8 or newer for the `scan_angle` setting to work correctly. - -## ROS parameters - -### Common ROS parameters - -Parameters shared by all supported models: - -| Parameter | Type | Default | Accepted values | Description | -| ------------ | ------ | ---------------- | -------------------------- | ---------------- | -| sensor_model | string | | See supported models | | -| return_mode | string | | See supported return modes | | -| frame_id | string | Sensor dependent | | ROS frame ID | -| scan_phase | double | 0.0 | degrees [0.0, 360.0] | Scan start angle | - -### Hesai specific parameters - -#### Supported return modes per model - -| Sensor model | return_mode | Mode | -| ------------ | -------------- | ------ | -| Pandar XT32M | Last | Single | -| Pandar XT32M | Strongest | Single | -| Pandar XT32M | LastStrongest | Dual | -| Pandar XT32M | First | Single | -| Pandar XT32M | LastFirst | Dual | -| Pandar XT32M | FirstStrongest | Dual | -| Pandar XT32M | Dual | Dual | -| --- | --- | --- | -| Pandar AT128 | Last | Single | -| Pandar AT128 | Strongest | Single | -| Pandar AT128 | LastStrongest | Dual | -| Pandar AT128 | First | Single | -| Pandar AT128 | LastFirst | Dual | -| Pandar AT128 | FirstStrongest | Dual | -| Pandar AT128 | Dual | Dual | -| --- | --- | --- | -| Pandar QT128 | Last | Single | -| Pandar QT128 | Strongest | Single | -| Pandar QT128 | LastStrongest | Dual | -| Pandar QT128 | First | Single | -| Pandar QT128 | LastFirst | Dual | -| Pandar QT128 | FirstStrongest | Dual | -| Pandar QT128 | Dual | Dual | -| --- | --- | --- | -| Pandar QT64 | Last | Single | -| Pandar QT64 | Dual | Dual | -| Pandar QT64 | First | Single | -| --- | --- | --- | -| Pandar 40P | Last | Single | -| Pandar 40P | Strongest | Single | -| Pandar 40P | Dual | Dual | -| --- | --- | --- | -| Pandar 64 | Last | Single | -| Pandar 64 | Strongest | Single | -| Pandar 64 | Dual | Dual | - -#### Hardware interface parameters - -| Parameter | Type | Default | Accepted values | Description | -| ------------------------------ | ------ | --------------- | ----------------- | ------------------------------ | -| frame_id | string | hesai | | ROS frame ID | -| sensor_ip | string | 192.168.1.201 | | Sensor IP | -| host_ip | string | 255.255.255.255 | | Host IP | -| data_port | uint16 | 2368 | | Sensor port | -| gnss_port | uint16 | 2369 | | GNSS port | -| frequency_ms | uint16 | 100 | milliseconds, > 0 | Time per scan | -| packet_mtu_size | uint16 | 1500 | | Packet MTU size | -| rotation_speed | uint16 | 600 | | Rotation speed | -| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | -| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | -| dual_return_distance_threshold | double | 0.1 | | Dual return distance threshold | -| diag_span | uint16 | 1000 | milliseconds, > 0 | Diagnostic span | -| setup_sensor | bool | True | True, False | Configure sensor settings | - -#### Driver parameters - -| Parameter | Type | Default | Accepted values | Description | -| ---------------- | ------ | ------- | --------------- | ---------------------- | -| frame_id | string | hesai | | ROS frame ID | -| calibration_file | string | | | LiDAR calibration file | -| correction_file | string | | | LiDAR correction file | - -### Velodyne specific parameters - -#### Supported return modes - -| return_mode | Mode | -| --------------- | ------------------ | -| SingleFirst | Single (First) | -| SingleStrongest | Single (Strongest) | -| SingleLast | Single (Last) | -| Dual | Dual | - -#### Hardware interface parameters - -| Parameter | Type | Default | Accepted values | Description | -| --------------- | ------ | --------------- | ----------------- | --------------- | -| frame_id | string | velodyne | | ROS frame ID | -| sensor_ip | string | 192.168.1.201 | | Sensor IP | -| host_ip | string | 255.255.255.255 | | Host IP | -| data_port | uint16 | 2368 | | Sensor port | -| gnss_port | uint16 | 2369 | | GNSS port | -| frequency_ms | uint16 | 100 | milliseconds, > 0 | Time per scan | -| packet_mtu_size | uint16 | 1500 | | Packet MTU size | -| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | -| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | - -#### Driver parameters - -| Parameter | Type | Default | Accepted values | Description | -| ---------------- | ------ | -------- | ---------------- | ----------------------------- | -| frame_id | string | velodyne | | ROS frame ID | -| calibration_file | string | | | LiDAR calibration file | -| min_range | double | 0.3 | meters, >= 0.3 | Minimum point range published | -| max_range | double | 300.0 | meters, <= 300.0 | Maximum point range published | -| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | -| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | - -## Software design overview - -![DriverOrganization](docs/diagram.png) - -## Hesai Sensor Setup - -New Hesai sensors do not provide a Web UI to verify and set up the sensor parameters. Instead, these offer a TCP-based protocol to obtain and set the configuration. Nebula sets these sensors at launch. However, settings such as Destination IP, Sensor IP, IP Mask, and Data Port might cause undesired sensor functioning if the driver gets mistakenly launched with inappropriate values. To overcome this problem, Nebula provides an additional setup script for these settings to avoid such scenarios. - -The script requires the installation of dependencies via pip: - -`$ pip3 install scripts/requirements.txt # first-time setup` - -Once the dependencies are installed, the setup script can be invoked using the following command: - -`$ python3 scripts/hesai_config --sensor-ip X.X.X.X` - -To set the parameters, please use the corresponding arguments as defined below: - -``` -usage: hesai_config.py [-h] --sensor-ip SENSOR_IP [--destination-ip DESTINATION_IP] - [--data-port DATA_PORT] [--new-sensor-ip NEW_SENSOR_IP] [--mask MASK] - -options: - -h, --help Show this help message and exit - --sensor-ip SENSOR_IP - The current sensor IP address - --destination-ip DESTINATION_IP - Change the current destination IP address to the given one - --data-port DATA_PORT - Change the current destination LiDAR data port to the given one - --new-sensor-ip NEW_SENSOR_IP - Change the current sensor IP address to the given one - --mask MASK Change the current net mask to the given one. You can pass it in either a - CIDR notation or dotted-decimal notation. -``` - -## How to evaluate performance - -You can evaluate Nebula performance on a given rosbag and sensor model using the below tools. -The profiling runner is most accurate when assigning isolated cores via the `-c `. -CPU frequencies are locked/unlocked automatically by the runner to increase repeatability. - -Run profiling for each version you want to compare: +To launch Nebula as a ROS 2 node with default parameters for your sensor model: ```bash -./scripts/profiling_runner.bash baseline -m Pandar64 -b ~/my_rosbag -c 2 -t 20 -n 3 -git checkout my_improved_branch -./scripts/profiling_runner.bash improved -m Pandar64 -b ~/my_rosbag -c 2 -t 20 -n 3 +ros2 launch nebula_ros *sensor_vendor_name*_launch_all_hw.xml sensor_model:=*sensor_model_name* ``` -Show results: +For example, for a Hesai Pandar40P sensor: ```bash -pip3 install scripts/requirements.txt # first-time setup -python3 scripts/plot_times.py baseline improved +ros2 launch nebula_ros hesai_launch_all_hw.xml sensor_model:=Pandar40P ``` diff --git a/build_depends.repos b/build_depends.repos index e28379f1a..337cf8720 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -2,5 +2,10 @@ repositories: # TCP version of transport drivers transport_drivers: type: git - url: https://github.com/MapIV/transport_drivers - version: boost + url: https://github.com/mojomex/transport_drivers + version: feat-set-rmem + # Continental compatible version of ROS 2 socket CAN + ros2_socketcan: + type: git + url: https://github.com/knzo25/ros2_socketcan + version: feat/continental_fd diff --git a/docs/about.md b/docs/about.md index e69de29bb..06a9f909e 100644 --- a/docs/about.md +++ b/docs/about.md @@ -0,0 +1,3 @@ +# About Nebula + +WIP, please check back soon! diff --git a/docs/add_sensor.md b/docs/add_sensor.md deleted file mode 100644 index 73e1c183b..000000000 --- a/docs/add_sensor.md +++ /dev/null @@ -1,7 +0,0 @@ -# How to add your sensor - -1. Add your sensor to the `SensorModel` enumeration class located in the `nebula_common.hpp` header inside the `nebula_common` package. -2. Add your sensor model string to the `SensorModelFromString` method in the `nebula_common.hpp` header inside the `nebula_common` package. -3. Write the sensor decoder for your sensor. This class is in charge of converting raw packets to point clouds. This class should implement the abstract class defined in `nebula_driver_base.hpp` inside the `nebula_decoders` package. Add methods as the sensor requires. -4. Write the sensor hardware interface. This class is in charge of obtaining raw packets from the sensor using the `transport_drivers` library, accumulating them to form a scan, and making them available for consumption through callbacks. This class should implement the `nebula_hw_interface_base.hpp` inside the `nebula_hw_interfaces` package. Add methods as the sensor requires. -5. Write the ROS wrappers. The ROS wrappers use the sensor libraries to obtain raw data, decode it and convert it to PointCloud2 ROS messages. The Decoder wrapper receives the configuration and calibration data from files and sends them as structures to the decoder and the hw_interface. diff --git a/docs/contribute.md b/docs/contribute.md new file mode 100644 index 000000000..f0a80b161 --- /dev/null +++ b/docs/contribute.md @@ -0,0 +1,3 @@ +# Contributing to Nebula + +WIP - please check back soon! diff --git a/docs/design.md b/docs/design.md index 30ef4a09f..de352d4a9 100644 --- a/docs/design.md +++ b/docs/design.md @@ -1,102 +1,5 @@ -# Nebula requirements +# Nebula design -The driver follows the following functional requirements: -ドライバは、次の機能的な要求に応えなければならない。 - -## Hardware interface independent - -The data acquisition and communication with the sensor must not be bound to a specific hardware interface. -センサでのデータ取得と通信は、特定のハードウェアに縛られてはならない。 - -### Why? - -Writing a general-purpose driver independent of the interface will support different types of hardware interfaces. -インターフェイスに依存しない汎用ドライバを書くことで、異なったタイプのハードウェアインターフェイスに対応することができる。 - -## Sensor Control - -The driver should obtain, set, and confirm the desired sensor configuration at launch, i.e., scan frequency, synchronization methods, etc. -ドライバは起動時に、望ましいセンサ設定（スキャン周波数、同期方法等）を取得、設定、確認できる必要がある。 - -### Why? - -The sensor control will ensure that the sensor works in the expected mode as it was initially intended. -これによって、センサが当初より意図していたモードで動いていることが確認できる。 - -## Configurable output cloud - -The ROS wrapper should be able to define the desired output format of the point cloud. i.e., customize the fields to be contained in the final output cloud. -ROS ラッパーは点群の望ましい出力フォーマットで定義できなければならない（最終的な出力クラウドに含まれるフィールドをカスタマイズできる） -In addition, it should be able to generate the corresponding 2D range image for the output point cloud. - -さらに、出力された点群を対応する二次元レンジ画像に生成することができなければならない。 -If the sensor allows it, have an option to add a minimum and maximum range distance as an option. For instance, if the user launches the driver with a range limit set to 0, the driver will not perform any filtering. -センサが許可している場合は、オプションとして最小値と最大値のレンジ距離を追加することができる。例えば、ユーザーが範囲制限を 0 に設定してドライバーを起動した場合、ドライバはフィルタリングを行わない。 -The driver should have an option to define if the output cloud is of a fixed size or dynamic size. If a fixed size is selected, the output cloud must include NaN values for those lasers without any return. If a dynamic size is selected, the lasers with no valid returns are to be removed. -ドライバは、出力クラウドを固定されたサイズか、もしくはダイナミックサイズにするかを定義するオプションが必要である。固定されたサイズを選択した場合、出力クラウドは、リターンのないレーザの NaN 値を含まなければならない。ダイナミックサイズを選択した場合、リターンのないレーザは削除される。 -Default behavior: Dynamic, remove invalid laser returns (NaN). -デフォルトの動作：ダイナミック、無効なレーザリターンを削除。（NaN) - -### Why? - -The configurable output will allow the generation of the point cloud according to the expected use case application. -これによって、予定したユースケースアプリケーションにより必要な出力を生成することができる。 - -## Sensor Metadata - -The driver should include the following metadata for each generated point cloud: -ドライバは、生成された点群それぞれに、次のメタデータが含まれなければならない： - -### Calibration data キャリブレーションデータ - -Contains the sensor calibration parameters used to generate the point cloud from the raw data. -生データから点群を生成するのに使用したセンサキャリブレーションパラメータ - -### Sensor settings センサ設定 - -The configuration mode in which the sensor is being executed: -センサが実行された設定モード - -### Synchronization mode (PTP, PPS/NMEA)　同期モード(PTP, PPS/NMEA) - -Sensor Type -センサタイプ -Sensor Model -センサモデル -Scan frequency -スキャン周波数 - -### Why? - -The processed point cloud or the raw data does not always contain the state on which the sensor was run. Moreover, having this information at hand will help identify, classify and understand the situation of the recorded data. -処理した点群、つまり生データは、常にセンサが作動した状態を含んでいるとは限らない。手元に情報があることは、記録データの状況を素早く把握し、分類し、理解することの助けになる。 - -## Multi echo compatible - -The sensor should consider the possibility of the future inclusion of more than two echos. -センサは２つ以上のエコーが将来的に含まれる可能性があることを考慮すべきである。 - -### Why? - -Multi-echo support will help to future-proof the driver according to new features included or used in new sensors. In addition, multiple echo support has been proven to improve sensor resilience against weather conditions such as rain, fog, and snow. -これによって、新しいセンサに搭載された、もしくは、開発された新機能によって、ドライバの将来性を高めることができる。複数のエコーに対応することで、雨や霧、雪などの天候条件に対して、センサの回復力が改善されることが証明されている。 - -## ROS independent - -The objects used inside the driver must be ROS independent. -ドライバ内に使用されている API 等の対象物は、ROS に依存してはならない。 - -### Why? - -The third-party dependency reduction allows any software to be quickly updated without waiting for external dependencies to be updated. -これにより、どんなソフトウェアでも、アップデートのための外部依存を待つ必要がなく、素早くアップデートすることができる。 - -## Offline ready - -The data parser API inside the driver should not be designed to expect the data to be received in a real-time stream. -ドライバ内のデータパーサ API は、データをリアルタイムストリームで受け取るように設計されるべきではない。 - -### Why? - -Offline processing will help process the data faster than in real-time. -これにより、データがセンサの代わりにログファイルから発信された時にデータ処理をリアルタイムより速く処理することができる。 +WIP - please check back soon! +For now, here is some related information about the Hesai decoder design: +[Hesai decoder design](hesai_decoder_design.md) diff --git a/docs/hesai_decoder_design.md b/docs/hesai_decoder_design.md index a3d48f0a3..827b87773 100644 --- a/docs/hesai_decoder_design.md +++ b/docs/hesai_decoder_design.md @@ -11,6 +11,7 @@ This way, runtime overhead for this generalization is `0`. ### Packet formats For all handled Hesai sensors, the packet structure follows this rough format: + 1. (optional) header: static sensor info and feature flags 2. body: point data 3. tail and other appendices: timestamp, operation mode info @@ -18,10 +19,11 @@ For all handled Hesai sensors, the packet structure follows this rough format: ### Decoding steps For all handled Hesai sensors, decoding a packet follows these steps: + ```python def unpack(packet): parse_and_validate(packet) - # return group: one (single-return) or more (multi-return) + # return group: one (single-return) or more (multi-return) # blocks that belong to the same azimuth for return_group in packet: if is_start_of_new_scan(return_group): @@ -40,17 +42,18 @@ def decode(return_group): append to pointcloud ``` -The steps marked with __*__ are model-specific: +The steps marked with **\*** are model-specific: -* angle correction -* timing correction -* return type assignment +- angle correction +- timing correction +- return type assignment ### Angle correction There are two approaches between all the supported sensors: -* Calibration file based -* Correction file based (currently only used by AT128) + +- Calibration file based +- Correction file based (currently only used by AT128) For both approaches, sin/cos lookup tables can be computed. However, the resolution and calculation of these tables is different. @@ -60,7 +63,7 @@ However, the resolution and calculation of these tables is different. For each laser channel, a fixed elevation angle and azimuth angle offset are defined in the calibration file. Thus, sin/cos for elevation are only a function of the laser channel (not dependent on azimuth) while those for azimuth are a function of azimuth AND elevation. -Lookup tables for elevation can thus be sized with `n_channels`, yielding a maximum size of +Lookup tables for elevation can thus be sized with `n_channels`, yielding a maximum size of `128 * sizeof(float) = 512B` each. For azimuth, the size is `n_channels * n_azimuths = n_channels * 360 * azimuth_resolution <= 128 * 36000`. @@ -94,9 +97,10 @@ While there is a wide range of different supported return modes (e.g. single (fi Differences only arise in multi-return (dual or triple) in the output order of the returns, and in the handling of some returns being duplicates (e.g. in dual(first, strongest), the first return coincides with the strongest one). Here is an exhaustive list of differences: -* For Dual (First, Last) `0x3B`, 128E3X, 128E4X and XT32 reverse the output order (Last, First) -* For Dual (Last, Strongest) `0x39`, all sensors except XT32M place the second strongest return in the even block if last == strongest -* For Dual (First, Strongest) `0x3c`, the same as for `0x39` holds. + +- For Dual (First, Last) `0x3B`, 128E3X, 128E4X and XT32 reverse the output order (Last, First) +- For Dual (Last, Strongest) `0x39`, all sensors except XT32M place the second strongest return in the even block if last == strongest +- For Dual (First, Strongest) `0x3c`, the same as for `0x39` holds. For all other return modes, duplicate points are output if the two returns coincide. @@ -119,9 +123,10 @@ Return mode handling has a default implementation that is supplemented by additi ### `AngleCorrector` The angle corrector has three main tasks: -* compute corrected azimuth/elevation for given azimuth and channel -* implement `hasScanCompleted()` logic that decides where one scan ends and the next starts -* compute and provide lookup tables for sin/cos/etc. + +- compute corrected azimuth/elevation for given azimuth and channel +- implement `hasScanCompleted()` logic that decides where one scan ends and the next starts +- compute and provide lookup tables for sin/cos/etc. The two angle correction types are calibration-based and correction-based. In both approaches, a file from the sensor is used to extract the angle correction for each azimuth/channel. For all approaches, cos/sin lookup tables in the appropriate size are generated (see requirements section above). @@ -133,9 +138,10 @@ It is a template class taking a sensor type `SensorT` from which packet type, an Thus, this unified decoder is an almost zero-cost abstraction. Its tasks are: -* parsing an incoming packet -* managing decode/output point buffers -* converting all points in the packet using the sensor-specific functions of `SensorT` where necessary + +- parsing an incoming packet +- managing decode/output point buffers +- converting all points in the packet using the sensor-specific functions of `SensorT` where necessary `HesaiDecoder` is a subclass of the existing `HesaiScanDecoder` to allow all template instantiations to be assigned to variables of the supertype. @@ -144,24 +150,24 @@ Its tasks are: To support a new sensor model, first familiarize with the already implemented decoders. Then, consult the new sensor's datasheet and identify the following parameters: -| Parameter | Chapter | Possible values | Notes | -|-|-|-|-| -| Header format | 3.1 | `Header12B`, `Header8B`, ... | `Header12B` is the standard and comprises the UDP pre-header and header (6+6B) mentioned in the data sheets | -| Blocks per packet | 3.1 | `2`, `6`, `10`, ... | | -| Number of channels | 3.1 | `32`, `40`, `64`, ... | | -| Unit format | 3.1 | `Unit3B`, `Unit4B`, ... | | -| Angle correction | App. 3 | `CALIBRATION`, `CORRECTION`, ... | The datasheet usually specifies whether a calibration/correction file is used | -| Timing correction | App. 2 | | There is usually a block and channel component. These come in the form of formulas/lookup tables. For most sensors, these depend on return mode and for some, features like high resolution mode, alternate firing etc. might change the timing | -| Return type handling | 3.1 | | Return modes are handled identically for most sensors but some re-order the returns or replace returns if there are duplicates | -| Bytes per second | 1.4 | | | -| Lowest supported frequency | 1.4 | `5 Hz`, `10 Hz`, ... | | - -| Chapter | Full title | -|-|-| -|1.4| Introduction > Specifications| -|3.1| Data Structure > Point Cloud Data Packet| -|App. 2| Absolute Time of Point Cloud Data| -|App. 3| Angle Correction| +| Parameter | Chapter | Possible values | Notes | +| -------------------------- | ------- | -------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| Header format | 3.1 | `Header12B`, `Header8B`, ... | `Header12B` is the standard and comprises the UDP pre-header and header (6+6B) mentioned in the data sheets | +| Blocks per packet | 3.1 | `2`, `6`, `10`, ... | | +| Number of channels | 3.1 | `32`, `40`, `64`, ... | | +| Unit format | 3.1 | `Unit3B`, `Unit4B`, ... | | +| Angle correction | App. 3 | `CALIBRATION`, `CORRECTION`, ... | The datasheet usually specifies whether a calibration/correction file is used | +| Timing correction | App. 2 | | There is usually a block and channel component. These come in the form of formulas/lookup tables. For most sensors, these depend on return mode and for some, features like high resolution mode, alternate firing etc. might change the timing | +| Return type handling | 3.1 | | Return modes are handled identically for most sensors but some re-order the returns or replace returns if there are duplicates | +| Bytes per second | 1.4 | | | +| Lowest supported frequency | 1.4 | `5 Hz`, `10 Hz`, ... | | + +| Chapter | Full title | +| ------- | ---------------------------------------- | +| 1.4 | Introduction > Specifications | +| 3.1 | Data Structure > Point Cloud Data Packet | +| App. 2 | Absolute Time of Point Cloud Data | +| App. 3 | Angle Correction | With this information, create a `PacketMySensor` struct and `SensorMySensor` class. Reuse already-defined structs as much as possible (c.f. `Packet128E3X` and `Packet128E4X`). @@ -169,7 +175,7 @@ Reuse already-defined structs as much as possible (c.f. `Packet128E3X` and `Pack Implement timing correction in `SensorMySensor` and define the class constants `float MIN_RANGE`, `float MAX_RANGE` and `size_t MAX_SCAN_BUFFER_POINTS`. The former two are used for filtering out too-close and too-far away points while the latter is used to -allocate pointcloud buffers. +allocate pointcloud buffers. Set `MAX_SCAN_BUFFER_POINTS = bytes_per_second / lowest_supported_frequency` from the parameters found above. If there are any non-standard features your sensor has, implement them as generically as possible to allow for future sensors to re-use your code. diff --git a/docs/index.md b/docs/index.md index ea4de66a7..e79da77a5 100644 --- a/docs/index.md +++ b/docs/index.md @@ -1,72 +1,38 @@ -# Nebula introduction +# Welcome to the Nebula documentation -The project is separated into four main parts: -プロジェクトは主となる 4 つのパートに分かれている： -Lidar ドライバ、ROS ラッパー、HWI インターフェイスだ。 +Welcome to the Nebula documentation. Here you will find information about the background of the project, how to install and use with ROS 2, and also how to add new sensors to the Nebula driver. -- Common: `nebula_common`. This packages contains the structures, data types, calibration and configuration definitions used among all the packages. -- Drivers: `nebula_decoders`. The Drivers take care of all the data parsing and conversion. Lidar ドライバは、全てのセンサ通信とデータパーシングを管理している。 -- ROS Nodes Wrappers: `nebula_ros`, The ROSWrappers are a lightweight layer responsible for the data conversion between the LidarDriver point cloud and the corresponding ROS counterparts. The ROSWrapper also provides methods for configuration and the obtention of the status information of the Lidar. ROS ラッパーは、Lidar ドライバ点群と対応する ROS のカウンターパーツのデータ変換を掌る軽量のレイヤである。 ROS ラッパーは、構成方法、Lidar のステータス情報取得の方法を提供している。 -- HWInterface: `nebula_hw_interfaces`. The HWInterface offers an abstraction layer between the parser and the sensor communication. HW インターフェイスは、パーサとセンサ間の抽象化レイヤを提供している。 +## About Nebula -# Nebula Common +Nebula is a sensor driver platform that is designed to provide a unified framework for as wide a variety of devices as possible. +While it primarily targets Ethernet-based LiDAR sensors, it aims to be easily extendable to support new sensors and interfaces. +Nebula works with ROS 2 and is the recommended sensor driver for the [Autoware](https://autoware.org/) project. The project aims to provide: -The Nebula common package contains structure definition such as configuration, calibration, point types. -It also contains other common status strings and conversions used among all the packages. +- A universal sensor driver + - Supporting an increasing number of LiDAR, radar and camera sensors +- 100% open source, with a no-binaries policy +- ROS 2 wrappers for easy inclusion in robotics and self-driving vehicle projects +- A driver solution to suit current Autoware requirements + - Interfaces and pointcloud type updates made in unison with Autoware developments -## Point Types +For more information, please refer to [About Nebula](about.md). -Nebula supports three point cloud output types. -However, it can easily be extended to support other custom point cloud types. +## Getting started -These definitions can be found in the `nebula_common/include/point_types.hpp`. +- [Installation](installation.md) +- [Launching with ROS 2](usage.md) -### PointXYZIR +## Nebula architecture -| Field | Type | Units | Description | -| ------------- | ------- | ----- | -------------------------------------------------------------------- | -| `x` | `float` | `m` | Contains the abscissa member of the point in cartesian coordinates. | -| `y` | `float` | `m` | Contains the ordinate member of the point in cartesian coordinates. | -| `z` | `float` | `m` | Contains the applicate member of the point in cartesian coordinates. | -| `intensity` | `uint8` | | Contains the laser energy return value as reported by the sensor. | -| `return type` | `uint8` | | Contains the lase return type according to the sensor configuration. | +- [Design](design.md) +- [Parameters](parameters.md) +- [Point cloud types](point_types.md) -### PointXYZICAETR +## Supported sensors -| Field | Type | Units | Description | -| ------------- | ------- | ----- | -------------------------------------------------------------------- | -| `x` | `float` | `m` | Contains the abscissa member of the point in cartesian coordinates. | -| `y` | `float` | `m` | Contains the ordinate member of the point in cartesian coordinates. | -| `z` | `float` | `m` | Contains the applicate member of the point in cartesian coordinates. | -| `intensity` | `uint8` | | Contains the laser energy return value as reported by the sensor. | -| `channel` | `uint8` | | Contains the laser channel id. | -| `azimuth` | `float` | `rad` | Contains the azimuth of the current point. | -| `elevation` | `float` | `rad` | Contains the elevation of the current point. | -| `timestamp` | `float` | `ns` | Contains the relative time to the triggered scan time. | -| `return type` | `uint8` | | Contains the lase return type according to the sensor configuration. | +- [Supported sensors](supported_sensors.md) -### PointXYZICATR +## Development -| Field | Type | Units | Description | -| ------------- | ------- | --------- | -------------------------------------------------------------------- | -| `x` | `float` | `m` | Contains the abscissa member of the point in cartesian coordinates. | -| `y` | `float` | `m` | Contains the ordinate member of the point in cartesian coordinates. | -| `z` | `float` | `m` | Contains the applicate member of the point in cartesian coordinates. | -| `intensity` | `uint8` | | Contains the laser energy return value as reported by the sensor. | -| `channel` | `uint8` | | Contains the laser channel id. | -| `azimuth` | `float` | `degrees` | Contains the azimuth of the current point. | -| `timestamp` | `float` | `ns` | Contains the relative time to the triggered scan time. | -| `return type` | `uint8` | | Contains the lase return type according to the sensor configuration. | - -### PointXYZIRADT - -| Field | Type | Units | Description | -| ------------- | ------- | --------- | -------------------------------------------------------------------------- | -| `x` | `float` | `m` | Contains the abscissa member of the point in cartesian coordinates. | -| `y` | `float` | `m` | Contains the ordinate member of the point in cartesian coordinates. | -| `z` | `float` | `m` | Contains the applicate member of the point in cartesian coordinates. | -| `intensity` | `uint8` | | Contains the laser energy return value as reported by the sensor. | -| `return type` | `uint8` | | Contains the lase return type according to the sensor configuration. | -| `azimuth` | `float` | `degrees` | Contains the azimuth of the current point. | -| `distance` | `float` | `m` | Contains the distance from the sensor origin to this echo on the XY plane. | -| `timestamp` | `float` | `ns` | Contains the relative time to the triggered scan time. | +- [Tutorials](tutorials.md) +- [Contributing](contribute.md) diff --git a/docs/installation.md b/docs/installation.md new file mode 100644 index 000000000..5f006bc35 --- /dev/null +++ b/docs/installation.md @@ -0,0 +1,45 @@ +# Installing Nebula + +## Requirements + +Nebula requires ROS 2 (Galactic or Humble) to build the ROS 2 wrapper. +Please see the [ROS 2 documentation](https://docs.ros.org/en/humble/index.html) for how to install. + +## Getting the source and building + +> **Note** +> +> Boost version 1.74.0 or later is required. A manual install may be required in Ubuntu versions earlier than 22.04. +> +> **Note** +> +> A [TCP enabled version of ROS' Transport Driver](https://github.com/mojomex/transport_drivers/tree/mutable-buffer-in-udp-callback) is required to use Nebula. +> It is installed automatically into your workspace using the below commands. However, if you already have ROS transport driver binaries installed, you will have to uninstall them to avoid conflicts (replace `humble` with your ROS distribution): +> `sudo apt remove ros-humble-udp-driver ros-humble-io-context` + +To build Nebula run the following commands in your workspace: + +```bash +# In workspace +mkdir src +git clone https://github.com/tier4/nebula.git src +# Import dependencies +vcs import src < src/build_depends.repos +rosdep install --from-paths src --ignore-src -y -r +# Build Nebula +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release +``` + +## Testing your build + +To run Nebula unit tests: + +```bash +colcon test --event-handlers console_cohesion+ --packages-above nebula_common +``` + +Show results: + +```bash +colcon test-result --all +``` diff --git a/docs/parameters.md b/docs/parameters.md new file mode 100644 index 000000000..8cb3c62a0 --- /dev/null +++ b/docs/parameters.md @@ -0,0 +1,121 @@ +# ROS parameters for supported sensors + +> **Note** +> +> The information on this page may be out of date. +> Please refer to the configuration in the relevant `*sensor_model*.param.yaml` file for you sensor, to confirm what parameters are available, + +## Common ROS parameters + +Parameters shared by all supported models: + +| Parameter | Type | Default | Accepted values | Description | +| ------------ | ------ | ---------------- | -------------------------- | ---------------- | +| sensor_model | string | | See supported models | | +| return_mode | string | | See supported return modes | | +| frame_id | string | Sensor dependent | | ROS frame ID | +| scan_phase | double | 0.0 | degrees [0.0, 360.0] | Scan start angle | + +## Hesai specific parameters + +### Supported return modes per model + +| Sensor model | return_mode | Mode | +| ------------ | -------------- | ------ | +| Pandar XT32M | Last | Single | +| Pandar XT32M | Strongest | Single | +| Pandar XT32M | LastStrongest | Dual | +| Pandar XT32M | First | Single | +| Pandar XT32M | LastFirst | Dual | +| Pandar XT32M | FirstStrongest | Dual | +| Pandar XT32M | Dual | Dual | +| --- | --- | --- | +| Pandar AT128 | Last | Single | +| Pandar AT128 | Strongest | Single | +| Pandar AT128 | LastStrongest | Dual | +| Pandar AT128 | First | Single | +| Pandar AT128 | LastFirst | Dual | +| Pandar AT128 | FirstStrongest | Dual | +| Pandar AT128 | Dual | Dual | +| --- | --- | --- | +| Pandar QT128 | Last | Single | +| Pandar QT128 | Strongest | Single | +| Pandar QT128 | LastStrongest | Dual | +| Pandar QT128 | First | Single | +| Pandar QT128 | LastFirst | Dual | +| Pandar QT128 | FirstStrongest | Dual | +| Pandar QT128 | Dual | Dual | +| --- | --- | --- | +| Pandar QT64 | Last | Single | +| Pandar QT64 | Dual | Dual | +| Pandar QT64 | First | Single | +| --- | --- | --- | +| Pandar 40P | Last | Single | +| Pandar 40P | Strongest | Single | +| Pandar 40P | Dual | Dual | +| --- | --- | --- | +| Pandar 64 | Last | Single | +| Pandar 64 | Strongest | Single | +| Pandar 64 | Dual | Dual | + +### Hardware interface parameters + +| Parameter | Type | Default | Accepted values | Description | +| ------------------------------ | ------ | --------------- | ----------------- | ------------------------------ | +| frame_id | string | hesai | | ROS frame ID | +| sensor_ip | string | 192.168.1.201 | | Sensor IP | +| host_ip | string | 255.255.255.255 | | Host IP | +| data_port | uint16 | 2368 | | Sensor port | +| gnss_port | uint16 | 2369 | | GNSS port | +| frequency_ms | uint16 | 100 | milliseconds, > 0 | Time per scan | +| packet_mtu_size | uint16 | 1500 | | Packet MTU size | +| rotation_speed | uint16 | 600 | | Rotation speed | +| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | +| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | +| dual_return_distance_threshold | double | 0.1 | | Dual return distance threshold | +| diag_span | uint16 | 1000 | milliseconds, > 0 | Diagnostic span | +| setup_sensor | bool | True | True, False | Configure sensor settings | + +### Driver parameters + +| Parameter | Type | Default | Accepted values | Description | +| ---------------- | ------ | ------- | --------------- | ---------------------- | +| frame_id | string | hesai | | ROS frame ID | +| calibration_file | string | | | LiDAR calibration file | +| correction_file | string | | | LiDAR correction file | + +## Velodyne specific parameters + +### Supported return modes + +| return_mode | Mode | +| --------------- | ------------------ | +| SingleFirst | Single (First) | +| SingleStrongest | Single (Strongest) | +| SingleLast | Single (Last) | +| Dual | Dual | + +### Hardware interface parameters + +| Parameter | Type | Default | Accepted values | Description | +| --------------- | ------ | --------------- | ----------------- | --------------- | +| frame_id | string | velodyne | | ROS frame ID | +| sensor_ip | string | 192.168.1.201 | | Sensor IP | +| host_ip | string | 255.255.255.255 | | Host IP | +| data_port | uint16 | 2368 | | Sensor port | +| gnss_port | uint16 | 2369 | | GNSS port | +| frequency_ms | uint16 | 100 | milliseconds, > 0 | Time per scan | +| packet_mtu_size | uint16 | 1500 | | Packet MTU size | +| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | +| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | + +### Driver parameters + +| Parameter | Type | Default | Accepted values | Description | +| ---------------- | ------ | -------- | ---------------- | ----------------------------- | +| frame_id | string | velodyne | | ROS frame ID | +| calibration_file | string | | | LiDAR calibration file | +| min_range | double | 0.3 | meters, >= 0.3 | Minimum point range published | +| max_range | double | 300.0 | meters, <= 300.0 | Maximum point range published | +| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | +| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | diff --git a/docs/point_types.md b/docs/point_types.md new file mode 100644 index 000000000..dcde09409 --- /dev/null +++ b/docs/point_types.md @@ -0,0 +1,60 @@ +# Nebula point cloud types + +Nebula currently supports the below point cloud output types. +However, it can easily be extended to support other custom point cloud types. + +These definitions can be found in the `nebula_common/include/point_types.hpp`. + +## PointXYZIR + +| Field | Type | Units | Description | +| ----------- | -------- | ----- | -------------------------------------------------- | +| `x` | `float` | `m` | Cartesian x coordinate. | +| `y` | `float` | `m` | Cartesian y coordinate. | +| `z` | `float` | `m` | Cartesian z coordinate. | +| padding | 4 bytes | | | +| `intensity` | `float` | | Intensity of the return as reported by the sensor. | +| `ring` | `uint16` | | Ring ID - only defined for rotational LiDARs. | + +## PointXYZICATR + +| Field | Type | Units | Description | +| ------------- | -------- | --------- | ------------------------------------------------------- | +| `x` | `float` | `m` | Cartesian x coordinate. | +| `y` | `float` | `m` | The point's cartesian y coordinate. | +| `z` | `float` | `m` | Cartesian z coordinate. | +| padding | 4 bytes | | | +| `intensity` | `uint8` | | Intensity of the return as reported by the sensor. | +| `channel` | `uint16` | | The ID of the laser channel that produced the point. | +| `azimuth` | `float` | `degrees` | Azimuth in polar coordinates. | +| `timestamp` | `uint32` | `ns` | Time of detection relative to the pointcloud timestamp. | +| `return type` | `uint8` | | Return (echo) type. | + +## PointXYZIRADT + +| Field | Type | Units | Description | +| ------------- | -------- | --------- | ------------------------------------------------------- | +| `x` | `float` | `m` | Cartesian x coordinate. | +| `y` | `float` | `m` | Cartesian y coordinate. | +| `z` | `float` | `m` | Cartesian z coordinate. | +| padding | 4 bytes | | | +| `intensity` | `float` | | Intensity of the return as reported by the sensor. | +| `return type` | `uint8` | | Return (echo) type. | +| `azimuth` | `float` | `degrees` | Azimuth in polar coordinates. | +| `distance` | `float` | `m` | Distance from the sensor origin. | +| `timestamp` | `double` | `ns` | Time of detection relative to the pointcloud timestamp. | + +## NebulaPoint = PointXYZIRCAEDT + +| Field | Type | Units | Description | +| ------------- | -------- | ----- | ------------------------------------------------------- | +| `x` | `float` | `m` | Cartesian x coordinate. | +| `y` | `float` | `m` | Cartesian y coordinate. | +| `z` | `float` | `m` | Cartesian z coordinate. | +| `intensity` | `uint8` | | Intensity of the return as reported by the sensor. | +| `return type` | `uint8` | | Return (echo) type. | +| `channel` | `uint16` | | Laser channel ID. | +| `azimuth` | `float` | `rad` | Azimuth in polar coordinates. | +| `elevation` | `float` | `rad` | Elevation in polar coordinates. | +| `distance` | `float` | `m` | Distance from the sensor origin. | +| `timestamp` | `uint32` | `ns` | Time of detection relative to the pointcloud timestamp. | diff --git a/docs/supported_sensors.md b/docs/supported_sensors.md new file mode 100644 index 000000000..55fb1a381 --- /dev/null +++ b/docs/supported_sensors.md @@ -0,0 +1,55 @@ +# Supported sensors + +Nebula currently supports the sensor models listed below. The test status column indicates how many of the sensors' features are supported. + +For all sensors, the respective configuration file is found under `nebula_ros/config///` where + +- `` is either lidar or radar, +- `` is the vendor of the sensor and +- `` is listed in the table below. + +The launch file for a given vendor is called `_launch_all_hw.xml`. +The `sensor_model` parameter below decides which sensor driver is launched. + +## Hesai LiDARs + +| Model | `sensor_model` | Configuration file | Test status | +| ------------ | -------------- | ----------------------- | ----------- | +| Pandar64 | Pandar64 | Pandar64.param.yaml | ✅ | +| Pandar 40P | Pandar40P | Pandar40P.param.yaml | ✅ | +| Pandar XT32 | PandarXT32 | PandarXT32.param.yaml | ✅ | +| Pandar XT32M | PandarXT32M | PandarXT32M.param.yaml | ⚠️ | +| Pandar QT64 | PandarQT64 | PandarQT64.param.yaml | ✅ | +| Pandar QT128 | PandarQT128 | PandarQT128.param.yaml | ⚠️ | +| Pandar AT128 | PandarAT128 | PandarAT128.param.yaml | ✅\* | +| Pandar OT128 | Pandar128E4X | Pandar128E4X.param.yaml | ⚠️ | + +\*: AT128 needs software version 3.50.8 or newer for the `scan_angle` setting to work correctly. + +## Velodyne LiDARs + +| Model | `sensor_model` | Configuration file | Test status | +| ------------ | -------------- | ------------------ | ----------- | +| VLP-16 | VLP16 | VLP16.param.yaml | ⚠️ | +| VLP-16-HiRes | VLP16 | | ❌ | +| VLP-32 | VLP32 | VLP32.param.yaml | ⚠️ | +| VLS-128 | VLS128 | VLS128.param.yaml | ⚠️ | + +## Robosense LiDARs + +| Model | `sensor_model` | Configuration file | Test status | +| ------ | -------------- | ------------------ | ----------- | +| Bpearl | Bpearl | Bpearl.param.yaml | ⚠️ | +| Helios | Helios | Helios.param.yaml | ⚠️ | + +## Continental radars + +| Model | `sensor_model` | Configuration file | Test status | +| ------ | -------------- | ------------------ | ----------- | +| ARS548 | ARS548 | ARS548.param.yaml | ⚠️ | +| SRR520 | SRR520 | SRR520.param.yaml | ⚠️ | + +Test status: +✅: complete +⚠️: some functionality yet to be tested +❌: untested diff --git a/docs/tutorials.md b/docs/tutorials.md new file mode 100644 index 000000000..9edf03660 --- /dev/null +++ b/docs/tutorials.md @@ -0,0 +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/mojomex/nebula/tree/single-node-refactor-tutorial). diff --git a/docs/usage.md b/docs/usage.md new file mode 100644 index 000000000..a14497449 --- /dev/null +++ b/docs/usage.md @@ -0,0 +1,21 @@ +# Running Nebula + +## Launch with default parameters + +To launch Nebula as a ROS 2 node with default parameters for your sensor model: + +```bash +ros2 launch nebula_ros *sensor_vendor_name*_launch_all_hw.xml sensor_model:=*sensor_model_name* +``` + +For example, for a Hesai Pandar40P sensor: + +```bash +ros2 launch nebula_ros hesai_launch_all_hw.xml sensor_model:=Pandar40P +``` + +Refer to the list of [supported sensors](supported_sensors.md) for more information on the avaliable sensors and configuration options. + +## Sensor configuration + +WIP diff --git a/local.cspell.json b/local.cspell.json deleted file mode 100644 index e69de29bb..000000000 diff --git a/mkdocs.yml b/mkdocs.yml index febbda15c..3e25eaaba 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -35,13 +35,18 @@ use_directory_urls: true ### Navigation ### nav: - Home: index.md + - About: about.md + - Installation: installation.md + - Launching: usage.md - Design: design.md - - About Nebula: about.md - - Add Your Sensor: add_sensor.md - - Hesai Decoder Design: hesai_decoder_design.md - - Nebula Common: nebula_common/links.md - - Nebula Decoders: nebula_decoders/links.md - - Nebula HW Interfaces: nebula_hw_interfaces/links.md + - Parameters: parameters.md + - Point cloud types: point_types.md + - Supported sensors: supported_sensors.md + - Tutorials: tutorials.md + - Contributing: contribute.md + - Nebula common: nebula_common/links.md + - Nebula decoders: nebula_decoders/links.md + - Nebula HW interfaces: nebula_hw_interfaces/links.md - Nebula ROS: nebula_ros/links.md ### Extra Settings ### @@ -50,49 +55,49 @@ plugins: projects: nebula_common: src-dirs: nebula_common/include - full-doc: True + full-doc: true doxy-cfg: FILE_PATTERNS: "*.hpp *.h" - RECURSIVE: True - EXTRACT_ALL: True - INLINE_SOURCES: True - ENABLE_PREPROCESSING: True - MACRO_EXPANSION: True + RECURSIVE: true + EXTRACT_ALL: true + INLINE_SOURCES: true + ENABLE_PREPROCESSING: true + MACRO_EXPANSION: true nebula_decoders: src-dirs: nebula_decoders/include - full-doc: True + full-doc: true doxy-cfg: FILE_PATTERNS: "*.hpp *.h" - RECURSIVE: True - EXTRACT_ALL: True - INLINE_SOURCES: True - ENABLE_PREPROCESSING: True - MACRO_EXPANSION: True + RECURSIVE: true + EXTRACT_ALL: true + INLINE_SOURCES: true + ENABLE_PREPROCESSING: true + MACRO_EXPANSION: true nebula_hw_interfaces: src-dirs: nebula_hw_interfaces/include - full-doc: True + full-doc: true doxy-cfg: FILE_PATTERNS: "*.hpp *.h" - RECURSIVE: True - EXTRACT_ALL: True - INLINE_SOURCES: True - ENABLE_PREPROCESSING: True - MACRO_EXPANSION: True + RECURSIVE: true + EXTRACT_ALL: true + INLINE_SOURCES: true + ENABLE_PREPROCESSING: true + MACRO_EXPANSION: true nebula_ros: src-dirs: nebula_ros/include - full-doc: True + full-doc: true doxy-cfg: FILE_PATTERNS: "*.hpp *.h" - RECURSIVE: True - EXTRACT_ALL: True - INLINE_SOURCES: True - ENABLE_PREPROCESSING: True - MACRO_EXPANSION: True + 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 + full-doc: true + debug: false + ignore-errors: false - search: lang: en diff --git a/nebula_common/CMakeLists.txt b/nebula_common/CMakeLists.txt index 4c146736c..2e4be4f14 100644 --- a/nebula_common/CMakeLists.txt +++ b/nebula_common/CMakeLists.txt @@ -2,13 +2,9 @@ cmake_minimum_required(VERSION 3.14) project(nebula_common) find_package(ament_cmake_auto REQUIRED) -find_package(PCL REQUIRED) find_package(PCL REQUIRED COMPONENTS common) -find_package(pcl_conversions REQUIRED) find_package(yaml-cpp REQUIRED) -ament_auto_find_build_dependencies() - # Default to C++17 if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -28,19 +24,35 @@ if(BUILD_TESTING) endif() include_directories( - include - SYSTEM - ${YAML_CPP_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${PCL_COMMON_INCLUDE_DIRS} + include + SYSTEM + ${YAML_CPP_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +link_libraries( + ${PCL_LIBRARIES} + ${YAML_CPP_LIBRARIES} +) + +add_library(nebula_common SHARED + src/nebula_common.cpp + src/velodyne/velodyne_calibration_decoder.cpp +) + +install(TARGETS nebula_common EXPORT export_nebula_common) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_nebula_common) + +ament_export_dependencies( + PCL + yaml-cpp ) -ament_auto_add_library(nebula_common SHARED - src/nebula_common.cpp - src/velodyne/velodyne_calibration_decoder.cpp - ) +ament_package() -ament_auto_package() # Set ROS_DISTRO macros set(ROS_DISTRO $ENV{ROS_DISTRO}) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 2e7ac2093..971b3bad7 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -14,26 +14,16 @@ #pragma once -#include -#include +#include "nebula_common/nebula_common.hpp" -#include "boost/endian/buffers.hpp" #include -#include +#include #include #include -#include -#include -#include -#include -#include -#include #include -#include #include -#include namespace nebula { @@ -57,13 +47,6 @@ struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase float configuration_vehicle_wheelbase{}; }; -/// @brief struct for Multiple ARS548 sensor configuration -struct MultiContinentalARS548SensorConfiguration : ContinentalARS548SensorConfiguration -{ - std::vector sensor_ips{}; - std::vector frame_ids{}; -}; - /// @brief Convert ContinentalARS548SensorConfiguration to string (Overloading the << /// operator) /// @param os @@ -72,42 +55,18 @@ struct MultiContinentalARS548SensorConfiguration : ContinentalARS548SensorConfig inline std::ostream & operator<<( std::ostream & os, ContinentalARS548SensorConfiguration const & arg) { - os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip - << ", BaseFrame: " << arg.base_frame << ", ObjectFrame: " << arg.object_frame - << ", ConfigurationHostPort: " << arg.configuration_host_port - << ", ConfigurationSensorPort: " << arg.configuration_sensor_port - << ", UseSensorTime: " << arg.use_sensor_time - << ", ConfigurationVehicleLength: " << arg.configuration_vehicle_length - << ", ConfigurationVehicleWidth: " << arg.configuration_vehicle_width - << ", ConfigurationVehicleHeight: " << arg.configuration_vehicle_height - << ", ConfigurationVehicleWheelbase: " << arg.configuration_vehicle_wheelbase; - return os; -} - -/// @brief Convert MultiContinentalARS548SensorConfiguration to string (Overloading the << -/// operator) -/// @param os -/// @param arg -/// @return stream -inline std::ostream & operator<<( - std::ostream & os, MultiContinentalARS548SensorConfiguration const & arg) -{ - std::stringstream sensor_ips_ss; - sensor_ips_ss << "["; - for (const auto sensor_ip : arg.sensor_ips) { - sensor_ips_ss << sensor_ip << ", "; - } - sensor_ips_ss << "]"; - - std::stringstream frame_ids_ss; - frame_ids_ss << "["; - for (const auto frame_id : arg.frame_ids) { - frame_ids_ss << frame_id << ", "; - } - frame_ids_ss << "]"; - - os << (ContinentalARS548SensorConfiguration)(arg) << ", MulticastIP: " << arg.multicast_ip - << ", SensorIPs: " << sensor_ips_ss.str() << ", FrameIds: " << frame_ids_ss.str(); + os << "Continental ARS548 Sensor Configuration:" << '\n'; + os << (EthernetSensorConfigurationBase)(arg) << '\n'; + os << "Multicast IP: " << arg.multicast_ip << '\n'; + os << "Base Frame: " << arg.base_frame << '\n'; + os << "Object Frame: " << arg.object_frame << '\n'; + os << "Host Port: " << arg.configuration_host_port << '\n'; + os << "Sensor Port: " << arg.configuration_sensor_port << '\n'; + os << "UseSensor Time: " << arg.use_sensor_time << '\n'; + os << "Vehicle Length: " << arg.configuration_vehicle_length << '\n'; + os << "Vehicle Width: " << arg.configuration_vehicle_width << '\n'; + os << "Vehicle Height: " << arg.configuration_vehicle_height << '\n'; + os << "Vehicle Wheelbase: " << arg.configuration_vehicle_wheelbase; return os; } @@ -327,159 +286,159 @@ struct HeaderPacket struct HeaderSomeIPPacket { - big_uint16_buf_t client_id; - big_uint16_buf_t session_id; - uint8_t protocol_version; - uint8_t interface_version; - uint8_t message_type; - uint8_t return_code; + big_uint16_buf_t client_id{}; + big_uint16_buf_t session_id{}; + uint8_t protocol_version{}; + uint8_t interface_version{}; + uint8_t message_type{}; + uint8_t return_code{}; }; struct HeaderE2EP07Packet { - big_uint64_buf_t crc; - big_uint32_buf_t length; - big_uint32_buf_t sqc; - big_uint32_buf_t data_id; + big_uint64_buf_t crc{}; + big_uint32_buf_t length{}; + big_uint32_buf_t sqc{}; + big_uint32_buf_t data_id{}; }; struct StampSyncStatusPacket { - big_uint32_buf_t timestamp_nanoseconds; - big_uint32_buf_t timestamp_seconds; - uint8_t timestamp_sync_status; + big_uint32_buf_t timestamp_nanoseconds{}; + big_uint32_buf_t timestamp_seconds{}; + uint8_t timestamp_sync_status{}; }; struct DetectionPacket { - big_float32_buf_t azimuth_angle; - big_float32_buf_t azimuth_angle_std; - uint8_t invalid_flags; - big_float32_buf_t elevation_angle; - big_float32_buf_t elevation_angle_std; - big_float32_buf_t range; - big_float32_buf_t range_std; - big_float32_buf_t range_rate; - big_float32_buf_t range_rate_std; - int8_t rcs; - big_uint16_buf_t measurement_id; - uint8_t positive_predictive_value; - uint8_t classification; - uint8_t multi_target_probability; - big_uint16_buf_t object_id; - uint8_t ambiguity_flag; - big_uint16_buf_t sort_index; + big_float32_buf_t azimuth_angle{}; + big_float32_buf_t azimuth_angle_std{}; + uint8_t invalid_flags{}; + big_float32_buf_t elevation_angle{}; + big_float32_buf_t elevation_angle_std{}; + big_float32_buf_t range{}; + big_float32_buf_t range_std{}; + big_float32_buf_t range_rate{}; + big_float32_buf_t range_rate_std{}; + int8_t rcs{}; + big_uint16_buf_t measurement_id{}; + uint8_t positive_predictive_value{}; + uint8_t classification{}; + uint8_t multi_target_probability{}; + big_uint16_buf_t object_id{}; + uint8_t ambiguity_flag{}; + big_uint16_buf_t sort_index{}; }; struct DetectionListPacket { - HeaderPacket header; - HeaderSomeIPPacket header_some_ip; - HeaderE2EP07Packet header_e2ep07; - StampSyncStatusPacket stamp; - big_uint32_buf_t event_data_qualifier; - uint8_t extended_qualifier; - big_uint16_buf_t origin_invalid_flags; - big_float32_buf_t origin_x_pos; - big_float32_buf_t origin_x_std; - big_float32_buf_t origin_y_pos; - big_float32_buf_t origin_y_std; - big_float32_buf_t origin_z_pos; - big_float32_buf_t origin_z_std; - big_float32_buf_t origin_roll; - big_float32_buf_t origin_roll_std; - big_float32_buf_t origin_pitch; - big_float32_buf_t origin_pitch_std; - big_float32_buf_t origin_yaw; - big_float32_buf_t origin_yaw_std; - uint8_t list_invalid_flags; + HeaderPacket header{}; + HeaderSomeIPPacket header_some_ip{}; + HeaderE2EP07Packet header_e2ep07{}; + StampSyncStatusPacket stamp{}; + big_uint32_buf_t event_data_qualifier{}; + uint8_t extended_qualifier{}; + big_uint16_buf_t origin_invalid_flags{}; + big_float32_buf_t origin_x_pos{}; + big_float32_buf_t origin_x_std{}; + big_float32_buf_t origin_y_pos{}; + big_float32_buf_t origin_y_std{}; + big_float32_buf_t origin_z_pos{}; + big_float32_buf_t origin_z_std{}; + big_float32_buf_t origin_roll{}; + big_float32_buf_t origin_roll_std{}; + big_float32_buf_t origin_pitch{}; + big_float32_buf_t origin_pitch_std{}; + big_float32_buf_t origin_yaw{}; + big_float32_buf_t origin_yaw_std{}; + uint8_t list_invalid_flags{}; DetectionPacket detections[MAX_DETECTIONS]; - big_float32_buf_t list_rad_vel_domain_min; - big_float32_buf_t list_rad_vel_domain_max; - big_uint32_buf_t number_of_detections; - big_float32_buf_t alignment_azimuth_correction; - big_float32_buf_t alignment_elevation_correction; - uint8_t alignment_status; + big_float32_buf_t list_rad_vel_domain_min{}; + big_float32_buf_t list_rad_vel_domain_max{}; + big_uint32_buf_t number_of_detections{}; + big_float32_buf_t alignment_azimuth_correction{}; + big_float32_buf_t alignment_elevation_correction{}; + uint8_t alignment_status{}; uint8_t reserved[14]; }; struct ObjectPacket { - big_uint16_buf_t status_sensor; - big_uint32_buf_t id; - big_uint16_buf_t age; - uint8_t status_measurement; - uint8_t status_movement; - big_uint16_buf_t position_invalid_flags; - uint8_t position_reference; - big_float32_buf_t position_x; - big_float32_buf_t position_x_std; - big_float32_buf_t position_y; - big_float32_buf_t position_y_std; - big_float32_buf_t position_z; - big_float32_buf_t position_z_std; - big_float32_buf_t position_covariance_xy; - big_float32_buf_t position_orientation; - big_float32_buf_t position_orientation_std; - uint8_t existence_invalid_flags; - big_float32_buf_t existence_probability; - big_float32_buf_t existence_ppv; - uint8_t classification_car; - uint8_t classification_truck; - uint8_t classification_motorcycle; - uint8_t classification_bicycle; - uint8_t classification_pedestrian; - uint8_t classification_animal; - uint8_t classification_hazard; - uint8_t classification_unknown; - uint8_t classification_overdrivable; - uint8_t classification_underdrivable; - uint8_t dynamics_abs_vel_invalid_flags; - big_float32_buf_t dynamics_abs_vel_x; - big_float32_buf_t dynamics_abs_vel_x_std; - big_float32_buf_t dynamics_abs_vel_y; - big_float32_buf_t dynamics_abs_vel_y_std; - big_float32_buf_t dynamics_abs_vel_covariance_xy; - uint8_t dynamics_rel_vel_invalid_flags; - big_float32_buf_t dynamics_rel_vel_x; - big_float32_buf_t dynamics_rel_vel_x_std; - big_float32_buf_t dynamics_rel_vel_y; - big_float32_buf_t dynamics_rel_vel_y_std; - big_float32_buf_t dynamics_rel_vel_covariance_xy; - uint8_t dynamics_abs_accel_invalid_flags; - big_float32_buf_t dynamics_abs_accel_x; - big_float32_buf_t dynamics_abs_accel_x_std; - big_float32_buf_t dynamics_abs_accel_y; - big_float32_buf_t dynamics_abs_accel_y_std; - big_float32_buf_t dynamics_abs_accel_covariance_xy; - uint8_t dynamics_rel_accel_invalid_flags; - big_float32_buf_t dynamics_rel_accel_x; - big_float32_buf_t dynamics_rel_accel_x_std; - big_float32_buf_t dynamics_rel_accel_y; - big_float32_buf_t dynamics_rel_accel_y_std; - big_float32_buf_t dynamics_rel_accel_covariance_xy; - uint8_t dynamics_orientation_invalid_flags; - big_float32_buf_t dynamics_orientation_rate_mean; - big_float32_buf_t dynamics_orientation_rate_std; - big_uint32_buf_t shape_length_status; - uint8_t shape_length_edge_invalid_flags; - big_float32_buf_t shape_length_edge_mean; - big_float32_buf_t shape_length_edge_std; - big_uint32_buf_t shape_width_status; - uint8_t shape_width_edge_invalid_flags; - big_float32_buf_t shape_width_edge_mean; - big_float32_buf_t shape_width_edge_std; + big_uint16_buf_t status_sensor{}; + big_uint32_buf_t id{}; + big_uint16_buf_t age{}; + uint8_t status_measurement{}; + uint8_t status_movement{}; + big_uint16_buf_t position_invalid_flags{}; + uint8_t position_reference{}; + big_float32_buf_t position_x{}; + big_float32_buf_t position_x_std{}; + big_float32_buf_t position_y{}; + big_float32_buf_t position_y_std{}; + big_float32_buf_t position_z{}; + big_float32_buf_t position_z_std{}; + big_float32_buf_t position_covariance_xy{}; + big_float32_buf_t position_orientation{}; + big_float32_buf_t position_orientation_std{}; + uint8_t existence_invalid_flags{}; + big_float32_buf_t existence_probability{}; + big_float32_buf_t existence_ppv{}; + uint8_t classification_car{}; + uint8_t classification_truck{}; + uint8_t classification_motorcycle{}; + uint8_t classification_bicycle{}; + uint8_t classification_pedestrian{}; + uint8_t classification_animal{}; + uint8_t classification_hazard{}; + uint8_t classification_unknown{}; + uint8_t classification_overdrivable{}; + uint8_t classification_underdrivable{}; + uint8_t dynamics_abs_vel_invalid_flags{}; + big_float32_buf_t dynamics_abs_vel_x{}; + big_float32_buf_t dynamics_abs_vel_x_std{}; + big_float32_buf_t dynamics_abs_vel_y{}; + big_float32_buf_t dynamics_abs_vel_y_std{}; + big_float32_buf_t dynamics_abs_vel_covariance_xy{}; + uint8_t dynamics_rel_vel_invalid_flags{}; + big_float32_buf_t dynamics_rel_vel_x{}; + big_float32_buf_t dynamics_rel_vel_x_std{}; + big_float32_buf_t dynamics_rel_vel_y{}; + big_float32_buf_t dynamics_rel_vel_y_std{}; + big_float32_buf_t dynamics_rel_vel_covariance_xy{}; + uint8_t dynamics_abs_accel_invalid_flags{}; + big_float32_buf_t dynamics_abs_accel_x{}; + big_float32_buf_t dynamics_abs_accel_x_std{}; + big_float32_buf_t dynamics_abs_accel_y{}; + big_float32_buf_t dynamics_abs_accel_y_std{}; + big_float32_buf_t dynamics_abs_accel_covariance_xy{}; + uint8_t dynamics_rel_accel_invalid_flags{}; + big_float32_buf_t dynamics_rel_accel_x{}; + big_float32_buf_t dynamics_rel_accel_x_std{}; + big_float32_buf_t dynamics_rel_accel_y{}; + big_float32_buf_t dynamics_rel_accel_y_std{}; + big_float32_buf_t dynamics_rel_accel_covariance_xy{}; + uint8_t dynamics_orientation_invalid_flags{}; + big_float32_buf_t dynamics_orientation_rate_mean{}; + big_float32_buf_t dynamics_orientation_rate_std{}; + big_uint32_buf_t shape_length_status{}; + uint8_t shape_length_edge_invalid_flags{}; + big_float32_buf_t shape_length_edge_mean{}; + big_float32_buf_t shape_length_edge_std{}; + big_uint32_buf_t shape_width_status{}; + uint8_t shape_width_edge_invalid_flags{}; + big_float32_buf_t shape_width_edge_mean{}; + big_float32_buf_t shape_width_edge_std{}; }; struct ObjectListPacket { - HeaderPacket header; - HeaderSomeIPPacket header_some_ip; - HeaderE2EP07Packet header_e2ep07; - StampSyncStatusPacket stamp; - big_uint32_buf_t event_data_qualifier; - uint8_t extended_qualifier; - uint8_t number_of_objects; + HeaderPacket header{}; + HeaderSomeIPPacket header_some_ip{}; + HeaderE2EP07Packet header_e2ep07{}; + StampSyncStatusPacket stamp{}; + big_uint32_buf_t event_data_qualifier{}; + uint8_t extended_qualifier{}; + uint8_t number_of_objects{}; ObjectPacket objects[MAX_OBJECTS]; }; @@ -513,24 +472,24 @@ struct StatusConfigurationPacket struct SensorStatusPacket { - HeaderPacket header; - StampSyncStatusPacket stamp; - uint8_t sw_version_major; - uint8_t sw_version_minor; - uint8_t sw_version_patch; - StatusConfigurationPacket status; - uint8_t configuration_counter; - uint8_t longitudinal_velocity_status; - uint8_t longitudinal_acceleration_status; - uint8_t lateral_acceleration_status; - uint8_t yaw_rate_status; - uint8_t steering_angle_status; - uint8_t driving_direction_status; - uint8_t characteristic_speed_status; - uint8_t radar_status; - uint8_t voltage_status; - uint8_t temperature_status; - uint8_t blockage_status; + HeaderPacket header{}; + StampSyncStatusPacket stamp{}; + uint8_t sw_version_major{}; + uint8_t sw_version_minor{}; + uint8_t sw_version_patch{}; + StatusConfigurationPacket status{}; + uint8_t configuration_counter{}; + uint8_t longitudinal_velocity_status{}; + uint8_t longitudinal_acceleration_status{}; + uint8_t lateral_acceleration_status{}; + uint8_t yaw_rate_status{}; + uint8_t steering_angle_status{}; + uint8_t driving_direction_status{}; + uint8_t characteristic_speed_status{}; + uint8_t radar_status{}; + uint8_t voltage_status{}; + uint8_t temperature_status{}; + uint8_t blockage_status{}; }; struct ConfigurationPacket @@ -545,17 +504,17 @@ struct ConfigurationPacket struct AccelerationLateralCoGPacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t acceleration_lateral; + big_float32_buf_t acceleration_lateral{}; uint8_t reserved1[22]; }; struct AccelerationLongitudinalCoGPacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t acceleration_lateral; + big_float32_buf_t acceleration_lateral{}; uint8_t reserved1[22]; }; @@ -569,51 +528,51 @@ struct CharacteristicSpeedPacket struct DrivingDirectionPacket { - HeaderPacket header; - uint8_t reserved0; - uint8_t driving_direction; + HeaderPacket header{}; + uint8_t reserved0{}; + uint8_t driving_direction{}; uint8_t reserved1[20]; }; struct SteeringAngleFrontAxlePacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t steering_angle_front_axle; + big_float32_buf_t steering_angle_front_axle{}; uint8_t reserved1[22]; }; struct VelocityVehiclePacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[3]; - big_float32_buf_t velocity_vehicle; + big_float32_buf_t velocity_vehicle{}; uint8_t reserved1[21]; }; struct YawRatePacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t yaw_rate; + big_float32_buf_t yaw_rate{}; uint8_t reserved1[22]; }; struct FilterStatusEntryPacket { - uint8_t active; - uint8_t data_index; - big_float32_buf_t min_value; - big_float32_buf_t max_value; + uint8_t active{}; + uint8_t data_index{}; + big_float32_buf_t min_value{}; + big_float32_buf_t max_value{}; }; struct FilterStatusPacket { - HeaderPacket header; - StampSyncStatusPacket stamp; - uint8_t filter_configuration_counter; - uint8_t detection_sort_index; - uint8_t object_sort_index; + HeaderPacket header{}; + StampSyncStatusPacket stamp{}; + uint8_t filter_configuration_counter{}; + uint8_t detection_sort_index{}; + uint8_t object_sort_index{}; FilterStatusEntryPacket detection_filters[DETECTION_FILTER_PROPERTIES_NUM]; FilterStatusEntryPacket object_filters[OBJECT_FILTER_PROPERTIES_NUM]; }; diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp new file mode 100644 index 000000000..34f1c3514 --- /dev/null +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -0,0 +1,293 @@ +// 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. + +#pragma once + +#include "nebula_common/nebula_common.hpp" + +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ + +/// @brief struct for SRR520 sensor configuration +struct ContinentalSRR520SensorConfiguration : CANSensorConfigurationBase +{ + std::string base_frame{}; + bool sync_use_bus_time{}; + float configuration_vehicle_wheelbase{}; +}; + +/// @brief Convert ContinentalSRR520SensorConfiguration to string (Overloading the << +/// operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<( + std::ostream & os, ContinentalSRR520SensorConfiguration const & arg) +{ + os << "Continental SRR520 Sensor Configuration:" << '\n'; + os << (CANSensorConfigurationBase)(arg) << '\n'; + os << "Base Frame: " << arg.base_frame << '\n'; + os << "Sync Use Bus Time: " << arg.sync_use_bus_time << '\n'; + os << "Vehicle Wheelbase: " << arg.configuration_vehicle_wheelbase; + return os; +} + +using boost::endian::big_float32_buf_t; +using boost::endian::big_uint16_buf_t; +using boost::endian::big_uint32_buf_t; +using boost::endian::big_uint64_buf_t; + +// CAN MSG IDS +constexpr int RDI_NEAR_HEADER_CAN_MESSAGE_ID = 900; +constexpr int RDI_NEAR_ELEMENT_CAN_MESSAGE_ID = 901; +constexpr int RDI_HRR_HEADER_CAN_MESSAGE_ID = 1100; +constexpr int RDI_HRR_ELEMENT_CAN_MESSAGE_ID = 1101; +constexpr int OBJECT_HEADER_CAN_MESSAGE_ID = 1200; +constexpr int OBJECT_CAN_MESSAGE_ID = 1201; +constexpr int CRC_LIST_CAN_MESSAGE_ID = 800; +constexpr int STATUS_CAN_MESSAGE_ID = 700; +constexpr int SYNC_FOLLOW_UP_CAN_MESSAGE_ID = 53; +constexpr int VEH_DYN_CAN_MESSAGE_ID = 600; +constexpr int SENSOR_CONFIG_CAN_MESSAGE_ID = 601; + +// CRC IDS +constexpr int NEAR_CRC_ID = 0; +constexpr int HRR_CRC_ID = 1; +constexpr int OBJECT_CRC_ID = 2; +constexpr int TIME_DOMAIN_ID = + 0; // For details, please refer to AUTOSAR's "Time Synchronization over CAN" document + +constexpr int RDI_NEAR_HEADER_PACKET_SIZE = 32; +constexpr int RDI_NEAR_ELEMENT_PACKET_SIZE = 64; +constexpr int RDI_HRR_HEADER_PACKET_SIZE = 32; +constexpr int RDI_HRR_ELEMENT_PACKET_SIZE = 64; +constexpr int OBJECT_HEADER_PACKET_SIZE = 32; +constexpr int OBJECT_PACKET_SIZE = 64; +constexpr int CRC_LIST_PACKET_SIZE = 4; +constexpr int STATUS_PACKET_SIZE = 64; +constexpr int SYNC_FOLLOW_UP_CAN_PACKET_SIZE = 8; +constexpr int VEH_DYN_CAN_PACKET_SIZE = 8; +constexpr int CONFIGURATION_PACKET_SIZE = 16; + +constexpr int RDI_NEAR_PACKET_NUM = 50; +constexpr int RDI_HRR_PACKET_NUM = 20; +constexpr int OBJECT_PACKET_NUM = 20; + +constexpr int FRAGMENTS_PER_DETECTION_PACKET = 10; +constexpr int FRAGMENTS_PER_OBJECT_PACKET = 2; +constexpr int DETECTION_FRAGMENT_SIZE = 6; +constexpr int OBJECT_FRAGMENT_SIZE = 31; + +constexpr int MAX_RDI_NEAR_DETECTIONS = RDI_NEAR_PACKET_NUM * FRAGMENTS_PER_DETECTION_PACKET; +constexpr int MAX_RDI_HRR_DETECTIONS = RDI_HRR_PACKET_NUM * FRAGMENTS_PER_DETECTION_PACKET; +constexpr int MAX_OBJECTS = OBJECT_PACKET_NUM * FRAGMENTS_PER_OBJECT_PACKET; + +#pragma pack(push, 1) + +struct StatusPacket +{ + big_uint32_buf_t u_time_stamp; // 0 + big_uint32_buf_t u_global_time_stamp_sec; // 4 + big_uint32_buf_t u_global_time_stamp_nsec; // 8 + uint8_t u_global_time_stamp_sync_status; // 12 + uint8_t u_sw_version_major; // 13 + uint8_t u_sw_version_minor; // 14 + uint8_t u_sw_version_patch; // 15 + uint8_t u_sensor_id; // 16 + big_uint16_buf_t u_long_pos; // 17 + big_uint16_buf_t u_lat_pos; // 19 + big_uint16_buf_t u_vert_pos; // 21 + big_uint16_buf_t u_long_pos_cog; // 23 + big_uint16_buf_t u_wheelbase; // 25 + big_uint16_buf_t u_yaw_angle; // 27 + big_uint16_buf_t u_cover_damping; // 29 + uint8_t u_plug_orientation; // 31 + uint8_t u_defective; // 32 + uint8_t u_supply_voltage_limit; // 33 + uint8_t u_sensor_off_temp; // 34 + uint8_t u_dynamics_invalid0; // 35 + uint8_t u_dynamics_invalid1; // 36 + uint8_t u_ext_disturbed; // 37 + uint8_t u_com_error; // 38 + big_uint16_buf_t u_sw_error; // 39 + uint8_t u_aln_status_azimuth_available; // 41 + big_uint16_buf_t u_aln_current_azimuth_std; // 42 + big_uint16_buf_t u_aln_current_azimuth; // 44 + big_uint16_buf_t u_aln_current_delta; // 46 + uint8_t reserved1[13]; // Reserved fields, no change needed + big_uint16_buf_t u_crc; // 61 + uint8_t u_sequence_counter; // 63 +}; + +struct ScanHeaderPacket +{ + big_uint32_buf_t u_time_stamp; // 0 + big_uint32_buf_t u_global_time_stamp_sec; // 4 + big_uint32_buf_t u_global_time_stamp_nsec; // 8 + uint8_t u_global_time_stamp_sync_status; // 12 + uint8_t u_signal_status; // 13 + uint8_t u_sequence_counter; // 14 + big_uint32_buf_t u_cycle_counter; // 15 + big_uint16_buf_t u_v_ambiguous; // 19 + big_uint16_buf_t u_max_range; // 21 + big_uint16_buf_t u_number_of_detections; // 23 + uint8_t reserved[7]; // 25 +}; + +struct DetectionFragmentPacket +{ + uint8_t data[DETECTION_FRAGMENT_SIZE]; +}; + +struct DetectionPacket +{ + DetectionFragmentPacket fragments[FRAGMENTS_PER_DETECTION_PACKET]; // 0 - 59 + uint8_t reserved0; // 60 + uint8_t reserved1; // 61 + uint8_t u_message_counter; // 62 + uint8_t u_sequence_counter; // 63 +}; + +struct ObjectHeaderPacket +{ + big_uint32_buf_t u_time_stamp; // 0 + big_uint32_buf_t u_global_time_stamp_sec; // 4 + big_uint32_buf_t u_global_time_stamp_nsec; // 8 + uint8_t u_global_time_stamp_sync_status; // 12 + uint8_t u_signal_status; // 13 + uint8_t u_sequence_counter; // 14 + big_uint32_buf_t u_cycle_counter; // 15 + big_uint16_buf_t u_ego_vx; // 19 + big_uint16_buf_t u_ego_yaw_rate; // 21 + uint8_t u_motion_type; // 23 + uint8_t u_number_of_objects; // 24 + uint8_t reserved[7]; // 25 +}; + +struct ObjectFragmentPacket +{ + uint8_t data[OBJECT_FRAGMENT_SIZE]; +}; + +struct ObjectPacket +{ + ObjectFragmentPacket fragments[FRAGMENTS_PER_OBJECT_PACKET]; // 0 - 61 + uint8_t u_message_counter; // 62 + uint8_t u_sequence_counter; // 63 +}; + +struct ConfigurationPacket +{ + uint8_t u_sensor_id; + big_uint16_buf_t u_long_pose; + big_uint16_buf_t u_lat_pose; + big_uint16_buf_t u_vert_pos; + big_uint16_buf_t u_long_pos_cog; + big_uint16_buf_t u_wheelbase; + big_uint16_buf_t u_yaw_angle; + big_uint16_buf_t u_cover_damping; + uint8_t u_plug_orientation_and_default_settings; +}; + +struct SyncPacket +{ + uint8_t u_type; + uint8_t u_crc; + uint8_t u_time_domain_and_sequence_counter; + uint8_t u_user_data0; + big_uint32_buf_t u_sync_time_sec; +}; + +struct FollowUpPacket +{ + uint8_t u_type; + uint8_t u_crc; + uint8_t u_time_domain_and_sequence_counter; + uint8_t u_reserved; + big_uint32_buf_t u_sync_time_nsec; +}; + +#pragma pack(pop) + +struct EIGEN_ALIGN16 PointSRR520Detection +{ + PCL_ADD_POINT4D; + float range; + float azimuth; + float range_rate; + float rcs; + uint8_t pdh00; + uint8_t pdh01; + uint8_t pdh02; + uint8_t pdh03; + uint8_t pdh04; + uint8_t pdh05; + float snr; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +// Note we only use a subset of the data since POINT_CLOUD_REGISTER_POINT_STRUCT has a limit in the +// number of fields +struct EIGEN_ALIGN16 PointSRR520Object +{ + PCL_ADD_POINT4D; + uint32_t id; + uint16_t age; + float orientation; + float rcs; + float score; + uint8_t object_status; + float dynamics_abs_vel_x; + float dynamics_abs_vel_y; + float dynamics_abs_acc_x; + float dynamics_abs_acc_y; + float box_length; + float box_width; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula + +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::continental_srr520::PointSRR520Detection, + (float, x, x)(float, y, y)(float, z, z)(float, azimuth, azimuth)(float, range, range)( + float, range_rate, range_rate)(int8_t, rcs, rcs)(uint8_t, pdh00, pdh00)(uint8_t, pdh01, pdh01)( + uint8_t, pdh02, + pdh02)(uint16_t, pdh03, pdh03)(uint8_t, pdh04, pdh04)(uint8_t, pdh05, pdh05)(float, snr, snr)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::continental_srr520::PointSRR520Object, + (float, x, x)(float, y, y)(float, z, z)(uint32_t, id, id)(uint16_t, age, age)( + float, orientation, + orientation)(float, rcs, rcs)(float, score, score)(uint8_t, object_status, object_status)( + float, dynamics_abs_vel_x, dynamics_abs_vel_x)(float, dynamics_abs_vel_y, dynamics_abs_vel_y)( + float, dynamics_abs_acc_x, dynamics_abs_acc_x)(float, dynamics_abs_acc_y, dynamics_abs_acc_y)( + float, box_length, box_length)(float, box_width, box_width)) diff --git a/nebula_common/include/nebula_common/continental/crc.hpp b/nebula_common/include/nebula_common/continental/crc.hpp new file mode 100644 index 000000000..733f5d0fd --- /dev/null +++ b/nebula_common/include/nebula_common/continental/crc.hpp @@ -0,0 +1,68 @@ +// 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. + +#pragma once + +#include + +template +int crc16_packets(Iterator begin, Iterator end, int payload_offset) +{ + uint16_t crc_word = 0xffff; + + for (Iterator it = begin; it != end; ++it) { + for (auto it2 = it->data.begin() + payload_offset; it2 != it->data.end(); ++it2) { + auto byte = *it2; + crc_word ^= byte << 8; + for (int i = 0; i < 8; i++) + crc_word = crc_word & 0x8000 ? (crc_word << 1) ^ 0x1021 : crc_word << 1; + } + } + + return crc_word; +} + +template +int crc16_packet(Iterator begin, Iterator end) +{ + uint16_t crc_word = 0xffff; + for (Iterator it = begin; it != end; ++it) { + crc_word ^= (*it) << 8; + for (int i = 0; i < 8; i++) + crc_word = crc_word & 0x8000 ? (crc_word << 1) ^ 0x1021 : crc_word << 1; + } + + return crc_word; +} + +template +uint8_t crc8h2f(Iterator begin, Iterator end) +{ + uint8_t crc_word = 0xFF; + uint8_t bit = 0; + + for (Iterator it = begin; it != end; ++it) { + crc_word ^= *it; + for (bit = 0; bit < 8; bit++) { + if ((crc_word & 0x80) != 0) { + crc_word <<= 1; + crc_word ^= 0x2F; + } else { + crc_word <<= 1; + } + } + } + + return crc_word ^ 0xFF; +} diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 9c6e5d555..b38038473 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -1,24 +1,47 @@ +// 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_HESAI_COMMON_H #define NEBULA_HESAI_COMMON_H #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" +#include "nebula_common/util/string_conversions.hpp" -#include +#include #include #include #include +#include #include +#include +#include +#include +#include namespace nebula { namespace drivers { /// @brief struct for Hesai sensor configuration -struct HesaiSensorConfiguration : LidarConfigurationBase +struct HesaiSensorConfiguration : public LidarConfigurationBase { + std::string multicast_ip; uint16_t gnss_port{}; - double scan_phase{}; + uint16_t sync_angle{}; + double cut_angle{}; double dual_return_distance_threshold{}; + std::string calibration_path; uint16_t rotation_speed; uint16_t cloud_min_angle; uint16_t cloud_max_angle; @@ -33,34 +56,59 @@ struct HesaiSensorConfiguration : LidarConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration const & arg) { - os << (LidarConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port - << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed - << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle - << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold - << ", PtpProfile:" << arg.ptp_profile << ", PtpDomain:" << std::to_string(arg.ptp_domain) - << ", PtpTransportType:" << arg.ptp_transport_type - << ", PtpSwitchType:" << arg.ptp_switch_type; + os << "Hesai Sensor Configuration:" << '\n'; + os << (LidarConfigurationBase)(arg) << '\n'; + os << "Multicast: " + << (arg.multicast_ip.empty() ? "disabled" : "enabled, group " + arg.multicast_ip) << '\n'; + os << "GNSS Port: " << arg.gnss_port << '\n'; + os << "Rotation Speed: " << arg.rotation_speed << '\n'; + os << "Sync Angle: " << arg.sync_angle << '\n'; + os << "Cut Angle: " << arg.cut_angle << '\n'; + os << "FoV Start: " << arg.cloud_min_angle << '\n'; + os << "FoV End: " << arg.cloud_max_angle << '\n'; + os << "Dual Return Distance Threshold: " << arg.dual_return_distance_threshold << '\n'; + os << "Calibration Path: " << arg.calibration_path << '\n'; + os << "PTP Profile: " << arg.ptp_profile << '\n'; + os << "PTP Domain: " << std::to_string(arg.ptp_domain) << '\n'; + os << "PTP Transport Type: " << arg.ptp_transport_type << '\n'; + os << "PTP Switch Type: " << arg.ptp_switch_type; return os; } +struct HesaiCalibrationConfigurationBase : public CalibrationConfigurationBase +{ + virtual nebula::Status LoadFromBytes(const std::vector & buf) = 0; + virtual nebula::Status LoadFromFile(const std::string & calibration_file) = 0; + virtual nebula::Status SaveToFileFromBytes( + const std::string & calibration_file, const std::vector & buf) = 0; + + [[nodiscard]] virtual std::tuple getFovPadding() const = 0; +}; + /// @brief struct for Hesai calibration configuration -struct HesaiCalibrationConfiguration : CalibrationConfigurationBase +struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase { std::map elev_angle_map; std::map azimuth_offset_map; - inline nebula::Status LoadFromFile(const std::string & calibration_file) + inline nebula::Status LoadFromFile(const std::string & calibration_file) override { std::ifstream ifs(calibration_file); if (!ifs) { return Status::INVALID_CALIBRATION_FILE; } std::ostringstream ss; - ss << ifs.rdbuf(); // reading data + ss << ifs.rdbuf(); // reading data ifs.close(); return LoadFromString(ss.str()); } + nebula::Status LoadFromBytes(const std::vector & buf) override + { + std::string calibration_string = std::string(buf.begin(), buf.end()); + return LoadFromString(calibration_string); + } + /// @brief Loading calibration data /// @param calibration_content /// @return Resulting status @@ -70,15 +118,13 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase ss << calibration_content; std::string line; constexpr size_t expected_cols = 3; - while(std::getline(ss, line)) { + while (std::getline(ss, line)) { boost::char_separator sep(","); boost::tokenizer> tok(line, sep); std::vector actual_tokens(tok.begin(), tok.end()); - if (actual_tokens.size() < expected_cols - || actual_tokens.size() > expected_cols - ) { - std::cerr << "Ignoring line with unexpected data:" << line << std::endl; + if (actual_tokens.size() < expected_cols || actual_tokens.size() > expected_cols) { + std::cerr << "Ignoring line with unexpected data: " << line << std::endl; continue; } @@ -88,10 +134,9 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase float azimuth = std::stof(actual_tokens[2]); elev_angle_map[laser_id - 1] = elevation; azimuth_offset_map[laser_id - 1] = azimuth; - } catch (const std::invalid_argument& ia) { + } catch (const std::invalid_argument & ia) { continue; } - } return Status::OK; } @@ -99,7 +144,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase /// @brief Saving calibration data (not used) /// @param calibration_file /// @return Resulting status - inline nebula::Status SaveFile(const std::string & calibration_file) + inline nebula::Status SaveToFile(const std::string & calibration_file) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -117,11 +162,19 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase return Status::OK; } + nebula::Status SaveToFileFromBytes( + const std::string & calibration_file, const std::vector & buf) override + { + std::string calibration_string = std::string(buf.begin(), buf.end()); + return SaveFileFromString(calibration_file, calibration_string); + } + /// @brief Saving calibration data from string /// @param calibration_file path /// @param calibration_string calibration string /// @return Resulting status - inline nebula::Status SaveFileFromString(const std::string & calibration_file, const std::string & calibration_string) + inline nebula::Status SaveFileFromString( + const std::string & calibration_file, const std::string & calibration_string) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -131,10 +184,23 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase ofs.close(); return Status::OK; } + + [[nodiscard]] std::tuple getFovPadding() const override + { + float min = INFINITY; + float max = -INFINITY; + + for (const auto & item : azimuth_offset_map) { + min = std::min(min, item.second); + max = std::max(max, item.second); + } + + return {-max, -min}; + } }; /// @brief struct for Hesai correction configuration (for AT) -struct HesaiCorrection +struct HesaiCorrection : public HesaiCalibrationConfigurationBase { uint16_t delimiter; uint8_t versionMajor; @@ -156,12 +222,11 @@ struct HesaiCorrection /// @brief Load correction data from file /// @param buf Binary buffer /// @return Resulting status - inline nebula::Status LoadFromBinary(const std::vector & buf) + inline nebula::Status LoadFromBytes(const std::vector & buf) override { size_t index; - for (index = 0; index < buf.size()-1; index++) { - if(buf[index]==0xee && buf[index+1]==0xff) - break; + for (index = 0; index < buf.size() - 1; index++) { + if (buf[index] == 0xee && buf[index + 1] == 0xff) break; } delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); versionMajor = buf[index + 2] & 0xff; @@ -258,7 +323,7 @@ struct HesaiCorrection /// @brief Load correction data from file /// @param correction_file path /// @return Resulting status - inline nebula::Status LoadFromFile(const std::string & correction_file) + inline nebula::Status LoadFromFile(const std::string & correction_file) override { std::ifstream ifs(correction_file, std::ios::in | std::ios::binary); if (!ifs) { @@ -268,10 +333,10 @@ struct HesaiCorrection // int cnt = 0; while (!ifs.eof()) { unsigned char c; - ifs.read((char *)&c, sizeof(unsigned char)); + ifs.read(reinterpret_cast(&c), sizeof(unsigned char)); buf.emplace_back(c); } - LoadFromBinary(buf); + LoadFromBytes(buf); ifs.close(); return Status::OK; @@ -281,31 +346,31 @@ struct HesaiCorrection /// @param correction_file path /// @param buf correction binary /// @return Resulting status - inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector & buf) + inline nebula::Status SaveToFileFromBytes( + const std::string & correction_file, const std::vector & buf) override { - std::cerr << "Saving in:" << correction_file << "\n"; + std::cerr << "Saving in: " << correction_file << "\n"; std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); if (!ofs) { - std::cerr << "Could not create file:" << correction_file << "\n"; + std::cerr << "Could not create file: " << correction_file << "\n"; return Status::CANNOT_SAVE_FILE; } std::cerr << "Writing start...." << buf.size() << "\n"; bool sop_received = false; - for (const auto &byte : buf) { + for (const auto & byte : buf) { if (!sop_received) { if (byte == 0xEE) { std::cerr << "SOP received....\n"; sop_received = true; } } - if(sop_received) { + if (sop_received) { ofs << byte; } } std::cerr << "Closing file\n"; ofs.close(); - if(sop_received) - return Status::OK; + if (sop_received) return Status::OK; return Status::INVALID_CALIBRATION_FILE; } @@ -315,7 +380,7 @@ struct HesaiCorrection /// @param ch The channel id /// @param azi The precision azimuth in (0.01 / 256) degree unit /// @return The azimuth adjustment in 0.01 degree unit - int8_t getAzimuthAdjustV3(uint8_t ch, uint32_t azi) const + [[nodiscard]] int8_t getAzimuthAdjustV3(uint8_t ch, uint32_t azi) const { unsigned int i = std::floor(1.f * azi / STEP3); unsigned int l = azi - i * STEP3; @@ -327,13 +392,23 @@ struct HesaiCorrection /// @param ch The channel id /// @param azi The precision azimuth in (0.01 / 256) degree unit /// @return The elevation adjustment in 0.01 degree unit - int8_t getElevationAdjustV3(uint8_t ch, uint32_t azi) const + [[nodiscard]] int8_t getElevationAdjustV3(uint8_t ch, uint32_t azi) const { unsigned int i = std::floor(1.f * azi / STEP3); unsigned int l = azi - i * STEP3; float k = 1.f * l / STEP3; return round((1 - k) * elevationOffset[ch * 180 + i] + k * elevationOffset[ch * 180 + i + 1]); } + + [[nodiscard]] std::tuple getFovPadding() const override + { + // TODO(mojomex): calculate instead of hard-coding + // The reason this is tricky is that an upper bound over all azimuth/elevation combinations has + // to be found. For other sensors, this is only a function of elevation, so the search space is + // tiny compared to AT128. We should be able to find an upper bound of `getAzimuthAdjustV3` but + // I have not invested the time for now. + return {-5, 5}; + } }; /* @@ -362,28 +437,34 @@ inline ReturnMode ReturnModeFromStringHesai( const std::string & return_mode, const SensorModel & sensor_model) { switch (sensor_model) { + case SensorModel::HESAI_PANDARXT32: case SensorModel::HESAI_PANDARXT32M: - case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR128_E3X: case SensorModel::HESAI_PANDAR128_E4X: case SensorModel::HESAI_PANDARQT128: if (return_mode == "Last") return ReturnMode::LAST; if (return_mode == "Strongest") return ReturnMode::STRONGEST; - if (return_mode == "LastStrongest") return ReturnMode::DUAL_LAST_STRONGEST; + if (return_mode == "Dual" || return_mode == "LastStrongest") + return ReturnMode::DUAL_LAST_STRONGEST; if (return_mode == "First") return ReturnMode::FIRST; if (return_mode == "LastFirst") return ReturnMode::DUAL_LAST_FIRST; if (return_mode == "FirstStrongest") return ReturnMode::DUAL_FIRST_STRONGEST; - if (return_mode == "Dual") return ReturnMode::DUAL; break; case SensorModel::HESAI_PANDARQT64: if (return_mode == "Last") return ReturnMode::LAST; - if (return_mode == "Dual") return ReturnMode::DUAL; + if (return_mode == "Dual" || return_mode == "LastFirst") return ReturnMode::DUAL_LAST_FIRST; if (return_mode == "First") return ReturnMode::FIRST; break; - default: + case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR64: + case SensorModel::HESAI_PANDAR40P: if (return_mode == "Last") return ReturnMode::LAST; if (return_mode == "Strongest") return ReturnMode::STRONGEST; - if (return_mode == "Dual") return ReturnMode::DUAL; + if (return_mode == "Dual" || return_mode == "LastStrongest") + return ReturnMode::DUAL_LAST_STRONGEST; break; + default: + throw std::runtime_error("Unsupported sensor model: " + util::to_string(sensor_model)); } return ReturnMode::UNKNOWN; @@ -396,8 +477,9 @@ inline ReturnMode ReturnModeFromStringHesai( inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorModel & sensor_model) { switch (sensor_model) { + case SensorModel::HESAI_PANDARXT32: case SensorModel::HESAI_PANDARXT32M: - case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR128_E3X: case SensorModel::HESAI_PANDAR128_E4X: case SensorModel::HESAI_PANDARQT128: if (return_mode == 0) return ReturnMode::LAST; @@ -409,14 +491,18 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode break; case SensorModel::HESAI_PANDARQT64: if (return_mode == 0) return ReturnMode::LAST; - if (return_mode == 2) return ReturnMode::DUAL; + if (return_mode == 2) return ReturnMode::DUAL_LAST_FIRST; if (return_mode == 3) return ReturnMode::FIRST; break; - default: + case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR64: + case SensorModel::HESAI_PANDAR40P: if (return_mode == 0) return ReturnMode::LAST; if (return_mode == 1) return ReturnMode::STRONGEST; - if (return_mode == 2) return ReturnMode::DUAL; + if (return_mode == 2) return ReturnMode::DUAL_LAST_STRONGEST; break; + default: + throw std::runtime_error("Unsupported sensor model: " + util::to_string(sensor_model)); } return ReturnMode::UNKNOWN; @@ -429,28 +515,34 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorModel & sensor_model) { switch (sensor_model) { + case SensorModel::HESAI_PANDARXT32: case SensorModel::HESAI_PANDARXT32M: - case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR128_E3X: case SensorModel::HESAI_PANDAR128_E4X: case SensorModel::HESAI_PANDARQT128: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; - if (return_mode == ReturnMode::DUAL_LAST_STRONGEST - || return_mode == ReturnMode::DUAL) return 2; + if (return_mode == ReturnMode::DUAL || return_mode == ReturnMode::DUAL_LAST_STRONGEST) + return 2; if (return_mode == ReturnMode::FIRST) return 3; if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4; if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5; break; case SensorModel::HESAI_PANDARQT64: if (return_mode == ReturnMode::LAST) return 0; - if (return_mode == ReturnMode::DUAL) return 2; + if (return_mode == ReturnMode::DUAL || return_mode == ReturnMode::DUAL_LAST_FIRST) return 2; if (return_mode == ReturnMode::FIRST) return 3; break; - default: + case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR64: + case SensorModel::HESAI_PANDAR40P: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; - if (return_mode == ReturnMode::DUAL) return 2; + if (return_mode == ReturnMode::DUAL || return_mode == ReturnMode::DUAL_LAST_STRONGEST) + return 2; break; + default: + throw std::runtime_error("Unsupported sensor model: " + util::to_string(sensor_model)); } return -1; diff --git a/nebula_common/include/nebula_common/hesai/hesai_status.hpp b/nebula_common/include/nebula_common/hesai/hesai_status.hpp index e7547f666..a375bb606 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_status.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_status.hpp @@ -1,10 +1,23 @@ +// 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 HESAI_STATUS_HPP #define HESAI_STATUS_HPP #include "nebula_common/nebula_status.hpp" #include -#include namespace nebula { @@ -26,12 +39,13 @@ struct HesaiStatus : Status Type_end_of_Status = INVALID_RPM_ERROR } _hesai_type; HesaiStatus() : _type_num(static_cast(Status::OK)) { _type = static_cast(type()); } - HesaiStatus(Type v) : _type_num(static_cast(v)) { _type = v; } - HesaiStatus(HesaiType v) : _type_num(static_cast(v)), _hesai_type(v) + /* NOLINT(runtime/explicit) */ HesaiStatus(Type v) : _type_num(static_cast(v)) { _type = v; } + /* NOLINT(runtime/explicit) */ HesaiStatus(HesaiType v) + : _type_num(static_cast(v)), _hesai_type(v) { _type = Type::Type_end_of_Status; } - HesaiStatus(int type) : _type_num(type) {} + explicit HesaiStatus(int type) : _type_num(type) {} int type() const { return _type_num; } friend bool operator==(const HesaiStatus & L, const HesaiStatus & R) { diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 9d04a267a..36044a9e8 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -1,9 +1,25 @@ +// 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_COMMON_H #define NEBULA_COMMON_H #include + #include -#include + +#include #include #include #include @@ -324,10 +340,10 @@ enum class SensorModel { VELODYNE_HDL32, VELODYNE_VLP16, ROBOSENSE_HELIOS, - ROBOSENSE_BPEARL, ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, - CONTINENTAL_ARS548 + CONTINENTAL_ARS548, + CONTINENTAL_SRR520 }; /// @brief not used? @@ -342,24 +358,11 @@ enum class datatype { FLOAT64 = 8 }; -enum class PtpProfile { - IEEE_1588v2 = 0, - IEEE_802_1AS, - IEEE_802_1AS_AUTO, - PROFILE_UNKNOWN -}; +enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, UNKNOWN_PROFILE }; -enum class PtpTransportType { - UDP_IP = 0, - L2, - UNKNOWN_TRANSPORT -}; +enum class PtpTransportType { UDP_IP = 0, L2, UNKNOWN_TRANSPORT }; -enum class PtpSwitchType { - NON_TSN = 0, - TSN, - UNKNOWN_SWITCH -}; +enum class PtpSwitchType { NON_TSN = 0, TSN, UNKNOWN_SWITCH }; /// @brief not used? struct PointField @@ -428,9 +431,6 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::ROBOSENSE_HELIOS: os << "HELIOS"; break; - case SensorModel::ROBOSENSE_BPEARL: - os << "BPEARL"; - break; case SensorModel::ROBOSENSE_BPEARL_V3: os << "BPEARL V3.0"; break; @@ -440,6 +440,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::CONTINENTAL_ARS548: os << "ARS548"; break; + case SensorModel::CONTINENTAL_SRR520: + os << "SRR520"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -462,6 +465,18 @@ struct EthernetSensorConfigurationBase : SensorConfigurationBase uint16_t data_port; }; +/// @brief Base struct for CAN-based Sensor configuration +struct CANSensorConfigurationBase : SensorConfigurationBase +{ + std::string interface; + float receiver_timeout_sec{}; + float sender_timeout_sec{}; + /// @brief Socketcan filters, see the documentation of SocketCanReceiver::CanFilterList for + /// details + std::string filters{}; + bool use_bus_time{}; +}; + /// @brief Base struct for Lidar configuration struct LidarConfigurationBase : EthernetSensorConfigurationBase { @@ -482,7 +497,8 @@ struct LidarConfigurationBase : EthernetSensorConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, SensorConfigurationBase const & arg) { - os << "SensorModel: " << arg.sensor_model << ", FrameID: " << arg.frame_id; + os << "Sensor Model: " << arg.sensor_model << '\n'; + os << "Frame ID: " << arg.frame_id; return os; } @@ -492,8 +508,25 @@ inline std::ostream & operator<<(std::ostream & os, SensorConfigurationBase cons /// @return stream inline std::ostream & operator<<(std::ostream & os, EthernetSensorConfigurationBase const & arg) { - os << (SensorConfigurationBase)(arg) << ", HostIP: " << arg.host_ip - << ", SensorIP: " << arg.sensor_ip << ", DataPort: " << arg.data_port; + os << (SensorConfigurationBase)(arg) << '\n'; + os << "Host IP: " << arg.host_ip << '\n'; + os << "Sensor IP: " << arg.sensor_ip << '\n'; + os << "Data Port: " << arg.data_port; + return os; +} + +/// @brief Convert CANSensorConfigurationBase to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, CANSensorConfigurationBase const & arg) +{ + os << (SensorConfigurationBase)(arg) << '\n'; + os << "Interface: " << arg.interface << '\n'; + os << "Receiver Timeout (s): " << arg.receiver_timeout_sec << '\n'; + os << "Sender Timeout (s): " << arg.sender_timeout_sec << '\n'; + os << "Filters: " << arg.filters << '\n'; + os << "Use Bus Time: " << arg.use_bus_time; return os; } @@ -504,9 +537,11 @@ inline std::ostream & operator<<(std::ostream & os, EthernetSensorConfigurationB inline std::ostream & operator<<( std::ostream & os, nebula::drivers::LidarConfigurationBase const & arg) { - os << (EthernetSensorConfigurationBase)(arg) << ", ReturnMode: " << arg.return_mode - << ", Frequency: " << arg.frequency_ms << ", MTU: " << arg.packet_mtu_size - << ", Use sensor time: " << arg.use_sensor_time; + os << (EthernetSensorConfigurationBase)(arg) << '\n'; + os << "Return Mode: " << arg.return_mode << '\n'; + os << "Frequency: " << arg.frequency_ms << '\n'; + os << "MTU: " << arg.packet_mtu_size << '\n'; + os << "Use Sensor Time: " << arg.use_sensor_time; return os; } @@ -540,10 +575,12 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) if (sensor_model == "VLP16") return SensorModel::VELODYNE_VLP16; // Robosense if (sensor_model == "Helios") return SensorModel::ROBOSENSE_HELIOS; - if (sensor_model == "Bpearl") return SensorModel::ROBOSENSE_BPEARL; + if (sensor_model == "Bpearl" || sensor_model == "Bpearl_V4") + return SensorModel::ROBOSENSE_BPEARL_V4; if (sensor_model == "Bpearl_V3") return SensorModel::ROBOSENSE_BPEARL_V3; - if (sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4; + // Continental if (sensor_model == "ARS548") return SensorModel::CONTINENTAL_ARS548; + if (sensor_model == "SRR520") return SensorModel::CONTINENTAL_SRR520; return SensorModel::UNKNOWN; } @@ -585,14 +622,15 @@ inline std::string SensorModelToString(const SensorModel & sensor_model) // Robosense case SensorModel::ROBOSENSE_HELIOS: return "Helios"; - case SensorModel::ROBOSENSE_BPEARL: - return "Bpearl"; case SensorModel::ROBOSENSE_BPEARL_V3: return "Bpearl_V3"; case SensorModel::ROBOSENSE_BPEARL_V4: return "Bpearl_V4"; + // Continental case SensorModel::CONTINENTAL_ARS548: return "ARS548"; + case SensorModel::CONTINENTAL_SRR520: + return "SRR520"; default: return "UNKNOWN"; } @@ -618,13 +656,14 @@ inline PtpProfile PtpProfileFromString(const std::string & ptp_profile) { // Hesai auto tmp_str = ptp_profile; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2; if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS; if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO; - return PtpProfile::PROFILE_UNKNOWN; + return PtpProfile::UNKNOWN_PROFILE; } /// @brief Convert PtpProfile enum to string (Overloading the << operator) @@ -643,7 +682,7 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpProfile case PtpProfile::IEEE_802_1AS_AUTO: os << "IEEE_802.1AS Automotive"; break; - case PtpProfile::PROFILE_UNKNOWN: + case PtpProfile::UNKNOWN_PROFILE: os << "UNKNOWN"; break; } @@ -657,8 +696,9 @@ inline PtpTransportType PtpTransportTypeFromString(const std::string & transport { // Hesai auto tmp_str = transport_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "udp") return PtpTransportType::UDP_IP; if (tmp_str == "l2") return PtpTransportType::L2; @@ -692,8 +732,9 @@ inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type) { // Hesai auto tmp_str = switch_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "tsn") return PtpSwitchType::TSN; if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN; diff --git a/nebula_common/include/nebula_common/nebula_status.hpp b/nebula_common/include/nebula_common/nebula_status.hpp index f36d8b7d2..9f9eef655 100644 --- a/nebula_common/include/nebula_common/nebula_status.hpp +++ b/nebula_common/include/nebula_common/nebula_status.hpp @@ -1,8 +1,21 @@ +// 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_STATUS_HPP #define NEBULA_STATUS_HPP #include -#include namespace nebula { @@ -14,6 +27,7 @@ struct Status enum Type { OK = 0, UDP_CONNECTION_ERROR, + CAN_CONNECTION_ERROR, SENSOR_CONFIG_ERROR, INVALID_SENSOR_MODEL, INVALID_ECHO_MODE, @@ -27,8 +41,8 @@ struct Status Type_end_of_Status = ERROR_1 } _type; Status() : _type(Type::OK) {} - Status(Type v) : _type(v) {} - Status(int type) : _type(static_cast(type)) {} + /* NOLINT(runtime/explicit) */ Status(Type v) : _type(v) {} + explicit Status(int type) : _type(static_cast(type)) {} Type type() const { return _type; } friend bool operator==(const Status & L, const Status & R) { return L.type() == R.type(); } friend bool operator!=(const Status & L, const Status & R) { return L.type() != R.type(); } diff --git a/nebula_common/include/nebula_common/point_types.hpp b/nebula_common/include/nebula_common/point_types.hpp index 260b7b9db..db66f6941 100644 --- a/nebula_common/include/nebula_common/point_types.hpp +++ b/nebula_common/include/nebula_common/point_types.hpp @@ -1,9 +1,25 @@ +// 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_POINT_TYPES_H #define NEBULA_POINT_TYPES_H #include #include +#include + namespace nebula { namespace drivers @@ -63,20 +79,15 @@ using NebulaPointCloudPtr = pcl::PointCloud::Ptr; } // namespace drivers } // namespace nebula -POINT_CLOUD_REGISTER_POINT_STRUCT(nebula::drivers::PointXYZIR, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, - ring, ring)) +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::PointXYZIR, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)) -POINT_CLOUD_REGISTER_POINT_STRUCT(nebula::drivers::PointXYZIRADT, - (float, x, x) - (float, y, y) - (float, z, z) - (float, intensity, intensity) - (std::uint16_t, ring, ring) - (float, azimuth, azimuth) - (float, distance, distance) - (std::uint8_t, return_type, return_type) - (double, time_stamp, time_stamp)) +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::PointXYZIRADT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( + float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( + double, time_stamp, time_stamp)) POINT_CLOUD_REGISTER_POINT_STRUCT( nebula::drivers::PointXYZICATR, diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp index 1984642dc..d7bb496d4 100644 --- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -1,14 +1,27 @@ +// 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. + #pragma once #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include #include #include #include -#include #include +#include #include namespace nebula @@ -25,9 +38,6 @@ struct RobosenseSensorConfiguration : LidarConfigurationBase uint16_t gnss_port{}; // difop double scan_phase{}; // start/end angle double dual_return_distance_threshold{}; - uint16_t rotation_speed; - uint16_t cloud_min_angle; - uint16_t cloud_max_angle; }; /// @brief Convert RobosenseSensorConfiguration to string (Overloading the << operator) @@ -36,9 +46,10 @@ struct RobosenseSensorConfiguration : LidarConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, RobosenseSensorConfiguration const & arg) { - os << (LidarConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port - << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed - << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle; + os << "Robosense Sensor Configuration:" << '\n'; + os << (LidarConfigurationBase)(arg) << '\n'; + os << "GNSS Port: " << arg.gnss_port << '\n'; + os << "Scan Phase: " << arg.scan_phase; return os; } @@ -55,16 +66,6 @@ inline ReturnMode ReturnModeFromStringRobosense(const std::string & return_mode) return ReturnMode::UNKNOWN; } -size_t GetChannelSize(const SensorModel & model) -{ - switch (model) { - case SensorModel::ROBOSENSE_BPEARL_V3: - return 32; - case SensorModel::ROBOSENSE_HELIOS: - return 32; - } -} - struct ChannelCorrection { float azimuth{NAN}; @@ -88,7 +89,7 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase std::getline(stream, header); char sep; - int laser_id; + size_t laser_id; float elevation; float azimuth; Status load_status = Status::OK; @@ -188,10 +189,10 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase void CreateCorrectedChannels() { - for(auto& correction : calibration) { + for (auto & correction : calibration) { uint16_t channel = 0; - for(const auto& compare:calibration) { - if(compare.elevation < correction.elevation) ++channel; + for (const auto & compare : calibration) { + if (compare.elevation < correction.elevation) ++channel; } correction.channel = channel; } diff --git a/nebula_common/include/nebula_common/util/expected.hpp b/nebula_common/include/nebula_common/util/expected.hpp new file mode 100644 index 000000000..1d4333443 --- /dev/null +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -0,0 +1,109 @@ +// 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. + +#pragma once + +#include +#include +#include + +namespace nebula +{ +namespace util +{ + +struct bad_expected_access : public std::exception +{ + explicit bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {} + + const char * what() const noexcept override { return msg_.c_str(); } + +private: + const std::string msg_; +}; + +/// @brief A poor man's backport of C++23's std::expected. +/// +/// At any given time, holds exactly one of: expected value, or error value. +/// Provides functions for easy exception handling. +/// +/// More info here: https://en.cppreference.com/w/cpp/utility/expected +/// +/// @tparam T Type of the expected value +/// @tparam E Error type +template +struct expected +{ + /// @brief Whether the expected instance holds a value (as opposed to an error). + /// Call this before trying to access values via `value()`. + /// @return True if a value is contained, false if an error is contained + bool has_value() { return std::holds_alternative(value_); } + + /// @brief Retrieve the value, or throw `bad_expected_access` if an error is contained. + /// @return The value of type `T` + T value() + { + if (!has_value()) { + throw bad_expected_access("value() called but containing error"); + } + return std::get(value_); + } + + /// @brief Return the contained value, or, if an error is contained, return the given `default_` + /// instead. + /// @return The contained value, if any, else `default_` + T value_or(const T & default_) + { + if (has_value()) return value(); + return default_; + } + + /// @brief If the instance has a value, return the value, else throw std::runtime_error(error_msg) + /// @param error_msg The message to be included in the thrown exception + /// @return The value + T value_or_throw(const std::string & error_msg) + { + if (has_value()) return value(); + throw std::runtime_error(error_msg); + } + + /// @brief Retrieve the error, or throw `bad_expected_access` if a value is contained. + /// @return The error of type `E` + E error() + { + if (has_value()) { + throw bad_expected_access("error() called but containing value"); + } + return std::get(value_); + } + + /// @brief Return the contained error, or, if a value is contained, return the given `default_` + /// instead. + /// @return The contained error, if any, else `default_` + E error_or(const E & default_) + { + if (!has_value()) return error(); + return default_; + } + + expected(const T & value) { value_ = value; } // NOLINT(runtime/explicit) + + expected(const E & error) { value_ = error; } // NOLINT(runtime/explicit) + +private: + std::variant value_; +}; + +} // namespace util +} // namespace nebula diff --git a/nebula_common/include/nebula_common/util/string_conversions.hpp b/nebula_common/include/nebula_common/util/string_conversions.hpp new file mode 100644 index 000000000..df5cb2b48 --- /dev/null +++ b/nebula_common/include/nebula_common/util/string_conversions.hpp @@ -0,0 +1,44 @@ +// 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. + +#pragma once + +#include +#include +#include +#include + +namespace nebula::util +{ + +template +struct IsStreamable : std::false_type +{ +}; + +template +struct IsStreamable() << std::declval())> > +: std::true_type +{ +}; + +template +std::enable_if_t::value, std::string> to_string(const T & value) +{ + std::stringstream ss{}; + ss << value; + return ss.str(); +} + +} // namespace nebula::util diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_calibration_decoder.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_calibration_decoder.hpp index 9cbbb53ea..49b8a427f 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_calibration_decoder.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_calibration_decoder.hpp @@ -15,9 +15,6 @@ #include #include -#include -#include -#include #include #include #include @@ -61,12 +58,8 @@ class VelodyneCalibration bool initialized; public: - explicit VelodyneCalibration() - : distance_resolution_m(0.002f), initialized(false) - { - } - explicit VelodyneCalibration(const std::string & calibration_file) - : distance_resolution_m(0.002f) + VelodyneCalibration() : distance_resolution_m(0.002f), initialized(false) {} + explicit VelodyneCalibration(const std::string & calibration_file) : distance_resolution_m(0.002f) { read(calibration_file); } diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index b429e7b53..2c68eedb8 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp @@ -1,3 +1,17 @@ +// 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_VELODYNE_COMMON_H #define NEBULA_VELODYNE_COMMON_H @@ -5,8 +19,7 @@ #include "nebula_common/nebula_status.hpp" #include "nebula_common/velodyne/velodyne_calibration_decoder.hpp" -#include -#include +#include namespace nebula { namespace drivers @@ -26,9 +39,13 @@ struct VelodyneSensorConfiguration : LidarConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, VelodyneSensorConfiguration const & arg) { - os << (LidarConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port - << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed - << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle; + os << "Velodyne Sensor Configuration:" << '\n'; + os << (LidarConfigurationBase)(arg) << '\n'; + os << "GNSS Port: " << arg.gnss_port << '\n'; + os << "Scan Phase: " << arg.scan_phase << '\n'; + os << "Rotation Speed: " << arg.rotation_speed << '\n'; + os << "FoV Start: " << arg.cloud_min_angle << '\n'; + os << "FoV End: " << arg.cloud_max_angle; return os; } diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_status.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_status.hpp index 783f57af6..ac60e7994 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_status.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_status.hpp @@ -1,10 +1,23 @@ +// 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 VELODYNE_STATUS_HPP #define VELODYNE_STATUS_HPP #include "nebula_common/nebula_status.hpp" #include -#include namespace nebula { @@ -26,12 +39,16 @@ struct VelodyneStatus : Status Type_end_of_Status = INVALID_RPM_ERROR } _velo_type; VelodyneStatus() : _type_num(static_cast(Status::OK)) { _type = static_cast(type()); } - VelodyneStatus(Type v) : _type_num(static_cast(v)) { _type = v; } - VelodyneStatus(VelodyneType v) : _type_num(static_cast(v)), _velo_type(v) + /* NOLINT(runtime/explicit) */ VelodyneStatus(Type v) : _type_num(static_cast(v)) + { + _type = v; + } + /* NOLINT(runtime/explicit) */ VelodyneStatus(VelodyneType v) + : _type_num(static_cast(v)), _velo_type(v) { _type = Type::Type_end_of_Status; } - VelodyneStatus(int type) : _type_num(type) {} + explicit VelodyneStatus(int type) : _type_num(type) {} int type() const { return _type_num; } friend bool operator==(const VelodyneStatus & L, const VelodyneStatus & R) { diff --git a/nebula_common/package.xml b/nebula_common/package.xml index d769e273e..6ca9d47c9 100644 --- a/nebula_common/package.xml +++ b/nebula_common/package.xml @@ -2,7 +2,7 @@ nebula_common - 0.1.0 + 0.2.0 Nebula Common Libraries and headers MAP IV @@ -13,7 +13,6 @@ ros_environment libpcl-all-dev - pcl_conversions yaml-cpp ament_cmake_gtest diff --git a/nebula_common/src/nebula_common.cpp b/nebula_common/src/nebula_common.cpp index 7af599032..c87331421 100644 --- a/nebula_common/src/nebula_common.cpp +++ b/nebula_common/src/nebula_common.cpp @@ -1,3 +1,5 @@ +// Copyright 2024 TIER IV, Inc. + #include namespace nebula @@ -60,7 +62,7 @@ pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIRADT( point.ring = p.channel; point.azimuth = rad2deg(p.azimuth) * 100.0; point.distance = p.distance; - point.time_stamp = stamp + static_cast(p.time_stamp)*1e-9; + point.time_stamp = stamp + static_cast(p.time_stamp) * 1e-9; output_pointcloud->points.emplace_back(point); } diff --git a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp index 0c03c3c9e..d4d69e018 100644 --- a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp +++ b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp @@ -8,6 +8,9 @@ #include +#include +#include + #ifdef HAVE_NEW_YAMLCPP namespace YAML { @@ -25,22 +28,22 @@ namespace nebula { namespace drivers { -const std::string NUM_LASERS = "num_lasers"; -const std::string DISTANCE_RESOLUTION = "distance_resolution"; -const std::string LASERS = "lasers"; -const std::string LASER_ID = "laser_id"; -const std::string ROT_CORRECTION = "rot_correction"; -const std::string VERT_CORRECTION = "vert_correction"; -const std::string DIST_CORRECTION = "dist_correction"; -const std::string TWO_PT_CORRECTION_AVAILABLE = "two_pt_correction_available"; -const std::string DIST_CORRECTION_X = "dist_correction_x"; -const std::string DIST_CORRECTION_Y = "dist_correction_y"; -const std::string VERT_OFFSET_CORRECTION = "vert_offset_correction"; -const std::string HORIZ_OFFSET_CORRECTION = "horiz_offset_correction"; -const std::string MAX_INTENSITY = "max_intensity"; -const std::string MIN_INTENSITY = "min_intensity"; -const std::string FOCAL_DISTANCE = "focal_distance"; -const std::string FOCAL_SLOPE = "focal_slope"; +const char NUM_LASERS[] = "num_lasers"; +const char DISTANCE_RESOLUTION[] = "distance_resolution"; +const char LASERS[] = "lasers"; +const char LASER_ID[] = "laser_id"; +const char ROT_CORRECTION[] = "rot_correction"; +const char VERT_CORRECTION[] = "vert_correction"; +const char DIST_CORRECTION[] = "dist_correction"; +const char TWO_PT_CORRECTION_AVAILABLE[] = "two_pt_correction_available"; +const char DIST_CORRECTION_X[] = "dist_correction_x"; +const char DIST_CORRECTION_Y[] = "dist_correction_y"; +const char VERT_OFFSET_CORRECTION[] = "vert_offset_correction"; +const char HORIZ_OFFSET_CORRECTION[] = "horiz_offset_correction"; +const char MAX_INTENSITY[] = "max_intensity"; +const char MIN_INTENSITY[] = "min_intensity"; +const char FOCAL_DISTANCE[] = "focal_distance"; +const char FOCAL_SLOPE[] = "focal_slope"; /** Read calibration for a single laser. */ void operator>>(const YAML::Node & node, std::pair & correction) @@ -158,7 +161,6 @@ void operator>>(const YAML::Node & node, VelodyneCalibration & calibration) } if (next_index < num_lasers) { // anything found in this ring? - // store this ring number with its corresponding laser number calibration.laser_corrections[next_index].laser_ring = ring; next_angle = min_seen; diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index 31385de6f..e94afcf2a 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -1,10 +1,6 @@ cmake_minimum_required(VERSION 3.14) project(nebula_decoders) -find_package(ament_cmake_auto REQUIRED) - -ament_auto_find_build_dependencies() - # Default to C++17 if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -14,58 +10,152 @@ if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function) endif () + +find_package(ament_cmake_auto REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) find_package(angles REQUIRED) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) -find_package(yaml-cpp REQUIRED) +find_package(continental_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) find_package(nebula_common REQUIRED) +find_package(nebula_msgs REQUIRED) +find_package(pandar_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(rclcpp REQUIRED) find_package(robosense_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(velodyne_msgs REQUIRED) +find_package(yaml-cpp REQUIRED) -include_directories( - include - SYSTEM - ${YAML_CPP_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${PCL_COMMON_INCLUDE_DIRS} +include_directories(PUBLIC + include + SYSTEM + ${nebula_common_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} +) + +link_libraries( + ${nebula_common_TARGETS} + ${PCL_LIBRARIES} + ${pcl_conversions_LIBRARIES} + ${rclcpp_TARGETS} + ${sensor_msgs_TARGETS} ) # Lidar Decoders # Hesai -ament_auto_add_library(nebula_decoders_hesai SHARED - src/nebula_decoders_hesai/hesai_driver.cpp - ) +add_library(nebula_decoders_hesai SHARED + src/nebula_decoders_hesai/hesai_driver.cpp +) +target_link_libraries(nebula_decoders_hesai PUBLIC + ${pandar_msgs_TARGETS} +) + +target_include_directories(nebula_decoders_hesai PUBLIC + ${pandar_msgs_INCLUDE_DIRS} +) # Velodyne -ament_auto_add_library(nebula_decoders_velodyne SHARED - src/nebula_decoders_velodyne/velodyne_driver.cpp - src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp - src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp - src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp - ) +add_library(nebula_decoders_velodyne SHARED + src/nebula_decoders_velodyne/velodyne_driver.cpp + src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp + src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp + src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +) +target_link_libraries(nebula_decoders_velodyne PUBLIC + ${angles_msgs_TARGETS} + ${velodyne_msgs_TARGETS} +) +target_include_directories(nebula_decoders_velodyne PUBLIC + ${angles_INCLUDE_DIRS} + ${velodyne_msgs_INCLUDE_DIRS} +) # Robosense -ament_auto_add_library(nebula_decoders_robosense SHARED - src/nebula_decoders_robosense/robosense_driver.cpp - ) +add_library(nebula_decoders_robosense SHARED + src/nebula_decoders_robosense/robosense_driver.cpp +) +target_link_libraries(nebula_decoders_robosense PUBLIC + ${robosense_msgs_TARGETS} +) +target_include_directories(nebula_decoders_robosense PUBLIC + ${robosense_msgs_INCLUDE_DIRS} +) -ament_auto_add_library(nebula_decoders_robosense_info SHARED - src/nebula_decoders_robosense/robosense_info_driver.cpp - ) +add_library(nebula_decoders_robosense_info SHARED + src/nebula_decoders_robosense/robosense_info_driver.cpp +) +target_link_libraries(nebula_decoders_robosense_info PUBLIC + ${robosense_msgs_TARGETS} +) +target_include_directories(nebula_decoders_robosense_info PUBLIC + ${robosense_msgs_INCLUDE_DIRS} +) # Continental -ament_auto_add_library(nebula_decoders_continental SHARED - src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp - ) +add_library(nebula_decoders_continental SHARED + src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp + src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +) +target_link_libraries(nebula_decoders_continental PUBLIC + ${continental_msgs_TARGETS} + ${diagnostic_msgs_TARGETS} + ${boost_udp_driver_TARGETS} + ${nebula_common_TARGETS} + ${nebula_msgs_TARGETS} +) +target_include_directories(nebula_decoders_continental PUBLIC + ${continental_msgs_INCLUDE_DIRS} + ${diagnostic_msgs_INCLUDE_DIRS} + ${boost_udp_driver_INCLUDE_DIRS} + ${nebula_common_INCLUDE_DIRS} + ${nebula_msgs_INCLUDE_DIRS} +) + +install(TARGETS nebula_decoders_hesai EXPORT export_nebula_decoders_hesai) +install(TARGETS nebula_decoders_velodyne EXPORT export_nebula_decoders_velodyne) +install(TARGETS nebula_decoders_robosense EXPORT export_nebula_decoders_robosense) +install(TARGETS nebula_decoders_robosense_info EXPORT export_nebula_decoders_robosense_info) +install(TARGETS nebula_decoders_continental EXPORT export_nebula_decoders_continental) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() -ament_auto_package( - INSTALL_TO_SHARE - calibration - ) +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_nebula_decoders_hesai) +ament_export_targets(export_nebula_decoders_velodyne) +ament_export_targets(export_nebula_decoders_robosense) +ament_export_targets(export_nebula_decoders_robosense_info) +ament_export_targets(export_nebula_decoders_continental) + +install( + DIRECTORY calibration + DESTINATION share/${PROJECT_NAME} +) + +ament_export_dependencies( + PCL + pcl_conversions + angles + continental_msgs + diagnostic_msgs + nebula_common + nebula_msgs + pandar_msgs + rclcpp + robosense_msgs + sensor_msgs + velodyne_msgs + yaml-cpp +) + +ament_package() # Set ROS_DISTRO macros set(ROS_DISTRO $ENV{ROS_DISTRO}) diff --git a/nebula_decoders/calibration/velodyne/HDL32.yaml b/nebula_decoders/calibration/velodyne/HDL32.yaml index 7a0e6f6e4..e276ec43e 100644 --- a/nebula_decoders/calibration/velodyne/HDL32.yaml +++ b/nebula_decoders/calibration/velodyne/HDL32.yaml @@ -1,388 +1,388 @@ # standard Velodyne HDL-32E calibration parameters lasers: - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 0, - rot_correction: 0.0, - vert_correction: -0.5352924815866609, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 1, - rot_correction: 0.0, - vert_correction: -0.1628392174657417, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 2, - rot_correction: 0.0, - vert_correction: -0.5119050696099369, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 3, - rot_correction: 0.0, - vert_correction: -0.13962634015954636, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 4, - rot_correction: 0.0, - vert_correction: -0.4886921905584123, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 5, - rot_correction: 0.0, - vert_correction: -0.11641346285335104, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 6, - rot_correction: 0.0, - vert_correction: -0.4654793115068877, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 7, - rot_correction: 0.0, - vert_correction: -0.09302604738596851, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 8, - rot_correction: 0.0, - vert_correction: -0.44209189953016365, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 9, - rot_correction: 0.0, - vert_correction: -0.06981317007977318, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 10, - rot_correction: 0.0, - vert_correction: -0.4188790204786391, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 11, - rot_correction: 0.0, - vert_correction: -0.046600292773577856, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 12, - rot_correction: 0.0, - vert_correction: -0.39566614142711454, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 13, - rot_correction: 0.0, - vert_correction: -0.023212879051524585, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 14, - rot_correction: 0.0, - vert_correction: -0.3722787294503905, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 15, - rot_correction: 0.0, - vert_correction: 0.0, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 16, - rot_correction: 0.0, - vert_correction: -0.3490658503988659, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 17, - rot_correction: 0.0, - vert_correction: 0.023212879051524585, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 18, - rot_correction: 0.0, - vert_correction: -0.32585297134734137, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 19, - rot_correction: 0.0, - vert_correction: 0.046600292773577856, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 20, - rot_correction: 0.0, - vert_correction: -0.30246555937061725, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 21, - rot_correction: 0.0, - vert_correction: 0.06981317007977318, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 22, - rot_correction: 0.0, - vert_correction: -0.2792526803190927, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 23, - rot_correction: 0.0, - vert_correction: 0.09302604738596851, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 24, - rot_correction: 0.0, - vert_correction: -0.25603980126756815, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 25, - rot_correction: 0.0, - vert_correction: 0.11641346285335104, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 26, - rot_correction: 0.0, - vert_correction: -0.23265238929084414, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 27, - rot_correction: 0.0, - vert_correction: 0.13962634015954636, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 28, - rot_correction: 0.0, - vert_correction: -0.20943951023931956, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 29, - rot_correction: 0.0, - vert_correction: 0.1628392174657417, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 30, - rot_correction: 0.0, - vert_correction: -0.18622663118779495, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 31, - rot_correction: 0.0, - vert_correction: 0.18622663118779495, - vert_offset_correction: 0.0, + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 0, + rot_correction: 0.0, + vert_correction: -0.5352924815866609, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 1, + rot_correction: 0.0, + vert_correction: -0.1628392174657417, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 2, + rot_correction: 0.0, + vert_correction: -0.5119050696099369, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 3, + rot_correction: 0.0, + vert_correction: -0.13962634015954636, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 4, + rot_correction: 0.0, + vert_correction: -0.4886921905584123, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 5, + rot_correction: 0.0, + vert_correction: -0.11641346285335104, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 6, + rot_correction: 0.0, + vert_correction: -0.4654793115068877, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 7, + rot_correction: 0.0, + vert_correction: -0.09302604738596851, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 8, + rot_correction: 0.0, + vert_correction: -0.44209189953016365, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 9, + rot_correction: 0.0, + vert_correction: -0.06981317007977318, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 10, + rot_correction: 0.0, + vert_correction: -0.4188790204786391, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 11, + rot_correction: 0.0, + vert_correction: -0.046600292773577856, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 12, + rot_correction: 0.0, + vert_correction: -0.39566614142711454, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 13, + rot_correction: 0.0, + vert_correction: -0.023212879051524585, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 14, + rot_correction: 0.0, + vert_correction: -0.3722787294503905, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 15, + rot_correction: 0.0, + vert_correction: 0.0, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 16, + rot_correction: 0.0, + vert_correction: -0.3490658503988659, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 17, + rot_correction: 0.0, + vert_correction: 0.023212879051524585, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 18, + rot_correction: 0.0, + vert_correction: -0.32585297134734137, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 19, + rot_correction: 0.0, + vert_correction: 0.046600292773577856, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 20, + rot_correction: 0.0, + vert_correction: -0.30246555937061725, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 21, + rot_correction: 0.0, + vert_correction: 0.06981317007977318, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 22, + rot_correction: 0.0, + vert_correction: -0.2792526803190927, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 23, + rot_correction: 0.0, + vert_correction: 0.09302604738596851, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 24, + rot_correction: 0.0, + vert_correction: -0.25603980126756815, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 25, + rot_correction: 0.0, + vert_correction: 0.11641346285335104, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 26, + rot_correction: 0.0, + vert_correction: -0.23265238929084414, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 27, + rot_correction: 0.0, + vert_correction: 0.13962634015954636, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 28, + rot_correction: 0.0, + vert_correction: -0.20943951023931956, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 29, + rot_correction: 0.0, + vert_correction: 0.1628392174657417, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 30, + rot_correction: 0.0, + vert_correction: -0.18622663118779495, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 31, + rot_correction: 0.0, + vert_correction: 0.18622663118779495, + vert_offset_correction: 0.0, } num_lasers: 32 distance_resolution: 0.002 diff --git a/nebula_decoders/calibration/velodyne/HDL64e_s2.yaml b/nebula_decoders/calibration/velodyne/HDL64e_s2.yaml index 3bc719cd0..8c1c190dc 100644 --- a/nebula_decoders/calibration/velodyne/HDL64e_s2.yaml +++ b/nebula_decoders/calibration/velodyne/HDL64e_s2.yaml @@ -1,817 +1,817 @@ lasers: - - { - dist_correction: 1.5195264000000002, - dist_correction_x: 1.5500304, - dist_correction_y: 1.5231381, - focal_distance: 12.0, - focal_slope: 1.4, - horiz_offset_correction: 0.025999999, - laser_id: 0, - max_intensity: 235.0, - min_intensity: 30.0, - rot_correction: -0.1248942899601548, - vert_correction: -0.15304134919741974, - vert_offset_correction: 0.19548199, - } - - { - dist_correction: 1.5145139, - dist_correction_x: 1.5256960000000002, - dist_correction_y: 1.5491043, - focal_distance: 5.0, - focal_slope: 1.0, - horiz_offset_correction: -0.025999999, - laser_id: 1, - min_intensity: 40.0, - rot_correction: -0.06924466398252106, - vert_correction: -0.1458455539136526, - vert_offset_correction: 0.19601112, - } - - { - dist_correction: 1.4963768, - dist_correction_x: 1.5571011, - dist_correction_y: 1.5456782999999998, - focal_distance: 5.0, - focal_slope: 0.89999998, - horiz_offset_correction: 0.025999999, - laser_id: 2, - min_intensity: 60.0, - rot_correction: 0.0824016906676694, - vert_correction: 0.04339494149708345, - vert_offset_correction: 0.20969539999999998, - } - - { - dist_correction: 1.3771207, - dist_correction_x: 1.4103835, - dist_correction_y: 1.4343457000000002, - focal_distance: 9.5, - focal_slope: 1.1, - horiz_offset_correction: -0.025999999, - laser_id: 3, - min_intensity: 20.0, - rot_correction: 0.137808349360133, - vert_correction: 0.05196082373432465, - vert_offset_correction: 0.21031273, - } - - { - dist_correction: 1.2947885, - dist_correction_x: 1.3720455999999999, - dist_correction_y: 1.4025244000000001, - focal_distance: 10.0, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 4, - min_intensity: 50.0, - rot_correction: -0.011199603626188263, - vert_correction: -0.1358262679404349, - vert_offset_correction: 0.19674603999999998, - } - - { - dist_correction: 1.4395787000000002, - dist_correction_x: 1.4801956, - dist_correction_y: 1.5074649, - focal_distance: 7.0, - focal_slope: 1.0, - horiz_offset_correction: -0.025999999, - laser_id: 5, - min_intensity: 30.0, - rot_correction: 0.045795414990398894, - vert_correction: -0.12678532627942263, - vert_offset_correction: 0.19740747, - } - - { - dist_correction: 1.3618773, - dist_correction_x: 1.4004077000000001, - dist_correction_y: 1.3900876, - focal_distance: 5.0, - focal_slope: 1.1, - horiz_offset_correction: 0.025999999, - laser_id: 6, - min_intensity: 65.0, - rot_correction: -0.031646046452444135, - vert_correction: -0.1895564226946273, - vert_offset_correction: 0.19277746, - } - - { - dist_correction: 1.5325716, - dist_correction_x: 1.5337143, - dist_correction_y: 1.5452948000000002, - focal_distance: 6.0, - focal_slope: 1.0, - horiz_offset_correction: -0.025999999, - laser_id: 7, - min_intensity: 10.0, - rot_correction: 0.023408326257150665, - vert_correction: -0.18086723120040343, - vert_offset_correction: 0.19342419, - } - - { - dist_correction: 1.3743323, - dist_correction_x: 1.4474606, - dist_correction_y: 1.4533472, - focal_distance: 6.0, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 8, - min_intensity: 30.0, - rot_correction: 0.10065885915180103, - vert_correction: -0.11973897655818674, - vert_offset_correction: 0.19792192, - } - - { - dist_correction: 1.4969112000000002, - dist_correction_x: 1.4754176, - dist_correction_y: 1.4888799000000001, - focal_distance: 5.0, - focal_slope: 1.0, - horiz_offset_correction: -0.025999999, - laser_id: 9, - min_intensity: 10.0, - rot_correction: 0.15996423607269877, - vert_correction: -0.10924820210484752, - vert_offset_correction: 0.19868624000000001, - } - - { - dist_correction: 1.434263, - dist_correction_x: 1.4957901000000002, - dist_correction_y: 1.5191892999999999, - focal_distance: 5.0, - focal_slope: 0.89999998, - horiz_offset_correction: 0.025999999, - laser_id: 10, - min_intensity: 35.0, - rot_correction: 0.08313316164783874, - vert_correction: -0.17135657256846043, - vert_offset_correction: 0.19412970999999998, - } - - { - dist_correction: 1.5500841, - dist_correction_x: 1.542697, - dist_correction_y: 1.5716737, - focal_distance: 8.0, - focal_slope: 1.2, - horiz_offset_correction: -0.025999999, - laser_id: 11, - min_intensity: 30.0, - rot_correction: 0.13862572370075624, - vert_correction: -0.1630088948849621, - vert_offset_correction: 0.19474705, - } - - { - dist_correction: 1.3725992, - dist_correction_x: 1.4353555, - dist_correction_y: 1.3932405, - focal_distance: 11.0, - focal_slope: 1.1, - horiz_offset_correction: 0.025999999, - laser_id: 12, - min_intensity: 50.0, - rot_correction: -0.1255805045528199, - vert_correction: -0.04526314018308893, - vert_offset_correction: 0.20331627000000002, - } - - { - dist_correction: 1.3111591000000002, - dist_correction_x: 1.3470857, - dist_correction_y: 1.3652054000000002, - focal_distance: 6.0, - focal_slope: 0.69999999, - horiz_offset_correction: -0.025999999, - laser_id: 13, - min_intensity: 30.0, - rot_correction: -0.06716508498014805, - vert_correction: -0.03587557613263058, - vert_offset_correction: 0.20399239000000002, - } - - { - dist_correction: 1.5360803, - dist_correction_x: 1.5528035, - dist_correction_y: 1.5666801000000001, - focal_distance: 8.0, - focal_slope: 1.1, - horiz_offset_correction: 0.025999999, - laser_id: 14, - min_intensity: 20.0, - rot_correction: -0.14578889529014513, - vert_correction: -0.09893566067472198, - vert_offset_correction: 0.19943586, - } - - { - dist_correction: 1.4755448999999998, - dist_correction_x: 1.5032428, - dist_correction_y: 1.5326752, - focal_distance: 5.0, - focal_slope: 0.89999998, - horiz_offset_correction: -0.025999999, - laser_id: 15, - min_intensity: 30.0, - rot_correction: -0.08892898296002069, - vert_correction: -0.09062974348423608, - vert_offset_correction: 0.20003851, - } - - { - dist_correction: 1.4410587000000001, - dist_correction_x: 1.4870845, - dist_correction_y: 1.4627965, - focal_distance: 18.0, - focal_slope: 0.5, - horiz_offset_correction: 0.025999999, - laser_id: 16, - min_intensity: 40.0, - rot_correction: -0.013006177528832626, - vert_correction: -0.02770725895504266, - vert_offset_correction: 0.20458033, - } - - { - dist_correction: 1.4434521, - dist_correction_x: 1.4897508, - dist_correction_y: 1.4791382, - focal_distance: 11.0, - focal_slope: 0.80000001, - horiz_offset_correction: -0.025999999, - laser_id: 17, - min_intensity: 55.0, - rot_correction: 0.04596628970548614, - vert_correction: -0.02014825541794774, - vert_offset_correction: 0.20512417, - } - - { - dist_correction: 1.3243448000000002, - dist_correction_x: 1.3816666000000002, - dist_correction_y: 1.3647719, - focal_distance: 12.0, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 18, - min_intensity: 10.0, - rot_correction: -0.03237303019110658, - vert_correction: -0.08210824496987311, - vert_offset_correction: 0.20065581999999998, - } - - { - dist_correction: 1.4565853999999998, - dist_correction_x: 1.4340645, - dist_correction_y: 1.4550516, - focal_distance: 20.0, - focal_slope: 0.69999999, - horiz_offset_correction: -0.025999999, - laser_id: 19, - min_intensity: 30.0, - rot_correction: 0.02431275238487562, - vert_correction: -0.0727614640280461, - vert_offset_correction: 0.20133196000000003, - } - - { - dist_correction: 1.3313776, - dist_correction_x: 1.3819601000000001, - dist_correction_y: 1.3847791, - focal_distance: 5.0, - focal_slope: 0.89999998, - horiz_offset_correction: 0.025999999, - laser_id: 20, - min_intensity: 45.0, - rot_correction: 0.10029393174916004, - vert_correction: -0.009929893699589059, - vert_offset_correction: 0.20585909000000002, - } - - { - dist_correction: 1.3787535, - dist_correction_x: 1.3978789, - dist_correction_y: 1.4257806, - focal_distance: 10.0, - focal_slope: 1.1, - horiz_offset_correction: -0.025999999, - laser_id: 21, - min_intensity: 35.0, - rot_correction: 0.15867894419560363, - vert_correction: -0.0037977317325845416, - vert_offset_correction: 0.20630005, - } - - { - dist_correction: 1.3419412, - dist_correction_x: 1.4059189, - dist_correction_y: 1.4113303, - focal_distance: 11.0, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 22, - min_intensity: 40.0, - rot_correction: 0.08044522544540282, - vert_correction: -0.06482699129962814, - vert_offset_correction: 0.20190518999999998, - } - - { - dist_correction: 1.4338304, - dist_correction_x: 1.4212059, - dist_correction_y: 1.4900565000000001, - focal_distance: 11.0, - focal_slope: 1.1, - horiz_offset_correction: -0.025999999, - laser_id: 23, - min_intensity: 55.0, - rot_correction: 0.13867708175932542, - vert_correction: -0.05566171063737453, - vert_offset_correction: 0.20256662, - } - - { - dist_correction: 1.4930911, - dist_correction_x: 1.5572188, - dist_correction_y: 1.5182672, - focal_distance: 5.0, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 24, - min_intensity: 25.0, - rot_correction: -0.12505298227706313, - vert_correction: 0.06113007152996804, - vert_offset_correction: 0.21097416, - } - - { - dist_correction: 1.4367653000000002, - dist_correction_x: 1.4614236, - dist_correction_y: 1.4587589, - focal_distance: 3.0, - focal_slope: 1.0, - horiz_offset_correction: -0.025999999, - laser_id: 25, - min_intensity: 50.0, - rot_correction: -0.06814826559971075, - vert_correction: 0.06988221181432357, - vert_offset_correction: 0.21160620000000002, - } - - { - dist_correction: 1.5279892000000002, - dist_correction_x: 1.5686107999999999, - dist_correction_y: 1.55737, - focal_distance: 7.5, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 26, - min_intensity: 30.0, - rot_correction: -0.14531660046790917, - vert_correction: 0.00887576771486082, - vert_offset_correction: 0.20721134, - } - - { - dist_correction: 1.5077638, - dist_correction_x: 1.5453738000000001, - dist_correction_y: 1.5483368, - focal_distance: 4.0, - focal_slope: 0.89999998, - horiz_offset_correction: -0.025999999, - laser_id: 27, - min_intensity: 30.0, - rot_correction: -0.08904354811745085, - vert_correction: 0.017050959569839413, - vert_offset_correction: 0.20779928000000003, - } - - { - dist_correction: 1.3627797000000001, - dist_correction_x: 1.4232985, - dist_correction_y: 1.4116524000000001, - focal_distance: 4.0, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 28, - min_intensity: 50.0, - rot_correction: -0.012061568860271928, - vert_correction: 0.07842048992411524, - vert_offset_correction: 0.21222351, - } - - { - dist_correction: 1.3749608, - dist_correction_x: 1.4045798, - dist_correction_y: 1.4006630999999998, - focal_distance: 4.0, - focal_slope: 1.1, - horiz_offset_correction: -0.025999999, - laser_id: 29, - min_intensity: 40.0, - rot_correction: 0.043892943273872005, - vert_correction: 0.08674443287511571, - vert_offset_correction: 0.21282616000000001, - } - - { - dist_correction: 1.2940709000000001, - dist_correction_x: 1.392794, - dist_correction_y: 1.3225565000000001, - focal_distance: 8.0, - focal_slope: 0.89999998, - horiz_offset_correction: 0.025999999, - laser_id: 30, - min_intensity: 90.0, - rot_correction: -0.033068739374902546, - vert_correction: 0.02522388232225749, - vert_offset_correction: 0.20838722, - } - - { - dist_correction: 1.3907666, - dist_correction_x: 1.4365166, - dist_correction_y: 1.4501437000000001, - focal_distance: 9.0, - focal_slope: 1.0, - horiz_offset_correction: -0.025999999, - laser_id: 31, - min_intensity: 5.0, - rot_correction: 0.025535290948715324, - vert_correction: 0.034414332377654115, - vert_offset_correction: 0.20904865, - } - - { - dist_correction: 1.3461819, - dist_correction_x: 1.3678523000000002, - dist_correction_y: 1.3552880999999999, - focal_distance: 11.0, - focal_slope: 1.5, - horiz_offset_correction: 0.025999999, - laser_id: 32, - min_intensity: 45.0, - rot_correction: -0.13309965698710405, - vert_correction: -0.39666389380060213, - vert_offset_correction: 0.10812234999999999, - } - - { - dist_correction: 1.2487466, - dist_correction_x: 1.2929747, - dist_correction_y: 1.3438803, - focal_distance: 5.0, - focal_slope: 0.89999998, - horiz_offset_correction: -0.025999999, - laser_id: 33, - min_intensity: 35.0, - rot_correction: -0.07241159183553281, - vert_correction: -0.39021216204755743, - vert_offset_correction: 0.10859233, - } - - { - dist_correction: 1.5016498000000003, - dist_correction_x: 1.5384890999999998, - dist_correction_y: 1.5506186, - focal_distance: 4.5, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 34, - min_intensity: 20.0, - rot_correction: 0.08292710807634829, - vert_correction: -0.2010672181773803, - vert_offset_correction: 0.12148494, - } - - { - dist_correction: 1.2495708, - dist_correction_x: 1.2891127, - dist_correction_y: 1.2943669, - focal_distance: 11.0, - focal_slope: 1.3, - horiz_offset_correction: -0.025999999, - laser_id: 35, - min_intensity: 25.0, - rot_correction: 0.14006506182829093, - vert_correction: -0.19103771853737994, - vert_offset_correction: 0.12213274, - } - - { - dist_correction: 1.2674536, - dist_correction_x: 1.3298663, - dist_correction_y: 1.3577469, - focal_distance: 6.0, - focal_slope: 0.89999998, - horiz_offset_correction: 0.025999999, - laser_id: 36, - min_intensity: 35.0, - rot_correction: -0.012004346320883118, - vert_correction: -0.3819670695815035, - vert_offset_correction: 0.10918932, - } - - { - dist_correction: 1.2481956, - dist_correction_x: 1.28627, - dist_correction_y: 1.29235, - focal_distance: 14.5, - focal_slope: 1.5, - horiz_offset_correction: -0.025999999, - laser_id: 37, - min_intensity: 30.0, - rot_correction: 0.04832190474636683, - vert_correction: -0.3718940414299584, - vert_offset_correction: 0.10991334, - } - - { - dist_correction: 1.4516722, - dist_correction_x: 1.4842308, - dist_correction_y: 1.4868384000000001, - focal_distance: 4.0, - focal_slope: 1.1, - horiz_offset_correction: 0.025999999, - laser_id: 38, - min_intensity: 55.0, - rot_correction: -0.03583078923869515, - vert_correction: -0.4336284663746853, - vert_offset_correction: 0.1053787, - } - - { - dist_correction: 1.3843003999999999, - dist_correction_x: 1.4010727, - dist_correction_y: 1.4332015999999999, - focal_distance: 6.5, - focal_slope: 1.3, - horiz_offset_correction: -0.025999999, - laser_id: 39, - min_intensity: 40.0, - rot_correction: 0.026883486237381612, - vert_correction: -0.4261966798867682, - vert_offset_correction: 0.10593759000000001, - } - - { - dist_correction: 1.244738, - dist_correction_x: 1.3131859000000001, - dist_correction_y: 1.354245, - focal_distance: 6.5, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 40, - min_intensity: 25.0, - rot_correction: 0.1076316048478218, - vert_correction: -0.3640637439139335, - vert_offset_correction: 0.11047223, - } - - { - dist_correction: 1.3284543, - dist_correction_x: 1.3295518000000002, - dist_correction_y: 1.3809489, - focal_distance: 12.5, - focal_slope: 1.7, - horiz_offset_correction: -0.025999999, - laser_id: 41, - min_intensity: 50.0, - rot_correction: 0.1679293847080498, - vert_correction: -0.3511493721000192, - vert_offset_correction: 0.11138678, - } - - { - dist_correction: 1.4055542, - dist_correction_x: 1.4426663, - dist_correction_y: 1.430847, - focal_distance: 1.5, - focal_slope: 1.1, - horiz_offset_correction: 0.025999999, - laser_id: 42, - min_intensity: 10.0, - rot_correction: 0.0861156692854385, - vert_correction: -0.4163231777753111, - vert_offset_correction: 0.10667431000000001, - } - - { - dist_correction: 1.3529747, - dist_correction_x: 1.3834917999999998, - dist_correction_y: 1.4029074, - focal_distance: 6.0, - focal_slope: 1.2, - horiz_offset_correction: -0.025999999, - laser_id: 43, - min_intensity: 35.0, - rot_correction: 0.15024020221305323, - vert_correction: -0.4046365927302973, - vert_offset_correction: 0.10753805, - } - - { - dist_correction: 1.2407380000000001, - dist_correction_x: 1.3171521000000002, - dist_correction_y: 1.2836400000000001, - focal_distance: 15.0, - focal_slope: 1.6, - horiz_offset_correction: 0.025999999, - laser_id: 44, - max_intensity: 220.0, - rot_correction: -0.12998527227122358, - vert_correction: -0.28892197890806653, - vert_offset_correction: 0.11568009, - } - - { - dist_correction: 1.5288051999999999, - dist_correction_x: 1.5904514, - dist_correction_y: 1.6288455, - focal_distance: 10.0, - focal_slope: 0.89999998, - horiz_offset_correction: -0.025999999, - laser_id: 45, - rot_correction: -0.07009281894983248, - vert_correction: -0.28120381879648226, - vert_offset_correction: 0.11620087, - } - - { - dist_correction: 1.5423979, - dist_correction_x: 1.5919412, - dist_correction_y: 1.610177, - focal_distance: 13.0, - focal_slope: 1.7, - horiz_offset_correction: 0.025999999, - laser_id: 46, - rot_correction: -0.1533711640661691, - vert_correction: -0.34156398894148127, - vert_offset_correction: 0.11205999, - } - - { - dist_correction: 1.3086252999999999, - dist_correction_x: 1.3737134, - dist_correction_y: 1.4177727, - focal_distance: 5.5, - focal_slope: 0.89999998, - horiz_offset_correction: -0.025999999, - laser_id: 47, - rot_correction: -0.0927890927600715, - vert_correction: -0.3348332488542128, - vert_offset_correction: 0.11252997, - } - - { - dist_correction: 1.3405330000000002, - dist_correction_x: 1.4512436, - dist_correction_y: 1.4471467999999998, - focal_distance: 13.5, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 48, - rot_correction: -0.012004348415278218, - vert_correction: -0.2738300470529015, - vert_offset_correction: 0.11669625, - } - - { - dist_correction: 1.2994545, - dist_correction_x: 1.3971343999999999, - dist_correction_y: 1.36849, - focal_distance: 14.0, - focal_slope: 1.0, - horiz_offset_correction: -0.025999999, - laser_id: 49, - rot_correction: 0.047471358677980184, - vert_correction: -0.2649030020882158, - vert_offset_correction: 0.11729325, - } - - { - dist_correction: 1.4499762999999999, - dist_correction_x: 1.5111346, - dist_correction_y: 1.542395, - focal_distance: 13.0, - focal_slope: 1.2, - horiz_offset_correction: 0.025999999, - laser_id: 50, - rot_correction: -0.03378136079915033, - vert_correction: -0.32678797913421975, - vert_offset_correction: 0.11308886, - } - - { - dist_correction: 1.3516956, - dist_correction_x: 1.4231836999999998, - dist_correction_y: 1.4301146, - focal_distance: 13.0, - focal_slope: 1.1, - horiz_offset_correction: -0.025999999, - laser_id: 51, - rot_correction: 0.025334358173250228, - vert_correction: -0.3179609589889659, - vert_offset_correction: 0.11369856, - } - - { - dist_correction: 1.2374236, - dist_correction_x: 1.308008, - dist_correction_y: 1.3549429000000002, - focal_distance: 5.0, - focal_slope: 0.89999998, - horiz_offset_correction: 0.025999999, - laser_id: 52, - rot_correction: 0.10481975724981128, - vert_correction: -0.25478428121685354, - vert_offset_correction: 0.11796646000000001, - } - - { - dist_correction: 1.3730562000000002, - dist_correction_x: 1.403678, - dist_correction_y: 1.4347415000000001, - focal_distance: 13.0, - focal_slope: 1.4, - horiz_offset_correction: -0.025999999, - laser_id: 53, - rot_correction: 0.16311715242247551, - vert_correction: -0.24576616497179882, - vert_offset_correction: 0.11856346000000001, - } - - { - dist_correction: 1.4445888, - dist_correction_x: 1.5025624, - dist_correction_y: 1.5189749000000001, - focal_distance: 10.0, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 54, - rot_correction: 0.08384971878954978, - vert_correction: -0.31000962288931516, - vert_offset_correction: 0.11424474999999999, - } - - { - dist_correction: 1.3861449, - dist_correction_x: 1.4228815000000001, - dist_correction_y: 1.4620186000000002, - focal_distance: 15.0, - focal_slope: 1.6, - horiz_offset_correction: -0.025999999, - laser_id: 55, - rot_correction: 0.14374613400834294, - vert_correction: -0.2986601307360315, - vert_offset_correction: 0.11501958000000001, - } - - { - dist_correction: 1.4635341, - dist_correction_x: 1.5232283000000002, - dist_correction_y: 1.5063837000000002, - focal_distance: 13.0, - focal_slope: 1.3, - horiz_offset_correction: 0.025999999, - laser_id: 56, - rot_correction: -0.12741928396895377, - vert_correction: -0.18393445537456576, - vert_offset_correction: 0.12259002000000001, - } - - { - dist_correction: 1.3715826000000002, - dist_correction_x: 1.4186972, - dist_correction_y: 1.4328435, - focal_distance: 3.5, - focal_slope: 1.0, - horiz_offset_correction: -0.025999999, - laser_id: 57, - rot_correction: -0.06853129555735342, - vert_correction: -0.1744342722087932, - vert_offset_correction: 0.12319972, - } - - { - dist_correction: 1.4644831999999999, - dist_correction_x: 1.5095984000000002, - dist_correction_y: 1.5149265, - focal_distance: 11.0, - focal_slope: 1.4, - horiz_offset_correction: 0.025999999, - laser_id: 58, - rot_correction: -0.1497316045423758, - vert_correction: -0.23400086557751998, - vert_offset_correction: 0.11933828, - } - - { - dist_correction: 1.3074548, - dist_correction_x: 1.3521214000000001, - dist_correction_y: 1.3830151000000002, - focal_distance: 5.0, - focal_slope: 0.89999998, - horiz_offset_correction: -0.025999999, - laser_id: 59, - rot_correction: -0.08973572126418226, - vert_correction: -0.22644383426247983, - vert_offset_correction: 0.11983367, - } - - { - dist_correction: 1.4378391, - dist_correction_x: 1.4799687000000001, - dist_correction_y: 1.4889211, - focal_distance: 5.0, - focal_slope: 1.1, - horiz_offset_correction: 0.025999999, - laser_id: 60, - rot_correction: -0.011751946229237658, - vert_correction: -0.1678843098347878, - vert_offset_correction: 0.12361888, - } - - { - dist_correction: 1.3585466, - dist_correction_x: 1.4135242, - dist_correction_y: 1.4170488, - focal_distance: 2.0, - focal_slope: 0.89999998, - horiz_offset_correction: -0.025999999, - laser_id: 61, - rot_correction: 0.04489272721060892, - vert_correction: -0.158331168143522, - vert_offset_correction: 0.12422858, - } - - { - dist_correction: 1.4478067, - dist_correction_x: 1.5442529, - dist_correction_y: 1.5512207, - focal_distance: 10.0, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 62, - rot_correction: -0.0340408982402389, - vert_correction: -0.21671681763514258, - vert_offset_correction: 0.12046877, - } - - { - dist_correction: 1.4329738, - dist_correction_x: 1.4817114, - dist_correction_y: 1.4954124, - focal_distance: 9.0, - focal_slope: 0.80000001, - horiz_offset_correction: -0.025999999, - laser_id: 63, - rot_correction: 0.024857907722065305, - vert_correction: -0.2106649408137298, - vert_offset_correction: 0.12086253, + - { + dist_correction: 1.5195264000000002, + dist_correction_x: 1.5500304, + dist_correction_y: 1.5231381, + focal_distance: 12.0, + focal_slope: 1.4, + horiz_offset_correction: 0.025999999, + laser_id: 0, + max_intensity: 235.0, + min_intensity: 30.0, + rot_correction: -0.1248942899601548, + vert_correction: -0.15304134919741974, + vert_offset_correction: 0.19548199, + } + - { + dist_correction: 1.5145139, + dist_correction_x: 1.5256960000000002, + dist_correction_y: 1.5491043, + focal_distance: 5.0, + focal_slope: 1.0, + horiz_offset_correction: -0.025999999, + laser_id: 1, + min_intensity: 40.0, + rot_correction: -0.06924466398252106, + vert_correction: -0.1458455539136526, + vert_offset_correction: 0.19601112, + } + - { + dist_correction: 1.4963768, + dist_correction_x: 1.5571011, + dist_correction_y: 1.5456782999999998, + focal_distance: 5.0, + focal_slope: 0.89999998, + horiz_offset_correction: 0.025999999, + laser_id: 2, + min_intensity: 60.0, + rot_correction: 0.0824016906676694, + vert_correction: 0.04339494149708345, + vert_offset_correction: 0.20969539999999998, + } + - { + dist_correction: 1.3771207, + dist_correction_x: 1.4103835, + dist_correction_y: 1.4343457000000002, + focal_distance: 9.5, + focal_slope: 1.1, + horiz_offset_correction: -0.025999999, + laser_id: 3, + min_intensity: 20.0, + rot_correction: 0.137808349360133, + vert_correction: 0.05196082373432465, + vert_offset_correction: 0.21031273, + } + - { + dist_correction: 1.2947885, + dist_correction_x: 1.3720455999999999, + dist_correction_y: 1.4025244000000001, + focal_distance: 10.0, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 4, + min_intensity: 50.0, + rot_correction: -0.011199603626188263, + vert_correction: -0.1358262679404349, + vert_offset_correction: 0.19674603999999998, + } + - { + dist_correction: 1.4395787000000002, + dist_correction_x: 1.4801956, + dist_correction_y: 1.5074649, + focal_distance: 7.0, + focal_slope: 1.0, + horiz_offset_correction: -0.025999999, + laser_id: 5, + min_intensity: 30.0, + rot_correction: 0.045795414990398894, + vert_correction: -0.12678532627942263, + vert_offset_correction: 0.19740747, + } + - { + dist_correction: 1.3618773, + dist_correction_x: 1.4004077000000001, + dist_correction_y: 1.3900876, + focal_distance: 5.0, + focal_slope: 1.1, + horiz_offset_correction: 0.025999999, + laser_id: 6, + min_intensity: 65.0, + rot_correction: -0.031646046452444135, + vert_correction: -0.1895564226946273, + vert_offset_correction: 0.19277746, + } + - { + dist_correction: 1.5325716, + dist_correction_x: 1.5337143, + dist_correction_y: 1.5452948000000002, + focal_distance: 6.0, + focal_slope: 1.0, + horiz_offset_correction: -0.025999999, + laser_id: 7, + min_intensity: 10.0, + rot_correction: 0.023408326257150665, + vert_correction: -0.18086723120040343, + vert_offset_correction: 0.19342419, + } + - { + dist_correction: 1.3743323, + dist_correction_x: 1.4474606, + dist_correction_y: 1.4533472, + focal_distance: 6.0, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 8, + min_intensity: 30.0, + rot_correction: 0.10065885915180103, + vert_correction: -0.11973897655818674, + vert_offset_correction: 0.19792192, + } + - { + dist_correction: 1.4969112000000002, + dist_correction_x: 1.4754176, + dist_correction_y: 1.4888799000000001, + focal_distance: 5.0, + focal_slope: 1.0, + horiz_offset_correction: -0.025999999, + laser_id: 9, + min_intensity: 10.0, + rot_correction: 0.15996423607269877, + vert_correction: -0.10924820210484752, + vert_offset_correction: 0.19868624000000001, + } + - { + dist_correction: 1.434263, + dist_correction_x: 1.4957901000000002, + dist_correction_y: 1.5191892999999999, + focal_distance: 5.0, + focal_slope: 0.89999998, + horiz_offset_correction: 0.025999999, + laser_id: 10, + min_intensity: 35.0, + rot_correction: 0.08313316164783874, + vert_correction: -0.17135657256846043, + vert_offset_correction: 0.19412970999999998, + } + - { + dist_correction: 1.5500841, + dist_correction_x: 1.542697, + dist_correction_y: 1.5716737, + focal_distance: 8.0, + focal_slope: 1.2, + horiz_offset_correction: -0.025999999, + laser_id: 11, + min_intensity: 30.0, + rot_correction: 0.13862572370075624, + vert_correction: -0.1630088948849621, + vert_offset_correction: 0.19474705, + } + - { + dist_correction: 1.3725992, + dist_correction_x: 1.4353555, + dist_correction_y: 1.3932405, + focal_distance: 11.0, + focal_slope: 1.1, + horiz_offset_correction: 0.025999999, + laser_id: 12, + min_intensity: 50.0, + rot_correction: -0.1255805045528199, + vert_correction: -0.04526314018308893, + vert_offset_correction: 0.20331627000000002, + } + - { + dist_correction: 1.3111591000000002, + dist_correction_x: 1.3470857, + dist_correction_y: 1.3652054000000002, + focal_distance: 6.0, + focal_slope: 0.69999999, + horiz_offset_correction: -0.025999999, + laser_id: 13, + min_intensity: 30.0, + rot_correction: -0.06716508498014805, + vert_correction: -0.03587557613263058, + vert_offset_correction: 0.20399239000000002, + } + - { + dist_correction: 1.5360803, + dist_correction_x: 1.5528035, + dist_correction_y: 1.5666801000000001, + focal_distance: 8.0, + focal_slope: 1.1, + horiz_offset_correction: 0.025999999, + laser_id: 14, + min_intensity: 20.0, + rot_correction: -0.14578889529014513, + vert_correction: -0.09893566067472198, + vert_offset_correction: 0.19943586, + } + - { + dist_correction: 1.4755448999999998, + dist_correction_x: 1.5032428, + dist_correction_y: 1.5326752, + focal_distance: 5.0, + focal_slope: 0.89999998, + horiz_offset_correction: -0.025999999, + laser_id: 15, + min_intensity: 30.0, + rot_correction: -0.08892898296002069, + vert_correction: -0.09062974348423608, + vert_offset_correction: 0.20003851, + } + - { + dist_correction: 1.4410587000000001, + dist_correction_x: 1.4870845, + dist_correction_y: 1.4627965, + focal_distance: 18.0, + focal_slope: 0.5, + horiz_offset_correction: 0.025999999, + laser_id: 16, + min_intensity: 40.0, + rot_correction: -0.013006177528832626, + vert_correction: -0.02770725895504266, + vert_offset_correction: 0.20458033, + } + - { + dist_correction: 1.4434521, + dist_correction_x: 1.4897508, + dist_correction_y: 1.4791382, + focal_distance: 11.0, + focal_slope: 0.80000001, + horiz_offset_correction: -0.025999999, + laser_id: 17, + min_intensity: 55.0, + rot_correction: 0.04596628970548614, + vert_correction: -0.02014825541794774, + vert_offset_correction: 0.20512417, + } + - { + dist_correction: 1.3243448000000002, + dist_correction_x: 1.3816666000000002, + dist_correction_y: 1.3647719, + focal_distance: 12.0, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 18, + min_intensity: 10.0, + rot_correction: -0.03237303019110658, + vert_correction: -0.08210824496987311, + vert_offset_correction: 0.20065581999999998, + } + - { + dist_correction: 1.4565853999999998, + dist_correction_x: 1.4340645, + dist_correction_y: 1.4550516, + focal_distance: 20.0, + focal_slope: 0.69999999, + horiz_offset_correction: -0.025999999, + laser_id: 19, + min_intensity: 30.0, + rot_correction: 0.02431275238487562, + vert_correction: -0.0727614640280461, + vert_offset_correction: 0.20133196000000003, + } + - { + dist_correction: 1.3313776, + dist_correction_x: 1.3819601000000001, + dist_correction_y: 1.3847791, + focal_distance: 5.0, + focal_slope: 0.89999998, + horiz_offset_correction: 0.025999999, + laser_id: 20, + min_intensity: 45.0, + rot_correction: 0.10029393174916004, + vert_correction: -0.009929893699589059, + vert_offset_correction: 0.20585909000000002, + } + - { + dist_correction: 1.3787535, + dist_correction_x: 1.3978789, + dist_correction_y: 1.4257806, + focal_distance: 10.0, + focal_slope: 1.1, + horiz_offset_correction: -0.025999999, + laser_id: 21, + min_intensity: 35.0, + rot_correction: 0.15867894419560363, + vert_correction: -0.0037977317325845416, + vert_offset_correction: 0.20630005, + } + - { + dist_correction: 1.3419412, + dist_correction_x: 1.4059189, + dist_correction_y: 1.4113303, + focal_distance: 11.0, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 22, + min_intensity: 40.0, + rot_correction: 0.08044522544540282, + vert_correction: -0.06482699129962814, + vert_offset_correction: 0.20190518999999998, + } + - { + dist_correction: 1.4338304, + dist_correction_x: 1.4212059, + dist_correction_y: 1.4900565000000001, + focal_distance: 11.0, + focal_slope: 1.1, + horiz_offset_correction: -0.025999999, + laser_id: 23, + min_intensity: 55.0, + rot_correction: 0.13867708175932542, + vert_correction: -0.05566171063737453, + vert_offset_correction: 0.20256662, + } + - { + dist_correction: 1.4930911, + dist_correction_x: 1.5572188, + dist_correction_y: 1.5182672, + focal_distance: 5.0, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 24, + min_intensity: 25.0, + rot_correction: -0.12505298227706313, + vert_correction: 0.06113007152996804, + vert_offset_correction: 0.21097416, + } + - { + dist_correction: 1.4367653000000002, + dist_correction_x: 1.4614236, + dist_correction_y: 1.4587589, + focal_distance: 3.0, + focal_slope: 1.0, + horiz_offset_correction: -0.025999999, + laser_id: 25, + min_intensity: 50.0, + rot_correction: -0.06814826559971075, + vert_correction: 0.06988221181432357, + vert_offset_correction: 0.21160620000000002, + } + - { + dist_correction: 1.5279892000000002, + dist_correction_x: 1.5686107999999999, + dist_correction_y: 1.55737, + focal_distance: 7.5, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 26, + min_intensity: 30.0, + rot_correction: -0.14531660046790917, + vert_correction: 0.00887576771486082, + vert_offset_correction: 0.20721134, + } + - { + dist_correction: 1.5077638, + dist_correction_x: 1.5453738000000001, + dist_correction_y: 1.5483368, + focal_distance: 4.0, + focal_slope: 0.89999998, + horiz_offset_correction: -0.025999999, + laser_id: 27, + min_intensity: 30.0, + rot_correction: -0.08904354811745085, + vert_correction: 0.017050959569839413, + vert_offset_correction: 0.20779928000000003, + } + - { + dist_correction: 1.3627797000000001, + dist_correction_x: 1.4232985, + dist_correction_y: 1.4116524000000001, + focal_distance: 4.0, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 28, + min_intensity: 50.0, + rot_correction: -0.012061568860271928, + vert_correction: 0.07842048992411524, + vert_offset_correction: 0.21222351, + } + - { + dist_correction: 1.3749608, + dist_correction_x: 1.4045798, + dist_correction_y: 1.4006630999999998, + focal_distance: 4.0, + focal_slope: 1.1, + horiz_offset_correction: -0.025999999, + laser_id: 29, + min_intensity: 40.0, + rot_correction: 0.043892943273872005, + vert_correction: 0.08674443287511571, + vert_offset_correction: 0.21282616000000001, + } + - { + dist_correction: 1.2940709000000001, + dist_correction_x: 1.392794, + dist_correction_y: 1.3225565000000001, + focal_distance: 8.0, + focal_slope: 0.89999998, + horiz_offset_correction: 0.025999999, + laser_id: 30, + min_intensity: 90.0, + rot_correction: -0.033068739374902546, + vert_correction: 0.02522388232225749, + vert_offset_correction: 0.20838722, + } + - { + dist_correction: 1.3907666, + dist_correction_x: 1.4365166, + dist_correction_y: 1.4501437000000001, + focal_distance: 9.0, + focal_slope: 1.0, + horiz_offset_correction: -0.025999999, + laser_id: 31, + min_intensity: 5.0, + rot_correction: 0.025535290948715324, + vert_correction: 0.034414332377654115, + vert_offset_correction: 0.20904865, + } + - { + dist_correction: 1.3461819, + dist_correction_x: 1.3678523000000002, + dist_correction_y: 1.3552880999999999, + focal_distance: 11.0, + focal_slope: 1.5, + horiz_offset_correction: 0.025999999, + laser_id: 32, + min_intensity: 45.0, + rot_correction: -0.13309965698710405, + vert_correction: -0.39666389380060213, + vert_offset_correction: 0.10812234999999999, + } + - { + dist_correction: 1.2487466, + dist_correction_x: 1.2929747, + dist_correction_y: 1.3438803, + focal_distance: 5.0, + focal_slope: 0.89999998, + horiz_offset_correction: -0.025999999, + laser_id: 33, + min_intensity: 35.0, + rot_correction: -0.07241159183553281, + vert_correction: -0.39021216204755743, + vert_offset_correction: 0.10859233, + } + - { + dist_correction: 1.5016498000000003, + dist_correction_x: 1.5384890999999998, + dist_correction_y: 1.5506186, + focal_distance: 4.5, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 34, + min_intensity: 20.0, + rot_correction: 0.08292710807634829, + vert_correction: -0.2010672181773803, + vert_offset_correction: 0.12148494, + } + - { + dist_correction: 1.2495708, + dist_correction_x: 1.2891127, + dist_correction_y: 1.2943669, + focal_distance: 11.0, + focal_slope: 1.3, + horiz_offset_correction: -0.025999999, + laser_id: 35, + min_intensity: 25.0, + rot_correction: 0.14006506182829093, + vert_correction: -0.19103771853737994, + vert_offset_correction: 0.12213274, + } + - { + dist_correction: 1.2674536, + dist_correction_x: 1.3298663, + dist_correction_y: 1.3577469, + focal_distance: 6.0, + focal_slope: 0.89999998, + horiz_offset_correction: 0.025999999, + laser_id: 36, + min_intensity: 35.0, + rot_correction: -0.012004346320883118, + vert_correction: -0.3819670695815035, + vert_offset_correction: 0.10918932, + } + - { + dist_correction: 1.2481956, + dist_correction_x: 1.28627, + dist_correction_y: 1.29235, + focal_distance: 14.5, + focal_slope: 1.5, + horiz_offset_correction: -0.025999999, + laser_id: 37, + min_intensity: 30.0, + rot_correction: 0.04832190474636683, + vert_correction: -0.3718940414299584, + vert_offset_correction: 0.10991334, + } + - { + dist_correction: 1.4516722, + dist_correction_x: 1.4842308, + dist_correction_y: 1.4868384000000001, + focal_distance: 4.0, + focal_slope: 1.1, + horiz_offset_correction: 0.025999999, + laser_id: 38, + min_intensity: 55.0, + rot_correction: -0.03583078923869515, + vert_correction: -0.4336284663746853, + vert_offset_correction: 0.1053787, + } + - { + dist_correction: 1.3843003999999999, + dist_correction_x: 1.4010727, + dist_correction_y: 1.4332015999999999, + focal_distance: 6.5, + focal_slope: 1.3, + horiz_offset_correction: -0.025999999, + laser_id: 39, + min_intensity: 40.0, + rot_correction: 0.026883486237381612, + vert_correction: -0.4261966798867682, + vert_offset_correction: 0.10593759000000001, + } + - { + dist_correction: 1.244738, + dist_correction_x: 1.3131859000000001, + dist_correction_y: 1.354245, + focal_distance: 6.5, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 40, + min_intensity: 25.0, + rot_correction: 0.1076316048478218, + vert_correction: -0.3640637439139335, + vert_offset_correction: 0.11047223, + } + - { + dist_correction: 1.3284543, + dist_correction_x: 1.3295518000000002, + dist_correction_y: 1.3809489, + focal_distance: 12.5, + focal_slope: 1.7, + horiz_offset_correction: -0.025999999, + laser_id: 41, + min_intensity: 50.0, + rot_correction: 0.1679293847080498, + vert_correction: -0.3511493721000192, + vert_offset_correction: 0.11138678, + } + - { + dist_correction: 1.4055542, + dist_correction_x: 1.4426663, + dist_correction_y: 1.430847, + focal_distance: 1.5, + focal_slope: 1.1, + horiz_offset_correction: 0.025999999, + laser_id: 42, + min_intensity: 10.0, + rot_correction: 0.0861156692854385, + vert_correction: -0.4163231777753111, + vert_offset_correction: 0.10667431000000001, + } + - { + dist_correction: 1.3529747, + dist_correction_x: 1.3834917999999998, + dist_correction_y: 1.4029074, + focal_distance: 6.0, + focal_slope: 1.2, + horiz_offset_correction: -0.025999999, + laser_id: 43, + min_intensity: 35.0, + rot_correction: 0.15024020221305323, + vert_correction: -0.4046365927302973, + vert_offset_correction: 0.10753805, + } + - { + dist_correction: 1.2407380000000001, + dist_correction_x: 1.3171521000000002, + dist_correction_y: 1.2836400000000001, + focal_distance: 15.0, + focal_slope: 1.6, + horiz_offset_correction: 0.025999999, + laser_id: 44, + max_intensity: 220.0, + rot_correction: -0.12998527227122358, + vert_correction: -0.28892197890806653, + vert_offset_correction: 0.11568009, + } + - { + dist_correction: 1.5288051999999999, + dist_correction_x: 1.5904514, + dist_correction_y: 1.6288455, + focal_distance: 10.0, + focal_slope: 0.89999998, + horiz_offset_correction: -0.025999999, + laser_id: 45, + rot_correction: -0.07009281894983248, + vert_correction: -0.28120381879648226, + vert_offset_correction: 0.11620087, + } + - { + dist_correction: 1.5423979, + dist_correction_x: 1.5919412, + dist_correction_y: 1.610177, + focal_distance: 13.0, + focal_slope: 1.7, + horiz_offset_correction: 0.025999999, + laser_id: 46, + rot_correction: -0.1533711640661691, + vert_correction: -0.34156398894148127, + vert_offset_correction: 0.11205999, + } + - { + dist_correction: 1.3086252999999999, + dist_correction_x: 1.3737134, + dist_correction_y: 1.4177727, + focal_distance: 5.5, + focal_slope: 0.89999998, + horiz_offset_correction: -0.025999999, + laser_id: 47, + rot_correction: -0.0927890927600715, + vert_correction: -0.3348332488542128, + vert_offset_correction: 0.11252997, + } + - { + dist_correction: 1.3405330000000002, + dist_correction_x: 1.4512436, + dist_correction_y: 1.4471467999999998, + focal_distance: 13.5, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 48, + rot_correction: -0.012004348415278218, + vert_correction: -0.2738300470529015, + vert_offset_correction: 0.11669625, + } + - { + dist_correction: 1.2994545, + dist_correction_x: 1.3971343999999999, + dist_correction_y: 1.36849, + focal_distance: 14.0, + focal_slope: 1.0, + horiz_offset_correction: -0.025999999, + laser_id: 49, + rot_correction: 0.047471358677980184, + vert_correction: -0.2649030020882158, + vert_offset_correction: 0.11729325, + } + - { + dist_correction: 1.4499762999999999, + dist_correction_x: 1.5111346, + dist_correction_y: 1.542395, + focal_distance: 13.0, + focal_slope: 1.2, + horiz_offset_correction: 0.025999999, + laser_id: 50, + rot_correction: -0.03378136079915033, + vert_correction: -0.32678797913421975, + vert_offset_correction: 0.11308886, + } + - { + dist_correction: 1.3516956, + dist_correction_x: 1.4231836999999998, + dist_correction_y: 1.4301146, + focal_distance: 13.0, + focal_slope: 1.1, + horiz_offset_correction: -0.025999999, + laser_id: 51, + rot_correction: 0.025334358173250228, + vert_correction: -0.3179609589889659, + vert_offset_correction: 0.11369856, + } + - { + dist_correction: 1.2374236, + dist_correction_x: 1.308008, + dist_correction_y: 1.3549429000000002, + focal_distance: 5.0, + focal_slope: 0.89999998, + horiz_offset_correction: 0.025999999, + laser_id: 52, + rot_correction: 0.10481975724981128, + vert_correction: -0.25478428121685354, + vert_offset_correction: 0.11796646000000001, + } + - { + dist_correction: 1.3730562000000002, + dist_correction_x: 1.403678, + dist_correction_y: 1.4347415000000001, + focal_distance: 13.0, + focal_slope: 1.4, + horiz_offset_correction: -0.025999999, + laser_id: 53, + rot_correction: 0.16311715242247551, + vert_correction: -0.24576616497179882, + vert_offset_correction: 0.11856346000000001, + } + - { + dist_correction: 1.4445888, + dist_correction_x: 1.5025624, + dist_correction_y: 1.5189749000000001, + focal_distance: 10.0, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 54, + rot_correction: 0.08384971878954978, + vert_correction: -0.31000962288931516, + vert_offset_correction: 0.11424474999999999, + } + - { + dist_correction: 1.3861449, + dist_correction_x: 1.4228815000000001, + dist_correction_y: 1.4620186000000002, + focal_distance: 15.0, + focal_slope: 1.6, + horiz_offset_correction: -0.025999999, + laser_id: 55, + rot_correction: 0.14374613400834294, + vert_correction: -0.2986601307360315, + vert_offset_correction: 0.11501958000000001, + } + - { + dist_correction: 1.4635341, + dist_correction_x: 1.5232283000000002, + dist_correction_y: 1.5063837000000002, + focal_distance: 13.0, + focal_slope: 1.3, + horiz_offset_correction: 0.025999999, + laser_id: 56, + rot_correction: -0.12741928396895377, + vert_correction: -0.18393445537456576, + vert_offset_correction: 0.12259002000000001, + } + - { + dist_correction: 1.3715826000000002, + dist_correction_x: 1.4186972, + dist_correction_y: 1.4328435, + focal_distance: 3.5, + focal_slope: 1.0, + horiz_offset_correction: -0.025999999, + laser_id: 57, + rot_correction: -0.06853129555735342, + vert_correction: -0.1744342722087932, + vert_offset_correction: 0.12319972, + } + - { + dist_correction: 1.4644831999999999, + dist_correction_x: 1.5095984000000002, + dist_correction_y: 1.5149265, + focal_distance: 11.0, + focal_slope: 1.4, + horiz_offset_correction: 0.025999999, + laser_id: 58, + rot_correction: -0.1497316045423758, + vert_correction: -0.23400086557751998, + vert_offset_correction: 0.11933828, + } + - { + dist_correction: 1.3074548, + dist_correction_x: 1.3521214000000001, + dist_correction_y: 1.3830151000000002, + focal_distance: 5.0, + focal_slope: 0.89999998, + horiz_offset_correction: -0.025999999, + laser_id: 59, + rot_correction: -0.08973572126418226, + vert_correction: -0.22644383426247983, + vert_offset_correction: 0.11983367, + } + - { + dist_correction: 1.4378391, + dist_correction_x: 1.4799687000000001, + dist_correction_y: 1.4889211, + focal_distance: 5.0, + focal_slope: 1.1, + horiz_offset_correction: 0.025999999, + laser_id: 60, + rot_correction: -0.011751946229237658, + vert_correction: -0.1678843098347878, + vert_offset_correction: 0.12361888, + } + - { + dist_correction: 1.3585466, + dist_correction_x: 1.4135242, + dist_correction_y: 1.4170488, + focal_distance: 2.0, + focal_slope: 0.89999998, + horiz_offset_correction: -0.025999999, + laser_id: 61, + rot_correction: 0.04489272721060892, + vert_correction: -0.158331168143522, + vert_offset_correction: 0.12422858, + } + - { + dist_correction: 1.4478067, + dist_correction_x: 1.5442529, + dist_correction_y: 1.5512207, + focal_distance: 10.0, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 62, + rot_correction: -0.0340408982402389, + vert_correction: -0.21671681763514258, + vert_offset_correction: 0.12046877, + } + - { + dist_correction: 1.4329738, + dist_correction_x: 1.4817114, + dist_correction_y: 1.4954124, + focal_distance: 9.0, + focal_slope: 0.80000001, + horiz_offset_correction: -0.025999999, + laser_id: 63, + rot_correction: 0.024857907722065305, + vert_correction: -0.2106649408137298, + vert_offset_correction: 0.12086253, } num_lasers: 64 distance_resolution: 0.002 diff --git a/nebula_decoders/calibration/velodyne/HDL64e_s3.yaml b/nebula_decoders/calibration/velodyne/HDL64e_s3.yaml index a398407e9..df8f54e11 100755 --- a/nebula_decoders/calibration/velodyne/HDL64e_s3.yaml +++ b/nebula_decoders/calibration/velodyne/HDL64e_s3.yaml @@ -1,788 +1,788 @@ lasers: - - { - dist_correction: 1.4139490000000001, - dist_correction_x: 1.4198446999999998, - dist_correction_y: 1.4058145, - focal_distance: 10.5, - focal_slope: 1.85, - horiz_offset_correction: 0.025999999, - laser_id: 0, - min_intensity: 5, - rot_correction: -0.07648247457737148, - vert_correction: -0.1261818455292898, - vert_offset_correction: 0.21569468, - } - - { - dist_correction: 1.5094685, - dist_correction_x: 1.5403023, - dist_correction_y: 1.5012577999999999, - focal_distance: 15.0, - focal_slope: 1.2, - horiz_offset_correction: -0.025999999, - laser_id: 1, - min_intensity: 10, - rot_correction: -0.03932070772308096, - vert_correction: -0.11953745909742221, - vert_offset_correction: 0.21520962000000002, - } - - { - dist_correction: 1.4579400999999999, - dist_correction_x: 1.4846666, - dist_correction_y: 1.4228239, - focal_distance: 24.0, - focal_slope: 1.05, - horiz_offset_correction: 0.025999999, - laser_id: 2, - min_intensity: 30, - rot_correction: 0.06005350008746038, - vert_correction: 0.00356118725906175, - vert_offset_correction: 0.20631704, - } - - { - dist_correction: 1.5046124, - dist_correction_x: 1.5335535999999999, - dist_correction_y: 1.5401996, - focal_distance: 14.5, - focal_slope: 1.7, - horiz_offset_correction: -0.025999999, - laser_id: 3, - min_intensity: 10, - rot_correction: 0.09919490315516696, - vert_correction: 0.010306535403103584, - vert_offset_correction: 0.20583200000000001, - } - - { - dist_correction: 1.4370993, - dist_correction_x: 1.4714845, - dist_correction_y: 1.4524646, - focal_distance: 15.0, - focal_slope: 0.40000001, - horiz_offset_correction: 0.025999999, - laser_id: 4, - min_intensity: 30, - rot_correction: -0.00095355016485159, - vert_correction: -0.1144967440141401, - vert_offset_correction: 0.21484217000000003, - } - - { - dist_correction: 1.3536626999999999, - dist_correction_x: 1.3909094, - dist_correction_y: 1.3700326999999999, - focal_distance: 17.0, - focal_slope: 1.45, - horiz_offset_correction: -0.025999999, - laser_id: 5, - min_intensity: 30, - rot_correction: 0.03824387099052699, - vert_correction: -0.10803612329921503, - vert_offset_correction: 0.21437181, - } - - { - dist_correction: 1.4117873, - dist_correction_x: 1.4350795, - dist_correction_y: 1.3829624999999999, - focal_distance: 10.5, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 6, - min_intensity: 10, - rot_correction: -0.015297255529962315, - vert_correction: -0.14944538101702426, - vert_offset_correction: 0.21739968999999998, - } - - { - dist_correction: 1.4269646, - dist_correction_x: 1.451161, - dist_correction_y: 1.4328421, - focal_distance: 14.0, - focal_slope: 1.3, - horiz_offset_correction: -0.025999999, - laser_id: 7, - min_intensity: 10, - rot_correction: 0.024255809272700053, - vert_correction: -0.14344353231329066, - vert_offset_correction: 0.21695873, - } - - { - dist_correction: 1.3808284000000002, - dist_correction_x: 1.421472, - dist_correction_y: 1.3838283000000002, - focal_distance: 15.0, - focal_slope: 0.94999999, - horiz_offset_correction: 0.025999999, - laser_id: 8, - min_intensity: 30, - rot_correction: 0.07354442571180776, - vert_correction: -0.10278016723125998, - vert_offset_correction: 0.21398966000000003, - } - - { - dist_correction: 1.3644336000000001, - dist_correction_x: 1.3931616, - dist_correction_y: 1.3852122, - focal_distance: 12.5, - focal_slope: 1.95, - horiz_offset_correction: -0.025999999, - laser_id: 9, - min_intensity: 10, - rot_correction: 0.11317857886058363, - vert_correction: -0.09569590023202451, - vert_offset_correction: 0.21347521, - } - - { - dist_correction: 1.4102663000000002, - dist_correction_x: 1.4163402, - dist_correction_y: 1.3592377, - focal_distance: 12.5, - focal_slope: 1.95, - horiz_offset_correction: 0.025999999, - laser_id: 10, - min_intensity: 10, - rot_correction: 0.05954228616823424, - vert_correction: -0.1386345201601863, - vert_offset_correction: 0.21660597, - } - - { - dist_correction: 1.5019737000000002, - dist_correction_x: 1.532475, - dist_correction_y: 1.5237663000000001, - focal_distance: 13.0, - focal_slope: 1.9, - horiz_offset_correction: -0.025999999, - laser_id: 11, - min_intensity: 10, - rot_correction: 0.09906092120980835, - vert_correction: -0.13181073657049397, - vert_offset_correction: 0.21610622, - } - - { - dist_correction: 1.4399905000000002, - dist_correction_x: 1.4887962, - dist_correction_y: 1.4455237, - focal_distance: 14.5, - focal_slope: 1.7, - horiz_offset_correction: 0.025999999, - laser_id: 12, - max_intensity: 235, - rot_correction: -0.07574798243226695, - vert_correction: -0.054438932963427306, - vert_offset_correction: 0.21049141, - } - - { - dist_correction: 1.5207593, - dist_correction_x: 1.5401453, - dist_correction_y: 1.5190082, - focal_distance: 20.5, - focal_slope: 1.25, - horiz_offset_correction: -0.025999999, - laser_id: 13, - max_intensity: 240, - rot_correction: -0.037047761947550245, - vert_correction: -0.048730533448148504, - vert_offset_correction: 0.21007986, - } - - { - dist_correction: 1.4746239, - dist_correction_x: 1.5300989999999999, - dist_correction_y: 1.4663734, - focal_distance: 12.5, - focal_slope: 1.9, - horiz_offset_correction: 0.025999999, - laser_id: 14, - max_intensity: 245, - rot_correction: -0.0895955468906376, - vert_correction: -0.08961595328025192, - vert_offset_correction: 0.21303425, - } - - { - dist_correction: 1.4697629, - dist_correction_x: 1.5051735, - dist_correction_y: 1.4558601, - focal_distance: 19.5, - focal_slope: 0.94999999, - horiz_offset_correction: -0.025999999, - laser_id: 15, - max_intensity: 245, - rot_correction: -0.05072966149865084, - vert_correction: -0.0839353306800186, - vert_offset_correction: 0.21262267999999998, - } - - { - dist_correction: 1.4143376, - dist_correction_x: 1.4432597000000003, - dist_correction_y: 1.4038483, - focal_distance: 24.0, - focal_slope: 0.69999999, - horiz_offset_correction: 0.025999999, - laser_id: 16, - max_intensity: 245, - rot_correction: -0.001864320564407547, - vert_correction: -0.043427018903405855, - vert_offset_correction: 0.20969770000000001, - } - - { - dist_correction: 1.4815961, - dist_correction_x: 1.5173615, - dist_correction_y: 1.4871606, - focal_distance: 14.5, - focal_slope: 1.65, - horiz_offset_correction: -0.025999999, - laser_id: 17, - rot_correction: 0.03747852840556422, - vert_correction: -0.03648801216715539, - vert_offset_correction: 0.20919794, - } - - { - dist_correction: 1.420331, - dist_correction_x: 1.4755219, - dist_correction_y: 1.3919118000000001, - focal_distance: 17.5, - focal_slope: 1.4, - horiz_offset_correction: 0.025999999, - laser_id: 18, - rot_correction: -0.015194972429011364, - vert_correction: -0.07906187922560139, - vert_offset_correction: 0.21226994000000002, - } - - { - dist_correction: 1.4900717, - dist_correction_x: 1.5143349000000002, - dist_correction_y: 1.4829907, - focal_distance: 24.0, - focal_slope: 1.25, - horiz_offset_correction: -0.025999999, - laser_id: 19, - rot_correction: 0.02391977928648433, - vert_correction: -0.07255814015150577, - vert_offset_correction: 0.21179958, - } - - { - dist_correction: 1.4823865, - dist_correction_x: 1.5357741999999999, - dist_correction_y: 1.4956403, - focal_distance: 24.0, - focal_slope: 1.1, - horiz_offset_correction: 0.025999999, - laser_id: 20, - rot_correction: 0.07264374331532833, - vert_correction: -0.031587736747464255, - vert_offset_correction: 0.20884519999999998, - } - - { - dist_correction: 1.5553201, - dist_correction_x: 1.5698593, - dist_correction_y: 1.5624425, - focal_distance: 15.5, - focal_slope: 1.55, - horiz_offset_correction: -0.025999999, - laser_id: 21, - rot_correction: 0.11143265968730214, - vert_correction: -0.025460288914769372, - vert_offset_correction: 0.20840424, - } - - { - dist_correction: 1.4437845999999999, - dist_correction_x: 1.4909378000000002, - dist_correction_y: 1.4347861, - focal_distance: 20.5, - focal_slope: 1.2, - horiz_offset_correction: 0.025999999, - laser_id: 22, - rot_correction: 0.05942554283989759, - vert_correction: -0.06665878416276627, - vert_offset_correction: 0.21137333000000003, - } - - { - dist_correction: 1.4939513, - dist_correction_x: 1.5194868, - dist_correction_y: 1.5106346, - focal_distance: 14.5, - focal_slope: 1.65, - horiz_offset_correction: -0.025999999, - laser_id: 23, - rot_correction: 0.09797042203986979, - vert_correction: -0.06055112836378901, - vert_offset_correction: 0.21093236999999998, - } - - { - dist_correction: 1.4521244999999998, - dist_correction_x: 1.4884882, - dist_correction_y: 1.4649333, - focal_distance: 11.5, - focal_slope: 2.0, - horiz_offset_correction: 0.025999999, - laser_id: 24, - rot_correction: -0.07658879305408597, - vert_correction: 0.015620435355027301, - vert_offset_correction: 0.20544985000000002, - } - - { - dist_correction: 1.5351622, - dist_correction_x: 1.524973, - dist_correction_y: 1.5029565, - focal_distance: 18.5, - focal_slope: 1.25, - horiz_offset_correction: -0.025999999, - laser_id: 25, - rot_correction: -0.0379098725675606, - vert_correction: 0.022567997346205196, - vert_offset_correction: 0.20495010000000002, - } - - { - dist_correction: 1.5321327, - dist_correction_x: 1.5833431999999998, - dist_correction_y: 1.5359726, - focal_distance: 13.0, - focal_slope: 1.9, - horiz_offset_correction: 0.025999999, - laser_id: 26, - rot_correction: -0.0904730670226138, - vert_correction: -0.019943931467746017, - vert_offset_correction: 0.20800737000000002, - } - - { - dist_correction: 1.5166023000000002, - dist_correction_x: 1.4874829, - dist_correction_y: 1.4625635, - focal_distance: 21.5, - focal_slope: 1.1, - horiz_offset_correction: -0.025999999, - laser_id: 27, - rot_correction: -0.051308328902808065, - vert_correction: -0.013200099491225388, - vert_offset_correction: 0.20752234000000003, - } - - { - dist_correction: 1.4549194, - dist_correction_x: 1.4688664, - dist_correction_y: 1.4194371000000001, - focal_distance: 13.5, - focal_slope: 1.83, - horiz_offset_correction: 0.025999999, - laser_id: 28, - rot_correction: -0.0019143155208309244, - vert_correction: 0.027266617424120905, - vert_offset_correction: 0.20461203000000003, - } - - { - dist_correction: 1.5617532, - dist_correction_x: 1.5887217999999999, - dist_correction_y: 1.5643922000000001, - focal_distance: 18.5, - focal_slope: 1.35, - horiz_offset_correction: -0.025999999, - laser_id: 29, - rot_correction: 0.03681404490741569, - vert_correction: 0.03421014630846329, - vert_offset_correction: 0.20411228, - } - - { - dist_correction: 1.5132924, - dist_correction_x: 1.5661812000000002, - dist_correction_y: 1.5102689, - focal_distance: 18.5, - focal_slope: 1.35, - horiz_offset_correction: 0.025999999, - laser_id: 30, - rot_correction: -0.015542064486559148, - vert_correction: -0.007885903531460533, - vert_offset_correction: 0.20714018, - } - - { - dist_correction: 1.5638524, - dist_correction_x: 1.5621136000000002, - dist_correction_y: 1.5383017, - focal_distance: 21.5, - focal_slope: 1.15, - horiz_offset_correction: -0.025999999, - laser_id: 31, - rot_correction: 0.023284423588222334, - vert_correction: -0.0015491891506552067, - vert_offset_correction: 0.20668451000000002, - } - - { - dist_correction: 1.3077792, - dist_correction_x: 1.3433452, - dist_correction_y: 1.2840973, - focal_distance: 10.5, - focal_slope: 2.0, - horiz_offset_correction: 0.025999999, - laser_id: 32, - rot_correction: -0.1278634161583795, - vert_correction: -0.39021216204755743, - vert_offset_correction: 0.15970787, - } - - { - dist_correction: 1.4303885, - dist_correction_x: 1.4429931999999999, - dist_correction_y: 1.4607234, - focal_distance: 0.25, - focal_slope: 1.0, - horiz_offset_correction: -0.025999999, - laser_id: 33, - rot_correction: -0.066528586091226, - vert_correction: -0.38355018793281753, - vert_offset_correction: 0.15922519, - } - - { - dist_correction: 1.3746819, - dist_correction_x: 1.3873923, - dist_correction_y: 1.4089924999999999, - focal_distance: 0.25, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 34, - rot_correction: 0.08854290740283328, - vert_correction: -0.197138848550284, - vert_offset_correction: 0.14656122, - } - - { - dist_correction: 1.3814778, - dist_correction_x: 1.3932765, - dist_correction_y: 1.4301869, - focal_distance: 11.0, - focal_slope: 2.0, - horiz_offset_correction: -0.025999999, - laser_id: 35, - rot_correction: 0.14547956884148489, - vert_correction: -0.18768579625563228, - vert_offset_correction: 0.14595152, - } - - { - dist_correction: 1.4313307, - dist_correction_x: 1.4734517, - dist_correction_y: 1.4390849000000001, - focal_distance: 9.5, - focal_slope: 1.85, - horiz_offset_correction: 0.025999999, - laser_id: 36, - rot_correction: -0.005071588212420635, - vert_correction: -0.3750836655445631, - vert_offset_correction: 0.15861549, - } - - { - dist_correction: 1.3414835, - dist_correction_x: 1.3820609, - dist_correction_y: 1.355589, - focal_distance: 9.5, - focal_slope: 1.4, - horiz_offset_correction: -0.025999999, - laser_id: 37, - rot_correction: 0.05427589303135225, - vert_correction: -0.36762882325722723, - vert_offset_correction: 0.15808201, - } - - { - dist_correction: 1.4895827, - dist_correction_x: 1.5283238000000001, - dist_correction_y: 1.4803529, - focal_distance: 9.0, - focal_slope: 1.65, - horiz_offset_correction: 0.025999999, - laser_id: 38, - rot_correction: -0.028143856558087547, - vert_correction: -0.4285668719175616, - vert_offset_correction: 0.16254044, - } - - { - dist_correction: 1.3727733000000002, - dist_correction_x: 1.3623596, - dist_correction_y: 1.392661, - focal_distance: 0.25, - focal_slope: 1.15, - horiz_offset_correction: -0.025999999, - laser_id: 39, - rot_correction: 0.03172272051181349, - vert_correction: -0.4214410765541042, - vert_offset_correction: 0.16200695, - } - - { - dist_correction: 1.4348983999999998, - dist_correction_x: 1.4775407000000003, - dist_correction_y: 1.4712174999999998, - focal_distance: 0.25, - focal_slope: 0.92000002, - horiz_offset_correction: 0.025999999, - laser_id: 40, - rot_correction: 0.11411698480351568, - vert_correction: -0.3565455112681652, - vert_offset_correction: 0.15729448, - } - - { - dist_correction: 1.335618, - dist_correction_x: 1.3519691, - dist_correction_y: 1.3315152000000001, - focal_distance: 11.200000000000001, - focal_slope: 2.0, - horiz_offset_correction: -0.025999999, - laser_id: 41, - rot_correction: 0.1735498527902839, - vert_correction: -0.34717860842539194, - vert_offset_correction: 0.15663397, - } - - { - dist_correction: 1.4905824, - dist_correction_x: 1.488313, - dist_correction_y: 1.4948479000000001, - focal_distance: 0.25, - focal_slope: 1.1, - horiz_offset_correction: 0.025999999, - laser_id: 42, - rot_correction: 0.09564756333839054, - vert_correction: -0.4110102035460227, - vert_offset_correction: 0.16123213, - } - - { - dist_correction: 1.3288423, - dist_correction_x: 1.3409723, - dist_correction_y: 1.3440451, - focal_distance: 10.0, - focal_slope: 1.95, - horiz_offset_correction: -0.025999999, - laser_id: 43, - rot_correction: 0.15538288292189534, - vert_correction: -0.40083030888437793, - vert_offset_correction: 0.16048269, - } - - { - dist_correction: 1.3771290999999999, - dist_correction_x: 1.3773239000000002, - dist_correction_y: 1.3497290000000002, - focal_distance: 11.5, - focal_slope: 2.0, - horiz_offset_correction: 0.025999999, - laser_id: 44, - rot_correction: -0.12413605610123538, - vert_correction: -0.2840315837972709, - vert_offset_correction: 0.15228986, - } - - { - dist_correction: 1.3367807, - dist_correction_x: 1.370152, - dist_correction_y: 1.370934, - focal_distance: 0.25, - focal_slope: 0.44999999, - horiz_offset_correction: -0.025999999, - laser_id: 45, - rot_correction: -0.06413447432303407, - vert_correction: -0.27761533458791926, - vert_offset_correction: 0.15185799, - } - - { - dist_correction: 1.4788651, - dist_correction_x: 1.5014308, - dist_correction_y: 1.4727454, - focal_distance: 11.5, - focal_slope: 2.0, - horiz_offset_correction: 0.025999999, - laser_id: 46, - rot_correction: -0.14872602785785152, - vert_correction: -0.3359268721635124, - vert_offset_correction: 0.15584644, - } - - { - dist_correction: 1.3220766000000002, - dist_correction_x: 1.3654865, - dist_correction_y: 1.3739757000000001, - focal_distance: 0.25, - focal_slope: 0.94999999, - horiz_offset_correction: -0.025999999, - laser_id: 47, - rot_correction: -0.08707280959256145, - vert_correction: -0.33026751989087316, - vert_offset_correction: 0.15545268, - } - - { - dist_correction: 1.4612433999999999, - dist_correction_x: 1.5250436, - dist_correction_y: 1.4817635999999998, - focal_distance: 13.0, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 48, - rot_correction: -0.006799911150716457, - vert_correction: -0.26851710773019805, - vert_offset_correction: 0.15124829, - } - - { - dist_correction: 1.3407353000000002, - dist_correction_x: 1.3478844, - dist_correction_y: 1.3154279, - focal_distance: 13.5, - focal_slope: 1.8, - horiz_offset_correction: -0.025999999, - laser_id: 49, - rot_correction: 0.05242808851765633, - vert_correction: -0.26051854302098837, - vert_offset_correction: 0.1507148, - } - - { - dist_correction: 1.3465115, - dist_correction_x: 1.3874431999999999, - dist_correction_y: 1.3508052000000002, - focal_distance: 12.0, - focal_slope: 1.85, - horiz_offset_correction: 0.025999999, - laser_id: 50, - rot_correction: -0.028383715411859876, - vert_correction: -0.3216451745070007, - vert_offset_correction: 0.15485568000000002, - } - - { - dist_correction: 1.3629696999999998, - dist_correction_x: 1.4037315000000001, - dist_correction_y: 1.3869237, - focal_distance: 12.0, - focal_slope: 1.1, - horiz_offset_correction: -0.025999999, - laser_id: 51, - rot_correction: 0.031843273893907245, - vert_correction: -0.3135280670349956, - vert_offset_correction: 0.15429679000000002, - } - - { - dist_correction: 1.4472754, - dist_correction_x: 1.4817390000000001, - dist_correction_y: 1.4782272, - focal_distance: 2.0, - focal_slope: 1.0, - horiz_offset_correction: 0.025999999, - laser_id: 52, - rot_correction: 0.10955275158734502, - vert_correction: -0.24941678290173272, - vert_offset_correction: 0.14997807999999999, - } - - { - dist_correction: 1.3800671, - dist_correction_x: 1.3796414, - dist_correction_y: 1.3943782, - focal_distance: 11.200000000000001, - focal_slope: 2.0, - horiz_offset_correction: -0.025999999, - laser_id: 53, - rot_correction: 0.16876716543828738, - vert_correction: -0.2409525119882134, - vert_offset_correction: 0.14941919, - } - - { - dist_correction: 1.5190169, - dist_correction_x: 1.5619051, - dist_correction_y: 1.5511705, - focal_distance: 2.5, - focal_slope: 1.05, - horiz_offset_correction: 0.025999999, - laser_id: 54, - rot_correction: 0.09037283276367176, - vert_correction: -0.30313513748485243, - vert_offset_correction: 0.15358547, - } - - { - dist_correction: 1.3803656000000002, - dist_correction_x: 1.3960698, - dist_correction_y: 1.386106, - focal_distance: 12.5, - focal_slope: 1.9, - horiz_offset_correction: -0.025999999, - laser_id: 55, - rot_correction: 0.14871534295217081, - vert_correction: -0.29323634555253386, - vert_offset_correction: 0.15291226, - } - - { - dist_correction: 1.5827696, - dist_correction_x: 1.6050609, - dist_correction_y: 1.5844797, - focal_distance: 10.0, - focal_slope: 1.95, - horiz_offset_correction: 0.025999999, - laser_id: 56, - rot_correction: -0.12100867217308608, - vert_correction: -0.17760463486977288, - vert_offset_correction: 0.14530372, - } - - { - dist_correction: 1.3843919, - dist_correction_x: 1.3915251000000002, - dist_correction_y: 1.4156927, - focal_distance: 0.25, - focal_slope: 0.94999999, - horiz_offset_correction: -0.025999999, - laser_id: 57, - rot_correction: -0.06431965899265843, - vert_correction: -0.17006926832673774, - vert_offset_correction: 0.14482104, - } - - { - dist_correction: 1.5491273, - dist_correction_x: 1.5298964000000002, - dist_correction_y: 1.5107637, - focal_distance: 11.5, - focal_slope: 2.0, - horiz_offset_correction: 0.025999999, - laser_id: 58, - rot_correction: -0.14486168214370612, - vert_correction: -0.22974121500510264, - vert_offset_correction: 0.14868247, - } - - { - dist_correction: 1.3646004, - dist_correction_x: 1.3803583000000001, - dist_correction_y: 1.4061511, - focal_distance: 0.25, - focal_slope: 1.0, - horiz_offset_correction: -0.025999999, - laser_id: 59, - rot_correction: -0.0852480451703211, - vert_correction: -0.22391891879349718, - vert_offset_correction: 0.14830141, - } - - { - dist_correction: 1.5239935, - dist_correction_x: 1.560786, - dist_correction_y: 1.5632524, - focal_distance: 5.0, - focal_slope: 1.15, - horiz_offset_correction: 0.025999999, - laser_id: 60, - rot_correction: -0.005967226432828876, - vert_correction: -0.16231529056824404, - vert_offset_correction: 0.14432566, - } - - { - dist_correction: 1.4112029000000001, - dist_correction_x: 1.4223886, - dist_correction_y: 1.4542918, - focal_distance: 2.5, - focal_slope: 1.05, - horiz_offset_correction: -0.025999999, - laser_id: 61, - rot_correction: 0.050600613599087636, - vert_correction: -0.15314424682880032, - vert_offset_correction: 0.14374136, - } - - { - dist_correction: 1.5013637, - dist_correction_x: 1.5208610999999999, - dist_correction_y: 1.5006433000000001, - focal_distance: 16.5, - focal_slope: 1.25, - horiz_offset_correction: 0.025999999, - laser_id: 62, - rot_correction: -0.027436902217920986, - vert_correction: -0.21457119711920336, - vert_offset_correction: 0.14769171, - } - - { - dist_correction: 1.4423058, - dist_correction_x: 1.4454633000000001, - dist_correction_y: 1.4321198000000002, - focal_distance: 9.0, - focal_slope: 1.45, - horiz_offset_correction: -0.025999999, - laser_id: 63, - rot_correction: 0.0290479702718764, - vert_correction: -0.2079266762969834, - vert_offset_correction: 0.14725984, + - { + dist_correction: 1.4139490000000001, + dist_correction_x: 1.4198446999999998, + dist_correction_y: 1.4058145, + focal_distance: 10.5, + focal_slope: 1.85, + horiz_offset_correction: 0.025999999, + laser_id: 0, + min_intensity: 5, + rot_correction: -0.07648247457737148, + vert_correction: -0.1261818455292898, + vert_offset_correction: 0.21569468, + } + - { + dist_correction: 1.5094685, + dist_correction_x: 1.5403023, + dist_correction_y: 1.5012577999999999, + focal_distance: 15.0, + focal_slope: 1.2, + horiz_offset_correction: -0.025999999, + laser_id: 1, + min_intensity: 10, + rot_correction: -0.03932070772308096, + vert_correction: -0.11953745909742221, + vert_offset_correction: 0.21520962000000002, + } + - { + dist_correction: 1.4579400999999999, + dist_correction_x: 1.4846666, + dist_correction_y: 1.4228239, + focal_distance: 24.0, + focal_slope: 1.05, + horiz_offset_correction: 0.025999999, + laser_id: 2, + min_intensity: 30, + rot_correction: 0.06005350008746038, + vert_correction: 0.00356118725906175, + vert_offset_correction: 0.20631704, + } + - { + dist_correction: 1.5046124, + dist_correction_x: 1.5335535999999999, + dist_correction_y: 1.5401996, + focal_distance: 14.5, + focal_slope: 1.7, + horiz_offset_correction: -0.025999999, + laser_id: 3, + min_intensity: 10, + rot_correction: 0.09919490315516696, + vert_correction: 0.010306535403103584, + vert_offset_correction: 0.20583200000000001, + } + - { + dist_correction: 1.4370993, + dist_correction_x: 1.4714845, + dist_correction_y: 1.4524646, + focal_distance: 15.0, + focal_slope: 0.40000001, + horiz_offset_correction: 0.025999999, + laser_id: 4, + min_intensity: 30, + rot_correction: -0.00095355016485159, + vert_correction: -0.1144967440141401, + vert_offset_correction: 0.21484217000000003, + } + - { + dist_correction: 1.3536626999999999, + dist_correction_x: 1.3909094, + dist_correction_y: 1.3700326999999999, + focal_distance: 17.0, + focal_slope: 1.45, + horiz_offset_correction: -0.025999999, + laser_id: 5, + min_intensity: 30, + rot_correction: 0.03824387099052699, + vert_correction: -0.10803612329921503, + vert_offset_correction: 0.21437181, + } + - { + dist_correction: 1.4117873, + dist_correction_x: 1.4350795, + dist_correction_y: 1.3829624999999999, + focal_distance: 10.5, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 6, + min_intensity: 10, + rot_correction: -0.015297255529962315, + vert_correction: -0.14944538101702426, + vert_offset_correction: 0.21739968999999998, + } + - { + dist_correction: 1.4269646, + dist_correction_x: 1.451161, + dist_correction_y: 1.4328421, + focal_distance: 14.0, + focal_slope: 1.3, + horiz_offset_correction: -0.025999999, + laser_id: 7, + min_intensity: 10, + rot_correction: 0.024255809272700053, + vert_correction: -0.14344353231329066, + vert_offset_correction: 0.21695873, + } + - { + dist_correction: 1.3808284000000002, + dist_correction_x: 1.421472, + dist_correction_y: 1.3838283000000002, + focal_distance: 15.0, + focal_slope: 0.94999999, + horiz_offset_correction: 0.025999999, + laser_id: 8, + min_intensity: 30, + rot_correction: 0.07354442571180776, + vert_correction: -0.10278016723125998, + vert_offset_correction: 0.21398966000000003, + } + - { + dist_correction: 1.3644336000000001, + dist_correction_x: 1.3931616, + dist_correction_y: 1.3852122, + focal_distance: 12.5, + focal_slope: 1.95, + horiz_offset_correction: -0.025999999, + laser_id: 9, + min_intensity: 10, + rot_correction: 0.11317857886058363, + vert_correction: -0.09569590023202451, + vert_offset_correction: 0.21347521, + } + - { + dist_correction: 1.4102663000000002, + dist_correction_x: 1.4163402, + dist_correction_y: 1.3592377, + focal_distance: 12.5, + focal_slope: 1.95, + horiz_offset_correction: 0.025999999, + laser_id: 10, + min_intensity: 10, + rot_correction: 0.05954228616823424, + vert_correction: -0.1386345201601863, + vert_offset_correction: 0.21660597, + } + - { + dist_correction: 1.5019737000000002, + dist_correction_x: 1.532475, + dist_correction_y: 1.5237663000000001, + focal_distance: 13.0, + focal_slope: 1.9, + horiz_offset_correction: -0.025999999, + laser_id: 11, + min_intensity: 10, + rot_correction: 0.09906092120980835, + vert_correction: -0.13181073657049397, + vert_offset_correction: 0.21610622, + } + - { + dist_correction: 1.4399905000000002, + dist_correction_x: 1.4887962, + dist_correction_y: 1.4455237, + focal_distance: 14.5, + focal_slope: 1.7, + horiz_offset_correction: 0.025999999, + laser_id: 12, + max_intensity: 235, + rot_correction: -0.07574798243226695, + vert_correction: -0.054438932963427306, + vert_offset_correction: 0.21049141, + } + - { + dist_correction: 1.5207593, + dist_correction_x: 1.5401453, + dist_correction_y: 1.5190082, + focal_distance: 20.5, + focal_slope: 1.25, + horiz_offset_correction: -0.025999999, + laser_id: 13, + max_intensity: 240, + rot_correction: -0.037047761947550245, + vert_correction: -0.048730533448148504, + vert_offset_correction: 0.21007986, + } + - { + dist_correction: 1.4746239, + dist_correction_x: 1.5300989999999999, + dist_correction_y: 1.4663734, + focal_distance: 12.5, + focal_slope: 1.9, + horiz_offset_correction: 0.025999999, + laser_id: 14, + max_intensity: 245, + rot_correction: -0.0895955468906376, + vert_correction: -0.08961595328025192, + vert_offset_correction: 0.21303425, + } + - { + dist_correction: 1.4697629, + dist_correction_x: 1.5051735, + dist_correction_y: 1.4558601, + focal_distance: 19.5, + focal_slope: 0.94999999, + horiz_offset_correction: -0.025999999, + laser_id: 15, + max_intensity: 245, + rot_correction: -0.05072966149865084, + vert_correction: -0.0839353306800186, + vert_offset_correction: 0.21262267999999998, + } + - { + dist_correction: 1.4143376, + dist_correction_x: 1.4432597000000003, + dist_correction_y: 1.4038483, + focal_distance: 24.0, + focal_slope: 0.69999999, + horiz_offset_correction: 0.025999999, + laser_id: 16, + max_intensity: 245, + rot_correction: -0.001864320564407547, + vert_correction: -0.043427018903405855, + vert_offset_correction: 0.20969770000000001, + } + - { + dist_correction: 1.4815961, + dist_correction_x: 1.5173615, + dist_correction_y: 1.4871606, + focal_distance: 14.5, + focal_slope: 1.65, + horiz_offset_correction: -0.025999999, + laser_id: 17, + rot_correction: 0.03747852840556422, + vert_correction: -0.03648801216715539, + vert_offset_correction: 0.20919794, + } + - { + dist_correction: 1.420331, + dist_correction_x: 1.4755219, + dist_correction_y: 1.3919118000000001, + focal_distance: 17.5, + focal_slope: 1.4, + horiz_offset_correction: 0.025999999, + laser_id: 18, + rot_correction: -0.015194972429011364, + vert_correction: -0.07906187922560139, + vert_offset_correction: 0.21226994000000002, + } + - { + dist_correction: 1.4900717, + dist_correction_x: 1.5143349000000002, + dist_correction_y: 1.4829907, + focal_distance: 24.0, + focal_slope: 1.25, + horiz_offset_correction: -0.025999999, + laser_id: 19, + rot_correction: 0.02391977928648433, + vert_correction: -0.07255814015150577, + vert_offset_correction: 0.21179958, + } + - { + dist_correction: 1.4823865, + dist_correction_x: 1.5357741999999999, + dist_correction_y: 1.4956403, + focal_distance: 24.0, + focal_slope: 1.1, + horiz_offset_correction: 0.025999999, + laser_id: 20, + rot_correction: 0.07264374331532833, + vert_correction: -0.031587736747464255, + vert_offset_correction: 0.20884519999999998, + } + - { + dist_correction: 1.5553201, + dist_correction_x: 1.5698593, + dist_correction_y: 1.5624425, + focal_distance: 15.5, + focal_slope: 1.55, + horiz_offset_correction: -0.025999999, + laser_id: 21, + rot_correction: 0.11143265968730214, + vert_correction: -0.025460288914769372, + vert_offset_correction: 0.20840424, + } + - { + dist_correction: 1.4437845999999999, + dist_correction_x: 1.4909378000000002, + dist_correction_y: 1.4347861, + focal_distance: 20.5, + focal_slope: 1.2, + horiz_offset_correction: 0.025999999, + laser_id: 22, + rot_correction: 0.05942554283989759, + vert_correction: -0.06665878416276627, + vert_offset_correction: 0.21137333000000003, + } + - { + dist_correction: 1.4939513, + dist_correction_x: 1.5194868, + dist_correction_y: 1.5106346, + focal_distance: 14.5, + focal_slope: 1.65, + horiz_offset_correction: -0.025999999, + laser_id: 23, + rot_correction: 0.09797042203986979, + vert_correction: -0.06055112836378901, + vert_offset_correction: 0.21093236999999998, + } + - { + dist_correction: 1.4521244999999998, + dist_correction_x: 1.4884882, + dist_correction_y: 1.4649333, + focal_distance: 11.5, + focal_slope: 2.0, + horiz_offset_correction: 0.025999999, + laser_id: 24, + rot_correction: -0.07658879305408597, + vert_correction: 0.015620435355027301, + vert_offset_correction: 0.20544985000000002, + } + - { + dist_correction: 1.5351622, + dist_correction_x: 1.524973, + dist_correction_y: 1.5029565, + focal_distance: 18.5, + focal_slope: 1.25, + horiz_offset_correction: -0.025999999, + laser_id: 25, + rot_correction: -0.0379098725675606, + vert_correction: 0.022567997346205196, + vert_offset_correction: 0.20495010000000002, + } + - { + dist_correction: 1.5321327, + dist_correction_x: 1.5833431999999998, + dist_correction_y: 1.5359726, + focal_distance: 13.0, + focal_slope: 1.9, + horiz_offset_correction: 0.025999999, + laser_id: 26, + rot_correction: -0.0904730670226138, + vert_correction: -0.019943931467746017, + vert_offset_correction: 0.20800737000000002, + } + - { + dist_correction: 1.5166023000000002, + dist_correction_x: 1.4874829, + dist_correction_y: 1.4625635, + focal_distance: 21.5, + focal_slope: 1.1, + horiz_offset_correction: -0.025999999, + laser_id: 27, + rot_correction: -0.051308328902808065, + vert_correction: -0.013200099491225388, + vert_offset_correction: 0.20752234000000003, + } + - { + dist_correction: 1.4549194, + dist_correction_x: 1.4688664, + dist_correction_y: 1.4194371000000001, + focal_distance: 13.5, + focal_slope: 1.83, + horiz_offset_correction: 0.025999999, + laser_id: 28, + rot_correction: -0.0019143155208309244, + vert_correction: 0.027266617424120905, + vert_offset_correction: 0.20461203000000003, + } + - { + dist_correction: 1.5617532, + dist_correction_x: 1.5887217999999999, + dist_correction_y: 1.5643922000000001, + focal_distance: 18.5, + focal_slope: 1.35, + horiz_offset_correction: -0.025999999, + laser_id: 29, + rot_correction: 0.03681404490741569, + vert_correction: 0.03421014630846329, + vert_offset_correction: 0.20411228, + } + - { + dist_correction: 1.5132924, + dist_correction_x: 1.5661812000000002, + dist_correction_y: 1.5102689, + focal_distance: 18.5, + focal_slope: 1.35, + horiz_offset_correction: 0.025999999, + laser_id: 30, + rot_correction: -0.015542064486559148, + vert_correction: -0.007885903531460533, + vert_offset_correction: 0.20714018, + } + - { + dist_correction: 1.5638524, + dist_correction_x: 1.5621136000000002, + dist_correction_y: 1.5383017, + focal_distance: 21.5, + focal_slope: 1.15, + horiz_offset_correction: -0.025999999, + laser_id: 31, + rot_correction: 0.023284423588222334, + vert_correction: -0.0015491891506552067, + vert_offset_correction: 0.20668451000000002, + } + - { + dist_correction: 1.3077792, + dist_correction_x: 1.3433452, + dist_correction_y: 1.2840973, + focal_distance: 10.5, + focal_slope: 2.0, + horiz_offset_correction: 0.025999999, + laser_id: 32, + rot_correction: -0.1278634161583795, + vert_correction: -0.39021216204755743, + vert_offset_correction: 0.15970787, + } + - { + dist_correction: 1.4303885, + dist_correction_x: 1.4429931999999999, + dist_correction_y: 1.4607234, + focal_distance: 0.25, + focal_slope: 1.0, + horiz_offset_correction: -0.025999999, + laser_id: 33, + rot_correction: -0.066528586091226, + vert_correction: -0.38355018793281753, + vert_offset_correction: 0.15922519, + } + - { + dist_correction: 1.3746819, + dist_correction_x: 1.3873923, + dist_correction_y: 1.4089924999999999, + focal_distance: 0.25, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 34, + rot_correction: 0.08854290740283328, + vert_correction: -0.197138848550284, + vert_offset_correction: 0.14656122, + } + - { + dist_correction: 1.3814778, + dist_correction_x: 1.3932765, + dist_correction_y: 1.4301869, + focal_distance: 11.0, + focal_slope: 2.0, + horiz_offset_correction: -0.025999999, + laser_id: 35, + rot_correction: 0.14547956884148489, + vert_correction: -0.18768579625563228, + vert_offset_correction: 0.14595152, + } + - { + dist_correction: 1.4313307, + dist_correction_x: 1.4734517, + dist_correction_y: 1.4390849000000001, + focal_distance: 9.5, + focal_slope: 1.85, + horiz_offset_correction: 0.025999999, + laser_id: 36, + rot_correction: -0.005071588212420635, + vert_correction: -0.3750836655445631, + vert_offset_correction: 0.15861549, + } + - { + dist_correction: 1.3414835, + dist_correction_x: 1.3820609, + dist_correction_y: 1.355589, + focal_distance: 9.5, + focal_slope: 1.4, + horiz_offset_correction: -0.025999999, + laser_id: 37, + rot_correction: 0.05427589303135225, + vert_correction: -0.36762882325722723, + vert_offset_correction: 0.15808201, + } + - { + dist_correction: 1.4895827, + dist_correction_x: 1.5283238000000001, + dist_correction_y: 1.4803529, + focal_distance: 9.0, + focal_slope: 1.65, + horiz_offset_correction: 0.025999999, + laser_id: 38, + rot_correction: -0.028143856558087547, + vert_correction: -0.4285668719175616, + vert_offset_correction: 0.16254044, + } + - { + dist_correction: 1.3727733000000002, + dist_correction_x: 1.3623596, + dist_correction_y: 1.392661, + focal_distance: 0.25, + focal_slope: 1.15, + horiz_offset_correction: -0.025999999, + laser_id: 39, + rot_correction: 0.03172272051181349, + vert_correction: -0.4214410765541042, + vert_offset_correction: 0.16200695, + } + - { + dist_correction: 1.4348983999999998, + dist_correction_x: 1.4775407000000003, + dist_correction_y: 1.4712174999999998, + focal_distance: 0.25, + focal_slope: 0.92000002, + horiz_offset_correction: 0.025999999, + laser_id: 40, + rot_correction: 0.11411698480351568, + vert_correction: -0.3565455112681652, + vert_offset_correction: 0.15729448, + } + - { + dist_correction: 1.335618, + dist_correction_x: 1.3519691, + dist_correction_y: 1.3315152000000001, + focal_distance: 11.200000000000001, + focal_slope: 2.0, + horiz_offset_correction: -0.025999999, + laser_id: 41, + rot_correction: 0.1735498527902839, + vert_correction: -0.34717860842539194, + vert_offset_correction: 0.15663397, + } + - { + dist_correction: 1.4905824, + dist_correction_x: 1.488313, + dist_correction_y: 1.4948479000000001, + focal_distance: 0.25, + focal_slope: 1.1, + horiz_offset_correction: 0.025999999, + laser_id: 42, + rot_correction: 0.09564756333839054, + vert_correction: -0.4110102035460227, + vert_offset_correction: 0.16123213, + } + - { + dist_correction: 1.3288423, + dist_correction_x: 1.3409723, + dist_correction_y: 1.3440451, + focal_distance: 10.0, + focal_slope: 1.95, + horiz_offset_correction: -0.025999999, + laser_id: 43, + rot_correction: 0.15538288292189534, + vert_correction: -0.40083030888437793, + vert_offset_correction: 0.16048269, + } + - { + dist_correction: 1.3771290999999999, + dist_correction_x: 1.3773239000000002, + dist_correction_y: 1.3497290000000002, + focal_distance: 11.5, + focal_slope: 2.0, + horiz_offset_correction: 0.025999999, + laser_id: 44, + rot_correction: -0.12413605610123538, + vert_correction: -0.2840315837972709, + vert_offset_correction: 0.15228986, + } + - { + dist_correction: 1.3367807, + dist_correction_x: 1.370152, + dist_correction_y: 1.370934, + focal_distance: 0.25, + focal_slope: 0.44999999, + horiz_offset_correction: -0.025999999, + laser_id: 45, + rot_correction: -0.06413447432303407, + vert_correction: -0.27761533458791926, + vert_offset_correction: 0.15185799, + } + - { + dist_correction: 1.4788651, + dist_correction_x: 1.5014308, + dist_correction_y: 1.4727454, + focal_distance: 11.5, + focal_slope: 2.0, + horiz_offset_correction: 0.025999999, + laser_id: 46, + rot_correction: -0.14872602785785152, + vert_correction: -0.3359268721635124, + vert_offset_correction: 0.15584644, + } + - { + dist_correction: 1.3220766000000002, + dist_correction_x: 1.3654865, + dist_correction_y: 1.3739757000000001, + focal_distance: 0.25, + focal_slope: 0.94999999, + horiz_offset_correction: -0.025999999, + laser_id: 47, + rot_correction: -0.08707280959256145, + vert_correction: -0.33026751989087316, + vert_offset_correction: 0.15545268, + } + - { + dist_correction: 1.4612433999999999, + dist_correction_x: 1.5250436, + dist_correction_y: 1.4817635999999998, + focal_distance: 13.0, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 48, + rot_correction: -0.006799911150716457, + vert_correction: -0.26851710773019805, + vert_offset_correction: 0.15124829, + } + - { + dist_correction: 1.3407353000000002, + dist_correction_x: 1.3478844, + dist_correction_y: 1.3154279, + focal_distance: 13.5, + focal_slope: 1.8, + horiz_offset_correction: -0.025999999, + laser_id: 49, + rot_correction: 0.05242808851765633, + vert_correction: -0.26051854302098837, + vert_offset_correction: 0.1507148, + } + - { + dist_correction: 1.3465115, + dist_correction_x: 1.3874431999999999, + dist_correction_y: 1.3508052000000002, + focal_distance: 12.0, + focal_slope: 1.85, + horiz_offset_correction: 0.025999999, + laser_id: 50, + rot_correction: -0.028383715411859876, + vert_correction: -0.3216451745070007, + vert_offset_correction: 0.15485568000000002, + } + - { + dist_correction: 1.3629696999999998, + dist_correction_x: 1.4037315000000001, + dist_correction_y: 1.3869237, + focal_distance: 12.0, + focal_slope: 1.1, + horiz_offset_correction: -0.025999999, + laser_id: 51, + rot_correction: 0.031843273893907245, + vert_correction: -0.3135280670349956, + vert_offset_correction: 0.15429679000000002, + } + - { + dist_correction: 1.4472754, + dist_correction_x: 1.4817390000000001, + dist_correction_y: 1.4782272, + focal_distance: 2.0, + focal_slope: 1.0, + horiz_offset_correction: 0.025999999, + laser_id: 52, + rot_correction: 0.10955275158734502, + vert_correction: -0.24941678290173272, + vert_offset_correction: 0.14997807999999999, + } + - { + dist_correction: 1.3800671, + dist_correction_x: 1.3796414, + dist_correction_y: 1.3943782, + focal_distance: 11.200000000000001, + focal_slope: 2.0, + horiz_offset_correction: -0.025999999, + laser_id: 53, + rot_correction: 0.16876716543828738, + vert_correction: -0.2409525119882134, + vert_offset_correction: 0.14941919, + } + - { + dist_correction: 1.5190169, + dist_correction_x: 1.5619051, + dist_correction_y: 1.5511705, + focal_distance: 2.5, + focal_slope: 1.05, + horiz_offset_correction: 0.025999999, + laser_id: 54, + rot_correction: 0.09037283276367176, + vert_correction: -0.30313513748485243, + vert_offset_correction: 0.15358547, + } + - { + dist_correction: 1.3803656000000002, + dist_correction_x: 1.3960698, + dist_correction_y: 1.386106, + focal_distance: 12.5, + focal_slope: 1.9, + horiz_offset_correction: -0.025999999, + laser_id: 55, + rot_correction: 0.14871534295217081, + vert_correction: -0.29323634555253386, + vert_offset_correction: 0.15291226, + } + - { + dist_correction: 1.5827696, + dist_correction_x: 1.6050609, + dist_correction_y: 1.5844797, + focal_distance: 10.0, + focal_slope: 1.95, + horiz_offset_correction: 0.025999999, + laser_id: 56, + rot_correction: -0.12100867217308608, + vert_correction: -0.17760463486977288, + vert_offset_correction: 0.14530372, + } + - { + dist_correction: 1.3843919, + dist_correction_x: 1.3915251000000002, + dist_correction_y: 1.4156927, + focal_distance: 0.25, + focal_slope: 0.94999999, + horiz_offset_correction: -0.025999999, + laser_id: 57, + rot_correction: -0.06431965899265843, + vert_correction: -0.17006926832673774, + vert_offset_correction: 0.14482104, + } + - { + dist_correction: 1.5491273, + dist_correction_x: 1.5298964000000002, + dist_correction_y: 1.5107637, + focal_distance: 11.5, + focal_slope: 2.0, + horiz_offset_correction: 0.025999999, + laser_id: 58, + rot_correction: -0.14486168214370612, + vert_correction: -0.22974121500510264, + vert_offset_correction: 0.14868247, + } + - { + dist_correction: 1.3646004, + dist_correction_x: 1.3803583000000001, + dist_correction_y: 1.4061511, + focal_distance: 0.25, + focal_slope: 1.0, + horiz_offset_correction: -0.025999999, + laser_id: 59, + rot_correction: -0.0852480451703211, + vert_correction: -0.22391891879349718, + vert_offset_correction: 0.14830141, + } + - { + dist_correction: 1.5239935, + dist_correction_x: 1.560786, + dist_correction_y: 1.5632524, + focal_distance: 5.0, + focal_slope: 1.15, + horiz_offset_correction: 0.025999999, + laser_id: 60, + rot_correction: -0.005967226432828876, + vert_correction: -0.16231529056824404, + vert_offset_correction: 0.14432566, + } + - { + dist_correction: 1.4112029000000001, + dist_correction_x: 1.4223886, + dist_correction_y: 1.4542918, + focal_distance: 2.5, + focal_slope: 1.05, + horiz_offset_correction: -0.025999999, + laser_id: 61, + rot_correction: 0.050600613599087636, + vert_correction: -0.15314424682880032, + vert_offset_correction: 0.14374136, + } + - { + dist_correction: 1.5013637, + dist_correction_x: 1.5208610999999999, + dist_correction_y: 1.5006433000000001, + focal_distance: 16.5, + focal_slope: 1.25, + horiz_offset_correction: 0.025999999, + laser_id: 62, + rot_correction: -0.027436902217920986, + vert_correction: -0.21457119711920336, + vert_offset_correction: 0.14769171, + } + - { + dist_correction: 1.4423058, + dist_correction_x: 1.4454633000000001, + dist_correction_y: 1.4321198000000002, + focal_distance: 9.0, + focal_slope: 1.45, + horiz_offset_correction: -0.025999999, + laser_id: 63, + rot_correction: 0.0290479702718764, + vert_correction: -0.2079266762969834, + vert_offset_correction: 0.14725984, } num_lasers: 64 distance_resolution: 0.002 diff --git a/nebula_decoders/calibration/velodyne/HDL64e_utexas.yaml b/nebula_decoders/calibration/velodyne/HDL64e_utexas.yaml index 3b474f225..6a6156031 100644 --- a/nebula_decoders/calibration/velodyne/HDL64e_utexas.yaml +++ b/nebula_decoders/calibration/velodyne/HDL64e_utexas.yaml @@ -1,900 +1,900 @@ # University of Texas HDL-64E calibration parameters lasers: - - { - dist_correction: 0.100000001490116, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 0, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0698131695389748, - two_pt_correction_available: false, - vert_correction: -0.124932751059532, - vert_offset_correction: 0, - } - - { - dist_correction: 0.280000001192093, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 1, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0392699092626572, - two_pt_correction_available: false, - vert_correction: -0.118993431329727, - vert_offset_correction: 0, - } - - { - dist_correction: 0.319999992847443, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 2, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0698131695389748, - two_pt_correction_available: false, - vert_correction: 0.0055470340885222, - vert_offset_correction: 0, - } - - { - dist_correction: 0.230000004172325, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 3, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.104719758033752, - two_pt_correction_available: false, - vert_correction: 0.0114863449707627, - vert_offset_correction: 0, - } - - { - dist_correction: 0.0700000002980232, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 4, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.013962633907795, - two_pt_correction_available: false, - vert_correction: -0.113056324422359, - vert_offset_correction: 0, - } - - { - dist_correction: 0.0900000035762787, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 5, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0392699092626572, - two_pt_correction_available: false, - vert_correction: -0.107121199369431, - vert_offset_correction: 0, - } - - { - dist_correction: 0.119999997317791, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 6, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0, - two_pt_correction_available: false, - vert_correction: -0.148716226220131, - vert_offset_correction: 0, - } - - { - dist_correction: 0.200000002980232, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 7, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0226892791688442, - two_pt_correction_available: false, - vert_correction: -0.142765983939171, - vert_offset_correction: 0, - } - - { - dist_correction: 0.129999995231628, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 8, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0820304751396179, - two_pt_correction_available: false, - vert_correction: -0.101187855005264, - vert_offset_correction: 0, - } - - { - dist_correction: 0.159999996423721, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 9, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.113446399569511, - two_pt_correction_available: false, - vert_correction: -0.0952560678124428, - vert_offset_correction: 0, - } - - { - dist_correction: 0.159999996423721, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 10, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0698131695389748, - two_pt_correction_available: false, - vert_correction: -0.136818811297417, - vert_offset_correction: 0, - } - - { - dist_correction: 0.159999996423721, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 11, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.104719758033752, - two_pt_correction_available: false, - vert_correction: -0.130874469876289, - vert_offset_correction: 0, - } - - { - dist_correction: 0.129999995231628, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 12, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.068067841231823, - two_pt_correction_available: false, - vert_correction: -0.05375986546278, - vert_offset_correction: 0, - } - - { - dist_correction: 0.200000002980232, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 13, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0349065847694874, - two_pt_correction_available: false, - vert_correction: -0.0478330813348293, - vert_offset_correction: 0, - } - - { - dist_correction: 0.170000001788139, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 14, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0837758108973503, - two_pt_correction_available: false, - vert_correction: -0.0893256440758705, - vert_offset_correction: 0, - } - - { - dist_correction: 0.239999994635582, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 15, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0479965545237064, - two_pt_correction_available: false, - vert_correction: -0.0833963677287102, - vert_offset_correction: 0, - } - - { - dist_correction: 0.180000007152557, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 16, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.00872664619237185, - two_pt_correction_available: false, - vert_correction: -0.0419059917330742, - vert_offset_correction: 0, - } - - { - dist_correction: 0.0599999986588955, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 17, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0392699092626572, - two_pt_correction_available: false, - vert_correction: -0.035978376865387, - vert_offset_correction: 0, - } - - { - dist_correction: 0.140000000596046, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 18, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0, - two_pt_correction_available: false, - vert_correction: -0.0774680152535439, - vert_offset_correction: 0, - } - - { - dist_correction: 0.150000005960464, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 19, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0305432621389627, - two_pt_correction_available: false, - vert_correction: -0.0715404152870178, - vert_offset_correction: 0, - } - - { - dist_correction: 0.219999998807907, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 20, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0855211317539215, - two_pt_correction_available: false, - vert_correction: -0.0300500374287367, - vert_offset_correction: 0, - } - - { - dist_correction: 0.140000000596046, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 21, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.109083078801632, - two_pt_correction_available: false, - vert_correction: -0.024120757356286, - vert_offset_correction: 0, - } - - { - dist_correction: 0.0799999982118607, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 22, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0698131695389748, - two_pt_correction_available: false, - vert_correction: -0.0656133219599724, - vert_offset_correction: 0, - } - - { - dist_correction: 0.159999996423721, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 23, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.104719758033752, - two_pt_correction_available: false, - vert_correction: -0.0596865378320217, - vert_offset_correction: 0, - } - - { - dist_correction: 0.119999997317791, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 24, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0610865242779255, - two_pt_correction_available: false, - vert_correction: 0.0174280721694231, - vert_offset_correction: 0, - } - - { - dist_correction: 0.219999998807907, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 25, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0349065847694874, - two_pt_correction_available: false, - vert_correction: 0.0233724191784859, - vert_offset_correction: 0, - } - - { - dist_correction: 0.159999996423721, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 26, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0785398185253143, - two_pt_correction_available: false, - vert_correction: -0.0181903336197138, - vert_offset_correction: 0, - } - - { - dist_correction: 0.259999990463257, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 27, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0471238903701305, - two_pt_correction_available: false, - vert_correction: -0.0122585473582149, - vert_offset_correction: 0, - } - - { - dist_correction: 0.140000000596046, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 28, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0104719763621688, - two_pt_correction_available: false, - vert_correction: 0.0293195936828852, - vert_offset_correction: 0, - } - - { - dist_correction: 0.219999998807907, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 29, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0349065847694874, - two_pt_correction_available: false, - vert_correction: 0.0352698266506195, - vert_offset_correction: 0, - } - - { - dist_correction: 0.209999993443489, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 30, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.00436332309618592, - two_pt_correction_available: false, - vert_correction: -0.00632520206272602, - vert_offset_correction: 0, - } - - { - dist_correction: 0.140000000596046, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 31, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0296705979853868, - two_pt_correction_available: false, - vert_correction: -0.000390077300835401, - vert_offset_correction: 0, - } - - { - dist_correction: 0.119999997317791, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 32, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.122173048555851, - two_pt_correction_available: false, - vert_correction: -0.396850973367691, - vert_offset_correction: 0, - } - - { - dist_correction: 0.0199999995529652, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 33, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0610865242779255, - two_pt_correction_available: false, - vert_correction: -0.387918144464493, - vert_offset_correction: 0, - } - - { - dist_correction: 0.100000001490116, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 34, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0959931090474129, - two_pt_correction_available: false, - vert_correction: -0.200955957174301, - vert_offset_correction: 0, - } - - { - dist_correction: 0.230000004172325, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 35, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.148352980613708, - two_pt_correction_available: false, - vert_correction: -0.192023113369942, - vert_offset_correction: 0, - } - - { - dist_correction: 0.170000001788139, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 36, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.00872664619237185, - two_pt_correction_available: false, - vert_correction: -0.378992766141891, - vert_offset_correction: 0, - } - - { - dist_correction: 0.150000005960464, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 37, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0610865242779255, - two_pt_correction_available: false, - vert_correction: -0.370074152946472, - vert_offset_correction: 0, - } - - { - dist_correction: 0.0500000007450581, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 38, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0174532923847437, - two_pt_correction_available: false, - vert_correction: -0.43128889799118, - vert_offset_correction: 0, - } - - { - dist_correction: 0.270000010728836, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 39, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0349065847694874, - two_pt_correction_available: false, - vert_correction: -0.423701733350754, - vert_offset_correction: 0, - } - - { - dist_correction: 0.180000007152557, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 40, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.122173048555851, - two_pt_correction_available: false, - vert_correction: -0.361161530017853, - vert_offset_correction: 0, - } - - { - dist_correction: 0.189999997615814, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 41, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.174532920122147, - two_pt_correction_available: false, - vert_correction: -0.352254241704941, - vert_offset_correction: 0, - } - - { - dist_correction: 0.100000001490116, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 42, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.104719758033752, - two_pt_correction_available: false, - vert_correction: -0.414742022752762, - vert_offset_correction: 0, - } - - { - dist_correction: 0.200000002980232, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 43, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.165806278586388, - two_pt_correction_available: false, - vert_correction: -0.405792057514191, - vert_offset_correction: 0, - } - - { - dist_correction: 0.219999998807907, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 44, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.113446399569511, - two_pt_correction_available: false, - vert_correction: -0.28999200463295, - vert_offset_correction: 0, - } - - { - dist_correction: 0.200000002980232, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 45, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0567231997847557, - two_pt_correction_available: false, - vert_correction: -0.281101644039154, - vert_offset_correction: 0, - } - - { - dist_correction: 0.150000005960464, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 46, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.13962633907795, - two_pt_correction_available: false, - vert_correction: -0.343351542949677, - vert_offset_correction: 0, - } - - { - dist_correction: 0.219999998807907, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 47, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0872664600610733, - two_pt_correction_available: false, - vert_correction: -0.334452718496323, - vert_offset_correction: 0, - } - - { - dist_correction: 0.0599999986588955, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 48, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0, - two_pt_correction_available: false, - vert_correction: -0.272210210561752, - vert_offset_correction: 0, - } - - { - dist_correction: 0.129999995231628, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 49, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0558505356311798, - two_pt_correction_available: false, - vert_correction: -0.26331701874733, - vert_offset_correction: 0, - } - - { - dist_correction: 0.00999999977648258, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 50, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0174532923847437, - two_pt_correction_available: false, - vert_correction: -0.323895305395126, - vert_offset_correction: 0, - } - - { - dist_correction: 0.150000005960464, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 51, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0349065847694874, - two_pt_correction_available: false, - vert_correction: -0.316663861274719, - vert_offset_correction: 0, - } - - { - dist_correction: 0.200000002980232, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 52, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.122173048555851, - two_pt_correction_available: false, - vert_correction: -0.254421383142471, - vert_offset_correction: 0, - } - - { - dist_correction: 0.239999994635582, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 53, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.165806278586388, - two_pt_correction_available: false, - vert_correction: -0.245522528886795, - vert_offset_correction: 0, - } - - { - dist_correction: 0.180000007152557, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 54, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.104719758033752, - two_pt_correction_available: false, - vert_correction: -0.307772427797318, - vert_offset_correction: 0, - } - - { - dist_correction: 0.219999998807907, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 55, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.157079637050629, - two_pt_correction_available: false, - vert_correction: -0.298882067203522, - vert_offset_correction: 0, - } - - { - dist_correction: 0.140000000596046, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 56, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.104719758033752, - two_pt_correction_available: false, - vert_correction: -0.183082059025764, - vert_offset_correction: 0, - } - - { - dist_correction: 0.319999992847443, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 57, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0610865242779255, - two_pt_correction_available: false, - vert_correction: -0.17413204908371, - vert_offset_correction: 0, - } - - { - dist_correction: 0.200000002980232, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 58, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.13962633907795, - two_pt_correction_available: false, - vert_correction: -0.236619830131531, - vert_offset_correction: 0, - } - - { - dist_correction: 0.25, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 59, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0785398185253143, - two_pt_correction_available: false, - vert_correction: -0.22771255671978, - vert_offset_correction: 0, - } - - { - dist_correction: 0.170000001788139, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 60, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0, - two_pt_correction_available: false, - vert_correction: -0.16517236828804, - vert_offset_correction: 0, - } - - { - dist_correction: 0.230000004172325, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 61, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0436332300305367, - two_pt_correction_available: false, - vert_correction: -0.156202226877213, - vert_offset_correction: 0, - } - - { - dist_correction: 0.150000005960464, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 62, - max_intensity: 255, - min_intensity: 0, - rot_correction: -0.0174532923847437, - two_pt_correction_available: false, - vert_correction: -0.218799933791161, - vert_offset_correction: 0, - } - - { - dist_correction: 0.230000004172325, - dist_correction_x: 0, - dist_correction_y: 0, - focal_distance: 0, - focal_slope: 0, - laser_id: 63, - max_intensity: 255, - min_intensity: 0, - rot_correction: 0.0314159244298935, - two_pt_correction_available: false, - vert_correction: -0.209881335496902, - vert_offset_correction: 0, + - { + dist_correction: 0.100000001490116, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 0, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0698131695389748, + two_pt_correction_available: false, + vert_correction: -0.124932751059532, + vert_offset_correction: 0, + } + - { + dist_correction: 0.280000001192093, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 1, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0392699092626572, + two_pt_correction_available: false, + vert_correction: -0.118993431329727, + vert_offset_correction: 0, + } + - { + dist_correction: 0.319999992847443, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 2, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0698131695389748, + two_pt_correction_available: false, + vert_correction: 0.0055470340885222, + vert_offset_correction: 0, + } + - { + dist_correction: 0.230000004172325, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 3, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.104719758033752, + two_pt_correction_available: false, + vert_correction: 0.0114863449707627, + vert_offset_correction: 0, + } + - { + dist_correction: 0.0700000002980232, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 4, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.013962633907795, + two_pt_correction_available: false, + vert_correction: -0.113056324422359, + vert_offset_correction: 0, + } + - { + dist_correction: 0.0900000035762787, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 5, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0392699092626572, + two_pt_correction_available: false, + vert_correction: -0.107121199369431, + vert_offset_correction: 0, + } + - { + dist_correction: 0.119999997317791, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 6, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0, + two_pt_correction_available: false, + vert_correction: -0.148716226220131, + vert_offset_correction: 0, + } + - { + dist_correction: 0.200000002980232, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 7, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0226892791688442, + two_pt_correction_available: false, + vert_correction: -0.142765983939171, + vert_offset_correction: 0, + } + - { + dist_correction: 0.129999995231628, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 8, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0820304751396179, + two_pt_correction_available: false, + vert_correction: -0.101187855005264, + vert_offset_correction: 0, + } + - { + dist_correction: 0.159999996423721, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 9, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.113446399569511, + two_pt_correction_available: false, + vert_correction: -0.0952560678124428, + vert_offset_correction: 0, + } + - { + dist_correction: 0.159999996423721, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 10, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0698131695389748, + two_pt_correction_available: false, + vert_correction: -0.136818811297417, + vert_offset_correction: 0, + } + - { + dist_correction: 0.159999996423721, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 11, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.104719758033752, + two_pt_correction_available: false, + vert_correction: -0.130874469876289, + vert_offset_correction: 0, + } + - { + dist_correction: 0.129999995231628, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 12, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.068067841231823, + two_pt_correction_available: false, + vert_correction: -0.05375986546278, + vert_offset_correction: 0, + } + - { + dist_correction: 0.200000002980232, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 13, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0349065847694874, + two_pt_correction_available: false, + vert_correction: -0.0478330813348293, + vert_offset_correction: 0, + } + - { + dist_correction: 0.170000001788139, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 14, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0837758108973503, + two_pt_correction_available: false, + vert_correction: -0.0893256440758705, + vert_offset_correction: 0, + } + - { + dist_correction: 0.239999994635582, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 15, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0479965545237064, + two_pt_correction_available: false, + vert_correction: -0.0833963677287102, + vert_offset_correction: 0, + } + - { + dist_correction: 0.180000007152557, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 16, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.00872664619237185, + two_pt_correction_available: false, + vert_correction: -0.0419059917330742, + vert_offset_correction: 0, + } + - { + dist_correction: 0.0599999986588955, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 17, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0392699092626572, + two_pt_correction_available: false, + vert_correction: -0.035978376865387, + vert_offset_correction: 0, + } + - { + dist_correction: 0.140000000596046, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 18, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0, + two_pt_correction_available: false, + vert_correction: -0.0774680152535439, + vert_offset_correction: 0, + } + - { + dist_correction: 0.150000005960464, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 19, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0305432621389627, + two_pt_correction_available: false, + vert_correction: -0.0715404152870178, + vert_offset_correction: 0, + } + - { + dist_correction: 0.219999998807907, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 20, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0855211317539215, + two_pt_correction_available: false, + vert_correction: -0.0300500374287367, + vert_offset_correction: 0, + } + - { + dist_correction: 0.140000000596046, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 21, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.109083078801632, + two_pt_correction_available: false, + vert_correction: -0.024120757356286, + vert_offset_correction: 0, + } + - { + dist_correction: 0.0799999982118607, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 22, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0698131695389748, + two_pt_correction_available: false, + vert_correction: -0.0656133219599724, + vert_offset_correction: 0, + } + - { + dist_correction: 0.159999996423721, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 23, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.104719758033752, + two_pt_correction_available: false, + vert_correction: -0.0596865378320217, + vert_offset_correction: 0, + } + - { + dist_correction: 0.119999997317791, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 24, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0610865242779255, + two_pt_correction_available: false, + vert_correction: 0.0174280721694231, + vert_offset_correction: 0, + } + - { + dist_correction: 0.219999998807907, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 25, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0349065847694874, + two_pt_correction_available: false, + vert_correction: 0.0233724191784859, + vert_offset_correction: 0, + } + - { + dist_correction: 0.159999996423721, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 26, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0785398185253143, + two_pt_correction_available: false, + vert_correction: -0.0181903336197138, + vert_offset_correction: 0, + } + - { + dist_correction: 0.259999990463257, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 27, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0471238903701305, + two_pt_correction_available: false, + vert_correction: -0.0122585473582149, + vert_offset_correction: 0, + } + - { + dist_correction: 0.140000000596046, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 28, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0104719763621688, + two_pt_correction_available: false, + vert_correction: 0.0293195936828852, + vert_offset_correction: 0, + } + - { + dist_correction: 0.219999998807907, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 29, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0349065847694874, + two_pt_correction_available: false, + vert_correction: 0.0352698266506195, + vert_offset_correction: 0, + } + - { + dist_correction: 0.209999993443489, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 30, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.00436332309618592, + two_pt_correction_available: false, + vert_correction: -0.00632520206272602, + vert_offset_correction: 0, + } + - { + dist_correction: 0.140000000596046, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 31, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0296705979853868, + two_pt_correction_available: false, + vert_correction: -0.000390077300835401, + vert_offset_correction: 0, + } + - { + dist_correction: 0.119999997317791, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 32, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.122173048555851, + two_pt_correction_available: false, + vert_correction: -0.396850973367691, + vert_offset_correction: 0, + } + - { + dist_correction: 0.0199999995529652, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 33, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0610865242779255, + two_pt_correction_available: false, + vert_correction: -0.387918144464493, + vert_offset_correction: 0, + } + - { + dist_correction: 0.100000001490116, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 34, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0959931090474129, + two_pt_correction_available: false, + vert_correction: -0.200955957174301, + vert_offset_correction: 0, + } + - { + dist_correction: 0.230000004172325, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 35, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.148352980613708, + two_pt_correction_available: false, + vert_correction: -0.192023113369942, + vert_offset_correction: 0, + } + - { + dist_correction: 0.170000001788139, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 36, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.00872664619237185, + two_pt_correction_available: false, + vert_correction: -0.378992766141891, + vert_offset_correction: 0, + } + - { + dist_correction: 0.150000005960464, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 37, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0610865242779255, + two_pt_correction_available: false, + vert_correction: -0.370074152946472, + vert_offset_correction: 0, + } + - { + dist_correction: 0.0500000007450581, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 38, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0174532923847437, + two_pt_correction_available: false, + vert_correction: -0.43128889799118, + vert_offset_correction: 0, + } + - { + dist_correction: 0.270000010728836, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 39, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0349065847694874, + two_pt_correction_available: false, + vert_correction: -0.423701733350754, + vert_offset_correction: 0, + } + - { + dist_correction: 0.180000007152557, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 40, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.122173048555851, + two_pt_correction_available: false, + vert_correction: -0.361161530017853, + vert_offset_correction: 0, + } + - { + dist_correction: 0.189999997615814, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 41, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.174532920122147, + two_pt_correction_available: false, + vert_correction: -0.352254241704941, + vert_offset_correction: 0, + } + - { + dist_correction: 0.100000001490116, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 42, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.104719758033752, + two_pt_correction_available: false, + vert_correction: -0.414742022752762, + vert_offset_correction: 0, + } + - { + dist_correction: 0.200000002980232, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 43, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.165806278586388, + two_pt_correction_available: false, + vert_correction: -0.405792057514191, + vert_offset_correction: 0, + } + - { + dist_correction: 0.219999998807907, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 44, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.113446399569511, + two_pt_correction_available: false, + vert_correction: -0.28999200463295, + vert_offset_correction: 0, + } + - { + dist_correction: 0.200000002980232, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 45, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0567231997847557, + two_pt_correction_available: false, + vert_correction: -0.281101644039154, + vert_offset_correction: 0, + } + - { + dist_correction: 0.150000005960464, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 46, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.13962633907795, + two_pt_correction_available: false, + vert_correction: -0.343351542949677, + vert_offset_correction: 0, + } + - { + dist_correction: 0.219999998807907, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 47, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0872664600610733, + two_pt_correction_available: false, + vert_correction: -0.334452718496323, + vert_offset_correction: 0, + } + - { + dist_correction: 0.0599999986588955, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 48, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0, + two_pt_correction_available: false, + vert_correction: -0.272210210561752, + vert_offset_correction: 0, + } + - { + dist_correction: 0.129999995231628, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 49, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0558505356311798, + two_pt_correction_available: false, + vert_correction: -0.26331701874733, + vert_offset_correction: 0, + } + - { + dist_correction: 0.00999999977648258, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 50, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0174532923847437, + two_pt_correction_available: false, + vert_correction: -0.323895305395126, + vert_offset_correction: 0, + } + - { + dist_correction: 0.150000005960464, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 51, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0349065847694874, + two_pt_correction_available: false, + vert_correction: -0.316663861274719, + vert_offset_correction: 0, + } + - { + dist_correction: 0.200000002980232, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 52, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.122173048555851, + two_pt_correction_available: false, + vert_correction: -0.254421383142471, + vert_offset_correction: 0, + } + - { + dist_correction: 0.239999994635582, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 53, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.165806278586388, + two_pt_correction_available: false, + vert_correction: -0.245522528886795, + vert_offset_correction: 0, + } + - { + dist_correction: 0.180000007152557, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 54, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.104719758033752, + two_pt_correction_available: false, + vert_correction: -0.307772427797318, + vert_offset_correction: 0, + } + - { + dist_correction: 0.219999998807907, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 55, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.157079637050629, + two_pt_correction_available: false, + vert_correction: -0.298882067203522, + vert_offset_correction: 0, + } + - { + dist_correction: 0.140000000596046, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 56, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.104719758033752, + two_pt_correction_available: false, + vert_correction: -0.183082059025764, + vert_offset_correction: 0, + } + - { + dist_correction: 0.319999992847443, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 57, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0610865242779255, + two_pt_correction_available: false, + vert_correction: -0.17413204908371, + vert_offset_correction: 0, + } + - { + dist_correction: 0.200000002980232, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 58, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.13962633907795, + two_pt_correction_available: false, + vert_correction: -0.236619830131531, + vert_offset_correction: 0, + } + - { + dist_correction: 0.25, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 59, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0785398185253143, + two_pt_correction_available: false, + vert_correction: -0.22771255671978, + vert_offset_correction: 0, + } + - { + dist_correction: 0.170000001788139, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 60, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0, + two_pt_correction_available: false, + vert_correction: -0.16517236828804, + vert_offset_correction: 0, + } + - { + dist_correction: 0.230000004172325, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 61, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0436332300305367, + two_pt_correction_available: false, + vert_correction: -0.156202226877213, + vert_offset_correction: 0, + } + - { + dist_correction: 0.150000005960464, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 62, + max_intensity: 255, + min_intensity: 0, + rot_correction: -0.0174532923847437, + two_pt_correction_available: false, + vert_correction: -0.218799933791161, + vert_offset_correction: 0, + } + - { + dist_correction: 0.230000004172325, + dist_correction_x: 0, + dist_correction_y: 0, + focal_distance: 0, + focal_slope: 0, + laser_id: 63, + max_intensity: 255, + min_intensity: 0, + rot_correction: 0.0314159244298935, + two_pt_correction_available: false, + vert_correction: -0.209881335496902, + vert_offset_correction: 0, } num_lasers: 64 distance_resolution: 0.002 diff --git a/nebula_decoders/calibration/velodyne/VLP16.yaml b/nebula_decoders/calibration/velodyne/VLP16.yaml index b2e57adca..e545e40a0 100644 --- a/nebula_decoders/calibration/velodyne/VLP16.yaml +++ b/nebula_decoders/calibration/velodyne/VLP16.yaml @@ -1,195 +1,195 @@ lasers: - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 0, - rot_correction: 0.0, - vert_correction: -0.2617993877991494, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 1, - rot_correction: 0.0, - vert_correction: 0.017453292519943295, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 2, - rot_correction: 0.0, - vert_correction: -0.22689280275926285, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 3, - rot_correction: 0.0, - vert_correction: 0.05235987755982989, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 4, - rot_correction: 0.0, - vert_correction: -0.19198621771937624, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 5, - rot_correction: 0.0, - vert_correction: 0.08726646259971647, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 6, - rot_correction: 0.0, - vert_correction: -0.15707963267948966, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 7, - rot_correction: 0.0, - vert_correction: 0.12217304763960307, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 8, - rot_correction: 0.0, - vert_correction: -0.12217304763960307, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 9, - rot_correction: 0.0, - vert_correction: 0.15707963267948966, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 10, - rot_correction: 0.0, - vert_correction: -0.08726646259971647, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 11, - rot_correction: 0.0, - vert_correction: 0.19198621771937624, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 12, - rot_correction: 0.0, - vert_correction: -0.05235987755982989, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 13, - rot_correction: 0.0, - vert_correction: 0.22689280275926285, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 14, - rot_correction: 0.0, - vert_correction: -0.017453292519943295, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 15, - rot_correction: 0.0, - vert_correction: 0.2617993877991494, - vert_offset_correction: 0.0, + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 0, + rot_correction: 0.0, + vert_correction: -0.2617993877991494, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 1, + rot_correction: 0.0, + vert_correction: 0.017453292519943295, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 2, + rot_correction: 0.0, + vert_correction: -0.22689280275926285, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 3, + rot_correction: 0.0, + vert_correction: 0.05235987755982989, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 4, + rot_correction: 0.0, + vert_correction: -0.19198621771937624, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 5, + rot_correction: 0.0, + vert_correction: 0.08726646259971647, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 6, + rot_correction: 0.0, + vert_correction: -0.15707963267948966, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 7, + rot_correction: 0.0, + vert_correction: 0.12217304763960307, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 8, + rot_correction: 0.0, + vert_correction: -0.12217304763960307, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 9, + rot_correction: 0.0, + vert_correction: 0.15707963267948966, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 10, + rot_correction: 0.0, + vert_correction: -0.08726646259971647, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 11, + rot_correction: 0.0, + vert_correction: 0.19198621771937624, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 12, + rot_correction: 0.0, + vert_correction: -0.05235987755982989, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 13, + rot_correction: 0.0, + vert_correction: 0.22689280275926285, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 14, + rot_correction: 0.0, + vert_correction: -0.017453292519943295, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 15, + rot_correction: 0.0, + vert_correction: 0.2617993877991494, + vert_offset_correction: 0.0, } num_lasers: 16 distance_resolution: 0.002 diff --git a/nebula_decoders/calibration/velodyne/VLP16_hires.yaml b/nebula_decoders/calibration/velodyne/VLP16_hires.yaml index 1a9eb9b0a..9a8c97e63 100644 --- a/nebula_decoders/calibration/velodyne/VLP16_hires.yaml +++ b/nebula_decoders/calibration/velodyne/VLP16_hires.yaml @@ -1,195 +1,195 @@ lasers: - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 0, - rot_correction: 0.0, - vert_correction: -0.17453292519943295, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 1, - rot_correction: 0.0, - vert_correction: 0.011635528346628864, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 2, - rot_correction: 0.0, - vert_correction: -0.15126186850617523, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 3, - rot_correction: 0.0, - vert_correction: 0.03490658503988659, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 4, - rot_correction: 0.0, - vert_correction: -0.1279908118129175, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 5, - rot_correction: 0.0, - vert_correction: 0.05817764173314432, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 6, - rot_correction: 0.0, - vert_correction: -0.10471975511965977, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 7, - rot_correction: 0.0, - vert_correction: 0.08144869842640205, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 8, - rot_correction: 0.0, - vert_correction: -0.08144869842640205, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 9, - rot_correction: 0.0, - vert_correction: 0.10471975511965977, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 10, - rot_correction: 0.0, - vert_correction: -0.05817764173314432, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 11, - rot_correction: 0.0, - vert_correction: 0.1279908118129175, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 12, - rot_correction: 0.0, - vert_correction: -0.03490658503988659, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 13, - rot_correction: 0.0, - vert_correction: 0.15126186850617523, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 14, - rot_correction: 0.0, - vert_correction: -0.011635528346628864, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 15, - rot_correction: 0.0, - vert_correction: 0.17453292519943295, - vert_offset_correction: 0.0, + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 0, + rot_correction: 0.0, + vert_correction: -0.17453292519943295, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 1, + rot_correction: 0.0, + vert_correction: 0.011635528346628864, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 2, + rot_correction: 0.0, + vert_correction: -0.15126186850617523, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 3, + rot_correction: 0.0, + vert_correction: 0.03490658503988659, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 4, + rot_correction: 0.0, + vert_correction: -0.1279908118129175, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 5, + rot_correction: 0.0, + vert_correction: 0.05817764173314432, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 6, + rot_correction: 0.0, + vert_correction: -0.10471975511965977, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 7, + rot_correction: 0.0, + vert_correction: 0.08144869842640205, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 8, + rot_correction: 0.0, + vert_correction: -0.08144869842640205, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 9, + rot_correction: 0.0, + vert_correction: 0.10471975511965977, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 10, + rot_correction: 0.0, + vert_correction: -0.05817764173314432, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 11, + rot_correction: 0.0, + vert_correction: 0.1279908118129175, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 12, + rot_correction: 0.0, + vert_correction: -0.03490658503988659, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 13, + rot_correction: 0.0, + vert_correction: 0.15126186850617523, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 14, + rot_correction: 0.0, + vert_correction: -0.011635528346628864, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 15, + rot_correction: 0.0, + vert_correction: 0.17453292519943295, + vert_offset_correction: 0.0, } num_lasers: 16 distance_resolution: 0.002 diff --git a/nebula_decoders/calibration/velodyne/VLP32.yaml b/nebula_decoders/calibration/velodyne/VLP32.yaml index de3ddd829..32e7681dc 100644 --- a/nebula_decoders/calibration/velodyne/VLP32.yaml +++ b/nebula_decoders/calibration/velodyne/VLP32.yaml @@ -1,387 +1,387 @@ lasers: - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 0, - rot_correction: -0.024434609527920613, - vert_correction: -0.4363323129985824, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 1, - rot_correction: 0.07330382858376185, - vert_correction: -0.017453292519943295, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 2, - rot_correction: -0.024434609527920613, - vert_correction: -0.029094638630745476, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 3, - rot_correction: 0.024434609527920613, - vert_correction: -0.2729520417193932, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 4, - rot_correction: -0.024434609527920613, - vert_correction: -0.19739673840055869, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 5, - rot_correction: 0.024434609527920613, - vert_correction: 0.0, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 6, - rot_correction: -0.07330382858376185, - vert_correction: -0.011641346110802179, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 7, - rot_correction: 0.024434609527920613, - vert_correction: -0.15433946575385857, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 8, - rot_correction: -0.024434609527920613, - vert_correction: -0.12660618393966866, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 9, - rot_correction: 0.07330382858376185, - vert_correction: 0.005811946409141118, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 10, - rot_correction: -0.024434609527920613, - vert_correction: -0.005811946409141118, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 11, - rot_correction: 0.024434609527920613, - vert_correction: -0.10730284241261137, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 12, - rot_correction: -0.07330382858376185, - vert_correction: -0.0930784090088576, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 13, - rot_correction: 0.024434609527920613, - vert_correction: 0.02326523892908441, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 14, - rot_correction: -0.07330382858376185, - vert_correction: 0.011641346110802179, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 15, - rot_correction: 0.024434609527920613, - vert_correction: -0.06981317007977318, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 16, - rot_correction: -0.024434609527920613, - vert_correction: -0.08145451619057535, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 17, - rot_correction: 0.07330382858376185, - vert_correction: 0.029094638630745476, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 18, - rot_correction: -0.024434609527920613, - vert_correction: 0.017453292519943295, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 19, - rot_correction: 0.07330382858376185, - vert_correction: -0.06400122367063206, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 20, - rot_correction: -0.07330382858376185, - vert_correction: -0.058171823968971005, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 21, - rot_correction: 0.024434609527920613, - vert_correction: 0.058171823968971005, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 22, - rot_correction: -0.024434609527920613, - vert_correction: 0.04071853144902771, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 23, - rot_correction: 0.024434609527920613, - vert_correction: -0.04654793115068877, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 24, - rot_correction: -0.024434609527920613, - vert_correction: -0.05235987755982989, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 25, - rot_correction: 0.024434609527920613, - vert_correction: 0.12217304763960307, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 26, - rot_correction: -0.024434609527920613, - vert_correction: 0.08145451619057535, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 27, - rot_correction: 0.07330382858376185, - vert_correction: -0.04071853144902771, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 28, - rot_correction: -0.07330382858376185, - vert_correction: -0.03490658503988659, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 29, - rot_correction: 0.024434609527920613, - vert_correction: 0.2617993877991494, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 30, - rot_correction: -0.024434609527920613, - vert_correction: 0.18034487160857407, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 31, - rot_correction: 0.024434609527920613, - vert_correction: -0.02326523892908441, - vert_offset_correction: 0.0, + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 0, + rot_correction: -0.024434609527920613, + vert_correction: -0.4363323129985824, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 1, + rot_correction: 0.07330382858376185, + vert_correction: -0.017453292519943295, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 2, + rot_correction: -0.024434609527920613, + vert_correction: -0.029094638630745476, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 3, + rot_correction: 0.024434609527920613, + vert_correction: -0.2729520417193932, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 4, + rot_correction: -0.024434609527920613, + vert_correction: -0.19739673840055869, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 5, + rot_correction: 0.024434609527920613, + vert_correction: 0.0, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 6, + rot_correction: -0.07330382858376185, + vert_correction: -0.011641346110802179, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 7, + rot_correction: 0.024434609527920613, + vert_correction: -0.15433946575385857, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 8, + rot_correction: -0.024434609527920613, + vert_correction: -0.12660618393966866, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 9, + rot_correction: 0.07330382858376185, + vert_correction: 0.005811946409141118, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 10, + rot_correction: -0.024434609527920613, + vert_correction: -0.005811946409141118, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 11, + rot_correction: 0.024434609527920613, + vert_correction: -0.10730284241261137, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 12, + rot_correction: -0.07330382858376185, + vert_correction: -0.0930784090088576, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 13, + rot_correction: 0.024434609527920613, + vert_correction: 0.02326523892908441, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 14, + rot_correction: -0.07330382858376185, + vert_correction: 0.011641346110802179, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 15, + rot_correction: 0.024434609527920613, + vert_correction: -0.06981317007977318, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 16, + rot_correction: -0.024434609527920613, + vert_correction: -0.08145451619057535, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 17, + rot_correction: 0.07330382858376185, + vert_correction: 0.029094638630745476, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 18, + rot_correction: -0.024434609527920613, + vert_correction: 0.017453292519943295, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 19, + rot_correction: 0.07330382858376185, + vert_correction: -0.06400122367063206, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 20, + rot_correction: -0.07330382858376185, + vert_correction: -0.058171823968971005, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 21, + rot_correction: 0.024434609527920613, + vert_correction: 0.058171823968971005, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 22, + rot_correction: -0.024434609527920613, + vert_correction: 0.04071853144902771, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 23, + rot_correction: 0.024434609527920613, + vert_correction: -0.04654793115068877, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 24, + rot_correction: -0.024434609527920613, + vert_correction: -0.05235987755982989, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 25, + rot_correction: 0.024434609527920613, + vert_correction: 0.12217304763960307, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 26, + rot_correction: -0.024434609527920613, + vert_correction: 0.08145451619057535, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 27, + rot_correction: 0.07330382858376185, + vert_correction: -0.04071853144902771, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 28, + rot_correction: -0.07330382858376185, + vert_correction: -0.03490658503988659, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 29, + rot_correction: 0.024434609527920613, + vert_correction: 0.2617993877991494, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 30, + rot_correction: -0.024434609527920613, + vert_correction: 0.18034487160857407, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 31, + rot_correction: 0.024434609527920613, + vert_correction: -0.02326523892908441, + vert_offset_correction: 0.0, } num_lasers: 32 distance_resolution: 0.004 diff --git a/nebula_decoders/calibration/velodyne/VLP32MR.yaml b/nebula_decoders/calibration/velodyne/VLP32MR.yaml index de3ddd829..32e7681dc 100644 --- a/nebula_decoders/calibration/velodyne/VLP32MR.yaml +++ b/nebula_decoders/calibration/velodyne/VLP32MR.yaml @@ -1,387 +1,387 @@ lasers: - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 0, - rot_correction: -0.024434609527920613, - vert_correction: -0.4363323129985824, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 1, - rot_correction: 0.07330382858376185, - vert_correction: -0.017453292519943295, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 2, - rot_correction: -0.024434609527920613, - vert_correction: -0.029094638630745476, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 3, - rot_correction: 0.024434609527920613, - vert_correction: -0.2729520417193932, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 4, - rot_correction: -0.024434609527920613, - vert_correction: -0.19739673840055869, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 5, - rot_correction: 0.024434609527920613, - vert_correction: 0.0, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 6, - rot_correction: -0.07330382858376185, - vert_correction: -0.011641346110802179, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 7, - rot_correction: 0.024434609527920613, - vert_correction: -0.15433946575385857, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 8, - rot_correction: -0.024434609527920613, - vert_correction: -0.12660618393966866, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 9, - rot_correction: 0.07330382858376185, - vert_correction: 0.005811946409141118, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 10, - rot_correction: -0.024434609527920613, - vert_correction: -0.005811946409141118, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 11, - rot_correction: 0.024434609527920613, - vert_correction: -0.10730284241261137, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 12, - rot_correction: -0.07330382858376185, - vert_correction: -0.0930784090088576, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 13, - rot_correction: 0.024434609527920613, - vert_correction: 0.02326523892908441, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 14, - rot_correction: -0.07330382858376185, - vert_correction: 0.011641346110802179, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 15, - rot_correction: 0.024434609527920613, - vert_correction: -0.06981317007977318, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 16, - rot_correction: -0.024434609527920613, - vert_correction: -0.08145451619057535, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 17, - rot_correction: 0.07330382858376185, - vert_correction: 0.029094638630745476, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 18, - rot_correction: -0.024434609527920613, - vert_correction: 0.017453292519943295, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 19, - rot_correction: 0.07330382858376185, - vert_correction: -0.06400122367063206, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 20, - rot_correction: -0.07330382858376185, - vert_correction: -0.058171823968971005, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 21, - rot_correction: 0.024434609527920613, - vert_correction: 0.058171823968971005, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 22, - rot_correction: -0.024434609527920613, - vert_correction: 0.04071853144902771, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 23, - rot_correction: 0.024434609527920613, - vert_correction: -0.04654793115068877, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 24, - rot_correction: -0.024434609527920613, - vert_correction: -0.05235987755982989, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 25, - rot_correction: 0.024434609527920613, - vert_correction: 0.12217304763960307, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 26, - rot_correction: -0.024434609527920613, - vert_correction: 0.08145451619057535, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 27, - rot_correction: 0.07330382858376185, - vert_correction: -0.04071853144902771, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 28, - rot_correction: -0.07330382858376185, - vert_correction: -0.03490658503988659, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 29, - rot_correction: 0.024434609527920613, - vert_correction: 0.2617993877991494, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 30, - rot_correction: -0.024434609527920613, - vert_correction: 0.18034487160857407, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - horiz_offset_correction: 0.0, - laser_id: 31, - rot_correction: 0.024434609527920613, - vert_correction: -0.02326523892908441, - vert_offset_correction: 0.0, + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 0, + rot_correction: -0.024434609527920613, + vert_correction: -0.4363323129985824, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 1, + rot_correction: 0.07330382858376185, + vert_correction: -0.017453292519943295, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 2, + rot_correction: -0.024434609527920613, + vert_correction: -0.029094638630745476, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 3, + rot_correction: 0.024434609527920613, + vert_correction: -0.2729520417193932, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 4, + rot_correction: -0.024434609527920613, + vert_correction: -0.19739673840055869, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 5, + rot_correction: 0.024434609527920613, + vert_correction: 0.0, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 6, + rot_correction: -0.07330382858376185, + vert_correction: -0.011641346110802179, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 7, + rot_correction: 0.024434609527920613, + vert_correction: -0.15433946575385857, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 8, + rot_correction: -0.024434609527920613, + vert_correction: -0.12660618393966866, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 9, + rot_correction: 0.07330382858376185, + vert_correction: 0.005811946409141118, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 10, + rot_correction: -0.024434609527920613, + vert_correction: -0.005811946409141118, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 11, + rot_correction: 0.024434609527920613, + vert_correction: -0.10730284241261137, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 12, + rot_correction: -0.07330382858376185, + vert_correction: -0.0930784090088576, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 13, + rot_correction: 0.024434609527920613, + vert_correction: 0.02326523892908441, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 14, + rot_correction: -0.07330382858376185, + vert_correction: 0.011641346110802179, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 15, + rot_correction: 0.024434609527920613, + vert_correction: -0.06981317007977318, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 16, + rot_correction: -0.024434609527920613, + vert_correction: -0.08145451619057535, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 17, + rot_correction: 0.07330382858376185, + vert_correction: 0.029094638630745476, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 18, + rot_correction: -0.024434609527920613, + vert_correction: 0.017453292519943295, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 19, + rot_correction: 0.07330382858376185, + vert_correction: -0.06400122367063206, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 20, + rot_correction: -0.07330382858376185, + vert_correction: -0.058171823968971005, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 21, + rot_correction: 0.024434609527920613, + vert_correction: 0.058171823968971005, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 22, + rot_correction: -0.024434609527920613, + vert_correction: 0.04071853144902771, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 23, + rot_correction: 0.024434609527920613, + vert_correction: -0.04654793115068877, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 24, + rot_correction: -0.024434609527920613, + vert_correction: -0.05235987755982989, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 25, + rot_correction: 0.024434609527920613, + vert_correction: 0.12217304763960307, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 26, + rot_correction: -0.024434609527920613, + vert_correction: 0.08145451619057535, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 27, + rot_correction: 0.07330382858376185, + vert_correction: -0.04071853144902771, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 28, + rot_correction: -0.07330382858376185, + vert_correction: -0.03490658503988659, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 29, + rot_correction: 0.024434609527920613, + vert_correction: 0.2617993877991494, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 30, + rot_correction: -0.024434609527920613, + vert_correction: 0.18034487160857407, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + horiz_offset_correction: 0.0, + laser_id: 31, + rot_correction: 0.024434609527920613, + vert_correction: -0.02326523892908441, + vert_offset_correction: 0.0, } num_lasers: 32 distance_resolution: 0.004 diff --git a/nebula_decoders/calibration/velodyne/VLS128.yaml b/nebula_decoders/calibration/velodyne/VLS128.yaml index dc7057b83..8c783c4a2 100644 --- a/nebula_decoders/calibration/velodyne/VLS128.yaml +++ b/nebula_decoders/calibration/velodyne/VLS128.yaml @@ -1,1411 +1,1411 @@ lasers: - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 0, - rot_correction: -0.1108982206717197, - vert_correction: -0.2049365607691742, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 1, - rot_correction: -0.07937757438070212, - vert_correction: -0.034732052114687155, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 2, - rot_correction: -0.04768239516448509, - vert_correction: 0.059341194567807204, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 3, - rot_correction: -0.015899949485668342, - vert_correction: -0.09232791743050003, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 4, - rot_correction: 0.015899949485668342, - vert_correction: -0.013613568165555772, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 5, - rot_correction: 0.04768239516448509, - vert_correction: 0.0804596785169386, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 6, - rot_correction: 0.07937757438070212, - vert_correction: -0.07120943348136864, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 7, - rot_correction: 0.1108982206717197, - vert_correction: 0.022863813201125717, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 8, - rot_correction: -0.1108982206717197, - vert_correction: -0.11344640137963143, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 9, - rot_correction: -0.07937757438070212, - vert_correction: -0.01937315469713706, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 10, - rot_correction: -0.04768239516448509, - vert_correction: 0.0747000919853573, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 11, - rot_correction: -0.015899949485668342, - vert_correction: -0.07696902001294993, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 12, - rot_correction: 0.015899949485668342, - vert_correction: 0.0017453292519943296, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 13, - rot_correction: 0.04768239516448509, - vert_correction: 0.11309733552923257, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 14, - rot_correction: 0.07937757438070212, - vert_correction: -0.05585053606381855, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 15, - rot_correction: 0.1108982206717197, - vert_correction: 0.03822271061867582, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 16, - rot_correction: -0.1108982206717197, - vert_correction: -0.06736970912698112, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 17, - rot_correction: -0.07937757438070212, - vert_correction: 0.026703537555513242, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 18, - rot_correction: -0.04768239516448509, - vert_correction: -0.16133823605435582, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 19, - rot_correction: -0.015899949485668342, - vert_correction: -0.030892327760299633, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 20, - rot_correction: 0.015899949485668342, - vert_correction: 0.04782202150464463, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 21, - rot_correction: 0.04768239516448509, - vert_correction: -0.10384709049366261, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 22, - rot_correction: 0.07937757438070212, - vert_correction: -0.009773843811168246, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 23, - rot_correction: 0.1108982206717197, - vert_correction: 0.08429940287132612, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 24, - rot_correction: -0.1108982206717197, - vert_correction: -0.05201081170943102, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 25, - rot_correction: -0.07937757438070212, - vert_correction: 0.04206243497306335, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 26, - rot_correction: -0.04768239516448509, - vert_correction: -0.1096066770252439, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 27, - rot_correction: -0.015899949485668342, - vert_correction: -0.015533430342749533, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 28, - rot_correction: 0.015899949485668342, - vert_correction: 0.06318091892219473, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 29, - rot_correction: 0.04768239516448509, - vert_correction: -0.08848819307611251, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 30, - rot_correction: 0.07937757438070212, - vert_correction: 0.005585053606381855, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 31, - rot_correction: 0.1108982206717197, - vert_correction: 0.1322959573011702, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 32, - rot_correction: -0.1108982206717197, - vert_correction: -0.005934119456780721, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 33, - rot_correction: -0.07937757438070212, - vert_correction: 0.09040805525330627, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 34, - rot_correction: -0.04768239516448509, - vert_correction: -0.0635299847725936, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 35, - rot_correction: -0.015899949485668342, - vert_correction: 0.030543261909900768, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 36, - rot_correction: 0.015899949485668342, - vert_correction: -0.4363323129985824, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 37, - rot_correction: 0.04768239516448509, - vert_correction: -0.04241150082346221, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 38, - rot_correction: 0.07937757438070212, - vert_correction: 0.05166174585903215, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 39, - rot_correction: 0.1108982206717197, - vert_correction: -0.10000736613927509, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 40, - rot_correction: -0.1108982206717197, - vert_correction: 0.00942477796076938, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 41, - rot_correction: -0.07937757438070212, - vert_correction: 0.16929693744344995, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 42, - rot_correction: -0.04768239516448509, - vert_correction: -0.04817108735504349, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 43, - rot_correction: -0.015899949485668342, - vert_correction: 0.04590215932745086, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 44, - rot_correction: 0.015899949485668342, - vert_correction: -0.13351768777756623, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 45, - rot_correction: 0.04768239516448509, - vert_correction: -0.027052603405912107, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 46, - rot_correction: 0.07937757438070212, - vert_correction: 0.06702064327658225, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 47, - rot_correction: 0.1108982206717197, - vert_correction: -0.08464846872172498, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 48, - rot_correction: -0.1108982206717197, - vert_correction: 0.05550147021341968, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 49, - rot_correction: -0.07937757438070212, - vert_correction: -0.09616764178488756, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 50, - rot_correction: -0.04768239516448509, - vert_correction: -0.0020943951023931952, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 51, - rot_correction: -0.015899949485668342, - vert_correction: 0.10000736613927509, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 52, - rot_correction: 0.015899949485668342, - vert_correction: -0.07504915783575616, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 53, - rot_correction: 0.04768239516448509, - vert_correction: 0.019024088846738195, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 54, - rot_correction: 0.07937757438070212, - vert_correction: -0.2799857186049304, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 55, - rot_correction: 0.1108982206717197, - vert_correction: -0.038571776469074684, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 56, - rot_correction: -0.1108982206717197, - vert_correction: 0.07086036763096977, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 57, - rot_correction: -0.07937757438070212, - vert_correction: -0.08080874436733745, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 58, - rot_correction: -0.04768239516448509, - vert_correction: 0.013264502315156905, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 59, - rot_correction: -0.015899949485668342, - vert_correction: 0.2617993877991494, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 60, - rot_correction: 0.015899949485668342, - vert_correction: -0.05969026041820607, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 61, - rot_correction: 0.04768239516448509, - vert_correction: 0.03438298626428829, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 62, - rot_correction: 0.07937757438070212, - vert_correction: -0.11955505376161157, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 63, - rot_correction: 0.1108982206717197, - vert_correction: -0.023212879051524585, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 64, - rot_correction: -0.1108982206717197, - vert_correction: -0.09808750396208132, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 65, - rot_correction: -0.07937757438070212, - vert_correction: -0.004014257279586958, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 66, - rot_correction: -0.04768239516448509, - vert_correction: 0.0947713783832921, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 67, - rot_correction: -0.015899949485668342, - vert_correction: -0.06161012259539983, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 68, - rot_correction: 0.015899949485668342, - vert_correction: 0.01710422666954443, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 69, - rot_correction: 0.04768239516448509, - vert_correction: -0.34177037412552963, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 70, - rot_correction: 0.07937757438070212, - vert_correction: -0.040491638646268445, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 71, - rot_correction: 0.1108982206717197, - vert_correction: 0.053581608036225914, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 72, - rot_correction: -0.1108982206717197, - vert_correction: -0.08272860654453122, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 73, - rot_correction: -0.07937757438070212, - vert_correction: 0.011344640137963142, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 74, - rot_correction: -0.04768239516448509, - vert_correction: 0.20507618710933373, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 75, - rot_correction: -0.015899949485668342, - vert_correction: -0.046251225177849735, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 76, - rot_correction: 0.015899949485668342, - vert_correction: 0.03246312408709453, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 77, - rot_correction: 0.04768239516448509, - vert_correction: -0.12479104151759457, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 78, - rot_correction: 0.07937757438070212, - vert_correction: -0.025132741228718343, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 79, - rot_correction: 0.1108982206717197, - vert_correction: 0.06894050545377602, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 80, - rot_correction: -0.1108982206717197, - vert_correction: -0.03665191429188092, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 81, - rot_correction: -0.07937757438070212, - vert_correction: 0.05742133239061344, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 82, - rot_correction: -0.04768239516448509, - vert_correction: -0.0942477796076938, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 83, - rot_correction: -0.015899949485668342, - vert_correction: -0.00017453292519943296, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 84, - rot_correction: 0.015899949485668342, - vert_correction: 0.07853981633974483, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 85, - rot_correction: 0.04768239516448509, - vert_correction: -0.07312929565856241, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 86, - rot_correction: 0.07937757438070212, - vert_correction: 0.020943951023931952, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 87, - rot_correction: 0.1108982206717197, - vert_correction: -0.23675391303303078, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 88, - rot_correction: -0.1108982206717197, - vert_correction: -0.02129301687433082, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 89, - rot_correction: -0.07937757438070212, - vert_correction: 0.07278022980816354, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 90, - rot_correction: -0.04768239516448509, - vert_correction: -0.07888888219014369, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 91, - rot_correction: -0.015899949485668342, - vert_correction: 0.015184364492350668, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 92, - rot_correction: 0.015899949485668342, - vert_correction: 0.10611601852125524, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 93, - rot_correction: 0.04768239516448509, - vert_correction: -0.05777039824101231, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 94, - rot_correction: 0.07937757438070212, - vert_correction: 0.03630284844148206, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 95, - rot_correction: 0.1108982206717197, - vert_correction: -0.11606439525762292, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 96, - rot_correction: -0.1108982206717197, - vert_correction: 0.024783675378319478, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 97, - rot_correction: -0.07937757438070212, - vert_correction: -0.18057176441133332, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 98, - rot_correction: -0.04768239516448509, - vert_correction: -0.032812189937493394, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 99, - rot_correction: -0.015899949485668342, - vert_correction: 0.061261056745000965, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 100, - rot_correction: 0.015899949485668342, - vert_correction: -0.10576695267085637, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 101, - rot_correction: 0.04768239516448509, - vert_correction: -0.011693705988362009, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 102, - rot_correction: 0.07937757438070212, - vert_correction: 0.08237954069413235, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 103, - rot_correction: 0.1108982206717197, - vert_correction: -0.06928957130417489, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 104, - rot_correction: -0.1108982206717197, - vert_correction: 0.04014257279586958, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 105, - rot_correction: -0.07937757438070212, - vert_correction: -0.11152653920243766, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 106, - rot_correction: -0.04768239516448509, - vert_correction: -0.017453292519943295, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 107, - rot_correction: -0.015899949485668342, - vert_correction: 0.07661995416255106, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 108, - rot_correction: 0.015899949485668342, - vert_correction: -0.09040805525330627, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 109, - rot_correction: 0.04768239516448509, - vert_correction: 0.003665191429188092, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 110, - rot_correction: 0.07937757438070212, - vert_correction: 0.12182398178920421, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 111, - rot_correction: 0.1108982206717197, - vert_correction: -0.05393067388662478, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 112, - rot_correction: -0.1108982206717197, - vert_correction: 0.08691739674931762, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 113, - rot_correction: -0.07937757438070212, - vert_correction: -0.06544984694978735, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 114, - rot_correction: -0.04768239516448509, - vert_correction: 0.028623399732707003, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 115, - rot_correction: -0.015899949485668342, - vert_correction: -0.1457698991265664, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 116, - rot_correction: 0.015899949485668342, - vert_correction: -0.044331363000655974, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 117, - rot_correction: 0.04768239516448509, - vert_correction: 0.04974188368183839, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 118, - rot_correction: 0.07937757438070212, - vert_correction: -0.10192722831646885, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 119, - rot_correction: 0.1108982206717197, - vert_correction: -0.007853981633974483, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 120, - rot_correction: -0.1108982206717197, - vert_correction: 0.14713125594312199, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 121, - rot_correction: -0.07937757438070212, - vert_correction: -0.05009094953223726, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 122, - rot_correction: -0.04768239516448509, - vert_correction: 0.0439822971502571, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 123, - rot_correction: -0.015899949485668342, - vert_correction: -0.10768681484805014, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 124, - rot_correction: 0.015899949485668342, - vert_correction: -0.02897246558310587, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 125, - rot_correction: 0.04768239516448509, - vert_correction: 0.0651007810993885, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 126, - rot_correction: 0.07937757438070212, - vert_correction: -0.08656833089891874, - vert_offset_correction: 0.0, - } - - { - dist_correction: 0.0, - dist_correction_x: 0.0, - dist_correction_y: 0.0, - focal_distance: 0.0, - focal_slope: 0.0, - laser_id: 127, - rot_correction: 0.1108982206717197, - vert_correction: 0.007504915783575617, - vert_offset_correction: 0.0, + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 0, + rot_correction: -0.1108982206717197, + vert_correction: -0.2049365607691742, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 1, + rot_correction: -0.07937757438070212, + vert_correction: -0.034732052114687155, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 2, + rot_correction: -0.04768239516448509, + vert_correction: 0.059341194567807204, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 3, + rot_correction: -0.015899949485668342, + vert_correction: -0.09232791743050003, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 4, + rot_correction: 0.015899949485668342, + vert_correction: -0.013613568165555772, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 5, + rot_correction: 0.04768239516448509, + vert_correction: 0.0804596785169386, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 6, + rot_correction: 0.07937757438070212, + vert_correction: -0.07120943348136864, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 7, + rot_correction: 0.1108982206717197, + vert_correction: 0.022863813201125717, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 8, + rot_correction: -0.1108982206717197, + vert_correction: -0.11344640137963143, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 9, + rot_correction: -0.07937757438070212, + vert_correction: -0.01937315469713706, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 10, + rot_correction: -0.04768239516448509, + vert_correction: 0.0747000919853573, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 11, + rot_correction: -0.015899949485668342, + vert_correction: -0.07696902001294993, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 12, + rot_correction: 0.015899949485668342, + vert_correction: 0.0017453292519943296, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 13, + rot_correction: 0.04768239516448509, + vert_correction: 0.11309733552923257, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 14, + rot_correction: 0.07937757438070212, + vert_correction: -0.05585053606381855, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 15, + rot_correction: 0.1108982206717197, + vert_correction: 0.03822271061867582, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 16, + rot_correction: -0.1108982206717197, + vert_correction: -0.06736970912698112, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 17, + rot_correction: -0.07937757438070212, + vert_correction: 0.026703537555513242, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 18, + rot_correction: -0.04768239516448509, + vert_correction: -0.16133823605435582, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 19, + rot_correction: -0.015899949485668342, + vert_correction: -0.030892327760299633, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 20, + rot_correction: 0.015899949485668342, + vert_correction: 0.04782202150464463, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 21, + rot_correction: 0.04768239516448509, + vert_correction: -0.10384709049366261, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 22, + rot_correction: 0.07937757438070212, + vert_correction: -0.009773843811168246, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 23, + rot_correction: 0.1108982206717197, + vert_correction: 0.08429940287132612, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 24, + rot_correction: -0.1108982206717197, + vert_correction: -0.05201081170943102, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 25, + rot_correction: -0.07937757438070212, + vert_correction: 0.04206243497306335, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 26, + rot_correction: -0.04768239516448509, + vert_correction: -0.1096066770252439, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 27, + rot_correction: -0.015899949485668342, + vert_correction: -0.015533430342749533, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 28, + rot_correction: 0.015899949485668342, + vert_correction: 0.06318091892219473, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 29, + rot_correction: 0.04768239516448509, + vert_correction: -0.08848819307611251, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 30, + rot_correction: 0.07937757438070212, + vert_correction: 0.005585053606381855, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 31, + rot_correction: 0.1108982206717197, + vert_correction: 0.1322959573011702, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 32, + rot_correction: -0.1108982206717197, + vert_correction: -0.005934119456780721, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 33, + rot_correction: -0.07937757438070212, + vert_correction: 0.09040805525330627, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 34, + rot_correction: -0.04768239516448509, + vert_correction: -0.0635299847725936, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 35, + rot_correction: -0.015899949485668342, + vert_correction: 0.030543261909900768, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 36, + rot_correction: 0.015899949485668342, + vert_correction: -0.4363323129985824, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 37, + rot_correction: 0.04768239516448509, + vert_correction: -0.04241150082346221, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 38, + rot_correction: 0.07937757438070212, + vert_correction: 0.05166174585903215, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 39, + rot_correction: 0.1108982206717197, + vert_correction: -0.10000736613927509, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 40, + rot_correction: -0.1108982206717197, + vert_correction: 0.00942477796076938, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 41, + rot_correction: -0.07937757438070212, + vert_correction: 0.16929693744344995, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 42, + rot_correction: -0.04768239516448509, + vert_correction: -0.04817108735504349, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 43, + rot_correction: -0.015899949485668342, + vert_correction: 0.04590215932745086, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 44, + rot_correction: 0.015899949485668342, + vert_correction: -0.13351768777756623, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 45, + rot_correction: 0.04768239516448509, + vert_correction: -0.027052603405912107, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 46, + rot_correction: 0.07937757438070212, + vert_correction: 0.06702064327658225, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 47, + rot_correction: 0.1108982206717197, + vert_correction: -0.08464846872172498, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 48, + rot_correction: -0.1108982206717197, + vert_correction: 0.05550147021341968, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 49, + rot_correction: -0.07937757438070212, + vert_correction: -0.09616764178488756, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 50, + rot_correction: -0.04768239516448509, + vert_correction: -0.0020943951023931952, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 51, + rot_correction: -0.015899949485668342, + vert_correction: 0.10000736613927509, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 52, + rot_correction: 0.015899949485668342, + vert_correction: -0.07504915783575616, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 53, + rot_correction: 0.04768239516448509, + vert_correction: 0.019024088846738195, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 54, + rot_correction: 0.07937757438070212, + vert_correction: -0.2799857186049304, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 55, + rot_correction: 0.1108982206717197, + vert_correction: -0.038571776469074684, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 56, + rot_correction: -0.1108982206717197, + vert_correction: 0.07086036763096977, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 57, + rot_correction: -0.07937757438070212, + vert_correction: -0.08080874436733745, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 58, + rot_correction: -0.04768239516448509, + vert_correction: 0.013264502315156905, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 59, + rot_correction: -0.015899949485668342, + vert_correction: 0.2617993877991494, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 60, + rot_correction: 0.015899949485668342, + vert_correction: -0.05969026041820607, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 61, + rot_correction: 0.04768239516448509, + vert_correction: 0.03438298626428829, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 62, + rot_correction: 0.07937757438070212, + vert_correction: -0.11955505376161157, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 63, + rot_correction: 0.1108982206717197, + vert_correction: -0.023212879051524585, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 64, + rot_correction: -0.1108982206717197, + vert_correction: -0.09808750396208132, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 65, + rot_correction: -0.07937757438070212, + vert_correction: -0.004014257279586958, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 66, + rot_correction: -0.04768239516448509, + vert_correction: 0.0947713783832921, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 67, + rot_correction: -0.015899949485668342, + vert_correction: -0.06161012259539983, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 68, + rot_correction: 0.015899949485668342, + vert_correction: 0.01710422666954443, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 69, + rot_correction: 0.04768239516448509, + vert_correction: -0.34177037412552963, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 70, + rot_correction: 0.07937757438070212, + vert_correction: -0.040491638646268445, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 71, + rot_correction: 0.1108982206717197, + vert_correction: 0.053581608036225914, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 72, + rot_correction: -0.1108982206717197, + vert_correction: -0.08272860654453122, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 73, + rot_correction: -0.07937757438070212, + vert_correction: 0.011344640137963142, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 74, + rot_correction: -0.04768239516448509, + vert_correction: 0.20507618710933373, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 75, + rot_correction: -0.015899949485668342, + vert_correction: -0.046251225177849735, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 76, + rot_correction: 0.015899949485668342, + vert_correction: 0.03246312408709453, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 77, + rot_correction: 0.04768239516448509, + vert_correction: -0.12479104151759457, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 78, + rot_correction: 0.07937757438070212, + vert_correction: -0.025132741228718343, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 79, + rot_correction: 0.1108982206717197, + vert_correction: 0.06894050545377602, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 80, + rot_correction: -0.1108982206717197, + vert_correction: -0.03665191429188092, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 81, + rot_correction: -0.07937757438070212, + vert_correction: 0.05742133239061344, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 82, + rot_correction: -0.04768239516448509, + vert_correction: -0.0942477796076938, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 83, + rot_correction: -0.015899949485668342, + vert_correction: -0.00017453292519943296, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 84, + rot_correction: 0.015899949485668342, + vert_correction: 0.07853981633974483, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 85, + rot_correction: 0.04768239516448509, + vert_correction: -0.07312929565856241, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 86, + rot_correction: 0.07937757438070212, + vert_correction: 0.020943951023931952, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 87, + rot_correction: 0.1108982206717197, + vert_correction: -0.23675391303303078, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 88, + rot_correction: -0.1108982206717197, + vert_correction: -0.02129301687433082, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 89, + rot_correction: -0.07937757438070212, + vert_correction: 0.07278022980816354, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 90, + rot_correction: -0.04768239516448509, + vert_correction: -0.07888888219014369, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 91, + rot_correction: -0.015899949485668342, + vert_correction: 0.015184364492350668, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 92, + rot_correction: 0.015899949485668342, + vert_correction: 0.10611601852125524, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 93, + rot_correction: 0.04768239516448509, + vert_correction: -0.05777039824101231, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 94, + rot_correction: 0.07937757438070212, + vert_correction: 0.03630284844148206, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 95, + rot_correction: 0.1108982206717197, + vert_correction: -0.11606439525762292, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 96, + rot_correction: -0.1108982206717197, + vert_correction: 0.024783675378319478, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 97, + rot_correction: -0.07937757438070212, + vert_correction: -0.18057176441133332, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 98, + rot_correction: -0.04768239516448509, + vert_correction: -0.032812189937493394, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 99, + rot_correction: -0.015899949485668342, + vert_correction: 0.061261056745000965, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 100, + rot_correction: 0.015899949485668342, + vert_correction: -0.10576695267085637, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 101, + rot_correction: 0.04768239516448509, + vert_correction: -0.011693705988362009, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 102, + rot_correction: 0.07937757438070212, + vert_correction: 0.08237954069413235, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 103, + rot_correction: 0.1108982206717197, + vert_correction: -0.06928957130417489, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 104, + rot_correction: -0.1108982206717197, + vert_correction: 0.04014257279586958, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 105, + rot_correction: -0.07937757438070212, + vert_correction: -0.11152653920243766, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 106, + rot_correction: -0.04768239516448509, + vert_correction: -0.017453292519943295, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 107, + rot_correction: -0.015899949485668342, + vert_correction: 0.07661995416255106, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 108, + rot_correction: 0.015899949485668342, + vert_correction: -0.09040805525330627, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 109, + rot_correction: 0.04768239516448509, + vert_correction: 0.003665191429188092, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 110, + rot_correction: 0.07937757438070212, + vert_correction: 0.12182398178920421, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 111, + rot_correction: 0.1108982206717197, + vert_correction: -0.05393067388662478, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 112, + rot_correction: -0.1108982206717197, + vert_correction: 0.08691739674931762, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 113, + rot_correction: -0.07937757438070212, + vert_correction: -0.06544984694978735, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 114, + rot_correction: -0.04768239516448509, + vert_correction: 0.028623399732707003, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 115, + rot_correction: -0.015899949485668342, + vert_correction: -0.1457698991265664, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 116, + rot_correction: 0.015899949485668342, + vert_correction: -0.044331363000655974, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 117, + rot_correction: 0.04768239516448509, + vert_correction: 0.04974188368183839, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 118, + rot_correction: 0.07937757438070212, + vert_correction: -0.10192722831646885, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 119, + rot_correction: 0.1108982206717197, + vert_correction: -0.007853981633974483, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 120, + rot_correction: -0.1108982206717197, + vert_correction: 0.14713125594312199, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 121, + rot_correction: -0.07937757438070212, + vert_correction: -0.05009094953223726, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 122, + rot_correction: -0.04768239516448509, + vert_correction: 0.0439822971502571, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 123, + rot_correction: -0.015899949485668342, + vert_correction: -0.10768681484805014, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 124, + rot_correction: 0.015899949485668342, + vert_correction: -0.02897246558310587, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 125, + rot_correction: 0.04768239516448509, + vert_correction: 0.0651007810993885, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 126, + rot_correction: 0.07937757438070212, + vert_correction: -0.08656833089891874, + vert_offset_correction: 0.0, + } + - { + dist_correction: 0.0, + dist_correction_x: 0.0, + dist_correction_y: 0.0, + focal_distance: 0.0, + focal_slope: 0.0, + laser_id: 127, + rot_correction: 0.1108982206717197, + vert_correction: 0.007504915783575617, + vert_offset_correction: 0.0, } num_lasers: 128 distance_resolution: 0.007 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp new file mode 100644 index 000000000..805a35fae --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp @@ -0,0 +1,50 @@ +// 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. + +#pragma once + +#include +namespace nebula::drivers +{ + +/** + * @brief Tests if `angle` is in the region of the circle defined by `start_angle` and `end_angle`. + * Notably, `end_angle` can be smaller than `start_angle`, in which case the region passes over the + * 360/0 deg bound. This function is unit-independent (but all angles have to have the same unit), + * so degrees, radians, and arbitrary scale factors can be used. + */ +template +bool angle_is_between( + T start_angle, T end_angle, T angle, bool start_inclusive = true, bool end_inclusive = true) +{ + if (!start_inclusive && angle == start_angle) return false; + if (!end_inclusive && angle == end_angle) return false; + + return (start_angle <= angle && angle <= end_angle) || + ((end_angle < start_angle) && (angle <= end_angle || start_angle <= angle)); +} + +/** + * @brief Normalizes an angle to the interval [0; max_angle]. This function is unit-independent. + * `max_angle` is 360 for degrees, 2 * M_PI for radians, and the corresponding scaled value for + * scaled units such as centi-degrees (36000). + */ +template +T normalize_angle(T angle, T max_angle) +{ + T factor = std::floor((1.0 * angle) / max_angle); + return angle - (factor * max_angle); +} + +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp index 785fa59c3..71aed2985 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp @@ -1,3 +1,17 @@ +// 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_DRIVER_BASE_H #define NEBULA_DRIVER_BASE_H diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index 139ea6d5e..b017e6efd 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -14,15 +14,15 @@ #pragma once +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp" + #include -#include #include #include #include #include #include -#include #include #include @@ -41,30 +41,16 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder explicit ContinentalARS548Decoder( - const std::shared_ptr & sensor_configuration); + const std::shared_ptr & sensor_configuration); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus() override; /// @brief Function for parsing NebulaPackets /// @param nebula_packets /// @return Resulting flag - bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) override; - - /// @brief Function for parsing detection lists - /// @param data - /// @return Resulting flag - bool ParseDetectionsListPacket( - const std::vector & data, const std_msgs::msg::Header & header); - - /// @brief Function for parsing object lists - /// @param data - /// @return Resulting flag - bool ParseObjectsListPacket( - const std::vector & data, const std_msgs::msg::Header & header); - - /// @brief Function for parsing sensor status messages - /// @param data - /// @return Resulting flag - bool ParseSensorStatusPacket( - const std::vector & data, const std_msgs::msg::Header & header); + bool ProcessPacket(std::unique_ptr packet_msg) override; /// @brief Register function to call when a new detection list is processed /// @param detection_list_callback @@ -86,17 +72,40 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder Status RegisterSensorStatusCallback( std::function sensor_status_callback); + /// @brief Register function to call when a new sensor status message is processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterPacketsCallback( + std::function)> packets_callback); + private: + /// @brief Function for parsing detection lists + /// @param data + /// @return Resulting flag + bool ParseDetectionsListPacket(const nebula_msgs::msg::NebulaPacket & packet_msg); + + /// @brief Function for parsing object lists + /// @param data + /// @return Resulting flag + bool ParseObjectsListPacket(const nebula_msgs::msg::NebulaPacket & packet_msg); + + /// @brief Function for parsing sensor status messages + /// @param data + /// @return Resulting flag + bool ParseSensorStatusPacket(const nebula_msgs::msg::NebulaPacket & packet_msg); + std::function msg)> - detection_list_callback_; + detection_list_callback_{}; std::function msg)> - object_list_callback_; - std::function sensor_status_callback_; + object_list_callback_{}; + std::function sensor_status_callback_{}; + std::function msg)> + nebula_packets_callback_{}; ContinentalARS548Status radar_status_{}; /// @brief SensorConfiguration for this decoder - std::shared_ptr sensor_configuration_; + std::shared_ptr config_ptr_{}; }; } // namespace continental_ars548 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp index e5f0f5efa..933490542 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -15,10 +15,10 @@ #ifndef NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP #define NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP -#include "nebula_common/point_types.hpp" +#include +#include -#include "nebula_msgs/msg/nebula_packet.hpp" -#include "nebula_msgs/msg/nebula_packets.hpp" +#include #include #include @@ -39,10 +39,14 @@ class ContinentalPacketsDecoder virtual ~ContinentalPacketsDecoder() = default; ContinentalPacketsDecoder() = default; - /// @brief Virtual function for parsing NebulaPackets based on packet structure - /// @param nebula_packets + /// @brief Get current status of this driver + /// @return Current status + virtual Status GetStatus() = 0; + + /// @brief Virtual function for parsing a NebulaPacket + /// @param packet_msg /// @return Resulting flag - virtual bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) = 0; + virtual bool ProcessPacket(std::unique_ptr packet_msg) = 0; }; } // namespace drivers } // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp new file mode 100644 index 000000000..c70cb4706 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp @@ -0,0 +1,211 @@ +// 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. + +#pragma once + +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +/// @brief Continental Radar decoder (SRR520) +class ContinentalSRR520Decoder : public ContinentalPacketsDecoder +{ +public: + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this decoder + explicit ContinentalSRR520Decoder( + const std::shared_ptr & sensor_configuration); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus() override; + + /// @brief Function for parsing NebulaPackets + /// @param nebula_packets + /// @return Resulting flag + bool ProcessPacket(std::unique_ptr packet_msg) override; + + /// @brief Register function to call whenever a new RDI near detection list is processed + /// @param detection_list_callback + /// @return Resulting status + Status RegisterNearDetectionListCallback( + std::function)> + detection_list_callback); + + /// @brief Register function to call whenever a new RDI HRR detection list is processed + /// @param detection_list_callback + /// @return Resulting status + Status RegisterHRRDetectionListCallback( + std::function)> + detection_list_callback); + + /// @brief Register function to call whenever a new object list is processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterObjectListCallback( + std::function)> + object_list_callback); + + /// @brief Register function to call whenever a new object list is processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterStatusCallback( + std::function)> status_callback); + + /// @brief Register function to call whenever a sync follow-up packet is processed + /// @param sync_follow_up_callback + /// @return Resulting status + Status RegisterSyncFollowUpCallback( + std::function sync_follow_up_callback); + + /// @brief Register function to call whenever enough packets have been processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterPacketsCallback( + std::function)> nebula_packets_callback); + + /// @brief Setting rclcpp::Logger + /// @param node Logger + void SetLogger(std::shared_ptr node); + +private: + /// @brief Process a new near detection header packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessNearHeaderPacket(std::unique_ptr packet_msg); + + /// @brief Process a new near element packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessNearElementPacket(std::unique_ptr packet_msg); + + /// @brief Process a new hrr header packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessHRRHeaderPacket(std::unique_ptr packet_msg); + + /// @brief Process a new hrr element packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessHRRElementPacket(std::unique_ptr packet_msg); + + /// @brief Process a new object header packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessObjectHeaderPacket(std::unique_ptr packet_msg); + + /// @brief Process a new object element packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessObjectElementPacket(std::unique_ptr packet_msg); + + /// @brief Process a new crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessCRCListPacket(std::unique_ptr packet_msg); + + /// @brief Process a new Near detections crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessNearCRCListPacket(std::unique_ptr packet_msg); + + /// @brief Process a new HRR crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessHRRCRCListPacket( + std::unique_ptr packet_msg); // cspell:ignore HRRCRC + + /// @brief Process a new objects crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessObjectCRCListPacket(std::unique_ptr packet_msg); + + /// @brief Process a new sensor status packet + /// @param buffer The buffer containing the status packet + /// @param stamp The stamp in nanoseconds + void ProcessSensorStatusPacket(std::unique_ptr packet_msg); + + /// @brief Process a new sensor status packet + /// @param buffer The buffer containing the status packet + /// @param stamp The stamp in nanoseconds + void ProcessSyncFollowUpPacket(std::unique_ptr packet_msg); + + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + + std::function msg)> + near_detection_list_callback_{}; + std::function msg)> + hrr_detection_list_callback_{}; + std::function msg)> + object_list_callback_{}; + std::function msg)> + status_callback_{}; + std::function sync_follow_up_callback_{}; + std::function msg)> + nebula_packets_callback_{}; + + std::unique_ptr rdi_near_packets_ptr_{}; + std::unique_ptr rdi_hrr_packets_ptr_{}; + std::unique_ptr object_packets_ptr_{}; + + std::unique_ptr near_detection_list_ptr_{}; + std::unique_ptr hrr_detection_list_ptr_{}; + std::unique_ptr object_list_ptr_{}; + + bool first_rdi_near_packet_{true}; + bool first_rdi_hrr_packet_{true}; + bool first_object_packet_{true}; + + ScanHeaderPacket rdi_near_header_packet_{}; + ScanHeaderPacket rdi_hrr_header_packet_{}; + ObjectHeaderPacket object_header_packet_{}; + + /// @brief SensorConfiguration for this decoder + std::shared_ptr + sensor_configuration_{}; + + std::shared_ptr parent_node_logger_ptr_{}; +}; + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp index 42fd0a008..bc275b347 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp @@ -1,14 +1,24 @@ -#pragma once +// 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_common/hesai/hesai_common.hpp" +#pragma once #include #include -namespace nebula -{ -namespace drivers +namespace nebula::drivers { struct CorrectedAngleData @@ -23,19 +33,11 @@ struct CorrectedAngleData /// @brief Handles angle correction for given azimuth/channel combinations, as well as trigonometry /// lookup tables +template class AngleCorrector { -protected: - const std::shared_ptr sensor_calibration_; - const std::shared_ptr sensor_correction_; - public: - AngleCorrector( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : sensor_calibration_(sensor_calibration), sensor_correction_(sensor_correction) - { - } + using correction_data_t = CorrectionDataT; /// @brief Get the corrected azimuth and elevation for a given block and channel, along with their /// sin/cos values. @@ -45,15 +47,10 @@ class AngleCorrector /// @return The corrected angles (azimuth, elevation) in radians and their sin/cos values virtual CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) = 0; - /// @brief Returns true if the current azimuth lies in a different (new) scan compared to the last - /// azimuth - /// @param current_azimuth The current azimuth value in the sensor's angle resolution - /// @param last_azimuth The last azimuth in the sensor's angle resolution - /// @param sync_azimuth The azimuth set in the sensor configuration, for which the - /// timestamp is aligned to the full second - /// @return true if the current azimuth is in a different scan than the last one, false otherwise - virtual bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0; + virtual bool passedEmitAngle(uint32_t last_azimuth, uint32_t current_azimuth) = 0; + virtual bool passedTimestampResetAngle(uint32_t last_azimuth, uint32_t current_azimuth) = 0; + virtual bool isInsideFoV(uint32_t last_azimuth, uint32_t current_azimuth) = 0; + virtual bool isInsideOverlap(uint32_t last_azimuth, uint32_t current_azimuth) = 0; }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp index 3e0c999b0..b10c664ef 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp @@ -1,44 +1,86 @@ +// 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. + #pragma once #include "nebula_common/hesai/hesai_common.hpp" +#include "nebula_decoders/nebula_decoders_common/angles.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" +#include + +#include +#include #include +#include +#include +#include +#include -namespace nebula -{ -namespace drivers +namespace nebula::drivers { template -class AngleCorrectorCalibrationBased : public AngleCorrector +class AngleCorrectorCalibrationBased : public AngleCorrector { private: - static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit; + static constexpr size_t MAX_AZIMUTH = 360 * AngleUnit; std::array elevation_angle_rad_{}; std::array azimuth_offset_rad_{}; - std::array block_azimuth_rad_{}; + std::array block_azimuth_rad_{}; std::array elevation_cos_{}; std::array elevation_sin_{}; - std::array, MAX_AZIMUTH_LEN> azimuth_cos_{}; - std::array, MAX_AZIMUTH_LEN> azimuth_sin_{}; + std::array, MAX_AZIMUTH> azimuth_cos_{}; + std::array, MAX_AZIMUTH> azimuth_sin_{}; public: - AngleCorrectorCalibrationBased( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : AngleCorrector(sensor_calibration, sensor_correction) + uint32_t emit_angle_raw_; + uint32_t timestamp_reset_angle_raw_; + uint32_t fov_start_raw_; + uint32_t fov_end_raw_; + + bool is_360_; + + explicit AngleCorrectorCalibrationBased( + const std::shared_ptr & sensor_calibration, + double fov_start_azimuth_deg, double fov_end_azimuth_deg, double scan_cut_azimuth_deg) { if (sensor_calibration == nullptr) { throw std::runtime_error( "Cannot instantiate AngleCorrectorCalibrationBased without calibration data"); } + // //////////////////////////////////////// + // Elevation lookup tables + // //////////////////////////////////////// + + int32_t correction_min = INT32_MAX; + int32_t correction_max = INT32_MIN; + + auto round_away_from_zero = [](float value) { + return (value < 0) ? std::floor(value) : std::ceil(value); + }; + for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { - float elevation_angle_deg = sensor_calibration->elev_angle_map[channel_id]; - float azimuth_offset_deg = sensor_calibration->azimuth_offset_map[channel_id]; + float elevation_angle_deg = sensor_calibration->elev_angle_map.at(channel_id); + float azimuth_offset_deg = sensor_calibration->azimuth_offset_map.at(channel_id); + + int32_t azimuth_offset_raw = round_away_from_zero(azimuth_offset_deg * AngleUnit); + correction_min = std::min(correction_min, azimuth_offset_raw); + correction_max = std::max(correction_max, azimuth_offset_raw); elevation_angle_rad_[channel_id] = deg2rad(elevation_angle_deg); azimuth_offset_rad_[channel_id] = deg2rad(azimuth_offset_deg); @@ -47,7 +89,42 @@ class AngleCorrectorCalibrationBased : public AngleCorrector elevation_sin_[channel_id] = sinf(elevation_angle_rad_[channel_id]); } - for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH_LEN; block_azimuth++) { + // //////////////////////////////////////// + // Raw azimuth threshold angles + // //////////////////////////////////////// + + int32_t emit_angle_raw = std::ceil(scan_cut_azimuth_deg * AngleUnit); + emit_angle_raw -= correction_min; + emit_angle_raw_ = normalize_angle(emit_angle_raw, MAX_AZIMUTH); + + int32_t fov_start_raw = std::floor(fov_start_azimuth_deg * AngleUnit); + fov_start_raw -= correction_max; + fov_start_raw_ = normalize_angle(fov_start_raw, MAX_AZIMUTH); + + int32_t fov_end_raw = std::ceil(fov_end_azimuth_deg * AngleUnit); + fov_end_raw -= correction_min; + fov_end_raw_ = normalize_angle(fov_end_raw, MAX_AZIMUTH); + + // Reset timestamp on FoV start if FoV < 360 deg and scan is cut at FoV end. + // Otherwise, reset timestamp on publish + is_360_ = + normalize_angle(fov_start_azimuth_deg, 360.) == normalize_angle(fov_end_azimuth_deg, 360.); + bool reset_timestamp_on_publish = is_360_ || (normalize_angle(fov_end_azimuth_deg, 360.) != + normalize_angle(scan_cut_azimuth_deg, 360.)); + + if (reset_timestamp_on_publish) { + int32_t timestamp_reset_angle_raw = std::floor(scan_cut_azimuth_deg * AngleUnit); + timestamp_reset_angle_raw -= correction_max; + timestamp_reset_angle_raw_ = normalize_angle(timestamp_reset_angle_raw, MAX_AZIMUTH); + } else { + timestamp_reset_angle_raw_ = fov_start_raw_; + } + + // //////////////////////////////////////// + // Azimuth lookup tables + // //////////////////////////////////////// + + for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH; block_azimuth++) { block_azimuth_rad_[block_azimuth] = deg2rad(block_azimuth / static_cast(AngleUnit)); for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { @@ -63,6 +140,8 @@ class AngleCorrectorCalibrationBased : public AngleCorrector CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override { float azimuth_rad = block_azimuth_rad_[block_azimuth] + azimuth_offset_rad_[channel_id]; + azimuth_rad = normalize_angle(azimuth_rad, M_PIf * 2); + float elevation_rad = elevation_angle_rad_[channel_id]; return { @@ -74,17 +153,28 @@ class AngleCorrectorCalibrationBased : public AngleCorrector elevation_cos_[channel_id]}; } - bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) override + bool passedEmitAngle(uint32_t last_azimuth, uint32_t current_azimuth) override + { + return angle_is_between(last_azimuth, current_azimuth, emit_angle_raw_, false); + } + + bool passedTimestampResetAngle(uint32_t last_azimuth, uint32_t current_azimuth) override + { + return angle_is_between(last_azimuth, current_azimuth, timestamp_reset_angle_raw_, false); + } + + bool isInsideFoV(uint32_t last_azimuth, uint32_t current_azimuth) override + { + if (is_360_) return true; + return angle_is_between(fov_start_raw_, fov_end_raw_, current_azimuth) || + angle_is_between(timestamp_reset_angle_raw_, emit_angle_raw_, last_azimuth); + } + + bool isInsideOverlap(uint32_t last_azimuth, uint32_t current_azimuth) override { - // Cut the scan when the azimuth passes over the sync_azimuth - uint32_t current_diff_from_sync = - (MAX_AZIMUTH_LEN + current_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN; - uint32_t last_diff_from_sync = - (MAX_AZIMUTH_LEN + last_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN; - - return current_diff_from_sync < last_diff_from_sync; + return angle_is_between(timestamp_reset_angle_raw_, emit_angle_raw_, current_azimuth) || + angle_is_between(timestamp_reset_angle_raw_, emit_angle_raw_, last_azimuth); } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp index be60078de..c044868bc 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp @@ -1,26 +1,63 @@ +// 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. + #pragma once #include "nebula_common/hesai/hesai_common.hpp" +#include "nebula_decoders/nebula_decoders_common/angles.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" -#include - -#define _(x) '"' << #x << "\": " << x << ", " +#include -namespace nebula -{ -namespace drivers +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers { template -class AngleCorrectorCorrectionBased : public AngleCorrector +class AngleCorrectorCorrectionBased : public AngleCorrector { private: - static constexpr size_t MAX_AZIMUTH_LENGTH = 360 * AngleUnit; + static constexpr size_t MAX_AZIMUTH = 360 * AngleUnit; + const std::shared_ptr correction_; rclcpp::Logger logger_; - std::array cos_{}; - std::array sin_{}; + std::array cos_{}; + std::array sin_{}; + + struct FrameAngleInfo + { + static constexpr uint32_t unset = UINT32_MAX; + uint32_t fov_start = unset; + uint32_t fov_end = unset; + uint32_t timestamp_reset = unset; + uint32_t scan_emit = unset; + }; + + std::vector frame_angle_info_; /// @brief For a given azimuth value, find its corresponding output field /// @param azimuth The azimuth to get the field for @@ -29,73 +66,182 @@ class AngleCorrectorCorrectionBased : public AngleCorrector { // Assumes that: // * none of the startFrames are defined as > 360 deg (< 0 not possible since they are unsigned) - // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg etc.) - // These assumptions hold for AT128E2X. - int field = sensor_correction_->frameNumber - 1; - for (size_t i = 0; i < sensor_correction_->frameNumber; ++i) { - if (azimuth < sensor_correction_->startFrame[i]) return field; + // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg + // etc.) These assumptions hold for AT128E2X. + int field = correction_->frameNumber - 1; + for (size_t i = 0; i < correction_->frameNumber; ++i) { + if (azimuth < correction_->startFrame[i]) return field; field = i; } - // This is never reached if sensor_correction_ is correct + // This is never reached if correction_ is correct return field; } + /// @brief For raw encoder angle `azi`, return whether all (any if `any == true`) channels' + /// corrected azimuths are greater (or equal if `eq_ok == true`) than `threshold`. + bool are_corrected_angles_above_threshold(uint32_t azi, double threshold, bool any, bool eq_ok) + { + for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { + auto azi_corr = getCorrectedAngleData(azi, channel_id).azimuth_rad; + if (!any && (azi_corr < threshold || (!eq_ok && azi_corr == threshold))) return false; + if (any && (azi_corr > threshold || (eq_ok && azi_corr == threshold))) return true; + } + + return !any; + } + + /// @brief Find and return the first raw encoder angle between the raw `start` and `end` endcoder + /// angles for which all (any if `any == true`) channels' corrected azimuth is greater than (or + /// equal to if `eq_ok == true`) `threshold`. Return `FrameAngleInfo::unset` if no angle is found. + uint32_t bin_search(uint32_t start, uint32_t end, double threshold, bool any, bool eq_ok) + { + if (start > end) return FrameAngleInfo::unset; + + if (end - start <= 1) { + bool result_start = are_corrected_angles_above_threshold( + normalize_angle(start, MAX_AZIMUTH), threshold, any, eq_ok); + if (result_start) return start; + return end; + } + + uint32_t next = (start + end) / 2; + + bool result_next = are_corrected_angles_above_threshold( + normalize_angle(next, MAX_AZIMUTH), threshold, any, eq_ok); + if (result_next) return bin_search(start, next, threshold, any, eq_ok); + return bin_search(next + 1, end, threshold, any, eq_ok); + } + public: - AngleCorrectorCorrectionBased( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : AngleCorrector(sensor_calibration, sensor_correction), - logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) + explicit AngleCorrectorCorrectionBased( + const std::shared_ptr & sensor_correction, double fov_start_azimuth_deg, + double fov_end_azimuth_deg, double scan_cut_azimuth_deg) + : correction_(sensor_correction), logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) { if (sensor_correction == nullptr) { throw std::runtime_error( - "Cannot instantiate AngleCorrectorCalibrationBased without calibration data"); + "Cannot instantiate AngleCorrectorCorrectionBased without correction data"); } logger_.set_level(rclcpp::Logger::Level::Debug); - for (size_t i = 0; i < MAX_AZIMUTH_LENGTH; ++i) { - float rad = 2.f * i * M_PI / MAX_AZIMUTH_LENGTH; + // //////////////////////////////////////// + // Trigonometry lookup tables + // //////////////////////////////////////// + + for (size_t i = 0; i < MAX_AZIMUTH; ++i) { + float rad = 2.f * i * M_PIf / MAX_AZIMUTH; cos_[i] = cosf(rad); sin_[i] = sinf(rad); } + + // //////////////////////////////////////// + // Scan start/end correction lookups + // //////////////////////////////////////// + + auto fov_start_rad = deg2rad(fov_start_azimuth_deg); + auto fov_end_rad = deg2rad(fov_end_azimuth_deg); + auto scan_cut_rad = deg2rad(scan_cut_azimuth_deg); + + // For each field (= mirror), find the raw block azimuths corresponding FoV start and end + for (size_t field_id = 0; field_id < correction_->frameNumber; ++field_id) { + auto frame_start = correction_->startFrame[field_id]; + auto frame_end = correction_->endFrame[field_id]; + if (frame_end < frame_start) frame_end += MAX_AZIMUTH; + + FrameAngleInfo & angle_info = frame_angle_info_.emplace_back(); + + angle_info.fov_start = bin_search(frame_start, frame_end, fov_start_rad, true, true); + angle_info.fov_end = bin_search(angle_info.fov_start, frame_end, fov_end_rad, false, true); + angle_info.scan_emit = + bin_search(angle_info.fov_start, angle_info.fov_end, scan_cut_rad, false, true); + angle_info.timestamp_reset = + bin_search(angle_info.fov_start, angle_info.fov_end, scan_cut_rad, true, true); + + if ( + angle_info.fov_start == FrameAngleInfo::unset || + angle_info.fov_end == FrameAngleInfo::unset || + angle_info.scan_emit == FrameAngleInfo::unset || + angle_info.timestamp_reset == FrameAngleInfo::unset) { + throw std::runtime_error("Not all necessary angles found!"); + } + + if (fov_start_rad == scan_cut_rad) { + angle_info.timestamp_reset = angle_info.fov_start; + angle_info.scan_emit = angle_info.fov_start; + } else if (fov_end_rad == scan_cut_rad) { + angle_info.timestamp_reset = angle_info.fov_start; + angle_info.scan_emit = angle_info.fov_end; + } + } } CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override { - const auto & correction = AngleCorrector::sensor_correction_; int field = findField(block_azimuth); - auto elevation = - correction->elevation[channel_id] + - correction->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); - elevation = (MAX_AZIMUTH_LENGTH + elevation) % MAX_AZIMUTH_LENGTH; + int32_t elevation = + correction_->elevation[channel_id] + + correction_->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); + elevation = (MAX_AZIMUTH + elevation) % MAX_AZIMUTH; - auto azimuth = (block_azimuth + MAX_AZIMUTH_LENGTH - correction->startFrame[field]) * 2 - - correction->azimuth[channel_id] + - correction->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); - azimuth = (MAX_AZIMUTH_LENGTH + azimuth) % MAX_AZIMUTH_LENGTH; + int32_t azimuth = + (block_azimuth + MAX_AZIMUTH - correction_->startFrame[field]) * 2 - + correction_->azimuth[channel_id] + + correction_->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); + azimuth = (MAX_AZIMUTH + azimuth) % MAX_AZIMUTH; - float azimuth_rad = 2.f * azimuth * M_PI / MAX_AZIMUTH_LENGTH; - float elevation_rad = 2.f * elevation * M_PI / MAX_AZIMUTH_LENGTH; + float azimuth_rad = 2.f * azimuth * M_PIf / MAX_AZIMUTH; + float elevation_rad = 2.f * elevation * M_PIf / MAX_AZIMUTH; return {azimuth_rad, elevation_rad, sin_[azimuth], cos_[azimuth], sin_[elevation], cos_[elevation]}; } - bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override + bool passedEmitAngle(uint32_t last_azimuth, uint32_t current_azimuth) override + { + for (const auto & frame_angles : frame_angle_info_) { + if (angle_is_between(last_azimuth, current_azimuth, frame_angles.scan_emit, false)) + return true; + } + + return false; + } + + bool passedTimestampResetAngle(uint32_t last_azimuth, uint32_t current_azimuth) override + { + for (const auto & frame_angles : frame_angle_info_) { + if (angle_is_between(last_azimuth, current_azimuth, frame_angles.timestamp_reset, false)) + return true; + } + + return false; + } + + bool isInsideFoV(uint32_t last_azimuth, uint32_t current_azimuth) override + { + for (const auto & frame_angles : frame_angle_info_) { + if ( + angle_is_between(frame_angles.fov_start, frame_angles.fov_end, current_azimuth, false) || + angle_is_between(frame_angles.fov_start, frame_angles.fov_end, last_azimuth, false)) + return true; + } + + return false; + } + + bool isInsideOverlap(uint32_t last_azimuth, uint32_t current_azimuth) override { - // For AT128, the scan is always cut at the beginning of the field: - // If we would cut at `sync_azimuth`, the points left of it would be - // from the previous field and therefore significantly older than the - // points right of it. - // This also means that the pointcloud timestamp is only at top of second - // if the `sync_azimuth` aligns with the beginning of the field (e.g. 30deg for AT128). - // The absolute point time for points at `sync_azimuth` is still at top of second. - return findField(current_azimuth) != findField(last_azimuth); + for (const auto & frame_angles : frame_angle_info_) { + if ( + angle_is_between(frame_angles.timestamp_reset, frame_angles.scan_emit, current_azimuth) || + angle_is_between(frame_angles.timestamp_reset, frame_angles.scan_emit, last_azimuth)) + return true; + } + + return false; } }; -} // namespace drivers -} // namespace nebula +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 7bdfd069e..b1b5bc798 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -1,24 +1,54 @@ +// 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. + #pragma once +#include "nebula_decoders/nebula_decoders_common/angles.hpp" +#include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" +#include +#include +#include +#include #include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include +#include +#include +#include +#include +#include -namespace nebula -{ -namespace drivers +namespace nebula::drivers { template class HesaiDecoder : public HesaiScanDecoder { + struct ScanCutAngles + { + float fov_min; + float fov_max; + float scan_emit_angle; + }; + protected: /// @brief Configuration for this decoder - const std::shared_ptr sensor_configuration_; + const std::shared_ptr sensor_configuration_; /// @brief The sensor definition, used for return mode and time offset handling SensorT sensor_{}; @@ -33,14 +63,16 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief The last decoded packet typename SensorT::packet_t packet_; - /// @brief The last azimuth processed - int last_phase_; + /// @brief The timestamp of the last completed scan in nanoseconds - uint64_t output_scan_timestamp_ns_; + uint64_t output_scan_timestamp_ns_ = 0; /// @brief The timestamp of the scan currently in progress - uint64_t decode_scan_timestamp_ns_; + uint64_t decode_scan_timestamp_ns_ = 0; /// @brief Whether a full scan has been processed - bool has_scanned_; + bool has_scanned_ = false; + + ScanCutAngles scan_cut_angles_; + uint32_t last_azimuth_ = 0; rclcpp::Logger logger_; @@ -52,17 +84,17 @@ class HesaiDecoder : public HesaiScanDecoder block_firing_offset_ns_; /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. - /// @param pandar_packet The incoming PandarPacket + /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully - bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) + bool parsePacket(const std::vector & packet) { - if (pandar_packet.size < sizeof(typename SensorT::packet_t)) { + if (packet.size() < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << pandar_packet.size << " | Expected at least:" - << sizeof(typename SensorT::packet_t)); + logger_, "Packet size mismatch: " << packet.size() << " | Expected at least: " + << sizeof(typename SensorT::packet_t)); return false; } - if (std::memcpy(&packet_, pandar_packet.data.data(), sizeof(typename SensorT::packet_t))) { + if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) { // FIXME(mojomex) do validation? // RCLCPP_DEBUG(logger_, "Packet parsed successfully"); return true; @@ -80,7 +112,7 @@ class HesaiDecoder : public HesaiScanDecoder void convertReturns(size_t start_block_id, size_t n_blocks) { uint64_t packet_timestamp_ns = hesai_packet::get_timestamp_ns(packet_); - uint32_t raw_azimuth = packet_.body.blocks[start_block_id].get_azimuth(); + uint32_t raw_azimuth = packet_.body.blocks[start_block_id].getAzimuth(); std::vector return_units; @@ -100,7 +132,7 @@ class HesaiDecoder : public HesaiScanDecoder continue; } - auto distance = getDistance(unit); + float distance = getDistance(unit); if ( distance < SensorT::MIN_RANGE || SensorT::MAX_RANGE < distance || @@ -140,43 +172,52 @@ class HesaiDecoder : public HesaiScanDecoder } } - NebulaPoint point; + CorrectedAngleData corrected_angle_data = + angle_corrector_.getCorrectedAngleData(raw_azimuth, channel_id); + float azimuth = corrected_angle_data.azimuth_rad; + + bool in_fov = angle_is_between(scan_cut_angles_.fov_min, scan_cut_angles_.fov_max, azimuth); + if (!in_fov) { + continue; + } + + bool in_current_scan = true; + + if ( + angle_corrector_.isInsideOverlap(last_azimuth_, raw_azimuth) && + angle_is_between( + scan_cut_angles_.scan_emit_angle, scan_cut_angles_.scan_emit_angle + deg2rad(20), + azimuth)) { + in_current_scan = false; + } + + auto pc = in_current_scan ? decode_pc_ : output_pc_; + uint64_t scan_timestamp_ns = + in_current_scan ? decode_scan_timestamp_ns_ : output_scan_timestamp_ns_; + + NebulaPoint & point = pc->emplace_back(); point.distance = distance; point.intensity = unit.reflectivity; - point.time_stamp = - getPointTimeRelative(packet_timestamp_ns, block_offset + start_block_id, channel_id); + point.time_stamp = getPointTimeRelative( + scan_timestamp_ns, packet_timestamp_ns, block_offset + start_block_id, channel_id); point.return_type = static_cast(return_type); point.channel = channel_id; - auto corrected_angle_data = angle_corrector_.getCorrectedAngleData(raw_azimuth, channel_id); - // The raw_azimuth and channel are only used as indices, sin/cos functions use the precise // corrected angles - float xyDistance = distance * corrected_angle_data.cos_elevation; - point.x = xyDistance * corrected_angle_data.sin_azimuth; - point.y = xyDistance * corrected_angle_data.cos_azimuth; + float xy_distance = distance * corrected_angle_data.cos_elevation; + point.x = xy_distance * corrected_angle_data.sin_azimuth; + point.y = xy_distance * corrected_angle_data.cos_azimuth; point.z = distance * corrected_angle_data.sin_elevation; // The driver wrapper converts to degrees, expects radians point.azimuth = corrected_angle_data.azimuth_rad; point.elevation = corrected_angle_data.elevation_rad; - - decode_pc_->emplace_back(point); } } } - /// @brief Checks whether the last processed block was the last block of a scan - /// @param current_phase The azimuth of the last processed block - /// @param sync_phase The azimuth set in the sensor configuration, for which the - /// timestamp is aligned to the full second - /// @return Whether the scan has completed - bool checkScanCompleted(uint32_t current_phase, uint32_t sync_phase) - { - return angle_corrector_.hasScanned(current_phase, last_phase_, sync_phase); - } - /// @brief Get the distance of the given unit in meters float getDistance(const typename SensorT::packet_t::body_t::block_t::unit_t & unit) { @@ -185,85 +226,107 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief Get timestamp of point in nanoseconds, relative to scan timestamp. Includes firing time /// offset correction for channel and block + /// @param scan_timestamp_ns Start timestamp of the current scan in nanoseconds /// @param packet_timestamp_ns The timestamp of the current PandarPacket in nanoseconds /// @param block_id The block index of the point /// @param channel_id The channel index of the point - uint32_t getPointTimeRelative(uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id) + uint32_t getPointTimeRelative( + uint64_t scan_timestamp_ns, uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id) { auto point_to_packet_offset_ns = sensor_.getPacketRelativePointTimeOffset(block_id, channel_id, packet_); - auto packet_to_scan_offset_ns = - static_cast(packet_timestamp_ns - decode_scan_timestamp_ns_); + auto packet_to_scan_offset_ns = static_cast(packet_timestamp_ns - scan_timestamp_ns); return packet_to_scan_offset_ns + point_to_packet_offset_ns; } public: /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder - /// @param calibration_configuration Calibration for this decoder (can be nullptr if - /// correction_configuration is set) - /// @param correction_configuration Correction for this decoder (can be nullptr if - /// calibration_configuration is set) + /// @param correction_data Calibration data for this decoder explicit HesaiDecoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + correction_data) : sensor_configuration_(sensor_configuration), - angle_corrector_(calibration_configuration, correction_configuration), + angle_corrector_( + correction_data, sensor_configuration_->cloud_min_angle, + sensor_configuration_->cloud_max_angle, sensor_configuration_->cut_angle), logger_(rclcpp::get_logger("HesaiDecoder")) { logger_.set_level(rclcpp::Logger::Level::Debug); - RCLCPP_INFO_STREAM(logger_, sensor_configuration_); + RCLCPP_INFO_STREAM(logger_, *sensor_configuration_); - decode_pc_.reset(new NebulaPointCloud); - output_pc_.reset(new NebulaPointCloud); + decode_pc_ = std::make_shared(); + output_pc_ = std::make_shared(); decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); + + scan_cut_angles_ = { + deg2rad(sensor_configuration_->cloud_min_angle), + deg2rad(sensor_configuration_->cloud_max_angle), deg2rad(sensor_configuration_->cut_angle)}; } - int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override + int unpack(const std::vector & packet) override { - if (!parsePacket(pandar_packet)) { + if (!parsePacket(packet)) { return -1; } + // This is the first scan, set scan timestamp to whatever packet arrived first if (decode_scan_timestamp_ns_ == 0) { - decode_scan_timestamp_ns_ = hesai_packet::get_timestamp_ns(packet_); + decode_scan_timestamp_ns_ = hesai_packet::get_timestamp_ns(packet_) + + sensor_.getEarliestPointTimeOffsetForBlock(0, packet_); } if (has_scanned_) { + output_pc_->clear(); has_scanned_ = false; } const size_t n_returns = hesai_packet::get_n_returns(packet_.tail.return_mode); - uint32_t current_azimuth; - for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += n_returns) { - current_azimuth = packet_.body.blocks[block_id].get_azimuth(); + auto block_azimuth = packet_.body.blocks[block_id].getAzimuth(); + + if (angle_corrector_.passedTimestampResetAngle(last_azimuth_, block_azimuth)) { + uint64_t new_scan_timestamp_ns = + hesai_packet::get_timestamp_ns(packet_) + + sensor_.getEarliestPointTimeOffsetForBlock(block_id, packet_); + + if (sensor_configuration_->cut_angle == sensor_configuration_->cloud_max_angle) { + // In the non-360 deg case, if the cut angle and FoV end coincide, the old pointcloud has + // already been swapped and published before the timestamp reset angle is reached. Thus, + // the `decode` pointcloud is now empty and will be decoded to. Reset its timestamp. + decode_scan_timestamp_ns_ = new_scan_timestamp_ns; + } else { + /// When not cutting at the end of the FoV (i.e. the FoV is 360 deg or a cut occurs + /// somewhere within a non-360 deg FoV), the current scan is still being decoded to the + /// `decode` pointcloud but at the same time, points for the next pointcloud are arriving + /// and will be decoded to the `output` pointcloud (please forgive the naming for now). + /// Thus, reset the output pointcloud's timestamp. + output_scan_timestamp_ns_ = new_scan_timestamp_ns; + } + } - bool scan_completed = checkScanCompleted( - current_azimuth, - sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS); + if (!angle_corrector_.isInsideFoV(last_azimuth_, block_azimuth)) { + last_azimuth_ = block_azimuth; + continue; + } - if (scan_completed) { + convertReturns(block_id, n_returns); + + if (angle_corrector_.passedEmitAngle(last_azimuth_, block_azimuth)) { + // The current `decode` pointcloud is ready for publishing, swap buffers to continue with + // the last `output` pointcloud as the `decode pointcloud. std::swap(decode_pc_, output_pc_); - decode_pc_->clear(); + std::swap(decode_scan_timestamp_ns_, output_scan_timestamp_ns_); has_scanned_ = true; - output_scan_timestamp_ns_ = decode_scan_timestamp_ns_; - - // A new scan starts within the current packet, so the new scan's timestamp must be - // calculated as the packet timestamp plus the lowest time offset of any point in the - // remainder of the packet - decode_scan_timestamp_ns_ = hesai_packet::get_timestamp_ns(packet_) + - sensor_.getEarliestPointTimeOffsetForBlock(block_id, packet_); } - convertReturns(block_id, n_returns); - last_phase_ = current_azimuth; + last_azimuth_ = block_azimuth; } - return last_phase_; + return last_azimuth_; } bool hasScanned() override { return has_scanned_; } @@ -275,5 +338,4 @@ class HesaiDecoder : public HesaiScanDecoder } }; -} // namespace drivers -} // namespace nebula +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp index dec30dfcd..b05d64406 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp @@ -1,14 +1,24 @@ +// 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. + #pragma once #include #include #include - -namespace nebula -{ -namespace drivers -{ -namespace hesai_packet +#include +namespace nebula::drivers::hesai_packet { // FIXME(mojomex) This is a workaround for the compiler being pedantic about casting `enum class`s @@ -47,7 +57,7 @@ struct DateTime /// @brief Get seconds since epoch /// @return Whole seconds since epoch - uint64_t get_seconds() const + [[nodiscard]] uint64_t getSeconds() const { std::tm tm{}; tm.tm_year = year - 1900 + YearOffset; @@ -68,11 +78,11 @@ struct SecondsSinceEpoch /// @brief Get seconds since epoch /// @return Whole seconds since epoch - uint64_t get_seconds() const + [[nodiscard]] uint64_t getSeconds() const { uint64_t seconds = 0; - for (int i = 0; i < 5; ++i) { - seconds = (seconds << 8) | this->seconds[i]; + for (unsigned char second : this->seconds) { + seconds = (seconds << 8) | second; } return seconds; } @@ -133,39 +143,39 @@ struct Block { uint16_t azimuth; UnitT units[UnitN]; - typedef UnitT unit_t; + using unit_t = UnitT; - uint32_t get_azimuth() const { return azimuth; } + [[nodiscard]] uint32_t getAzimuth() const { return azimuth; } }; template struct FineAzimuthBlock { - typedef UnitT unit_t; + using unit_t = UnitT; uint16_t azimuth; uint8_t fine_azimuth; UnitT units[UnitN]; - uint32_t get_azimuth() const { return (azimuth << 8) + fine_azimuth; } + [[nodiscard]] uint32_t getAzimuth() const { return (azimuth << 8) + fine_azimuth; } }; template struct SOBBlock { - typedef UnitT unit_t; + using unit_t = UnitT; /// @brief Start of Block, 0xFFEE uint16_t sob; uint16_t azimuth; UnitT units[UnitN]; - uint32_t get_azimuth() const { return azimuth; } + [[nodiscard]] uint32_t getAzimuth() const { return azimuth; } }; template struct Body { - typedef BlockT block_t; + using block_t = BlockT; BlockT blocks[BlockN]; }; @@ -190,7 +200,7 @@ struct PacketBase /// @brief Get the number of returns for a given return mode /// @param return_mode The return mode /// @return The number of returns -int get_n_returns(uint8_t return_mode) +inline int get_n_returns(uint8_t return_mode) { switch (return_mode) { case return_mode::SINGLE_FIRST: @@ -218,10 +228,11 @@ int get_n_returns(uint8_t return_mode) template uint64_t get_timestamp_ns(const PacketT & packet) { - return packet.tail.date_time.get_seconds() * 1000000000 + packet.tail.timestamp * 1000; + return packet.tail.date_time.getSeconds() * 1000000000 + packet.tail.timestamp * 1000; } -/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, multiplied by this value, yield the distance in meters. +/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, +/// multiplied by this value, yield the distance in meters. /// @tparam PacketT The packet type /// @param packet The packet to get the distance unit from /// @return The distance unit in meters @@ -232,6 +243,4 @@ double get_dis_unit(const PacketT & packet) return packet.header.dis_unit / 1000.; } -} // namespace hesai_packet -} // namespace drivers -} // namespace nebula +} // namespace nebula::drivers::hesai_packet diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp index f0b4e3f8b..91e2a7767 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp @@ -1,17 +1,27 @@ +// 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_WS_HESAI_SCAN_DECODER_HPP #define NEBULA_WS_HESAI_SCAN_DECODER_HPP -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/point_types.hpp" - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include #include +#include -namespace nebula -{ -namespace drivers +namespace nebula::drivers { /// @brief Base class for Hesai LiDAR decoder class HesaiScanDecoder @@ -26,9 +36,9 @@ class HesaiScanDecoder HesaiScanDecoder() = default; /// @brief Parses PandarPacket and add its points to the point cloud - /// @param pandar_packet The incoming PandarPacket + /// @param packet The incoming PandarPacket /// @return The last azimuth processed - virtual int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) = 0; + virtual int unpack(const std::vector & packet) = 0; /// @brief Indicates whether one full scan is ready /// @return Whether a scan is ready @@ -38,6 +48,6 @@ class HesaiScanDecoder /// @return A tuple of point cloud and timestamp in nanoseconds virtual std::tuple getPointcloud() = 0; }; -} // namespace drivers -} // namespace nebula +} // namespace nebula::drivers + #endif // NEBULA_WS_HESAI_SCAN_DECODER_HPP diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp index 78f8a11bc..6492d35b0 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp @@ -1,15 +1,30 @@ +// 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. + #pragma once -#include "nebula_common/nebula_common.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" +#include + +#include #include +#include -namespace nebula -{ -namespace drivers +namespace nebula::drivers { enum class AngleCorrectionType { CALIBRATION, CORRECTION }; @@ -28,7 +43,7 @@ class HesaiSensor /// blocks) /// @return true if the reflectivity of the unit is strictly greater than that of all other units /// in return_units, false otherwise - static bool is_strongest( + static bool isStrongestReturn( uint32_t return_idx, const std::vector & return_units) { @@ -51,7 +66,7 @@ class HesaiSensor /// length 2 for dual-return with both units having the same channel but coming from different /// blocks) /// @return true if the unit is identical to any other one in return_units, false otherwise - static bool is_duplicate( + static bool isDuplicateReturn( uint32_t return_idx, const std::vector & return_units) { @@ -71,12 +86,11 @@ class HesaiSensor }; public: - typedef PacketT packet_t; - typedef typename std::conditional< + using packet_t = PacketT; + using angle_corrector_t = typename std::conditional< (AngleCorrection == AngleCorrectionType::CALIBRATION), AngleCorrectorCalibrationBased, - AngleCorrectorCorrectionBased>::type - angle_corrector_t; + AngleCorrectorCorrectionBased>::type; HesaiSensor() = default; virtual ~HesaiSensor() = default; @@ -99,7 +113,7 @@ class HesaiSensor int getEarliestPointTimeOffsetForBlock(uint32_t start_block_id, const PacketT & packet) { unsigned int n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); - int min_offset_ns = 0xFFFFFFFF; // MAXINT + int min_offset_ns = 0x7FFFFFFF; // MAXINT (max. positive value) for (uint32_t block_id = start_block_id; block_id < start_block_id + n_returns; ++block_id) { for (uint32_t channel_id = 0; channel_id < PacketT::N_CHANNELS; ++channel_id) { @@ -129,7 +143,7 @@ class HesaiSensor hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx, const std::vector & return_units) { - if (is_duplicate(return_idx, return_units)) { + if (isDuplicateReturn(return_idx, return_units)) { return ReturnType::IDENTICAL; } @@ -143,7 +157,7 @@ class HesaiSensor case hesai_packet::return_mode::SINGLE_LAST: return ReturnType::LAST; case hesai_packet::return_mode::DUAL_LAST_STRONGEST: - if (is_strongest(return_idx, return_units)) { + if (isStrongestReturn(return_idx, return_units)) { return return_idx == 0 ? ReturnType::LAST_STRONGEST : ReturnType::STRONGEST; } else { return return_idx == 0 ? ReturnType::LAST : ReturnType::SECONDSTRONGEST; @@ -153,7 +167,7 @@ class HesaiSensor case hesai_packet::return_mode::DUAL_FIRST_LAST: return return_idx == 0 ? ReturnType::FIRST : ReturnType::LAST; case hesai_packet::return_mode::DUAL_FIRST_STRONGEST: - if (is_strongest(return_idx, return_units)) { + if (isStrongestReturn(return_idx, return_units)) { return return_idx == 0 ? ReturnType::FIRST_STRONGEST : ReturnType::STRONGEST; } else { return return_idx == 0 ? ReturnType::FIRST : ReturnType::SECONDSTRONGEST; @@ -177,5 +191,4 @@ class HesaiSensor } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp index 65155efb4..80c43425d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp @@ -1,11 +1,25 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp" -namespace nebula -{ -namespace drivers +#include + +namespace nebula::drivers { namespace hesai_packet @@ -52,7 +66,7 @@ struct Tail128E3X struct Packet128E3X : public PacketBase<2, 128, 2, 100> { - typedef Body, Packet128E3X::N_BLOCKS> body_t; + using body_t = Body, Packet128E3X::N_BLOCKS>; Header12B header; body_t body; uint32_t crc_body; @@ -208,12 +222,12 @@ class Pandar128E3X : public HesaiSensor static constexpr size_t MAX_SCAN_BUFFER_POINTS = 691200; int getPacketRelativePointTimeOffset( - uint32_t block_id, uint32_t channel_id, const packet_t & packet) + uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); int block_offset_ns = 3148 - 27778 * 2 * (2 - block_id - 1) / n_returns; - int channel_offset_ns; + int channel_offset_ns = 0; bool is_hires_mode = packet.tail.operational_state == OperationalState::HIGH_RESOLUTION; bool is_nearfield = (hesai_packet::get_dis_unit(packet) * packet.body.blocks[block_id].units[channel_id].distance) <= 2.85f; @@ -271,5 +285,4 @@ class Pandar128E3X : public HesaiSensor } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index 41cee89b3..9e5cc862f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -1,12 +1,26 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp" -namespace nebula -{ -namespace drivers +#include + +namespace nebula::drivers { namespace hesai_packet @@ -14,7 +28,7 @@ namespace hesai_packet #pragma pack(push, 1) -typedef Packet128E3X Packet128E4X; +using Packet128E4X = Packet128E3X; #pragma pack(pop) @@ -23,11 +37,11 @@ typedef Packet128E3X Packet128E4X; // FIXME(mojomex): // The OT128 datasheet has entirely different numbers (and more azimuth states). // With the current sensor version, the numbers from the new datasheet are incorrect -// (clouds do not sync to ToS but ToS+.052s) +// (clouds do not sync to ToS but ToS+.052s) class Pandar128E4X : public HesaiSensor { private: -enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; + enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; static constexpr int firing_time_offset_static_ns_[128] = { 49758, 43224, 36690, 30156, 21980, 15446, 8912, 2378, 49758, 43224, 36690, 30156, 2378, @@ -71,17 +85,17 @@ enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; static constexpr size_t MAX_SCAN_BUFFER_POINTS = 691200; int getPacketRelativePointTimeOffset( - uint32_t block_id, uint32_t channel_id, const packet_t & packet) + uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); - int block_offset_ns; + int block_offset_ns = 0; if (n_returns == 1) { block_offset_ns = -27778 * 2 * (2 - block_id - 1); } else { block_offset_ns = 0; } - int channel_offset_ns; + int channel_offset_ns = 0; bool is_hires_mode = packet.tail.operational_state == OperationalState::HIGH_RESOLUTION; auto azimuth_state = packet.tail.geAzimuthState(block_id); @@ -119,5 +133,4 @@ enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp index 12003721e..d53da2ff1 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp @@ -1,11 +1,23 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp" -namespace nebula -{ -namespace drivers +namespace nebula::drivers { namespace hesai_packet @@ -31,7 +43,7 @@ struct Tail40P struct Packet40P : public PacketBase<10, 40, 2, 100> { - typedef Body, Packet40P::N_BLOCKS> body_t; + using body_t = Body, Packet40P::N_BLOCKS>; body_t body; Tail40P tail; }; @@ -42,7 +54,7 @@ struct Packet40P : public PacketBase<10, 40, 2, 100> /// packet type without a header. /// @return 0.004 (4mm) template <> -double get_dis_unit(const Packet40P & /* packet */) +inline double get_dis_unit(const Packet40P & /* packet */) { return 4 / 1000.; } @@ -72,5 +84,4 @@ class Pandar40 : public HesaiSensor } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp index b7a6640f3..c3920fd98 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp @@ -1,12 +1,24 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp" -namespace nebula -{ -namespace drivers +namespace nebula::drivers { namespace hesai_packet @@ -14,10 +26,10 @@ namespace hesai_packet #pragma pack(push, 1) -typedef Tail40P Tail64; +using Tail64 = Tail40P; struct Packet64 : public PacketBase<6, 64, 2, 100> { - typedef Body, Packet64::N_BLOCKS> body_t; + using body_t = Body, Packet64::N_BLOCKS>; Header8B header; body_t body; Tail64 tail; @@ -52,5 +64,4 @@ class Pandar64 : public HesaiSensor } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp index e3c85e857..c009c4437 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp @@ -1,11 +1,23 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp" -namespace nebula -{ -namespace drivers +namespace nebula::drivers { namespace hesai_packet @@ -32,8 +44,8 @@ struct TailAT128E2X struct PacketAT128E2X : public PacketBase<2, 128, 2, 100 * 256> { - typedef Body, PacketAT128E2X::N_BLOCKS> - body_t; + using body_t = + Body, PacketAT128E2X::N_BLOCKS>; Header12B header; body_t body; uint32_t crc_body; @@ -73,7 +85,7 @@ class PandarAT128 uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); - int block_offset_ns; + int block_offset_ns = 0; if (n_returns == 1) { block_offset_ns = -9249 - 41666 * (2 - block_id); } else { @@ -84,5 +96,4 @@ class PandarAT128 } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp index ac136bc20..85d23ddc4 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp @@ -1,11 +1,23 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp" -namespace nebula -{ -namespace drivers +namespace nebula::drivers { namespace hesai_packet @@ -32,7 +44,7 @@ struct TailQT128C2X struct PacketQT128C2X : public PacketBase<2, 128, 2, 100> { - typedef Body, PacketQT128C2X::N_BLOCKS> body_t; + using body_t = Body, PacketQT128C2X::N_BLOCKS>; Header12B header; body_t body; uint32_t crc_body; @@ -88,7 +100,7 @@ class PandarQT128 : public HesaiSensor auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); int block_offset_ns = 9000 + 111110 * (2 - block_id - 1) / n_returns; - int channel_offset_ns; + int channel_offset_ns = 0; if (n_returns == 1) { channel_offset_ns = block_id % 2 == 0 ? loop1[channel_id] : loop2[channel_id]; } else { @@ -99,5 +111,4 @@ class PandarQT128 : public HesaiSensor } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp index 1b7c4e54d..14cc494d1 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp @@ -1,11 +1,23 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp" -namespace nebula -{ -namespace drivers +namespace nebula::drivers { namespace hesai_packet @@ -25,7 +37,7 @@ struct TailQT64 struct PacketQT64 : public PacketBase<4, 64, 2, 100> { - typedef Body, PacketQT64::N_BLOCKS> body_t; + using body_t = Body, PacketQT64::N_BLOCKS>; Header12B header; body_t body; TailQT64 tail; @@ -61,5 +73,4 @@ class PandarQT64 : public HesaiSensor } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp index c9a5e7326..1bbf58b24 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp @@ -1,11 +1,25 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp" -namespace nebula -{ -namespace drivers +#include + +namespace nebula::drivers { namespace hesai_packet @@ -25,7 +39,7 @@ struct TailXT32 struct PacketXT32 : public PacketBase<8, 32, 2, 100> { - typedef Body, PacketXT32::N_BLOCKS> body_t; + using body_t = Body, PacketXT32::N_BLOCKS>; Header12B header; body_t body; TailXT32 tail; @@ -73,5 +87,4 @@ class PandarXT32 : public HesaiSensor } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp index 080876470..120a2622a 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp @@ -1,12 +1,24 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp" -namespace nebula -{ -namespace drivers +namespace nebula::drivers { namespace hesai_packet @@ -14,10 +26,10 @@ namespace hesai_packet #pragma pack(push, 1) -typedef TailXT32 TailXT32M2X; +using TailXT32M2X = TailXT32; struct PacketXT32M2X : public PacketBase<6, 32, 3, 100> { - typedef Body, PacketXT32M2X::N_BLOCKS> body_t; + using body_t = Body, PacketXT32M2X::N_BLOCKS>; Header12B header; body_t body; TailXT32M2X tail; @@ -39,7 +51,7 @@ class PandarXT32M : public HesaiSensor uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); - int block_offset_ns; + int block_offset_ns = 0; if (n_returns < 3) { block_offset_ns = 5632 - 50000 * ((8 - block_id - 1) / n_returns); } else /* n_returns == 3 */ { @@ -55,5 +67,4 @@ class PandarXT32M : public HesaiSensor } }; -} // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index c83788965..db07a8545 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -1,28 +1,37 @@ +// 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_HESAI_DRIVER_H #define NEBULA_HESAI_DRIVER_H #include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" #include "nebula_common/point_types.hpp" -#include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" -#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp" - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" #include -#include -#include -#include +#include +#include +#include namespace nebula { namespace drivers { /// @brief Hesai driver -class HesaiDriver : NebulaDriverBase +class HesaiDriver { private: /// @brief Current driver status @@ -30,16 +39,22 @@ class HesaiDriver : NebulaDriverBase /// @brief Decoder according to the model std::shared_ptr scan_decoder_; + template + std::shared_ptr InitializeDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); + public: HesaiDriver() = delete; /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver (for AT) + /// @param calibration_configuration CalibrationConfiguration for this driver (either + /// HesaiCalibrationConfiguration for sensors other than AT128 or HesaiCorrection for AT128) explicit HesaiDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration = nullptr); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); /// @brief Get current status of this driver /// @return Current status @@ -49,13 +64,13 @@ class HesaiDriver : NebulaDriverBase /// @param calibration_configuration /// @return Resulting status Status SetCalibrationConfiguration( - const CalibrationConfigurationBase & calibration_configuration) override; + const HesaiCalibrationConfigurationBase & calibration_configuration); - /// @brief Convert PandarScan message to point cloud - /// @param pandar_scan Message - /// @return tuple of Point cloud and timestamp - std::tuple ConvertScanToPointcloud( - const std::shared_ptr & pandar_scan); + /// @brief Convert raw packet to pointcloud + /// @param packet Packet to convert + /// @return Tuple of pointcloud and timestamp + std::tuple ParseCloudPacket( + const std::vector & packet); }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp index 60b345861..afd22650d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once #include "nebula_common/robosense/robosense_common.hpp" @@ -28,11 +42,11 @@ struct CorrectedAngleData class AngleCorrector { protected: - const std::shared_ptr sensor_calibration_; + const std::shared_ptr sensor_calibration_; public: explicit AngleCorrector( - const std::shared_ptr & sensor_calibration) + const std::shared_ptr & sensor_calibration) : sensor_calibration_(sensor_calibration) { } @@ -54,4 +68,4 @@ class AngleCorrector }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp index 99bd0c02f..1cb9557cb 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp @@ -1,9 +1,24 @@ +// 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. + #pragma once #include "nebula_common/robosense/robosense_common.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp" #include +#include namespace nebula { @@ -14,20 +29,20 @@ template class AngleCorrectorCalibrationBased : public AngleCorrector { private: - static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit; + static constexpr size_t MAX_AZIMUTH = 360 * AngleUnit; std::array elevation_angle_rad_{}; std::array azimuth_offset_rad_{}; - std::array block_azimuth_rad_{}; + std::array block_azimuth_rad_{}; std::array elevation_cos_{}; std::array elevation_sin_{}; - std::array, MAX_AZIMUTH_LEN> azimuth_cos_{}; - std::array, MAX_AZIMUTH_LEN> azimuth_sin_{}; + std::array, MAX_AZIMUTH> azimuth_cos_{}; + std::array, MAX_AZIMUTH> azimuth_sin_{}; public: explicit AngleCorrectorCalibrationBased( - const std::shared_ptr & sensor_calibration) + const std::shared_ptr & sensor_calibration) : AngleCorrector(sensor_calibration) { if (sensor_calibration == nullptr) { @@ -47,7 +62,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector elevation_sin_[channel_id] = sinf(elevation_angle_rad_[channel_id]); } - for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH_LEN; block_azimuth++) { + for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH; block_azimuth++) { block_azimuth_rad_[block_azimuth] = deg2rad(block_azimuth / static_cast(AngleUnit)); for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { @@ -82,4 +97,4 @@ class AngleCorrectorCalibrationBased : public AngleCorrector }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp index 92e58e304..948f16271 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" @@ -8,8 +22,11 @@ #include #include #include +#include +#include +#include -using namespace boost::endian; +using namespace boost::endian; // NOLINT(build/namespaces) namespace nebula { @@ -239,7 +256,7 @@ class BpearlV3 : public RobosenseSensor< int getPacketRelativePointTimeOffset( const uint32_t block_id, const uint32_t channel_id, - const std::shared_ptr & sensor_configuration) override + const std::shared_ptr & sensor_configuration) override { if (sensor_configuration->return_mode == ReturnMode::DUAL) return firing_time_offset_ns_dual_[block_id][channel_id]; @@ -247,7 +264,7 @@ class BpearlV3 : public RobosenseSensor< return firing_time_offset_ns_single_[block_id][channel_id]; } - ReturnMode getReturnMode(const robosense_packet::bpearl_v3::InfoPacket & info_packet) + ReturnMode getReturnMode(const robosense_packet::bpearl_v3::InfoPacket & info_packet) override { switch (info_packet.return_mode.value()) { case DUAL_RETURN_FLAG: @@ -262,12 +279,12 @@ class BpearlV3 : public RobosenseSensor< } RobosenseCalibrationConfiguration getSensorCalibration( - const robosense_packet::bpearl_v3::InfoPacket & info_packet) + const robosense_packet::bpearl_v3::InfoPacket & info_packet) override { return info_packet.sensor_calibration.getCalibration(); } - bool getSyncStatus(const robosense_packet::bpearl_v3::InfoPacket & info_packet) + bool getSyncStatus(const robosense_packet::bpearl_v3::InfoPacket & info_packet) override { switch (info_packet.sync_status.value()) { case SYNC_STATUS_INVALID_FLAG: @@ -282,7 +299,7 @@ class BpearlV3 : public RobosenseSensor< } std::map getSensorInfo( - const robosense_packet::bpearl_v3::InfoPacket & info_packet) + const robosense_packet::bpearl_v3::InfoPacket & info_packet) override { std::map sensor_info; sensor_info["motor_speed"] = std::to_string(info_packet.motor_speed.value()); @@ -457,4 +474,4 @@ class BpearlV3 : public RobosenseSensor< } }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp index 681ad2ba2..231fe9e64 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" @@ -8,8 +22,11 @@ #include #include #include +#include +#include +#include -using namespace boost::endian; +using namespace boost::endian; // NOLINT(build/namespaces) namespace nebula { @@ -209,7 +226,7 @@ class BpearlV4 : public RobosenseSensor< int getPacketRelativePointTimeOffset( const uint32_t block_id, const uint32_t channel_id, - const std::shared_ptr & sensor_configuration) override + const std::shared_ptr & sensor_configuration) override { if (sensor_configuration->return_mode == ReturnMode::DUAL) return firing_time_offset_ns_dual_[block_id][channel_id]; @@ -217,7 +234,7 @@ class BpearlV4 : public RobosenseSensor< return firing_time_offset_ns_single_[block_id][channel_id]; } - ReturnMode getReturnMode(const robosense_packet::bpearl_v4::InfoPacket & info_packet) + ReturnMode getReturnMode(const robosense_packet::bpearl_v4::InfoPacket & info_packet) override { switch (info_packet.return_mode.value()) { case DUAL_RETURN_FLAG: @@ -234,12 +251,12 @@ class BpearlV4 : public RobosenseSensor< } RobosenseCalibrationConfiguration getSensorCalibration( - const robosense_packet::bpearl_v4::InfoPacket & info_packet) + const robosense_packet::bpearl_v4::InfoPacket & info_packet) override { return info_packet.sensor_calibration.getCalibration(); } - bool getSyncStatus(const robosense_packet::bpearl_v4::InfoPacket & info_packet) + bool getSyncStatus(const robosense_packet::bpearl_v4::InfoPacket & info_packet) override { switch (info_packet.time_sync_state.value()) { case SYNC_STATUS_INVALID_FLAG: @@ -254,7 +271,7 @@ class BpearlV4 : public RobosenseSensor< } std::map getSensorInfo( - const robosense_packet::bpearl_v4::InfoPacket & info_packet) + const robosense_packet::bpearl_v4::InfoPacket & info_packet) override { std::map sensor_info; sensor_info["motor_speed"] = std::to_string(info_packet.motor_speed_setting.value()); @@ -376,4 +393,4 @@ class BpearlV4 : public RobosenseSensor< } }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp index 0780e3c24..5da0320e5 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" @@ -8,8 +22,11 @@ #include #include #include +#include +#include +#include -using namespace boost::endian; +using namespace boost::endian; // NOLINT(build/namespaces) namespace nebula { @@ -248,7 +265,7 @@ class Helios int getPacketRelativePointTimeOffset( const uint32_t block_id, const uint32_t channel_id, - const std::shared_ptr & sensor_configuration) override + const std::shared_ptr & sensor_configuration) override { if (sensor_configuration->return_mode == ReturnMode::DUAL) return firing_time_offset_ns_dual_[block_id][channel_id]; @@ -256,7 +273,7 @@ class Helios return firing_time_offset_ns_single_[block_id][channel_id]; } - ReturnMode getReturnMode(const robosense_packet::helios::InfoPacket & info_packet) + ReturnMode getReturnMode(const robosense_packet::helios::InfoPacket & info_packet) override { switch (info_packet.return_mode.value()) { case DUAL_RETURN_FLAG: @@ -273,12 +290,12 @@ class Helios } RobosenseCalibrationConfiguration getSensorCalibration( - const robosense_packet::helios::InfoPacket & info_packet) + const robosense_packet::helios::InfoPacket & info_packet) override { return info_packet.sensor_calibration.getCalibration(); } - bool getSyncStatus(const robosense_packet::helios::InfoPacket & info_packet) + bool getSyncStatus(const robosense_packet::helios::InfoPacket & info_packet) override { switch (info_packet.sync_status.value()) { case SYNC_STATUS_INVALID_FLAG: @@ -293,7 +310,7 @@ class Helios } std::map getSensorInfo( - const robosense_packet::helios::InfoPacket & info_packet) + const robosense_packet::helios::InfoPacket & info_packet) override { std::map sensor_info; sensor_info["motor_speed"] = std::to_string(info_packet.motor_speed.value()); @@ -452,4 +469,4 @@ class Helios }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp index 79630bc23..48bf77eb2 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once #include "nebula_common/robosense/robosense_common.hpp" @@ -6,8 +20,10 @@ #include -#include "robosense_msgs/msg/robosense_packet.hpp" -#include "robosense_msgs/msg/robosense_scan.hpp" +#include +#include +#include +#include namespace nebula { @@ -18,7 +34,7 @@ class RobosenseDecoder : public RobosenseScanDecoder { protected: /// @brief Configuration for this decoder - const std::shared_ptr sensor_configuration_; + const std::shared_ptr sensor_configuration_; /// @brief The sensor definition, used for return mode and time offset handling SensorT sensor_{}; @@ -47,15 +63,15 @@ class RobosenseDecoder : public RobosenseScanDecoder /// @brief Validates and parses MsopPacket. Currently only checks size, not checksums etc. /// @param msop_packet The incoming MsopPacket /// @return Whether the packet was parsed successfully - bool parsePacket(const robosense_msgs::msg::RobosensePacket & msop_packet) + bool parsePacket(const std::vector & msop_packet) { - if (msop_packet.data.size() < sizeof(typename SensorT::packet_t)) { + if (msop_packet.size() < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << msop_packet.data.size() << " | Expected at least:" - << sizeof(typename SensorT::packet_t)); + logger_, "Packet size mismatch: " << msop_packet.size() << " | Expected at least: " + << sizeof(typename SensorT::packet_t)); return false; } - if (std::memcpy(&packet_, msop_packet.data.data(), sizeof(typename SensorT::packet_t))) { + if (std::memcpy(&packet_, msop_packet.data(), sizeof(typename SensorT::packet_t))) { return true; } @@ -189,8 +205,8 @@ class RobosenseDecoder : public RobosenseScanDecoder /// @param calibration_configuration Calibration for this decoder /// calibration_configuration is set) explicit RobosenseDecoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration) : sensor_configuration_(sensor_configuration), angle_corrector_(calibration_configuration), logger_(rclcpp::get_logger("RobosenseDecoder")) @@ -205,7 +221,7 @@ class RobosenseDecoder : public RobosenseScanDecoder output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); } - int unpack(const robosense_msgs::msg::RobosensePacket & msop_packet) override + int unpack(const std::vector & msop_packet) override { if (!parsePacket(msop_packet)) { return -1; @@ -265,4 +281,4 @@ class RobosenseDecoder : public RobosenseScanDecoder }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp index 86b9bc4aa..02a00792c 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp @@ -1,13 +1,27 @@ +// 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. + #pragma once #include "nebula_common/robosense/robosense_common.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" #include #include -#include +#include +#include #include namespace nebula @@ -36,8 +50,8 @@ class RobosenseInfoDecoder : public RobosenseInfoDecoderBase const auto packet_size = raw_packet.size(); if (packet_size < sizeof(typename SensorT::info_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << packet_size << " | Expected at least:" - << sizeof(typename SensorT::info_t)); + logger_, "Packet size mismatch: " << packet_size << " | Expected at least: " + << sizeof(typename SensorT::info_t)); return false; } try { @@ -81,4 +95,4 @@ class RobosenseInfoDecoder : public RobosenseInfoDecoderBase }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp index d97dc4e1c..87d8279f0 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp @@ -1,5 +1,25 @@ +// 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. + #pragma once +#include + +#include +#include +#include +#include namespace nebula { namespace drivers @@ -39,4 +59,4 @@ class RobosenseInfoDecoderBase }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp index 607c44aac..b4a7d3061 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp @@ -1,12 +1,30 @@ +// 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. + #pragma once -#include "boost/endian/buffers.hpp" +#include + +#include #include #include +#include +#include #include -using namespace boost::endian; +using namespace boost::endian; // NOLINT(build/namespaces) namespace nebula { @@ -34,7 +52,6 @@ struct Timestamp } }; - struct Unit { big_uint16_buf_t distance; @@ -137,9 +154,8 @@ struct ChannelAngleCorrection [[nodiscard]] float getAngle() const { - return sign.value() == ANGLE_SIGN_FLAG - ? static_cast(angle.value()) / 100.0f - : static_cast(angle.value()) / -100.0f; + return sign.value() == ANGLE_SIGN_FLAG ? static_cast(angle.value()) / 100.0f + : static_cast(angle.value()) / -100.0f; } }; @@ -218,7 +234,7 @@ struct SerialNumber /// @brief Get the number of returns for a given return mode /// @param return_mode The return mode /// @return The number of returns -size_t get_n_returns(ReturnMode return_mode) +inline size_t get_n_returns(ReturnMode return_mode) { if (return_mode == ReturnMode::DUAL) { return 2; @@ -257,11 +273,11 @@ double get_dis_unit(const PacketT & packet) /// @brief Convert raw angle value from packet to std::string /// @param raw_angle The raw angle value from the packet /// @return The angle as std::string -std::string get_float_value(const uint16_t & raw_angle) +inline std::string get_float_value(const uint16_t & raw_angle) { return std::to_string(static_cast(raw_angle) / 100.0f); } } // namespace robosense_packet } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp index c5beaa342..b87f4f078 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp @@ -1,12 +1,23 @@ +// 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. + #pragma once #include "nebula_common/point_types.hpp" -#include "nebula_common/robosense/robosense_common.hpp" - -#include "robosense_msgs/msg/robosense_packet.hpp" -#include "robosense_msgs/msg/robosense_scan.hpp" #include +#include namespace nebula { @@ -27,7 +38,7 @@ class RobosenseScanDecoder /// @brief Parses RobosensePacket and add its points to the point cloud /// @param msop_packet The incoming MsopPacket /// @return The last azimuth processed - virtual int unpack(const robosense_msgs::msg::RobosensePacket & msop_packet) = 0; + virtual int unpack(const std::vector & msop_packet) = 0; /// @brief Indicates whether one full scan is ready /// @return Whether a scan is ready diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp index 4e8b3c25a..b88e49181 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once #include "nebula_common/nebula_common.hpp" @@ -5,7 +19,11 @@ #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" #include -#include +#include +#include +#include +#include +#include namespace nebula { @@ -17,7 +35,6 @@ namespace drivers template class RobosenseSensor { -private: public: typedef PacketT packet_t; typedef InfoPacketT info_t; @@ -35,7 +52,7 @@ class RobosenseSensor /// @return The relative time offset in nanoseconds virtual int getPacketRelativePointTimeOffset( uint32_t block_id, uint32_t channel_id, - const std::shared_ptr & sensor_configuration) = 0; + const std::shared_ptr & sensor_configuration) = 0; /// @brief For a given start block index, find the earliest (lowest) relative time offset of any /// point in the packet in or after the start block @@ -45,7 +62,7 @@ class RobosenseSensor /// after the start block, in nanoseconds int getEarliestPointTimeOffsetForBlock( uint32_t start_block_id, - const std::shared_ptr & sensor_configuration) + const std::shared_ptr & sensor_configuration) { const auto n_returns = robosense_packet::get_n_returns(sensor_configuration->return_mode); int min_offset_ns = std::numeric_limits::max(); @@ -119,6 +136,14 @@ class RobosenseSensor return ReturnType::UNKNOWN; } } + + virtual ReturnMode getReturnMode(const info_t & info_packet) = 0; + + virtual RobosenseCalibrationConfiguration getSensorCalibration(const info_t & info_packet) = 0; + + virtual bool getSyncStatus(const info_t & info_packet) = 0; + + virtual std::map getSensorInfo(const info_t & info_packet) = 0; }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp index fe9968162..55ea8d31d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once #include "nebula_common/nebula_common.hpp" @@ -5,16 +19,13 @@ #include "nebula_common/point_types.hpp" #include "nebula_common/robosense/robosense_common.hpp" #include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp" - -#include "robosense_msgs/msg/robosense_packet.hpp" -#include "robosense_msgs/msg/robosense_scan.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp" #include -#include -#include -#include +#include +#include +#include namespace nebula { @@ -37,8 +48,9 @@ class RobosenseDriver : NebulaDriverBase /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver explicit RobosenseDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); /// @brief Get current status of this driver /// @return Current status @@ -53,8 +65,8 @@ class RobosenseDriver : NebulaDriverBase /// @brief Convert RobosenseScan message to point cloud /// @param robosense_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ConvertScanToPointcloud( - const std::shared_ptr & robosense_scan); + std::tuple ParseCloudPacket( + const std::vector & packet); }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp index bf44a3ce5..859e97c4e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp @@ -1,25 +1,32 @@ +// 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. + #pragma once #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "nebula_common/point_types.hpp" #include "nebula_common/robosense/robosense_common.hpp" -#include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp" -#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp" -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" - #include #include +#include +#include #include #include +#include namespace nebula { @@ -41,7 +48,7 @@ class RobosenseInfoDriver /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this driver explicit RobosenseInfoDriver( - const std::shared_ptr & sensor_configuration); + const std::shared_ptr & sensor_configuration); /// @brief Get current status of this driver /// @return Current status diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp index e08d0c1dd..5fab49cc9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp @@ -1,25 +1,40 @@ +// 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_WS_VELODYNE_SCAN_DECODER_HPP #define NEBULA_WS_VELODYNE_SCAN_DECODER_HPP + +#include +#include +#include #include +#include +#include + #include +#include #include #include #include #include +#include #include -#include - -#include "nebula_common/point_types.hpp" -#include "nebula_common/velodyne/velodyne_calibration_decoder.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" - -#include -#include - #include +#include namespace nebula { @@ -55,8 +70,8 @@ static const float VLP16_DSR_TOFFSET = 2.304f; // [µs] static const float VLP16_FIRING_TOFFSET = 55.296f; // [µs] /** Special Defines for VLP32 support **/ -static const float VLP32_CHANNEL_DURATION = 2.304f; // [µs] -static const float VLP32_SEQ_DURATION = 55.296f; // [µs] +static const float VLP32_CHANNEL_DURATION = 2.304f; // [µs] +static const float VLP32_SEQ_DURATION = 55.296f; // [µs] /** Special Definitions for VLS128 support **/ static const float VLP128_DISTANCE_RESOLUTION = 0.004f; // [m] @@ -73,6 +88,10 @@ static const float VLS128_CHANNEL_DURATION = static const float VLS128_SEQ_DURATION = 53.3f; // [µs] Sequence is a set of laser firings including recharging +static const size_t OFFSET_FIRST_AZIMUTH = 2; +static const size_t OFFSET_LAST_AZIMUTH = 1102; +static const uint32_t DEGREE_SUBDIVISIONS = 100; + /** \brief Raw Velodyne data block. * * Each block contains data from either the upper or lower laser @@ -136,23 +155,60 @@ enum RETURN_TYPE { /// @brief Base class for Velodyne LiDAR decoder class VelodyneScanDecoder { +private: + size_t processed_packets_{0}; + uint32_t prev_packet_first_azm_phased_{0}; + bool has_scanned_{false}; + protected: + /// @brief Checks if the current packet completes the ongoing scan. + /// TODO: this has been moved from velodyne_hw_interface.cpp and is a temporary solution until + /// the Velodyne decoder uses the same structure as Hesai/Robosense + /// @param packet The packet buffer to extract azimuths from + /// @param packet_seconds The seconds-since-epoch part of the packet's timestamp + /// @param phase The sensor's scan phase used for scan cutting + void checkAndHandleScanComplete( + const std::vector & packet, int32_t packet_seconds, const uint32_t phase) + { + if (has_scanned_) { + processed_packets_ = 0; + reset_pointcloud(packet_seconds); + } + + has_scanned_ = false; + processed_packets_++; + + uint32_t packet_first_azm = packet[OFFSET_FIRST_AZIMUTH]; // lower word of azimuth block 0 + packet_first_azm |= packet[OFFSET_FIRST_AZIMUTH + 1] << 8; // higher word of azimuth block 0 + + uint32_t packet_last_azm = packet[OFFSET_LAST_AZIMUTH]; + packet_last_azm |= packet[OFFSET_LAST_AZIMUTH + 1] << 8; + + const uint32_t max_azi = 360 * DEGREE_SUBDIVISIONS; + + uint32_t packet_first_azm_phased = (max_azi + packet_first_azm - phase) % max_azi; + uint32_t packet_last_azm_phased = (max_azi + packet_last_azm - phase) % max_azi; + + has_scanned_ = + processed_packets_ > 1 && (packet_last_azm_phased < packet_first_azm_phased || + packet_first_azm_phased < prev_packet_first_azm_phased_); + + prev_packet_first_azm_phased_ = packet_first_azm_phased; + } + /// @brief Decoded point cloud drivers::NebulaPointCloudPtr scan_pc_; /// @brief Point cloud overflowing from one cycle drivers::NebulaPointCloudPtr overflow_pc_; - uint16_t scan_phase_{}; - uint16_t last_phase_{}; - bool has_scanned_ = true; double dual_return_distance_threshold_{}; // Velodyne does this internally, this will not be // implemented here double scan_timestamp_{}; /// @brief SensorConfiguration for this decoder - std::shared_ptr sensor_configuration_; + std::shared_ptr sensor_configuration_; /// @brief Calibration for this decoder - std::shared_ptr calibration_configuration_; + std::shared_ptr calibration_configuration_; public: VelodyneScanDecoder(VelodyneScanDecoder && c) = delete; @@ -165,7 +221,7 @@ class VelodyneScanDecoder /// @brief Virtual function for parsing and shaping VelodynePacket /// @param pandar_packet - virtual void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0; + virtual void unpack(const std::vector & packet, int32_t packet_seconds) = 0; /// @brief Virtual function for parsing VelodynePacket based on packet structure /// @param pandar_packet /// @return Resulting flag @@ -173,7 +229,8 @@ class VelodyneScanDecoder /// @brief Virtual function for getting the flag indicating whether one cycle is ready /// @return Readied - virtual bool hasScanned() = 0; + bool hasScanned() { return has_scanned_; } + /// @brief Calculation of points in each packet /// @return # of points virtual int pointsPerPacket() = 0; @@ -182,8 +239,7 @@ class VelodyneScanDecoder /// @return tuple of Point cloud and timestamp virtual std::tuple get_pointcloud() = 0; /// @brief Resetting point cloud buffer - /// @param n_pts # of points - virtual void reset_pointcloud(size_t n_pts, double time_stamp) = 0; + virtual void reset_pointcloud(double time_stamp) = 0; /// @brief Resetting overflowed point cloud buffer virtual void reset_overflow(double time_stamp) = 0; }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp index 730f7045f..645b7fdaf 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp" @@ -25,14 +39,12 @@ class Vlp16Decoder : public VelodyneScanDecoder /// @param sensor_configuration SensorConfiguration for this decoder /// @param calibration_configuration Calibration for this decoder explicit Vlp16Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); /// @brief Parsing and shaping VelodynePacket /// @param velodyne_packet - void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - /// @brief Get the flag indicating whether one cycle is ready - /// @return Readied - bool hasScanned() override; + void unpack(const std::vector & packet, int32_t packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; @@ -40,8 +52,7 @@ class Vlp16Decoder : public VelodyneScanDecoder /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer - /// @param n_pts # of points - void reset_pointcloud(size_t n_pts, double time_stamp) override; + void reset_pointcloud(double time_stamp) override; /// @brief Resetting overflowed point cloud buffer void reset_overflow(double time_stamp) override; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp index 0ffa1e3ce..3d434af0c 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp" @@ -24,14 +38,12 @@ class Vlp32Decoder : public VelodyneScanDecoder /// @param sensor_configuration SensorConfiguration for this decoder /// @param calibration_configuration Calibration for this decoder explicit Vlp32Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); /// @brief Parsing and shaping VelodynePacket /// @param velodyne_packet - void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - /// @brief Get the flag indicating whether one cycle is ready - /// @return Readied - bool hasScanned() override; + void unpack(const std::vector & packet, int32_t packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; @@ -39,8 +51,7 @@ class Vlp32Decoder : public VelodyneScanDecoder /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer - /// @param n_pts # of points - void reset_pointcloud(size_t n_pts, double time_stamp) override; + void reset_pointcloud(double time_stamp) override; /// @brief Resetting overflowed point cloud buffer void reset_overflow(double time_stamp) override; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp index e440096e9..0dc4ba563 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once #include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp" @@ -6,6 +20,9 @@ #include #include +#include +#include +#include namespace nebula { @@ -21,14 +38,12 @@ class Vls128Decoder : public VelodyneScanDecoder /// @param sensor_configuration SensorConfiguration for this decoder /// @param calibration_configuration Calibration for this decoder explicit Vls128Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); /// @brief Parsing and shaping VelodynePacket /// @param velodyne_packet - void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - /// @brief Get the flag indicating whether one cycle is ready - /// @return Readied - bool hasScanned() override; + void unpack(const std::vector & packet, int32_t packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; @@ -36,8 +51,7 @@ class Vls128Decoder : public VelodyneScanDecoder /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer - /// @param n_pts # of points - void reset_pointcloud(size_t n_pts, double time_stamp) override; + void reset_pointcloud(double time_stamp) override; /// @brief Resetting overflowed point cloud buffer void reset_overflow(double time_stamp) override; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp index d0257b82d..746cec719 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp @@ -1,3 +1,17 @@ +// 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_VELODYNE_DRIVER_H #define NEBULA_VELODYNE_DRIVER_H @@ -14,8 +28,11 @@ #include #include +#include #include #include +#include +#include namespace nebula { @@ -36,8 +53,9 @@ class VelodyneDriver : NebulaDriverBase /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver VelodyneDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); /// @brief Setting CalibrationConfiguration (not used) /// @param calibration_configuration @@ -52,8 +70,8 @@ class VelodyneDriver : NebulaDriverBase /// @brief Convert VelodyneScan message to point cloud /// @param velodyne_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ConvertScanToPointcloud( - const std::shared_ptr & velodyne_scan); + std::tuple ParseCloudPacket( + const std::vector & packet, int32_t packet_seconds); }; } // namespace drivers diff --git a/nebula_decoders/package.xml b/nebula_decoders/package.xml index 462f3956e..85c7a9a87 100644 --- a/nebula_decoders/package.xml +++ b/nebula_decoders/package.xml @@ -2,7 +2,7 @@ nebula_decoders - 0.1.0 + 0.2.0 Nebula Decoders Library MAP IV @@ -20,7 +20,7 @@ nebula_msgs pandar_msgs pcl_conversions - radar_msgs + rclcpp robosense_msgs sensor_msgs velodyne_msgs diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index fb2a44f60..548867b7f 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -14,7 +14,7 @@ #include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" -#include "nebula_common/continental/continental_ars548.hpp" +#include #include #include @@ -26,10 +26,15 @@ namespace drivers namespace continental_ars548 { ContinentalARS548Decoder::ContinentalARS548Decoder( - const std::shared_ptr & + const std::shared_ptr & sensor_configuration) { - sensor_configuration_ = sensor_configuration; + config_ptr_ = sensor_configuration; +} + +Status ContinentalARS548Decoder::GetStatus() +{ + return Status::OK; } Status ContinentalARS548Decoder::RegisterDetectionListCallback( @@ -55,14 +60,17 @@ Status ContinentalARS548Decoder::RegisterSensorStatusCallback( return Status::OK; } -bool ContinentalARS548Decoder::ProcessPackets( - const nebula_msgs::msg::NebulaPackets & nebula_packets) +Status ContinentalARS548Decoder::RegisterPacketsCallback( + std::function)> nebula_packets_callback) { - if (nebula_packets.packets.size() != 1) { - return false; - } + nebula_packets_callback_ = std::move(nebula_packets_callback); + return Status::OK; +} - const auto & data = nebula_packets.packets[0].data; +bool ContinentalARS548Decoder::ProcessPacket( + std::unique_ptr packet_msg) +{ + const auto & data = packet_msg->data; if (data.size() < sizeof(HeaderPacket)) { return false; @@ -82,13 +90,13 @@ bool ContinentalARS548Decoder::ProcessPackets( return false; } - return ParseDetectionsListPacket(data, nebula_packets.header); + ParseDetectionsListPacket(*packet_msg); } else if (header.method_id.value() == OBJECT_LIST_METHOD_ID) { if (data.size() != OBJECT_LIST_UDP_PAYLOAD || header.length.value() != OBJECT_LIST_PDU_LENGTH) { return false; } - return ParseObjectsListPacket(data, nebula_packets.header); + ParseObjectsListPacket(*packet_msg); } else if (header.method_id.value() == SENSOR_STATUS_METHOD_ID) { if ( data.size() != SENSOR_STATUS_UDP_PAYLOAD || @@ -96,30 +104,39 @@ bool ContinentalARS548Decoder::ProcessPackets( return false; } - return ParseSensorStatusPacket(data, nebula_packets.header); + ParseSensorStatusPacket(*packet_msg); + } + + // Some messages are not parsed but are still sent to the user (e.g., filters) + if (nebula_packets_callback_) { + auto packets_msg = std::make_unique(); + packets_msg->packets.emplace_back(std::move(*packet_msg)); + packets_msg->header.stamp = packet_msg->stamp; + packets_msg->header.frame_id = config_ptr_->frame_id; + nebula_packets_callback_(std::move(packets_msg)); } return true; } bool ContinentalARS548Decoder::ParseDetectionsListPacket( - const std::vector & data, const std_msgs::msg::Header & header) + const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; DetectionListPacket detection_list; - assert(sizeof(DetectionListPacket) == data.size()); + assert(sizeof(DetectionListPacket) == packet_msg.data.size()); - std::memcpy(&detection_list, data.data(), sizeof(DetectionListPacket)); + std::memcpy(&detection_list, packet_msg.data.data(), sizeof(DetectionListPacket)); - msg.header.frame_id = sensor_configuration_->frame_id; + msg.header.frame_id = config_ptr_->frame_id; - if (sensor_configuration_->use_sensor_time) { + if (config_ptr_->use_sensor_time) { msg.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); msg.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); } else { - msg.header.stamp = header.stamp; + msg.header.stamp = packet_msg.stamp; } msg.stamp_sync_status = detection_list.stamp.timestamp_sync_status; @@ -222,29 +239,31 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket( detection_msg.range_rate_std = detection.range_rate_std.value(); } - detection_list_callback_(std::move(msg_ptr)); + if (detection_list_callback_) { + detection_list_callback_(std::move(msg_ptr)); + } return true; } bool ContinentalARS548Decoder::ParseObjectsListPacket( - const std::vector & data, const std_msgs::msg::Header & header) + const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; ObjectListPacket object_list; - assert(sizeof(ObjectListPacket) == data.size()); + assert(sizeof(ObjectListPacket) == packet_msg.data.size()); - std::memcpy(&object_list, data.data(), sizeof(object_list)); + std::memcpy(&object_list, packet_msg.data.data(), sizeof(object_list)); - msg.header.frame_id = sensor_configuration_->object_frame; + msg.header.frame_id = config_ptr_->object_frame; - if (sensor_configuration_->use_sensor_time) { + if (config_ptr_->use_sensor_time) { msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); msg.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); } else { - msg.header.stamp = header.stamp; + msg.header.stamp = packet_msg.stamp; } msg.stamp_sync_status = object_list.stamp.timestamp_sync_status; @@ -374,16 +393,18 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( object_msg.shape_width_edge_mean = object.shape_width_edge_mean.value(); } - object_list_callback_(std::move(msg_ptr)); + if (object_list_callback_) { + object_list_callback_(std::move(msg_ptr)); + } return true; } bool ContinentalARS548Decoder::ParseSensorStatusPacket( - const std::vector & data, [[maybe_unused]] const std_msgs::msg::Header & header) + const nebula_msgs::msg::NebulaPacket & packet_msg) { SensorStatusPacket sensor_status_packet; - std::memcpy(&sensor_status_packet, data.data(), sizeof(SensorStatusPacket)); + std::memcpy(&sensor_status_packet, packet_msg.data.data(), sizeof(SensorStatusPacket)); radar_status_.timestamp_nanoseconds = sensor_status_packet.stamp.timestamp_nanoseconds.value(); radar_status_.timestamp_seconds = sensor_status_packet.stamp.timestamp_seconds.value(); @@ -613,7 +634,9 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( radar_status_.status_total_count++; radar_status_.radar_invalid_count += sensor_status_packet.radar_status == 2 ? 1 : 0; - sensor_status_callback_(radar_status_); + if (sensor_status_callback_) { + sensor_status_callback_(radar_status_); + } return true; } diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp new file mode 100644 index 000000000..d559b5714 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -0,0 +1,1210 @@ +// 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_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp" + +#include +#include + +#include + +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +ContinentalSRR520Decoder::ContinentalSRR520Decoder( + const std::shared_ptr & + sensor_configuration) +{ + sensor_configuration_ = sensor_configuration; + + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); + + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); + + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); +} + +Status ContinentalSRR520Decoder::GetStatus() +{ + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterNearDetectionListCallback( + std::function)> + detection_list_callback) +{ + near_detection_list_callback_ = std::move(detection_list_callback); + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterHRRDetectionListCallback( + std::function)> + detection_list_callback) +{ + hrr_detection_list_callback_ = std::move(detection_list_callback); + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterObjectListCallback( + std::function)> + object_list_callback) +{ + object_list_callback_ = std::move(object_list_callback); + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterStatusCallback( + std::function)> status_callback) +{ + status_callback_ = std::move(status_callback); + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterSyncFollowUpCallback( + std::function sync_follow_up_callback) +{ + sync_follow_up_callback_ = std::move(sync_follow_up_callback); + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterPacketsCallback( + std::function)> nebula_packets_callback) +{ + nebula_packets_callback_ = std::move(nebula_packets_callback); + return Status::OK; +} + +bool ContinentalSRR520Decoder::ProcessPacket( + std::unique_ptr packet_msg) +{ + const uint32_t can_message_id = (static_cast(packet_msg->data[0]) << 24) | + (static_cast(packet_msg->data[1]) << 16) | + (static_cast(packet_msg->data[2]) << 8) | + static_cast(packet_msg->data[3]); + + std::size_t payload_size = packet_msg->data.size() - 4; + + if (can_message_id == RDI_NEAR_HEADER_CAN_MESSAGE_ID) { + if (payload_size != RDI_NEAR_HEADER_PACKET_SIZE) { + PrintError("RDI_NEAR_HEADER_CAN_MESSAGE_ID message with invalid size"); + return false; + } + ProcessNearHeaderPacket(std::move(packet_msg)); + } else if (can_message_id == RDI_NEAR_ELEMENT_CAN_MESSAGE_ID) { + if (payload_size != RDI_NEAR_ELEMENT_PACKET_SIZE) { + PrintError("RDI_NEAR_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessNearElementPacket(std::move(packet_msg)); + } else if (can_message_id == RDI_HRR_HEADER_CAN_MESSAGE_ID) { + if (payload_size != RDI_HRR_HEADER_PACKET_SIZE) { + PrintError("RDI_HRR_HEADER_CAN_MESSAGE_ID message with invalid size"); + return false; + } + ProcessHRRHeaderPacket(std::move(packet_msg)); + } else if (can_message_id == RDI_HRR_ELEMENT_CAN_MESSAGE_ID) { + if (payload_size != RDI_HRR_ELEMENT_PACKET_SIZE) { + PrintError("RDI_HRR_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessHRRElementPacket(std::move(packet_msg)); + } else if (can_message_id == OBJECT_HEADER_CAN_MESSAGE_ID) { + if (payload_size != OBJECT_HEADER_PACKET_SIZE) { + PrintError("OBJECT_HEADER_CAN_MESSAGE_ID message with invalid size"); + return false; + } + ProcessObjectHeaderPacket(std::move(packet_msg)); + } else if (can_message_id == OBJECT_CAN_MESSAGE_ID) { + if (payload_size != OBJECT_PACKET_SIZE) { + PrintError("OBJECT_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessObjectElementPacket(std::move(packet_msg)); + } else if (can_message_id == CRC_LIST_CAN_MESSAGE_ID) { + if (payload_size != CRC_LIST_PACKET_SIZE) { + PrintError("CRC_LIST_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessCRCListPacket(std::move(packet_msg)); + } else if (can_message_id == STATUS_CAN_MESSAGE_ID) { + if (payload_size != STATUS_PACKET_SIZE) { + PrintError("CRC_LIST_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessSensorStatusPacket(std::move(packet_msg)); + } else if (can_message_id == SYNC_FOLLOW_UP_CAN_MESSAGE_ID) { + if (payload_size != SYNC_FOLLOW_UP_CAN_PACKET_SIZE) { + PrintError("SYNC_FOLLOW_UP_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessSyncFollowUpPacket(std::move(packet_msg)); + } else if ( + can_message_id != VEH_DYN_CAN_MESSAGE_ID && can_message_id != SENSOR_CONFIG_CAN_MESSAGE_ID) { + PrintError("Unrecognized message ID=" + std::to_string(can_message_id)); + return false; + } + + return true; +} + +void ContinentalSRR520Decoder::ProcessNearHeaderPacket( + std::unique_ptr packet_msg) +{ + constexpr float V_AMBIGUOUS_RESOLUTION = 0.003051851f; + constexpr float V_AMBIGUOUS_MIN_VALUE = -100.f; + constexpr float MAX_RANGE_RESOLUTION = 0.1f; + + first_rdi_near_packet_ = false; + + static_assert(sizeof(ScanHeaderPacket) == RDI_NEAR_HEADER_PACKET_SIZE); + static_assert(sizeof(DetectionPacket) == RDI_NEAR_ELEMENT_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_NEAR_HEADER_PACKET_SIZE + 4); + + std::memcpy( + &rdi_near_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), + sizeof(ScanHeaderPacket)); + + assert( + rdi_near_header_packet_.u_global_time_stamp_sync_status >= 1 && + rdi_near_header_packet_.u_global_time_stamp_sync_status <= 3); + + near_detection_list_ptr_->header.frame_id = sensor_configuration_->frame_id; + + if (rdi_near_header_packet_.u_global_time_stamp_sync_status == 1) { + near_detection_list_ptr_->header.stamp.sec = + rdi_near_header_packet_.u_global_time_stamp_sec.value(); + near_detection_list_ptr_->header.stamp.nanosec = + rdi_near_header_packet_.u_global_time_stamp_nsec.value(); + } else { + near_detection_list_ptr_->header.stamp = packet_msg->stamp; + } + + rdi_near_packets_ptr_->header.stamp = packet_msg->stamp; + rdi_near_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + + near_detection_list_ptr_->internal_time_stamp_usec = rdi_near_header_packet_.u_time_stamp.value(); + near_detection_list_ptr_->global_time_stamp_sync_status = + rdi_near_header_packet_.u_global_time_stamp_sync_status; + near_detection_list_ptr_->signal_status = rdi_near_header_packet_.u_signal_status; + near_detection_list_ptr_->sequence_counter = rdi_near_header_packet_.u_sequence_counter; + near_detection_list_ptr_->cycle_counter = rdi_near_header_packet_.u_cycle_counter.value(); + near_detection_list_ptr_->v_ambiguous = + V_AMBIGUOUS_RESOLUTION * rdi_near_header_packet_.u_v_ambiguous.value() + V_AMBIGUOUS_MIN_VALUE; + near_detection_list_ptr_->max_range = + MAX_RANGE_RESOLUTION * rdi_near_header_packet_.u_max_range.value(); + + near_detection_list_ptr_->detections.reserve( + rdi_near_header_packet_.u_number_of_detections.value()); + + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessNearElementPacket( + std::unique_ptr packet_msg) +{ + constexpr auto RANGE_RESOLUTION = 0.024420024; + constexpr auto AZIMUTH_RESOLUTION = 0.006159986; + constexpr auto RANGE_RATE_RESOLUTION = 0.014662757; + constexpr auto RCS_RESOLUTION = 0.476190476; + constexpr auto SNR_RESOLUTION = 1.7; + + constexpr auto AZIMUTH_MIN_VALUE = -1.570796327; + constexpr auto RANGE_RATE_MIN_VALUE = -15.f; + constexpr auto RCS_MIN_VALUE = -40.f; + constexpr auto SNR_MIN_VALUE = 11.f; + + if (rdi_near_packets_ptr_->packets.size() == 0) { + if (!first_rdi_near_packet_) { + PrintError("Near element before header. This can happen during the first iteration"); + } + return; + } + + int parsed_detections = near_detection_list_ptr_->detections.size(); + + if ( + near_detection_list_ptr_->detections.size() >= + rdi_near_header_packet_.u_number_of_detections.value()) { + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + return; + } + + DetectionPacket detection_packet; + std::memcpy( + &detection_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(DetectionPacket)); + + static_assert(sizeof(DetectionPacket) == RDI_NEAR_ELEMENT_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_NEAR_ELEMENT_PACKET_SIZE + 4); + assert(rdi_near_header_packet_.u_sequence_counter == detection_packet.u_sequence_counter); + assert( + rdi_near_packets_ptr_->packets.size() == + static_cast(detection_packet.u_message_counter + 1)); + + for (const auto & fragment : detection_packet.fragments) { + continental_msgs::msg::ContinentalSrr520Detection detection_msg; + const auto & data = fragment.data; + + if (parsed_detections >= rdi_near_header_packet_.u_number_of_detections.value()) { + break; + } + + uint16_t u_range = + (static_cast(data[0]) << 4) | (static_cast(data[1] & 0xF0) >> 4); + assert(u_range <= 4095); + detection_msg.range = RANGE_RESOLUTION * u_range; + + uint16_t u_azimuth = + (static_cast(data[1] & 0x0f) << 5) | (static_cast(data[2] & 0xF8) >> 3); + assert(u_azimuth <= 510); + detection_msg.azimuth_angle = AZIMUTH_RESOLUTION * u_azimuth + AZIMUTH_MIN_VALUE; + + uint16_t u_range_rate = + (static_cast(data[2] & 0x07) << 8) | static_cast(data[3]); + assert(u_range_rate <= 2046); + detection_msg.range_rate = RANGE_RATE_RESOLUTION * u_range_rate + RANGE_RATE_MIN_VALUE; + + uint16_t u_rcs = (data[4] & 0xFE) >> 1; + assert(u_rcs <= 126); + detection_msg.rcs = RCS_RESOLUTION * u_rcs + RCS_MIN_VALUE; + + detection_msg.pdh00 = 100 * (data[4] & 0x01); + detection_msg.pdh01 = 100 * ((data[5] & 0x80) >> 7); + detection_msg.pdh02 = 100 * ((data[5] & 0x40) >> 6); + detection_msg.pdh03 = 100 * ((data[5] & 0x20) >> 5); + detection_msg.pdh04 = 100 * ((data[5] & 0x10) >> 4); + + uint8_t u_snr = data[5] & 0x0f; + detection_msg.snr = SNR_RESOLUTION * u_snr + SNR_MIN_VALUE; + + near_detection_list_ptr_->detections.push_back(detection_msg); + parsed_detections++; + } + + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( + std::unique_ptr packet_msg) +{ + constexpr float V_AMBIGUOUS_RESOLUTION = 0.003051851f; + constexpr float V_AMBIGUOUS_MIN_VALUE = -100.f; + constexpr float MAX_RANGE_RESOLUTION = 0.1f; + + first_rdi_hrr_packet_ = false; + + static_assert(sizeof(ScanHeaderPacket) == RDI_HRR_HEADER_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_HRR_HEADER_PACKET_SIZE + 4); + + std::memcpy( + &rdi_hrr_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), + sizeof(ScanHeaderPacket)); + + assert( + rdi_hrr_header_packet_.u_global_time_stamp_sync_status >= 1 && + rdi_hrr_header_packet_.u_global_time_stamp_sync_status <= 3); + + hrr_detection_list_ptr_->header.frame_id = sensor_configuration_->frame_id; + + if (rdi_hrr_header_packet_.u_global_time_stamp_sync_status == 1) { + hrr_detection_list_ptr_->header.stamp.sec = + rdi_hrr_header_packet_.u_global_time_stamp_sec.value(); + hrr_detection_list_ptr_->header.stamp.nanosec = + rdi_hrr_header_packet_.u_global_time_stamp_nsec.value(); + } else { + hrr_detection_list_ptr_->header.stamp = packet_msg->stamp; + } + + rdi_hrr_packets_ptr_->header.stamp = packet_msg->stamp; + rdi_hrr_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + + hrr_detection_list_ptr_->internal_time_stamp_usec = rdi_hrr_header_packet_.u_time_stamp.value(); + hrr_detection_list_ptr_->global_time_stamp_sync_status = + rdi_hrr_header_packet_.u_global_time_stamp_sync_status; + hrr_detection_list_ptr_->signal_status = rdi_hrr_header_packet_.u_signal_status; + hrr_detection_list_ptr_->sequence_counter = rdi_hrr_header_packet_.u_sequence_counter; + hrr_detection_list_ptr_->cycle_counter = rdi_hrr_header_packet_.u_cycle_counter.value(); + hrr_detection_list_ptr_->v_ambiguous = + V_AMBIGUOUS_RESOLUTION * rdi_hrr_header_packet_.u_v_ambiguous.value() + V_AMBIGUOUS_MIN_VALUE; + hrr_detection_list_ptr_->max_range = + MAX_RANGE_RESOLUTION * rdi_hrr_header_packet_.u_max_range.value(); + + hrr_detection_list_ptr_->detections.reserve( + rdi_hrr_header_packet_.u_number_of_detections.value()); + + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessHRRElementPacket( + std::unique_ptr packet_msg) +{ + constexpr auto RANGE_RESOLUTION = 0.024420024; + constexpr auto AZIMUTH_RESOLUTION = 0.006159986; + constexpr auto RANGE_RATE_RESOLUTION = 0.014662757; + constexpr auto RCS_RESOLUTION = 0.476190476; + constexpr auto SNR_RESOLUTION = 1.7f; + + constexpr auto AZIMUTH_MIN_VALUE = -1.570796327; + constexpr auto RANGE_RATE_MIN_VALUE = -15.f; + constexpr auto RCS_MIN_VALUE = -40.f; + constexpr auto SNR_MIN_VALUE = 11.f; + + if (rdi_hrr_packets_ptr_->packets.size() == 0) { + if (!first_rdi_hrr_packet_) { + PrintError("HRR element before header. This can happen during the first iteration"); + } + return; + } + + int parsed_detections = hrr_detection_list_ptr_->detections.size(); + + if ( + hrr_detection_list_ptr_->detections.size() >= + rdi_hrr_header_packet_.u_number_of_detections.value()) { + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + return; + } + + DetectionPacket detection_packet; + std::memcpy( + &detection_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(DetectionPacket)); + + static_assert(sizeof(DetectionPacket) == RDI_HRR_ELEMENT_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_HRR_ELEMENT_PACKET_SIZE + 4); + assert(rdi_hrr_header_packet_.u_sequence_counter == detection_packet.u_sequence_counter); + assert( + rdi_hrr_packets_ptr_->packets.size() == + static_cast(detection_packet.u_message_counter + 1)); + + for (const auto & fragment : detection_packet.fragments) { + continental_msgs::msg::ContinentalSrr520Detection detection_msg; + const auto & data = fragment.data; + + if (parsed_detections >= rdi_hrr_header_packet_.u_number_of_detections.value()) { + break; + } + + uint16_t u_range = + (static_cast(data[0]) << 4) | (static_cast(data[1] & 0xF0) >> 4); + assert(u_range <= 4095); + detection_msg.range = RANGE_RESOLUTION * u_range; + + uint16_t u_azimuth = + (static_cast(data[1] & 0x0f) << 5) | (static_cast(data[2] & 0xF8) >> 3); + assert(u_azimuth <= 510); + detection_msg.azimuth_angle = AZIMUTH_RESOLUTION * u_azimuth + AZIMUTH_MIN_VALUE; + + uint16_t u_range_rate = + (static_cast(data[2] & 0x07) << 8) | static_cast(data[3]); + assert(u_range_rate <= 2046); + detection_msg.range_rate = RANGE_RATE_RESOLUTION * u_range_rate + RANGE_RATE_MIN_VALUE; + + uint16_t u_rcs = (data[4] & 0xFE) >> 1; + assert(u_rcs <= 126); + detection_msg.rcs = RCS_RESOLUTION * u_rcs + RCS_MIN_VALUE; + + detection_msg.pdh00 = 100 * (data[4] & 0x01); + detection_msg.pdh01 = 100 * ((data[5] & 0x80) >> 7); + detection_msg.pdh02 = 100 * ((data[5] & 0x40) >> 6); + detection_msg.pdh03 = 100 * ((data[5] & 0x20) >> 5); + detection_msg.pdh04 = 100 * ((data[5] & 0x10) >> 4); + + uint8_t u_snr = data[5] & 0x0f; + detection_msg.snr = SNR_RESOLUTION * u_snr + SNR_MIN_VALUE; + + hrr_detection_list_ptr_->detections.push_back(detection_msg); + parsed_detections++; + } + + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( + std::unique_ptr packet_msg) +{ + constexpr auto VX_RESOLUTION = 0.003051851; + constexpr auto VX_MIN_VALUE = -100.f; + constexpr auto YAW_RATE_RESOLUTION = 9.58766e-05; + constexpr auto YAW_RATE_MIN_VALUE = -3.14159; + + first_object_packet_ = false; + + static_assert(sizeof(ObjectHeaderPacket) == OBJECT_HEADER_PACKET_SIZE); + assert(packet_msg->data.size() == OBJECT_HEADER_PACKET_SIZE + 4); + + std::memcpy( + &object_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), + sizeof(ObjectHeaderPacket)); + + assert( + object_header_packet_.u_global_time_stamp_sync_status >= 1 && + object_header_packet_.u_global_time_stamp_sync_status <= 3); + + object_list_ptr_->header.frame_id = sensor_configuration_->base_frame; + + if (object_header_packet_.u_global_time_stamp_sync_status == 1) { + object_list_ptr_->header.stamp.sec = object_header_packet_.u_global_time_stamp_sec.value(); + object_list_ptr_->header.stamp.nanosec = object_header_packet_.u_global_time_stamp_nsec.value(); + } else { + object_list_ptr_->header.stamp = packet_msg->stamp; + } + + object_packets_ptr_->header.stamp = packet_msg->stamp; + object_packets_ptr_->header.frame_id = sensor_configuration_->base_frame; + + object_list_ptr_->internal_time_stamp_usec = object_header_packet_.u_time_stamp.value(); + object_list_ptr_->global_time_stamp_sync_status = + object_header_packet_.u_global_time_stamp_sync_status; + object_list_ptr_->signal_status = object_header_packet_.u_signal_status; + object_list_ptr_->sequence_counter = object_header_packet_.u_sequence_counter; + object_list_ptr_->cycle_counter = object_header_packet_.u_cycle_counter.value(); + object_list_ptr_->ego_vx = VX_RESOLUTION * object_header_packet_.u_ego_vx.value() + VX_MIN_VALUE; + object_list_ptr_->ego_yaw_rate = + YAW_RATE_RESOLUTION * object_header_packet_.u_ego_yaw_rate.value() + YAW_RATE_MIN_VALUE; + object_list_ptr_->motion_type = object_header_packet_.u_motion_type; + + object_list_ptr_->objects.reserve(object_header_packet_.u_number_of_objects); + + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessObjectElementPacket( + std::unique_ptr packet_msg) +{ + constexpr auto DIST_RESOLUTION = 0.009155553; + constexpr auto V_ABS_RESOLUTION = 0.009156391; + constexpr auto A_ABS_RESOLUTION = 0.019569472; + constexpr auto DIST_STD_RESOLUTION = 0.001831166; + constexpr auto V_ABS_STD_RESOLUTION = 0.001831166; + constexpr auto A_ABS_STD_RESOLUTION = 0.001831166; + constexpr auto OBJECT_BOX_RESOLUTION = 0.007326007; + constexpr auto OBJECT_ORIENTATION_RESOLUTION = 0.001534729; + constexpr auto OBJECT_RCS_RESOLUTION = 0.024425989; + constexpr auto OBJECT_SCORE_RESOLUTION = 6.666666667; + + constexpr auto DIST_MIN_VALUE = -300.f; + constexpr auto V_ABS_MIN_VALUE = -75.f; + constexpr auto A_ABS_MIN_VALUE = -10.f; + constexpr auto OBJECT_ORIENTATION_MIN_VALUE = -3.14159; + constexpr auto OBJECT_RCS_MIN_VALUE = -50.f; + + if (object_packets_ptr_->packets.size() == 0) { + if (!first_object_packet_) { + PrintError("Object element before header. This can happen during the first iteration"); + } + return; + } + + if (object_list_ptr_->objects.size() >= object_header_packet_.u_number_of_objects) { + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + return; + } + + ObjectPacket object_packet; + std::memcpy(&object_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(ObjectPacket)); + + static_assert(sizeof(ObjectPacket) == OBJECT_PACKET_SIZE); + assert(packet_msg->data.size() == OBJECT_PACKET_SIZE + 4); + assert(object_header_packet_.u_sequence_counter == object_packet.u_sequence_counter); + assert( + object_packets_ptr_->packets.size() == + static_cast(object_packet.u_message_counter + 1)); + + for (const auto & fragment : object_packet.fragments) { + continental_msgs::msg::ContinentalSrr520Object object_msg; + const auto & data = fragment.data; + + if (object_list_ptr_->objects.size() >= object_header_packet_.u_number_of_objects) { + break; + } + + object_msg.object_id = data[0]; + + uint16_t u_dist_x = ((static_cast(data[1]) << 8) | data[2]); + uint16_t u_dist_y = ((static_cast(data[3]) << 8) | data[4]); + assert(u_dist_x <= 65534); + assert(u_dist_y <= 65534); + object_msg.dist_x = DIST_RESOLUTION * u_dist_x + DIST_MIN_VALUE; + object_msg.dist_y = DIST_RESOLUTION * u_dist_y + DIST_MIN_VALUE; + + uint16_t u_v_abs_x = + (static_cast(data[5]) << 6) | (static_cast(data[6] & 0xfc) >> 2); + assert(u_v_abs_x <= 16382); + object_msg.v_abs_x = V_ABS_RESOLUTION * u_v_abs_x + V_ABS_MIN_VALUE; + + uint16_t u_v_abs_y = (static_cast(data[6] & 0x03) << 12) | + (static_cast(data[7]) << 4) | + (static_cast(data[8] & 0xF0) >> 4); + assert(u_v_abs_y <= 16382); + object_msg.v_abs_y = V_ABS_RESOLUTION * u_v_abs_y + V_ABS_MIN_VALUE; + + uint16_t u_a_abs_x = + (static_cast(data[8] & 0x0f) << 6) | (static_cast(data[9] & 0xfc) >> 2); + assert(u_a_abs_x <= 1022); + object_msg.a_abs_x = A_ABS_RESOLUTION * u_a_abs_x + A_ABS_MIN_VALUE; + + uint16_t u_a_abs_y = + (static_cast(data[9] & 0x03) << 8) | static_cast(data[10]); + assert(u_a_abs_y <= 1022); + object_msg.a_abs_y = A_ABS_RESOLUTION * u_a_abs_y + A_ABS_MIN_VALUE; + + uint16_t u_dist_x_std = + (static_cast(data[11]) << 6) | (static_cast(data[12] & 0xfc) >> 2); + assert(u_dist_x_std <= 16383); + object_msg.dist_x_std = DIST_STD_RESOLUTION * u_dist_x_std; + + uint16_t u_dist_y_std = (static_cast(data[12] & 0x03) << 12) | + (static_cast(data[13]) << 4) | + (static_cast(data[14] & 0xF0) >> 4); + assert(u_dist_y_std <= 16383); + object_msg.dist_y_std = DIST_STD_RESOLUTION * u_dist_y_std; + + uint16_t u_v_abs_x_std = (static_cast(data[14] & 0x0f) << 10) | + (static_cast(data[15]) << 2) | + (static_cast(data[15] & 0x03) >> 6); + assert(u_v_abs_x_std <= 16383); + object_msg.v_abs_x_std = V_ABS_STD_RESOLUTION * u_v_abs_x_std; + + uint16_t u_v_abs_y_std = + (static_cast(data[16] & 0x3f) << 8) | static_cast(data[17]); + assert(u_v_abs_y_std <= 16383); + object_msg.v_abs_y_std = V_ABS_STD_RESOLUTION * u_v_abs_y_std; + + uint16_t u_a_abs_x_std = + (static_cast(data[18]) << 6) | (static_cast(data[19] & 0xfc) >> 2); + assert(u_a_abs_x_std <= 16383); + object_msg.a_abs_x_std = A_ABS_STD_RESOLUTION * u_a_abs_x_std; + + uint16_t u_a_abs_y_std = (static_cast(data[19] & 0x03) << 12) | + (static_cast(data[20]) << 4) | + (static_cast(data[21] & 0xF0) >> 4); + assert(u_a_abs_y_std <= 16383); + object_msg.a_abs_y_std = A_ABS_STD_RESOLUTION * u_a_abs_y_std; + + uint16_t u_box_length = + (static_cast(data[21] & 0x0f) << 8) | static_cast(data[22]); + assert(u_box_length <= 4095); + object_msg.box_length = OBJECT_BOX_RESOLUTION * u_box_length; + + uint16_t u_box_width = + (static_cast(data[23]) << 4) | (static_cast(data[24] & 0xF0) >> 4); + assert(u_box_width <= 4095); + object_msg.box_width = OBJECT_BOX_RESOLUTION * u_box_width; + + uint16_t u_orientation = + (static_cast(data[24] & 0x0f) << 8) | static_cast(data[25]); + assert(u_orientation <= 4094); + object_msg.orientation = + OBJECT_ORIENTATION_RESOLUTION * u_orientation + OBJECT_ORIENTATION_MIN_VALUE; + + uint16_t u_rcs = + (static_cast(data[26]) << 4) | (static_cast(data[27] & 0xF0) >> 4); + assert(u_rcs <= 4094); + object_msg.rcs = OBJECT_RCS_RESOLUTION * u_rcs + OBJECT_RCS_MIN_VALUE; + + uint8_t u_score = data[27] & 0x0f; + assert(u_score <= 15); + object_msg.score = OBJECT_SCORE_RESOLUTION * u_score; + + object_msg.life_cycles = (static_cast(data[28]) << 8) | data[29]; + assert(object_msg.life_cycles <= 65535); + + object_msg.box_valid = data[30] & 0x01; + object_msg.object_status = (data[30] & 0x06) >> 1; + + object_list_ptr_->objects.push_back(object_msg); + } + + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessCRCListPacket( + std::unique_ptr packet_msg) +{ + const auto crc_id = packet_msg->data[4]; // first 4 bits are the can id + + if (crc_id == NEAR_CRC_ID) { + ProcessNearCRCListPacket(std::move(packet_msg)); + } else if (crc_id == HRR_CRC_ID) { + ProcessHRRCRCListPacket(std::move(packet_msg)); // cspell: ignore HRRCRC + } else if (crc_id == OBJECT_CRC_ID) { + ProcessObjectCRCListPacket(std::move(packet_msg)); + } else { + PrintError(std::string("Unrecognized CRC id=") + std::to_string(crc_id)); + } +} + +void ContinentalSRR520Decoder::ProcessNearCRCListPacket( + std::unique_ptr packet_msg) +{ + if (rdi_near_packets_ptr_->packets.size() != RDI_NEAR_PACKET_NUM + 1) { + if (!first_rdi_near_packet_) { + PrintError("Incorrect number of RDI Near elements before CRC list"); + } + + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); + return; + } + + uint16_t transmitted_crc = + (static_cast(packet_msg->data[5]) << 8) | packet_msg->data[6]; + uint16_t computed_crc = + crc16_packets(rdi_near_packets_ptr_->packets.begin(), rdi_near_packets_ptr_->packets.end(), 4); + + if (transmitted_crc != computed_crc) { + PrintError( + "RDI Near: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); + + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); + return; + } + + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + + if (near_detection_list_callback_) { + near_detection_list_callback_(std::move(near_detection_list_ptr_)); + } + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(rdi_near_packets_ptr_)); + } + + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); +} + +void ContinentalSRR520Decoder::ProcessHRRCRCListPacket( + std::unique_ptr packet_msg) +{ + if (rdi_hrr_packets_ptr_->packets.size() != RDI_HRR_PACKET_NUM + 1) { + if (!first_rdi_hrr_packet_) { + PrintError("Incorrect number of RDI HRR elements before CRC list"); + } + + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); + return; + } + + uint16_t transmitted_crc = + (static_cast(packet_msg->data[5]) << 8) | packet_msg->data[6]; + uint16_t computed_crc = + crc16_packets(rdi_hrr_packets_ptr_->packets.begin(), rdi_hrr_packets_ptr_->packets.end(), 4); + + if (transmitted_crc != computed_crc) { + PrintError( + "RDI HRR: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); + return; + } + + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + + if (hrr_detection_list_callback_) { + hrr_detection_list_callback_(std::move(hrr_detection_list_ptr_)); + } + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(rdi_hrr_packets_ptr_)); + } + + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); +} + +void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( + std::unique_ptr packet_msg) +{ + if (object_packets_ptr_->packets.size() != OBJECT_PACKET_NUM + 1) { + if (!first_object_packet_) { + PrintError("Incorrect number of object packages before CRC list"); + } + + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); + return; + } + + uint16_t transmitted_crc = + (static_cast(packet_msg->data[5]) << 8) | packet_msg->data[6]; + uint16_t computed_crc = + crc16_packets(object_packets_ptr_->packets.begin(), object_packets_ptr_->packets.end(), 4); + + if (transmitted_crc != computed_crc) { + PrintError( + "Object: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); + + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); + return; + } + + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + + if (object_list_callback_) { + object_list_callback_(std::move(object_list_ptr_)); + } + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(object_packets_ptr_)); + } + + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); +} + +void ContinentalSRR520Decoder::ProcessSensorStatusPacket( + std::unique_ptr packet_msg) +{ + static_assert(sizeof(StatusPacket) == STATUS_PACKET_SIZE); + + constexpr float STATUS_DISTANCE_RESOLUTION = 1e-3f; + constexpr float STATUS_DISTANCE_MIN_VALUE = -32.767; + constexpr float STATUS_ANGLE_RESOLUTION = 9.58766f; + constexpr float STATUS_ANGLE_MIN_VALUE = -3.14159f; + constexpr auto STATUS_ANGLE_STD_RESOLUTION = 1.52593e-05; + + StatusPacket status_packet; + std::memcpy(&status_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(status_packet)); + + auto diagnostic_array_msg_ptr = std::make_unique(); + + diagnostic_array_msg_ptr->header.frame_id = sensor_configuration_->frame_id; + diagnostic_array_msg_ptr->header.stamp = packet_msg->stamp; + diagnostic_array_msg_ptr->status.resize(1); + + auto & diagnostic_status = diagnostic_array_msg_ptr->status.front(); + auto & diagnostic_values = diagnostic_status.values; + diagnostic_values.reserve(33); + diagnostic_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + diagnostic_status.message = "Sensor diagnostics for the SRR520"; + diagnostic_status.hardware_id = "SRR"; + diagnostic_status.name = "SRR"; + diagnostic_msgs::msg::KeyValue key_value; + + key_value.key = "time_stamp"; + key_value.value = std::to_string(status_packet.u_time_stamp.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "global_time_stamp_sec"; + key_value.value = std::to_string(status_packet.u_global_time_stamp_sec.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "global_time_stamp_nsec"; + key_value.value = std::to_string(status_packet.u_global_time_stamp_nsec.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "global_time_stamp_sync_status"; + key_value.value = std::to_string(status_packet.u_global_time_stamp_sync_status); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_global_time_stamp_sync_status != 1 + ? diagnostic_msgs::msg::DiagnosticStatus::WARN + : diagnostic_status.level; + + std::stringstream sw_version_ss; + sw_version_ss << static_cast(status_packet.u_sw_version_major) << "." + << static_cast(status_packet.u_sw_version_minor) << "." + << static_cast(status_packet.u_sw_version_patch); + key_value.key = "sw_version_patch"; + key_value.value = sw_version_ss.str(); + diagnostic_values.push_back(key_value); + + key_value.key = "sensor_id"; + key_value.value = std::to_string(status_packet.u_sensor_id); + diagnostic_values.push_back(key_value); + + key_value.key = "long_pos"; + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_long_pos.value() + STATUS_DISTANCE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "lat_pos"; + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_lat_pos.value() + STATUS_DISTANCE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "vert_pos"; + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_vert_pos.value() + STATUS_DISTANCE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "long_pos_cog"; + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_long_pos_cog.value() + STATUS_DISTANCE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "wheelbase"; + key_value.value = std::to_string(STATUS_DISTANCE_RESOLUTION * status_packet.u_wheelbase.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "yaw_angle"; + key_value.value = std::to_string( + STATUS_ANGLE_RESOLUTION * status_packet.u_yaw_angle.value() + STATUS_ANGLE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "cover_damping"; + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_cover_damping.value() + STATUS_DISTANCE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + uint8_t plug_orientation = status_packet.u_plug_orientation & 0b1; + key_value.key = "plug_orientation"; + key_value.value = plug_orientation == 0 ? "0:PLUG_BOTTOM" : "1:PLUG_TOP"; + diagnostic_values.push_back(key_value); + + std::vector defective_message_vector; + + if (status_packet.u_defective & 0x01) { + defective_message_vector.push_back("Current Near Scan RF Error"); + } + if (status_packet.u_defective & 0x02) { + defective_message_vector.push_back("Near Scan RF this OPC"); + } + if (status_packet.u_defective & 0x04) { + defective_message_vector.push_back("Current HRR Scan RF Error"); + } + if (status_packet.u_defective & 0x08) { + defective_message_vector.push_back("HRR Scan RF this OPC"); + } + if (status_packet.u_defective & 0x10) { + defective_message_vector.push_back("Current RF HW Error"); + } + if (status_packet.u_defective & 0x20) { + defective_message_vector.push_back("RF HW Error this OPC"); + } + if (status_packet.u_defective & 0x40) { + defective_message_vector.push_back("Current HW Error"); + } + if (status_packet.u_defective & 0x80) { + defective_message_vector.push_back("HW Error this OPC"); + } + + key_value.key = "defective"; + key_value.value = defective_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(defective_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_defective != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + std::vector supply_voltage_message_vector; + + if (status_packet.u_supply_voltage_limit & 0x01) { + supply_voltage_message_vector.push_back("Current Overvoltage Error"); + } + if (status_packet.u_supply_voltage_limit & 0x02) { + supply_voltage_message_vector.push_back("Overvoltage Error this OPC"); + } + if (status_packet.u_supply_voltage_limit & 0x04) { + supply_voltage_message_vector.push_back("Current Undervoltage Error"); + } + if (status_packet.u_supply_voltage_limit & 0x08) { + supply_voltage_message_vector.push_back("Undervoltage Error this OPC"); + } + + key_value.key = "supply_voltage_limit"; + key_value.value = supply_voltage_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(supply_voltage_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_supply_voltage_limit != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + std::vector temperature_message_vector; + + if (status_packet.u_sensor_off_temp & 0x01) { + temperature_message_vector.push_back("Current Overtemperature Error"); + } + if (status_packet.u_sensor_off_temp & 0x02) { + temperature_message_vector.push_back("Overtemperature Error this OPC"); + } + if (status_packet.u_sensor_off_temp & 0x04) { + temperature_message_vector.push_back("Current Undertemperature Error"); + } + if (status_packet.u_sensor_off_temp & 0x08) { + temperature_message_vector.push_back("Undertemperature Error this OPC"); + } + + key_value.key = "sensor_off_temp"; + key_value.value = temperature_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(temperature_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_sensor_off_temp != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + auto valid_flag_to_string = [](uint8_t status) -> std::string { + std::vector status_vector; + if (status == 0) { + return "Ok"; + } + if (status > 3) { + return "Invalid value"; + } + if (status & 0x01) { + status_vector.push_back("Current error"); + } + if (status & 0x02) { + status_vector.push_back("Error this OPC"); + } + + return boost::algorithm::join(status_vector, ", "); + }; + + uint8_t u_long_vel_invalid = status_packet.u_dynamics_invalid0 & 0b00000011; + key_value.key = "long_vel_invalid"; + key_value.value = valid_flag_to_string(u_long_vel_invalid); + diagnostic_values.push_back(key_value); + + uint8_t u_long_accel_invalid = (status_packet.u_dynamics_invalid0 & 0b00001100) >> 2; + key_value.key = "long_accel_invalid"; + key_value.value = valid_flag_to_string(u_long_accel_invalid); + diagnostic_values.push_back(key_value); + + uint8_t u_lat_accel_invalid = (status_packet.u_dynamics_invalid0 & 0b00110000) >> 4; + key_value.key = "lat_accel_invalid"; + key_value.value = valid_flag_to_string(u_lat_accel_invalid); + diagnostic_values.push_back(key_value); + + uint8_t u_long_dir_invalid = (status_packet.u_dynamics_invalid0 & 0b11000000) >> 6; + key_value.key = "long_dir_invalid"; + key_value.value = valid_flag_to_string(u_long_dir_invalid); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_dynamics_invalid0 != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::WARN + : diagnostic_status.level; + + uint8_t u_yaw_rate_invalid = status_packet.u_dynamics_invalid1 & 0b00000011; + key_value.key = "yaw_rate_invalid"; + key_value.value = valid_flag_to_string(u_yaw_rate_invalid); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_dynamics_invalid1 != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::WARN + : diagnostic_status.level; + + std::vector ext_disturbed_message_vector; + + if (status_packet.u_ext_disturbed & 0x01) { + ext_disturbed_message_vector.push_back("Current Near Scan RF Error"); + } + if (status_packet.u_ext_disturbed & 0x02) { + ext_disturbed_message_vector.push_back("Near Scan RF this OPC"); + } + if (status_packet.u_ext_disturbed & 0x04) { + ext_disturbed_message_vector.push_back("Current HRR Scan RF Error"); + } + if (status_packet.u_ext_disturbed & 0x08) { + ext_disturbed_message_vector.push_back("HRR Scan RF this OPC"); + } + if (status_packet.u_ext_disturbed & 0x10) { + ext_disturbed_message_vector.push_back("Current RF HW Error"); + } + if (status_packet.u_ext_disturbed & 0x20) { + ext_disturbed_message_vector.push_back("RF HW Error this OPC"); + } + if (status_packet.u_ext_disturbed & 0x40) { + ext_disturbed_message_vector.push_back("Current HW Error"); + } + if (status_packet.u_ext_disturbed & 0x80) { + ext_disturbed_message_vector.push_back("HW Error this OPC"); + } + + key_value.key = "ext_disturbed"; + key_value.value = ext_disturbed_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(ext_disturbed_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_ext_disturbed != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + key_value.key = "com_error"; + key_value.value = valid_flag_to_string(status_packet.u_com_error); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_com_error != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + std::vector sw_message_vector; + uint8_t u_sw_error = status_packet.u_sw_error.value() & 0xff; + + if (u_sw_error & 0x01) { + sw_message_vector.push_back("Current Internal SW Error"); + } + if (u_sw_error & 0x02) { + sw_message_vector.push_back("Internal SW Error this OPC"); + } + if (u_sw_error & 0x04) { + sw_message_vector.push_back("Reset Error"); + } + if (u_sw_error & 0x08) { + sw_message_vector.push_back("Current Nvm Integrity"); + } + if (u_sw_error & 0x10) { + sw_message_vector.push_back("Nvm Integrity Error this OPC"); + } + if (u_sw_error & 0x20) { + sw_message_vector.push_back("RF HW Error this OPC"); + } + if (u_sw_error & 0x40) { + sw_message_vector.push_back("Runtime Error"); + } + if (u_sw_error & 0x80) { + sw_message_vector.push_back("Last Sensor Config Message Rejected Upper"); + } + + key_value.key = "sw_error"; + key_value.value = + sw_message_vector.size() == 0 ? "Ok" : boost::algorithm::join(sw_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = (status_packet.u_sw_error.value() & 0x0f) != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + bool aln_driving = status_packet.u_aln_status_azimuth_available & 0b00000001; + bool aln_sensor = (status_packet.u_aln_status_azimuth_available & 0b00000010) >> 1; + + key_value.key = "aln_status_driving"; + key_value.value = aln_driving ? "Driving conditions met" : "Driving conditions NOT met"; + diagnostic_values.push_back(key_value); + + key_value.key = "aln_status_sensor"; + key_value.value = aln_sensor ? "Sensor is adjusted" : "Sensor is not adjusted"; + diagnostic_values.push_back(key_value); + + key_value.key = "aln_azimuth_available"; + key_value.value = + std::to_string((status_packet.u_aln_status_azimuth_available & 0b00000100) >> 2); + diagnostic_values.push_back(key_value); + + key_value.key = "aln_current_azimuth_std"; + key_value.value = + std::to_string(STATUS_ANGLE_STD_RESOLUTION * status_packet.u_aln_current_azimuth_std.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "aln_current_azimuth"; + key_value.value = std::to_string( + STATUS_ANGLE_RESOLUTION * status_packet.u_aln_current_azimuth.value() + STATUS_ANGLE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "aln_current_delta"; + key_value.value = std::to_string( + STATUS_ANGLE_RESOLUTION * status_packet.u_aln_current_delta.value() + STATUS_ANGLE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + uint16_t computed_crc = crc16_packet(packet_msg->data.begin() + 4, packet_msg->data.end() - 3); + key_value.key = "crc_check"; + key_value.value = + std::to_string(status_packet.u_crc.value()) + "|" + std::to_string(computed_crc); + diagnostic_values.push_back(key_value); + + key_value.key = "sequence_counter"; + key_value.value = std::to_string(status_packet.u_sequence_counter); + diagnostic_values.push_back(key_value); + + if (status_callback_) { + status_callback_(std::move(diagnostic_array_msg_ptr)); + } + + auto nebula_packets = std::make_unique(); + nebula_packets->header.stamp = packet_msg->stamp; + nebula_packets->header.frame_id = sensor_configuration_->frame_id; + nebula_packets->packets.emplace_back(std::move(*packet_msg)); + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(nebula_packets)); + } +} + +void ContinentalSRR520Decoder::ProcessSyncFollowUpPacket( + std::unique_ptr packet_msg) +{ + if (sync_follow_up_callback_) { + sync_follow_up_callback_(packet_msg->stamp); + } + + auto nebula_packets = std::make_unique(); + nebula_packets->header.stamp = packet_msg->stamp; + nebula_packets->header.frame_id = sensor_configuration_->frame_id; + nebula_packets->packets.emplace_back(std::move(*packet_msg)); + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(nebula_packets)); + } +} + +void ContinentalSRR520Decoder::SetLogger(std::shared_ptr logger) +{ + parent_node_logger_ptr_ = logger; +} + +void ContinentalSRR520Decoder::PrintInfo(std::string info) +{ + if (parent_node_logger_ptr_) { + RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); + } else { + std::cout << info << std::endl; + } +} + +void ContinentalSRR520Decoder::PrintError(std::string error) +{ + if (parent_node_logger_ptr_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); + } else { + std::cerr << error << std::endl; + } +} + +void ContinentalSRR520Decoder::PrintDebug(std::string debug) +{ + if (parent_node_logger_ptr_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); + } else { + std::cout << debug << std::endl; + } +} + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 49be4adac..22269bba7 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -1,3 +1,5 @@ +// Copyright 2024 TIER IV, Inc. + #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp" @@ -18,62 +20,63 @@ namespace nebula namespace drivers { HesaiDriver::HesaiDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_data) { // initialize proper parser from cloud config's model and echo mode driver_status_ = nebula::Status::OK; + switch (sensor_configuration->sensor_model) { - case SensorModel::UNKNOWN: - driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; - break; case SensorModel::HESAI_PANDAR64: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR40P: case SensorModel::HESAI_PANDAR40M: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARQT64: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARQT128: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARXT32: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARXT32M: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARAT128: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR128_E3X: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR128_E4X: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; + case SensorModel::UNKNOWN: + driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; + throw std::runtime_error("Invalid sensor model."); default: driver_status_ = nebula::Status::NOT_INITIALIZED; throw std::runtime_error("Driver not Implemented for selected sensor."); - break; } } -std::tuple HesaiDriver::ConvertScanToPointcloud( - const std::shared_ptr & pandar_scan) +template +std::shared_ptr HesaiDriver::InitializeDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration) +{ + using CalibT = typename SensorT::angle_corrector_t::correction_data_t; + return std::make_shared>( + sensor_configuration, std::dynamic_pointer_cast(calibration_configuration)); +} + +std::tuple HesaiDriver::ParseCloudPacket( + const std::vector & packet) { std::tuple pointcloud; auto logger = rclcpp::get_logger("HesaiDriver"); @@ -83,26 +86,23 @@ std::tuple HesaiDriver::ConvertScanToPoint return pointcloud; } - int cnt = 0, last_azimuth = 0; - for (auto & packet : pandar_scan->packets) { - last_azimuth = scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) { - pointcloud = scan_decoder_->getPointcloud(); - cnt++; - } + scan_decoder_->unpack(packet); + if (scan_decoder_->hasScanned()) { + pointcloud = scan_decoder_->getPointcloud(); } - if (cnt == 0) { - RCLCPP_ERROR_STREAM( - logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " - << "pointclouds were generated. Last azimuth: " << last_azimuth); - } + // todo + // if (cnt == 0) { + // RCLCPP_ERROR_STREAM( + // logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " + // << "pointclouds were generated. Last azimuth: " << last_azimuth); + // } return pointcloud; } Status HesaiDriver::SetCalibrationConfiguration( - const CalibrationConfigurationBase & calibration_configuration) + const HesaiCalibrationConfigurationBase & calibration_configuration) { throw std::runtime_error( "SetCalibrationConfiguration. Not yet implemented (" + diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp index c6cb23378..96984e120 100644 --- a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp @@ -1,8 +1,11 @@ +// Copyright 2024 TIER IV, Inc. + #include "nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp" #include "nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp" namespace nebula { @@ -10,8 +13,8 @@ namespace drivers { RobosenseDriver::RobosenseDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration) { // initialize proper parser from cloud config's model and echo mode driver_status_ = nebula::Status::OK; @@ -50,8 +53,8 @@ Status RobosenseDriver::SetCalibrationConfiguration( calibration_configuration.calibration_file + ")"); } -std::tuple RobosenseDriver::ConvertScanToPointcloud( - const std::shared_ptr & robosense_scan) +std::tuple RobosenseDriver::ParseCloudPacket( + const std::vector & packet) { std::tuple pointcloud; auto logger = rclcpp::get_logger("RobosenseDriver"); @@ -61,19 +64,9 @@ std::tuple RobosenseDriver::ConvertScanToP return pointcloud; } - int cnt = 0, last_azimuth = 0; - for (auto & packet : robosense_scan->packets) { - last_azimuth = scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) { - pointcloud = scan_decoder_->getPointcloud(); - cnt++; - } - } - - if (cnt == 0) { - RCLCPP_ERROR_STREAM( - logger, "Scanned " << robosense_scan->packets.size() << " packets, but no " - << "pointclouds were generated. Last azimuth: " << last_azimuth); + scan_decoder_->unpack(packet); + if (scan_decoder_->hasScanned()) { + pointcloud = scan_decoder_->getPointcloud(); } return pointcloud; diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp index 4d415afd2..cfdc566eb 100644 --- a/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp @@ -1,12 +1,19 @@ +// Copyright 2024 TIER IV, Inc. + #include "nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp" + namespace nebula { namespace drivers { RobosenseInfoDriver::RobosenseInfoDriver( - const std::shared_ptr & sensor_configuration) + const std::shared_ptr & sensor_configuration) { // initialize proper parser from cloud config's model and echo mode driver_status_ = nebula::Status::OK; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index 5a8cea2ec..d73eb29c4 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -1,10 +1,12 @@ +// Copyright 2024 TIER IV, Inc. + #include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp" +#include + #include #include -#include - namespace nebula { namespace drivers @@ -12,8 +14,9 @@ namespace drivers namespace vlp16 { Vlp16Decoder::Vlp16Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration) { sensor_configuration_ = sensor_configuration; calibration_configuration_ = calibration_configuration; @@ -56,11 +59,6 @@ Vlp16Decoder::Vlp16Decoder( phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100); } -bool Vlp16Decoder::hasScanned() -{ - return has_scanned_; -} - std::tuple Vlp16Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); @@ -87,11 +85,9 @@ int Vlp16Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING; } -void Vlp16Decoder::reset_pointcloud(size_t n_pts, double time_stamp) +void Vlp16Decoder::reset_pointcloud(double time_stamp) { scan_pc_->points.clear(); - max_pts_ = n_pts * pointsPerPacket(); - scan_pc_->points.reserve(max_pts_); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } @@ -138,12 +134,14 @@ void Vlp16Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) +void Vlp16Decoder::unpack(const std::vector & packet, int32_t packet_seconds) { - const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0]; + checkAndHandleScanComplete(packet, packet_seconds, phase_); + + const raw_packet_t * raw = (const raw_packet_t *)packet.data(); float last_azimuth_diff = 0; uint16_t azimuth_next; - const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX]; + const uint8_t return_mode = packet[RETURN_MODE_INDEX]; const bool dual_return = (return_mode == RETURN_MODE_DUAL); for (uint block = 0; block < BLOCKS_PER_PACKET; block++) { @@ -203,7 +201,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa block % 2 ? raw->blocks[block - 1].data[k + 1] : raw->blocks[block + 1].data[k + 1]; } // Apply timestamp if this is the first new packet in the scan. - auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); + auto block_timestamp = packet_seconds; if (scan_timestamp_ < 0) { scan_timestamp_ = block_timestamp; } @@ -216,7 +214,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa continue; } { - VelodyneLaserCorrection & corrections = + const VelodyneLaserCorrection & corrections = calibration_configuration_->velodyne_calibration.laser_corrections[dsr]; float distance = current_return.uint * calibration_configuration_->velodyne_calibration.distance_resolution_m; @@ -231,9 +229,10 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa float azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr * VLP16_DSR_TOFFSET) + (firing * VLP16_FIRING_TOFFSET)) / - VLP16_BLOCK_DURATION) - corrections.rot_correction * 180.0 / M_PI * 100; - - if (azimuth_corrected_f < 0.0){ + VLP16_BLOCK_DURATION) - + corrections.rot_correction * 180.0 / M_PI * 100; + + if (azimuth_corrected_f < 0.0) { azimuth_corrected_f += 36000.0; } const uint16_t azimuth_corrected = diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp index 92ee10df0..3bf88e2a3 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -1,10 +1,12 @@ +// Copyright 2024 TIER IV, Inc. + #include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp" +#include + #include #include -#include - namespace nebula { namespace drivers @@ -12,8 +14,9 @@ namespace drivers namespace vlp32 { Vlp32Decoder::Vlp32Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration) { sensor_configuration_ = sensor_configuration; calibration_configuration_ = calibration_configuration; @@ -56,11 +59,6 @@ Vlp32Decoder::Vlp32Decoder( } } -bool Vlp32Decoder::hasScanned() -{ - return has_scanned_; -} - std::tuple Vlp32Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); @@ -85,12 +83,10 @@ int Vlp32Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } -void Vlp32Decoder::reset_pointcloud(size_t n_pts, double time_stamp) +void Vlp32Decoder::reset_pointcloud(double time_stamp) { // scan_pc_.reset(new NebulaPointCloud); scan_pc_->points.clear(); - max_pts_ = n_pts * pointsPerPacket(); - scan_pc_->points.reserve(max_pts_); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } @@ -137,12 +133,14 @@ void Vlp32Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) +void Vlp32Decoder::unpack(const std::vector & packet, int32_t packet_seconds) { - const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0]; + checkAndHandleScanComplete(packet, packet_seconds, phase_); + + const raw_packet_t * raw = (const raw_packet_t *)packet.data(); float last_azimuth_diff = 0; uint16_t azimuth_next; - uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX]; + uint8_t return_mode = packet[RETURN_MODE_INDEX]; const bool dual_return = (return_mode == RETURN_MODE_DUAL); for (uint i = 0; i < BLOCKS_PER_PACKET; i++) { @@ -173,9 +171,8 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa // This makes the assumption the difference between the last block and the next packet is the // same as the last to the second to last. // Assumes RPM doesn't change much between blocks. - azimuth_diff = (i == static_cast(BLOCKS_PER_PACKET - (4 * dual_return) - 1)) - ? 0 - : last_azimuth_diff; + azimuth_diff = + (i == static_cast(BLOCKS_PER_PACKET - (4 * dual_return) - 1)) ? 0 : last_azimuth_diff; } for (uint j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) { @@ -199,7 +196,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa i % 2 ? raw->blocks[i - 1].data[k + 1] : raw->blocks[i + 1].data[k + 1]; } // Apply timestamp if this is the first new packet in the scan. - auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); + auto block_timestamp = packet_seconds; if (scan_timestamp_ < 0) { scan_timestamp_ = block_timestamp; } @@ -232,7 +229,9 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa raw->blocks[i].rotation >= sensor_configuration_->cloud_min_angle * 100))) { const float cos_vert_angle = corrections.cos_vert_correction; const float sin_vert_angle = corrections.sin_vert_correction; - float azimuth_corrected_f = azimuth + (azimuth_diff * VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION * j) - corrections.rot_correction * 180.0 / M_PI * 100; + float azimuth_corrected_f = + azimuth + (azimuth_diff * VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION * j) - + corrections.rot_correction * 180.0 / M_PI * 100; if (azimuth_corrected_f < 0) { azimuth_corrected_f += 36000; } diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp index 2e7c644db..9c683d48d 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -1,10 +1,12 @@ +// Copyright 2024 TIER IV, Inc. + #include "nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp" +#include + #include #include -#include - namespace nebula { namespace drivers @@ -12,8 +14,9 @@ namespace drivers namespace vls128 { Vls128Decoder::Vls128Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration) { sensor_configuration_ = sensor_configuration; calibration_configuration_ = calibration_configuration; @@ -56,11 +59,6 @@ Vls128Decoder::Vls128Decoder( } } -bool Vls128Decoder::hasScanned() -{ - return has_scanned_; -} - std::tuple Vls128Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); @@ -87,12 +85,10 @@ int Vls128Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } -void Vls128Decoder::reset_pointcloud(size_t n_pts, double time_stamp) +void Vls128Decoder::reset_pointcloud(double time_stamp) { // scan_pc_.reset(new NebulaPointCloud); scan_pc_->points.clear(); - max_pts_ = n_pts * pointsPerPacket(); - scan_pc_->points.reserve(max_pts_); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } @@ -139,12 +135,14 @@ void Vls128Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) +void Vls128Decoder::unpack(const std::vector & packet, int32_t packet_seconds) { - const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0]; + checkAndHandleScanComplete(packet, packet_seconds, phase_); + + const raw_packet_t * raw = (const raw_packet_t *)packet.data(); float last_azimuth_diff = 0; uint16_t azimuth_next; - const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX]; + const uint8_t return_mode = packet[RETURN_MODE_INDEX]; const bool dual_return = (return_mode == RETURN_MODE_DUAL); for (uint block = 0; block < static_cast(BLOCKS_PER_PACKET - (4 * dual_return)); block++) { @@ -223,7 +221,7 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p block % 2 ? raw->blocks[block - 1].data[k + 1] : raw->blocks[block + 1].data[k + 1]; } // Apply timestamp if this is the first new packet in the scan. - auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); + auto block_timestamp = packet_seconds; if (scan_timestamp_ < 0) { scan_timestamp_ = block_timestamp; } @@ -240,7 +238,7 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p j + bank_origin; // offset the laser in this block by which block it's in const uint firing_order = laser_number / 8; // VLS-128 fires 8 lasers at a time - VelodyneLaserCorrection & corrections = + const VelodyneLaserCorrection & corrections = calibration_configuration_->velodyne_calibration.laser_corrections[laser_number]; float distance = current_return.uint * VLP128_DISTANCE_RESOLUTION; @@ -249,9 +247,11 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p } // Correct for the laser rotation as a function of timing during the firings. - float azimuth_corrected_f = azimuth + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) - corrections.rot_correction * 180.0 / M_PI * 100; - - if (azimuth_corrected_f < 0.0){ + float azimuth_corrected_f = azimuth + + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) - + corrections.rot_correction * 180.0 / M_PI * 100; + + if (azimuth_corrected_f < 0.0) { azimuth_corrected_f += 36000.0; } const uint16_t azimuth_corrected = ((uint16_t)std::round(azimuth_corrected_f)) % 36000; @@ -340,10 +340,10 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p current_point.intensity = intensity; scan_pc_->points.emplace_back(current_point); } // 2nd scan area condition - } // distance condition - } // empty "else" - } // (uint j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) - } // scan area condition + } // distance condition + } // empty "else" + } // (uint j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) + } // scan area condition } // for (uint block = 0; block < static_cast < uint > (BLOCKS_PER_PACKET - (4 * dual_return)); // block++) } diff --git a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp index b0f664c8c..c173e6a9d 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp @@ -1,3 +1,5 @@ +// Copyright 2024 TIER IV, Inc. + #include "nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp" #include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp" @@ -9,8 +11,9 @@ namespace nebula namespace drivers { VelodyneDriver::VelodyneDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration) { // initialize proper parser from cloud config's model and echo mode driver_status_ = nebula::Status::OK; @@ -46,20 +49,22 @@ Status VelodyneDriver::SetCalibrationConfiguration( calibration_configuration.calibration_file + ")"); } -std::tuple VelodyneDriver::ConvertScanToPointcloud( - const std::shared_ptr & velodyne_scan) +std::tuple VelodyneDriver::ParseCloudPacket( + const std::vector & packet, int32_t packet_seconds) { std::tuple pointcloud; - if (driver_status_ == nebula::Status::OK) { - scan_decoder_->reset_pointcloud( - velodyne_scan->packets.size(), rclcpp::Time(velodyne_scan->packets.front().stamp).seconds()); - for (auto & packet : velodyne_scan->packets) { - scan_decoder_->unpack(packet); - } + + if (driver_status_ != nebula::Status::OK) { + auto logger = rclcpp::get_logger("VelodyneDriver"); + RCLCPP_ERROR(logger, "Driver not OK."); + return pointcloud; + } + + scan_decoder_->unpack(packet, packet_seconds); + if (scan_decoder_->hasScanned()) { pointcloud = scan_decoder_->get_pointcloud(); - } else { - std::cout << "not ok driver_status_ = " << driver_status_ << std::endl; } + return pointcloud; } Status VelodyneDriver::GetStatus() diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt index 774580c04..311105e30 100644 --- a/nebula_examples/CMakeLists.txt +++ b/nebula_examples/CMakeLists.txt @@ -11,49 +11,80 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_auto REQUIRED) -find_package(PCL REQUIRED COMPONENTS common) -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) -find_package(yaml-cpp REQUIRED) +find_package(nebula_common REQUIRED) find_package(nebula_decoders REQUIRED) find_package(nebula_ros REQUIRED) -find_package(nebula_common REQUIRED) - -ament_auto_find_build_dependencies() +find_package(PCL REQUIRED COMPONENTS common io) +find_package(rosbag2_cpp REQUIRED) +find_package(yaml-cpp REQUIRED) include_directories( - include - SYSTEM - ${YAML_CPP_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${PCL_COMMON_INCLUDE_DIRS} + include + SYSTEM + ${nebula_common_INCLUDE_DIRS} + ${nebula_decoders_INCLUDE_DIRS} + ${nebula_ros_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${rosbag2_cpp_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} ) -## HESAI + +link_libraries( + ${nebula_common_TARGETS} + ${rosbag2_cpp_TARGETS} + ${PCL_LIBRARIES} +) + +## Hesai # Offline Lib -ament_auto_add_library(hesai_ros_offline_extract_pcd SHARED - ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_pcd.cpp +add_library(hesai_ros_offline_extract_pcd SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_pcd.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/common/parameter_descriptors.cpp ) -target_link_libraries(hesai_ros_offline_extract_pcd ${PCL_LIBRARIES}) -ament_auto_add_executable(hesai_ros_offline_extract_pcd_node - ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_pcd_main.cpp +target_link_libraries(hesai_ros_offline_extract_pcd PUBLIC + nebula_decoders::nebula_decoders_hesai ) + +add_executable(hesai_ros_offline_extract_pcd_node + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_pcd_main.cpp +) + +target_link_libraries(hesai_ros_offline_extract_pcd_node PUBLIC + hesai_ros_offline_extract_pcd +) + # Extraction for TEST Lib -ament_auto_add_library(hesai_ros_offline_extract_bag_pcd SHARED - ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +add_library(hesai_ros_offline_extract_bag_pcd SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/common/parameter_descriptors.cpp +) +target_link_libraries(hesai_ros_offline_extract_bag_pcd PUBLIC + nebula_decoders::nebula_decoders_hesai +) + +add_executable(hesai_ros_offline_extract_bag_pcd_node + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp ) -target_link_libraries(hesai_ros_offline_extract_bag_pcd ${PCL_LIBRARIES}) -ament_auto_add_executable(hesai_ros_offline_extract_bag_pcd_node - ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp + +target_link_libraries(hesai_ros_offline_extract_bag_pcd_node PUBLIC + hesai_ros_offline_extract_bag_pcd ) ## Velodyne # Extraction for TEST Lib -ament_auto_add_library(velodyne_ros_offline_extract_bag_pcd SHARED - ${CMAKE_CURRENT_SOURCE_DIR}/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp +add_library(velodyne_ros_offline_extract_bag_pcd SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp +) +target_link_libraries(velodyne_ros_offline_extract_bag_pcd PUBLIC + nebula_decoders::nebula_decoders_velodyne ) -target_link_libraries(velodyne_ros_offline_extract_bag_pcd ${PCL_LIBRARIES}) -ament_auto_add_executable(velodyne_ros_offline_extract_bag_pcd_node - ${CMAKE_CURRENT_SOURCE_DIR}/src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp + +add_executable(velodyne_ros_offline_extract_bag_pcd_node + ${CMAKE_CURRENT_SOURCE_DIR}/src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp +) + +target_link_libraries(velodyne_ros_offline_extract_bag_pcd_node PUBLIC + velodyne_ros_offline_extract_bag_pcd ) if(BUILD_TESTING) @@ -62,18 +93,18 @@ if(BUILD_TESTING) endif() ament_auto_package( - INSTALL_TO_SHARE - launch + INSTALL_TO_SHARE + launch ) # Set ROS_DISTRO macros set(ROS_DISTRO $ENV{ROS_DISTRO}) if(${ROS_DISTRO} STREQUAL "rolling") -add_compile_definitions(ROS_DISTRO_ROLLING) + add_compile_definitions(ROS_DISTRO_ROLLING) elseif(${ROS_DISTRO} STREQUAL "foxy") -add_compile_definitions(ROS_DISTRO_FOXY) + add_compile_definitions(ROS_DISTRO_FOXY) elseif(${ROS_DISTRO} STREQUAL "galactic") -add_compile_definitions(ROS_DISTRO_GALACTIC) + add_compile_definitions(ROS_DISTRO_GALACTIC) elseif(${ROS_DISTRO} STREQUAL "humble") -add_compile_definitions(ROS_DISTRO_HUMBLE) + add_compile_definitions(ROS_DISTRO_HUMBLE) endif() diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp index a7681323c..247fd1ba0 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp @@ -15,18 +15,18 @@ #ifndef NEBULA_HesaiRosOfflineExtractBag_H #define NEBULA_HesaiRosOfflineExtractBag_H -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include +#include +#include +#include +#include +#include #include -#include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include +#include +#include #include #include @@ -35,7 +35,7 @@ namespace nebula { namespace ros { -class HesaiRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrapperBase +class HesaiRosOfflineExtractBag final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -46,12 +46,7 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrap Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); Status GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp index 711502e6a..d7afb4dcc 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp @@ -15,18 +15,16 @@ #ifndef NEBULA_HesaiRosOfflineExtractSample_H #define NEBULA_HesaiRosOfflineExtractSample_H -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include +#include +#include +#include +#include +#include #include -#include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include #include #include @@ -36,7 +34,7 @@ namespace nebula namespace ros { /// @brief Offline hesai driver usage example (Output PCD data) -class HesaiRosOfflineExtractSample final : public rclcpp::Node, NebulaDriverRosWrapperBase +class HesaiRosOfflineExtractSample final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -51,17 +49,7 @@ class HesaiRosOfflineExtractSample final : public rclcpp::Node, NebulaDriverRosW /// @return Resulting status Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration diff --git a/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp index c1910d294..64e3d7435 100644 --- a/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp @@ -15,31 +15,16 @@ #ifndef NEBULA_VelodyneRosOfflineExtractBag_H #define NEBULA_VelodyneRosOfflineExtractBag_H -#include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "nebula_common/velodyne/velodyne_calibration_decoder.hpp" #include "nebula_common/velodyne/velodyne_common.hpp" #include "nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" -#include "rosbag2_storage/storage_options.hpp" #include #include -#include #include #include -#include - #include #include @@ -48,21 +33,21 @@ namespace nebula namespace ros { /// @brief Offline velodyne driver usage example (Output PCD data) -class VelodyneRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrapperBase +class VelodyneRosOfflineExtractBag final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; /// @brief Initializing ros wrapper /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver /// @return Resulting status Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration); /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration diff --git a/nebula_examples/package.xml b/nebula_examples/package.xml index 6f1163279..e9e6f6456 100644 --- a/nebula_examples/package.xml +++ b/nebula_examples/package.xml @@ -2,7 +2,7 @@ nebula_examples - 0.1.0 + 0.2.0 Nebula Offline Processing Samples Perception Engine @@ -12,24 +12,15 @@ ament_cmake_auto ros_environment - diagnostic_msgs - diagnostic_updater libpcl-all-dev nebula_common nebula_decoders nebula_ros - pandar_msgs - pcl_conversions - pcl_msgs - rclcpp - rclcpp_components rosbag2_cpp - sensor_msgs - velodyne_msgs yaml-cpp + ament_cmake_gtest ament_lint_auto - ament_lint_common ament_cmake diff --git a/nebula_examples/src/common/parameter_descriptors.cpp b/nebula_examples/src/common/parameter_descriptors.cpp new file mode 100644 index 000000000..7f4cfed8c --- /dev/null +++ b/nebula_examples/src/common/parameter_descriptors.cpp @@ -0,0 +1,36 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/common/parameter_descriptors.hpp" + +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write() +{ + return rcl_interfaces::msg::ParameterDescriptor{}; +}; + +rcl_interfaces::msg::ParameterDescriptor param_read_only() +{ + return rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); +} + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step) +{ + return { + rcl_interfaces::msg::FloatingPointRange().set__from_value(start).set__to_value(stop).set__step( + step)}; +} + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step) +{ + return { + rcl_interfaces::msg::IntegerRange().set__from_value(start).set__to_value(stop).set__step(step)}; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 9c1266347..62f988243 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -17,10 +17,8 @@ #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" #include "rosbag2_cpp/writers/sequential_writer.hpp" #include "rosbag2_storage/storage_options.hpp" @@ -41,7 +39,7 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration, correction_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -56,12 +54,11 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -69,178 +66,70 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( Status HesaiRosOfflineExtractBag::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { - // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractBag::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) +Status HesaiRosOfflineExtractBag::GetStatus() { - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); + return wrapper_status_; } -Status HesaiRosOfflineExtractBag::GetStatus() {return wrapper_status_;} - Status HesaiRosOfflineExtractBag::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration) { + auto sensor_model_ = this->declare_parameter("sensor_model", "", param_read_only()); + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_); + auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); + sensor_configuration.return_mode = + nebula::drivers::ReturnModeFromStringHesai(return_mode_, sensor_configuration.sensor_model); + this->declare_parameter("frame_id", "pandar", param_read_only()); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = - // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); - nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); + descriptor.integer_range = int_range(0, 360, 1); + sensor_configuration.sync_angle = + declare_parameter("sync_angle", 0., param_read_only()); } + { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; + descriptor.floating_point_range = float_range(0, 360, 0.01); + sensor_configuration.cut_angle = declare_parameter("cut_angle", 0., param_read_only()); } + + calibration_configuration.calibration_file = + this->declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("bag_path", "", descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_path", "", descriptor); - out_path = this->get_parameter("out_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 2; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_num", 3, descriptor); - out_num = this->get_parameter("out_num").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 2; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("skip_num", 3, descriptor); - skip_num = this->get_parameter("skip_num").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 1; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("only_xyz", false, descriptor); - only_xyz = this->get_parameter("only_xyz").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + correction_file_path = + this->declare_parameter("correction_file", "", param_read_only()); } + bag_path = this->declare_parameter("bag_path", "", param_read_only()); + storage_id = this->declare_parameter("storage_id", "sqlite3", param_read_only()); + out_path = this->declare_parameter("out_path", "", param_read_only()); + format = this->declare_parameter("format", "cdr", param_read_only()); + out_num = this->declare_parameter("out_num", 3, param_read_only()); + skip_num = this->declare_parameter("skip_num", 3, param_read_only()); + only_xyz = this->declare_parameter("only_xyz", false, param_read_only()); + target_topic = this->declare_parameter("target_topic", "", param_read_only()); + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { return Status::INVALID_ECHO_MODE; } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + if (sensor_configuration.frame_id.empty()) { return Status::SENSOR_CONFIG_ERROR; } if (calibration_configuration.calibration_file.empty()) { @@ -268,7 +157,7 @@ Status HesaiRosOfflineExtractBag::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "Sensor Configuration: " << sensor_configuration); return Status::OK; } @@ -296,75 +185,83 @@ Status HesaiRosOfflineExtractBag::ReadBag() if (rcpputils::fs::create_directories(o_dir)) { std::cout << "created: " << o_dir << std::endl; } - // return Status::OK; - pcl::PCDWriter writer; + pcl::PCDWriter pcd_writer; - std::unique_ptr writer_; - bool needs_open = true; + std::unique_ptr bag_writer{}; storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; - { - rosbag2_cpp::Reader reader(std::make_unique()); - // reader.open(rosbag_directory.string()); - reader.open(storage_options, converter_options); - int cnt = 0; - int out_cnt = 0; - while (reader.has_next()) { - auto bag_message = reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num << std::endl; - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::Serialization serialization; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - // std::cout<<"Found data in topic " << bag_message->topic_name << ": " << - // extracted_test_msg.data << std::endl; - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud( - std::make_shared(extracted_msg)); - auto pointcloud = std::get<0>(pointcloud_ts); - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); - - if (needs_open) { - const rosbag2_storage::StorageOptions storage_options_w( - {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); - const rosbag2_cpp::ConverterOptions converter_options_w( - {rmw_get_serialization_format(), rmw_get_serialization_format()}); - writer_ = std::make_unique(); - writer_->open(storage_options_w, converter_options_w); - writer_->create_topic( - {bag_message->topic_name, "pandar_msgs/msg/PandarScan", rmw_get_serialization_format(), - ""}); - needs_open = false; - } - writer_->write(bag_message); - cnt++; - if (skip_num < cnt) { - out_cnt++; - if (only_xyz) { - pcl::PointCloud cloud_xyz; - pcl::copyPointCloud(*pointcloud, cloud_xyz); - writer.writeBinary((o_dir / fn).string(), cloud_xyz); - } else { - writer.writeBinary((o_dir / fn).string(), *pointcloud); - } - // writer.writeASCII((o_dir / fn).string(), *pointcloud); - } - if (out_num <= out_cnt) { - break; + converter_options.output_serialization_format = format; + + rosbag2_cpp::Reader reader(std::make_unique()); + reader.open(storage_options, converter_options); + int cnt = 0; + int out_cnt = 0; + while (reader.has_next()) { + auto bag_message = reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name != target_topic) { + continue; + } + + std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num << std::endl; + pandar_msgs::msg::PandarScan extracted_msg; + rclcpp::Serialization serialization; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + nebula_msgs::msg::NebulaPackets nebula_msg; + nebula_msg.header = extracted_msg.header; + for (auto & pkt : extracted_msg.packets) { + std::vector pkt_data(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size)); + auto pointcloud_ts = driver_ptr_->ParseCloudPacket(pkt_data); + auto pointcloud = std::get<0>(pointcloud_ts); + + nebula_msgs::msg::NebulaPacket nebula_pkt; + nebula_pkt.stamp = pkt.stamp; + nebula_pkt.data.swap(pkt_data); // move storage from `pkt_data` to `data` + nebula_msg.packets.push_back(nebula_pkt); + + if (!pointcloud) { + continue; + } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + if (!bag_writer) { + const rosbag2_storage::StorageOptions storage_options_w( + {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); + const rosbag2_cpp::ConverterOptions converter_options_w( + {rmw_get_serialization_format(), rmw_get_serialization_format()}); + bag_writer = std::make_unique(); + bag_writer->open(storage_options_w, converter_options_w); + bag_writer->create_topic( + {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(), + ""}); + } + + bag_writer->write(bag_message); + cnt++; + if (skip_num < cnt) { + out_cnt++; + if (only_xyz) { + pcl::PointCloud cloud_xyz; + pcl::copyPointCloud(*pointcloud, cloud_xyz); + pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz); + } else { + pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud); } } + + if (out_num <= out_cnt) { + break; + } } - // close on scope exit } return Status::OK; } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp index f0eed5055..db6a66285 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp @@ -36,7 +36,6 @@ int main(int argc, char * argv[]) if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); driver_status = hesai_driver->ReadBag(); - // exec.spin(); } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp index a57d9b96e..c1dd6d78c 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -17,7 +17,6 @@ #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/readers/sequential_reader.hpp" #include "rosbag2_storage/storage_options.hpp" @@ -41,7 +40,7 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration, correction_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -56,12 +55,11 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -69,151 +67,67 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( Status HesaiRosOfflineExtractSample::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractSample::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) +Status HesaiRosOfflineExtractSample::GetStatus() { - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); + return wrapper_status_; } -Status HesaiRosOfflineExtractSample::GetStatus() {return wrapper_status_;} - Status HesaiRosOfflineExtractSample::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration) { + auto sensor_model_ = this->declare_parameter("sensor_model", ""); + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_); + auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); + sensor_configuration.return_mode = + nebula::drivers::ReturnModeFromStringHesai(return_mode_, sensor_configuration.sensor_model); + sensor_configuration.frame_id = + declare_parameter("frame_id", "pandar", param_read_only()); + { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = - // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); - nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); + descriptor.integer_range = int_range(0, 360, 1); + sensor_configuration.sync_angle = + declare_parameter("sync_angle", 0., param_read_only()); } + { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; + descriptor.floating_point_range = float_range(0, 360, 0.01); + sensor_configuration.cut_angle = declare_parameter("cut_angle", 0., param_read_only()); } + + calibration_configuration.calibration_file = + declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("bag_path", "", descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_path", "", descriptor); - out_path = this->get_parameter("out_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + correction_file_path = declare_parameter("correction_file", "", param_read_only()); } + bag_path = declare_parameter("bag_path", "", param_read_only()); + storage_id = declare_parameter("storage_id", "sqlite3", param_read_only()); + out_path = declare_parameter("out_path", "", param_read_only()); + format = declare_parameter("format", "cdr", param_read_only()); + target_topic = declare_parameter("target_topic", "", param_read_only()); + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { return Status::INVALID_ECHO_MODE; } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + if (sensor_configuration.frame_id.empty()) { return Status::SENSOR_CONFIG_ERROR; } if (calibration_configuration.calibration_file.empty()) { @@ -241,7 +155,7 @@ Status HesaiRosOfflineExtractSample::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "Sensor Configuration: " << sensor_configuration); return Status::OK; } @@ -266,46 +180,46 @@ Status HesaiRosOfflineExtractSample::ReadBag() if (rcpputils::fs::create_directories(o_dir)) { std::cout << "created: " << o_dir << std::endl; } - // return Status::OK; pcl::PCDWriter writer; storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; - { - rosbag2_cpp::Reader reader(std::make_unique()); - // reader.open(rosbag_directory.string()); - reader.open(storage_options, converter_options); - while (reader.has_next()) { - auto bag_message = reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::Serialization serialization; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - // std::cout<<"Found data in topic " << bag_message->topic_name << ": " << - // extracted_test_msg.data << std::endl; - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - // nebula::drivers::NebulaPointCloudPtr pointcloud = - // driver_ptr_->ConvertScanToPointcloud( - // std::make_shared(extracted_msg)); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud( - std::make_shared(extracted_msg)); - auto pointcloud = std::get<0>(pointcloud_ts); - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); - writer.writeBinary((o_dir / fn).string(), *pointcloud); + converter_options.output_serialization_format = format; + + rosbag2_cpp::Reader reader(std::make_unique()); + reader.open(storage_options, converter_options); + while (reader.has_next()) { + auto bag_message = reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name != target_topic) { + continue; + } + + pandar_msgs::msg::PandarScan extracted_msg; + rclcpp::Serialization serialization; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + for (auto & pkt : extracted_msg.packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud = std::get<0>(pointcloud_ts); + + if (!pointcloud) { + continue; } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + writer.writeBinary((o_dir / fn).string(), *pointcloud); } - // close on scope exit } + return Status::OK; } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp index b504605e6..a4839dac0 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp @@ -37,7 +37,6 @@ int main(int argc, char * argv[]) if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); driver_status = hesai_driver->ReadBag(); - // exec.spin(); } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); } diff --git a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp index 913898b7c..5c9c67e39 100644 --- a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp @@ -14,6 +14,11 @@ #include "velodyne/velodyne_ros_offline_extract_bag_pcd.hpp" +#include +#include + +#include + namespace nebula { namespace ros @@ -27,36 +32,37 @@ VelodyneRosOfflineExtractBag::VelodyneRosOfflineExtractBag( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); + std::make_shared(calibration_configuration); - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + sensor_cfg_ptr_ = + std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } Status VelodyneRosOfflineExtractBag::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + driver_ptr_ = + std::make_shared(sensor_configuration, calibration_configuration); return driver_ptr_->GetStatus(); } -Status VelodyneRosOfflineExtractBag::GetStatus() {return wrapper_status_;} +Status VelodyneRosOfflineExtractBag::GetStatus() +{ + return wrapper_status_; +} Status VelodyneRosOfflineExtractBag::GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, @@ -338,37 +344,46 @@ Status VelodyneRosOfflineExtractBag::ReadBag() // nebula::drivers::NebulaPointCloudPtr pointcloud = // driver_ptr_->ConvertScanToPointcloud( // std::make_shared(extracted_msg)); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud( - std::make_shared(extracted_msg)); - auto pointcloud = std::get<0>(pointcloud_ts); - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - if (needs_open) { - const rosbag2_storage::StorageOptions storage_options_w( - {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); - const rosbag2_cpp::ConverterOptions converter_options_w( - {rmw_get_serialization_format(), rmw_get_serialization_format()}); - writer_ = std::make_unique(); - writer_->open(storage_options_w, converter_options_w); - writer_->create_topic( - {bag_message->topic_name, "velodyne_msgs/msg/VelodyneScan", - rmw_get_serialization_format(), ""}); - needs_open = false; - } - writer_->write(bag_message); - cnt++; - if (skip_num < cnt) { - out_cnt++; - if (only_xyz) { - pcl::PointCloud cloud_xyz; - pcl::copyPointCloud(*pointcloud, cloud_xyz); - writer.writeBinary((o_dir / fn).string(), cloud_xyz); - } else { - writer.writeBinary((o_dir / fn).string(), *pointcloud); + for (auto & pkt : extracted_msg.packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.data.size())), + pkt.stamp.sec); + auto pointcloud = std::get<0>(pointcloud_ts); + + if (!pointcloud) { + continue; + } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + if (needs_open) { + const rosbag2_storage::StorageOptions storage_options_w( + {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); + const rosbag2_cpp::ConverterOptions converter_options_w( + {rmw_get_serialization_format(), rmw_get_serialization_format()}); + writer_ = std::make_unique(); + writer_->open(storage_options_w, converter_options_w); + writer_->create_topic( + {bag_message->topic_name, "velodyne_msgs/msg/VelodyneScan", + rmw_get_serialization_format(), ""}); + needs_open = false; + } + writer_->write(bag_message); + cnt++; + if (skip_num < cnt) { + out_cnt++; + if (only_xyz) { + pcl::PointCloud cloud_xyz; + pcl::copyPointCloud(*pointcloud, cloud_xyz); + writer.writeBinary((o_dir / fn).string(), cloud_xyz); + } else { + writer.writeBinary((o_dir / fn).string(), *pointcloud); + } + } + if (out_num <= out_cnt) { + break; } - } - if (out_num <= out_cnt) { - break; } } } diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 32893d1f4..2577ea9a5 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -1,14 +1,6 @@ cmake_minimum_required(VERSION 3.14) project(nebula_hw_interfaces) -find_package(ament_cmake_auto REQUIRED) -find_package(PCL REQUIRED) -find_package(PCL REQUIRED COMPONENTS common) -find_package(pcl_conversions REQUIRED) -find_package(robosense_msgs REQUIRED) - -ament_auto_find_build_dependencies() - # Default to C++17 if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -18,35 +10,113 @@ if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function) endif () +find_package(ament_cmake_auto REQUIRED) +find_package(boost_tcp_driver) +find_package(boost_udp_driver) +find_package(nebula_common) +find_package(nebula_msgs) +find_package(pandar_msgs) +find_package(robosense_msgs) +find_package(ros2_socketcan) +find_package(velodyne_msgs) + +# Common includes for all targets include_directories( - include - SYSTEM - ${YAML_CPP_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${PCL_COMMON_INCLUDE_DIRS} + ${nebula_common_INCLUDE_DIRS} + include + SYSTEM ) -ament_auto_add_library(nebula_hw_interfaces_hesai SHARED - src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp - ) +link_libraries( + ${nebula_common_TARGETS} +) -ament_auto_add_library(nebula_hw_interfaces_velodyne SHARED - src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp - ) +add_library(nebula_hw_interfaces_hesai SHARED + src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +) +target_link_libraries(nebula_hw_interfaces_hesai PUBLIC + ${boost_tcp_driver_LIBRARIES} + ${boost_udp_driver_LIBRARIES} + ${pandar_msgs_TARGETS} +) -ament_auto_add_library(nebula_hw_interfaces_robosense SHARED - src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp - ) +target_include_directories(nebula_hw_interfaces_hesai PUBLIC + ${boost_tcp_driver_INCLUDE_DIRS} + ${boost_udp_driver_INCLUDE_DIRS} + ${pandar_msgs_INCLUDE_DIRS} +) -ament_auto_add_library(nebula_hw_interfaces_continental SHARED - src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp - src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp - ) +add_library(nebula_hw_interfaces_velodyne SHARED + src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +) +target_link_libraries(nebula_hw_interfaces_velodyne PUBLIC + ${boost_tcp_driver_LIBRARIES} + ${boost_udp_driver_LIBRARIES} + ${velodyne_msgs_TARGETS} +) +target_include_directories(nebula_hw_interfaces_velodyne PUBLIC + ${boost_udp_driver_INCLUDE_DIRS} + ${boost_tcp_driver_INCLUDE_DIRS} + ${velodyne_msgs_INCLUDE_DIRS} +) + +add_library(nebula_hw_interfaces_robosense SHARED + src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp +) +target_link_libraries(nebula_hw_interfaces_robosense PUBLIC + ${boost_tcp_driver_LIBRARIES} + ${boost_udp_driver_LIBRARIES} + ${robosense_msgs_TARGETS} + +) +target_include_directories(nebula_hw_interfaces_robosense PUBLIC + ${boost_udp_driver_INCLUDE_DIRS} + ${boost_tcp_driver_INCLUDE_DIRS} + ${robosense_msgs_INCLUDE_DIRS} +) + +add_library(nebula_hw_interfaces_continental SHARED + src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp + src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp +) +target_link_libraries(nebula_hw_interfaces_continental PUBLIC + ${boost_udp_driver_LIBRARIES} + ${nebula_msgs_TARGETS} + ${ros2_socketcan_LIBRARIES} +) +target_include_directories(nebula_hw_interfaces_continental PUBLIC + ${boost_udp_driver_INCLUDE_DIRS} + ${nebula_msgs_INCLUDE_DIRS} + ${ros2_socketcan_INCLUDE_DIRS} +) + +install(TARGETS nebula_hw_interfaces_hesai EXPORT export_nebula_hw_interfaces_hesai) +install(TARGETS nebula_hw_interfaces_velodyne EXPORT export_nebula_hw_interfaces_velodyne) +install(TARGETS nebula_hw_interfaces_robosense EXPORT export_nebula_hw_interfaces_robosense) +install(TARGETS nebula_hw_interfaces_continental EXPORT export_nebula_hw_interfaces_continental) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() -ament_auto_package() +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_nebula_hw_interfaces_hesai) +ament_export_targets(export_nebula_hw_interfaces_velodyne) +ament_export_targets(export_nebula_hw_interfaces_robosense) +ament_export_targets(export_nebula_hw_interfaces_continental) + +ament_export_dependencies( + boost_tcp_driver + boost_udp_driver + nebula_common + nebula_msgs + pandar_msgs + robosense_msgs + ros2_socketcan + velodyne_msgs +) + +ament_package() diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp index 5b46415e7..9044bf5c6 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp @@ -1,12 +1,24 @@ +// 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_HW_INTERFACE_BASE_H #define NEBULA_HW_INTERFACE_BASE_H #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "boost_udp_driver/udp_driver.hpp" -#include -#include +#include #include namespace nebula @@ -22,7 +34,7 @@ class NebulaHwInterfaceBase * @param buffer Buffer containing the data received from the UDP socket * @return Status::OK if no error occurred. */ - virtual void ReceiveSensorPacketCallback([[maybe_unused]] const std::vector & buffer) {} + virtual void ReceiveSensorPacketCallback([[maybe_unused]] std::vector & buffer) {} // virtual Status RegisterScanCallback( // std::function>>)> scan_callback) = 0; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index 3e150b18e..0563dc605 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -14,32 +14,16 @@ #ifndef NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H #define NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H -// 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 -#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 -#define BOOST_BIND_GLOBAL_PLACEHOLDERS -#endif -#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 -#define BOOST_ALLOW_DEPRECATED_HEADERS -#endif -#include -#include + +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" + #include #include -#include #include #include -#include - -#include -#include -#include #include -#include #include #include @@ -50,84 +34,31 @@ namespace drivers namespace continental_ars548 { /// @brief Hardware interface of the Continental ARS548 radar -class ContinentalARS548HwInterface : NebulaHwInterfaceBase +class ContinentalARS548HwInterface { -private: - std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; - std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_; - std::shared_ptr sensor_configuration_; - std::unique_ptr nebula_packets_ptr_; - std::function buffer)> - nebula_packets_reception_callback_; - - std::mutex sensor_status_mutex_; - - SensorStatusPacket sensor_status_packet_{}; - FilterStatusPacket filter_status_{}; - - std::shared_ptr parent_node_logger; - - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void PrintInfo(std::string info); - - /// @brief Printing the string to RCLCPP_ERROR_STREAM - /// @param error Target string - void PrintError(std::string error); - - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void PrintDebug(std::string debug); - - /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM - /// @param bytes Target byte vector - void PrintDebug(const std::vector & bytes); - public: /// @brief Constructor ContinentalARS548HwInterface(); - /// @brief Process a new filter status packet - /// @param buffer The buffer containing the status packet - void ProcessFilterStatusPacket(const std::vector & buffer); - - /// @brief Process a new data packet - /// @param buffer The buffer containing the data packet - void ProcessDataPacket(const std::vector & buffer); - - /// @brief Callback function to receive the Cloud Packet data from the UDP Driver - /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallbackWithSender( - const std::vector & buffer, const std::string & sender_ip); - - /// @brief Callback function to receive the Cloud Packet data from the UDP Driver - /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer) final; - /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart() final; + Status SensorInterfaceStart(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop() final; - - /// @brief Printing sensor configuration - /// @param sensor_configuration SensorConfiguration for this interface - /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + Status SensorInterfaceStop(); /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; + std::shared_ptr sensor_configuration); - /// @brief Registering callback for PandarScan - /// @param scan_callback Callback function + /// @brief Registering callback + /// @param callback Callback function /// @return Resulting status - Status RegisterScanCallback( - std::function)> scan_callback); + Status RegisterPacketCallback( + std::function)> packet_callback); /// @brief Set the sensor mounting parameters /// @param longitudinal_autosar Desired longitudinal value in autosar coordinates @@ -203,13 +134,38 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase /// @return Resulting status Status SetYawRate(float yaw_rate); - /// @brief Checking the current settings and changing the difference point - /// @return Resulting status - Status CheckAndSetConfig(); - /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr node); + +private: + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveSensorPacketCallbackWithSender( + std::vector & buffer, const std::string & sender_ip); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveSensorPacketCallback(std::vector & buffer); + + std::unique_ptr<::drivers::common::IoContext> sensor_io_context_ptr_; + std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_ptr_; + std::shared_ptr config_ptr_; + std::function)> packet_callback_; + + std::shared_ptr parent_node_logger_ptr_; }; } // namespace continental_ars548 } // namespace drivers diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp new file mode 100644 index 000000000..ddbbe1619 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -0,0 +1,148 @@ +// 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_CONTINENTAL_SRR520_HW_INTERFACE_H +#define NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H + +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +/// @brief Hardware interface of the Continental SRR520 radar +class ContinentalSRR520HwInterface +{ +public: + /// @brief Constructor + ContinentalSRR520HwInterface(); + + /// @brief Starting the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStart(); + + /// @brief Function for stopping the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStop(); + + /// @brief Setting sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status SetSensorConfiguration( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> + new_config_ptr); + + /// @brief Registering callback + /// @param callback Callback function + /// @return Resulting status + Status RegisterPacketCallback( + std::function)> packet_callback); + + /// @brief Sensor synchronization routine + void SensorSync(); + + /// @brief Process a new Sync Follow-up request + void SensorSyncFollowUp(builtin_interfaces::msg::Time stamp); + + /// @brief Configure the sensor + /// @param sensor_id Desired sensor id + /// @param longitudinal_autosar Desired longitudinal value in autosar coordinates + /// @param lateral_autosar Desired lateral value in AUTOSAR coordinates + /// @param vertical_autosar Desired vertical value in autosar coordinates + /// @param yaw_autosar Desired yaw value in autosar coordinates + /// @param longitudinal_cog Desired longitudinal cog + /// @param wheelbase Desired wheelbase + /// @param cover_damping Desired cover damping + /// @param plug_bottom Desired plug bottom + /// @param reset Rest the sensor to its default values + /// @return Resulting status + Status ConfigureSensor( + uint8_t sensor_id, float longitudinal_autosar, float lateral_autosar, float vertical_autosar, + float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, + bool plug_bottom, bool reset); + + /// @brief Set the current vehicle dynamics + /// @param longitudinal_acceleration Longitudinal acceleration + /// @param lateral_acceleration Lateral acceleration + /// @param yaw_rate Yaw rate + /// @param longitudinal_velocity Longitudinal velocity + /// @param driving_direction Driving direction + /// @return Resulting status + Status SetVehicleDynamics( + float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, + float longitudinal_velocity, bool standstill); + + /// @brief Setting rclcpp::Logger + /// @param node Logger + void SetLogger(std::shared_ptr node); + +private: + /// @brief Send a Fd frame + /// @param data a buffer containing the data to send + template + bool SendFrame(const std::array & data, int can_frame_id); + + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + + /// @brief Main loop of the CAN receiver thread + void ReceiveLoop(); + + std::unique_ptr<::drivers::socketcan::SocketCanReceiver> can_receiver_ptr_; + std::unique_ptr<::drivers::socketcan::SocketCanSender> can_sender_ptr_; + std::unique_ptr receiver_thread_ptr_; + + std::shared_ptr config_ptr_; + std::function buffer)> + nebula_packet_callback_; + + std::mutex receiver_mutex_; + bool sensor_interface_active_{}; + + uint8_t sync_counter_{0}; + bool sync_follow_up_sent_{true}; + builtin_interfaces::msg::Time last_sync_stamp_; + + std::shared_ptr parent_node_logger_ptr_; +}; +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp deleted file mode 100644 index b029ae40e..000000000 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// 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_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H -#define NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H -// 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 -#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 -#define BOOST_BIND_GLOBAL_PLACEHOLDERS -#endif -#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 -#define BOOST_ALLOW_DEPRECATED_HEADERS -#endif -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace drivers -{ -namespace continental_ars548 -{ -/// @brief Hardware interface of the Continental ARS548 radar -class MultiContinentalARS548HwInterface : NebulaHwInterfaceBase -{ -private: - std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; - std::vector> sensor_udp_drivers_; - std::shared_ptr sensor_configuration_; - std::unique_ptr nebula_packets_ptr_; - std::function buffer, const std::string & sensor_ip)> - nebula_packets_reception_callback_; - - std::mutex sensor_status_mutex_; - - SensorStatusPacket sensor_status_packet_{}; - FilterStatusPacket filter_status_{}; - std::unordered_map sensor_ip_to_frame_; - - std::shared_ptr parent_node_logger; - - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void PrintInfo(std::string info); - - /// @brief Printing the string to RCLCPP_ERROR_STREAM - /// @param error Target string - void PrintError(std::string error); - - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void PrintDebug(std::string debug); - - /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM - /// @param bytes Target byte vector - void PrintDebug(const std::vector & bytes); - -public: - /// @brief Constructor - MultiContinentalARS548HwInterface(); - - /// @brief Process a new filter status packet - /// @param buffer The buffer containing the status packet - void ProcessFilterStatusPacket(const std::vector & buffer); - - /// @brief Process a new data packet - /// @param buffer The buffer containing the data packet - void ProcessDataPacket(const std::vector & buffer, const std::string & sensor_ip); - - /// @brief Callback function to receive the Cloud Packet data from the UDP Driver - /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback( - const std::vector & buffer, const std::string & sensor_ip); - - /// @brief Starting the interface that handles UDP streams - /// @return Resulting status - Status SensorInterfaceStart() final; - - /// @brief Function for stopping the interface that handles UDP streams - /// @return Resulting status - Status SensorInterfaceStop() final; - - /// @brief Printing sensor configuration - /// @param sensor_configuration SensorConfiguration for this interface - /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; - - /// @brief Setting sensor configuration - /// @param sensor_configuration SensorConfiguration for this interface - /// @return Resulting status - Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; - - /// @brief Registering callback for PandarScan - /// @param scan_callback Callback function - /// @return Resulting status - Status RegisterScanCallback( - std::function, const std::string &)> - scan_callback); - - /// @brief Set the current lateral acceleration - /// @param lateral_acceleration Current lateral acceleration - /// @return Resulting status - Status SetAccelerationLateralCog(float lateral_acceleration); - - /// @brief Set the current longitudinal acceleration - /// @param longitudinal_acceleration Current longitudinal acceleration - /// @return Resulting status - Status SetAccelerationLongitudinalCog(float longitudinal_acceleration); - - /// @brief Set the characteristic speed - /// @param characteristic_speed Characteristic speed - /// @return Resulting status - Status SetCharacteristicSpeed(float characteristic_speed); - - /// @brief Set the current direction - /// @param direction Current driving direction - /// @return Resulting status - Status SetDrivingDirection(int direction); - - /// @brief Set the current steering angle - /// @param angle_rad Current steering angle in radians - /// @return Resulting status - Status SetSteeringAngleFrontAxle(float angle_rad); - - /// @brief Set the current vehicle velocity - /// @param velocity Current vehicle velocity - /// @return Resulting status - Status SetVelocityVehicle(float velocity); - - /// @brief Set the current yaw rate - /// @param yaw_rate Current yaw rate - /// @return Resulting status - Status SetYawRate(float yaw_rate); - - /// @brief Checking the current settings and changing the difference point - /// @return Resulting status - Status CheckAndSetConfig(); - - /// @brief Setting rclcpp::Logger - /// @param node Logger - void SetLogger(std::shared_ptr node); -}; -} // namespace continental_ars548 -} // namespace drivers -} // namespace nebula - -#endif // NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp index 415225309..f3bb7b073 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp @@ -1,20 +1,43 @@ +// 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 HESAI_CMD_RESPONSE_HPP #define HESAI_CMD_RESPONSE_HPP #include +#include #include -#include +#include +#include #include +#include +#include + +using namespace boost::endian; // NOLINT(build/namespaces) namespace nebula { + +#pragma pack(push, 1) + /// @brief PTP STATUS struct of PTC_COMMAND_PTP_DIAGNOSTICS struct HesaiPtpDiagStatus { - long long master_offset; - int ptp_state; - int elapsed_millisec; + big_int64_buf_t master_offset; + big_int32_buf_t ptp_state; + big_int32_buf_t elapsed_millisec; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpDiagStatus const & arg) { @@ -31,35 +54,20 @@ struct HesaiPtpDiagStatus /// @brief PTP TLV PORT_DATA_SET struct of PTC_COMMAND_PTP_DIAGNOSTICS struct HesaiPtpDiagPort { - std::vector portIdentity = std::vector(10); - int portState; - int logMinDelayReqInterval; - long long peerMeanPathDelay; - int logAnnounceInterval; - int announceReceiptTimeout; - int logSyncInterval; - int delayMechanism; - int logMinPdelayReqInterval; - int versionNumber; - - HesaiPtpDiagPort() {} - HesaiPtpDiagPort(const HesaiPtpDiagPort & arg) - { - std::copy(arg.portIdentity.begin(), arg.portIdentity.end(), portIdentity.begin()); - portState = arg.portState; - logMinDelayReqInterval = arg.logMinDelayReqInterval; - peerMeanPathDelay = arg.peerMeanPathDelay; - logAnnounceInterval = arg.logAnnounceInterval; - announceReceiptTimeout = arg.announceReceiptTimeout; - logSyncInterval = arg.logSyncInterval; - delayMechanism = arg.delayMechanism; - logMinPdelayReqInterval = arg.logMinPdelayReqInterval; - versionNumber = arg.versionNumber; - } + uint8_t portIdentity[10]; + big_int32_buf_t portState; + big_int32_buf_t logMinDelayReqInterval; + big_int64_buf_t peerMeanPathDelay; + big_int32_buf_t logAnnounceInterval; + big_int32_buf_t announceReceiptTimeout; + big_int32_buf_t logSyncInterval; + big_int32_buf_t delayMechanism; + big_int32_buf_t logMinPdelayReqInterval; + big_int32_buf_t versionNumber; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpDiagPort const & arg) { - os << "portIdentity: " << std::string(arg.portIdentity.begin(), arg.portIdentity.end()); + os << "portIdentity: " << std::string(std::begin(arg.portIdentity), std::end(arg.portIdentity)); os << ", "; os << "portState: " << arg.portState; os << ", "; @@ -86,28 +94,14 @@ struct HesaiPtpDiagPort /// @brief LinuxPTP TLV TIME_STATUS_NP struct of PTC_COMMAND_PTP_DIAGNOSTICS struct HesaiPtpDiagTime { - long long master_offset; - long long ingress_time; - int cumulativeScaledRateOffset; - int scaledLastGmPhaseChange; - int gmTimeBaseIndicator; - std::vector lastGmPhaseChange = std::vector(12); - int gmPresent; - long long gmIdentity; - - HesaiPtpDiagTime() {} - HesaiPtpDiagTime(const HesaiPtpDiagTime & arg) - { - master_offset = arg.master_offset; - ingress_time = arg.ingress_time; - cumulativeScaledRateOffset = arg.cumulativeScaledRateOffset; - scaledLastGmPhaseChange = arg.scaledLastGmPhaseChange; - gmTimeBaseIndicator = arg.gmTimeBaseIndicator; - std::copy( - arg.lastGmPhaseChange.begin(), arg.lastGmPhaseChange.end(), lastGmPhaseChange.begin()); - gmPresent = arg.gmPresent; - gmIdentity = arg.gmIdentity; - } + big_int64_buf_t master_offset; + big_int64_buf_t ingress_time; + big_int32_buf_t cumulativeScaledRateOffset; + big_int32_buf_t scaledLastGmPhaseChange; + big_int16_buf_t gmTimeBaseIndicator; + uint8_t lastGmPhaseChange[12]; + big_int32_buf_t gmPresent; + big_int64_buf_t gmIdentity; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpDiagTime const & arg) { @@ -121,8 +115,9 @@ struct HesaiPtpDiagTime os << ", "; os << "gmTimeBaseIndicator: " << arg.gmTimeBaseIndicator; os << ", "; + // FIXME: lastGmPhaseChange is a binary number, displaying it as string is incorrect os << "lastGmPhaseChange: " - << std::string(arg.lastGmPhaseChange.begin(), arg.lastGmPhaseChange.end()); + << std::string(std::begin(arg.lastGmPhaseChange), std::end(arg.lastGmPhaseChange)); os << ", "; os << "gmPresent: " << arg.gmPresent; os << ", "; @@ -135,10 +130,10 @@ struct HesaiPtpDiagTime /// @brief LinuxPTP TLV GRANDMASTER_SETTINGS_NP struct of PTC_COMMAND_PTP_DIAGNOSTICS struct HesaiPtpDiagGrandmaster { - int clockQuality; - int utc_offset; - int time_flags; - int time_source; + big_int32_buf_t clockQuality; + big_int16_buf_t utc_offset; + int8_t time_flags; + int8_t time_source; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpDiagGrandmaster const & arg) { @@ -146,9 +141,9 @@ struct HesaiPtpDiagGrandmaster os << ", "; os << "utc_offset: " << arg.utc_offset; os << ", "; - os << "time_flags: " << arg.time_flags; + os << "time_flags: " << +arg.time_flags; os << ", "; - os << "time_source: " << arg.time_source; + os << "time_source: " << +arg.time_source; return os; } @@ -157,81 +152,73 @@ struct HesaiPtpDiagGrandmaster /// @brief struct of PTC_COMMAND_GET_INVENTORY_INFO struct HesaiInventory { - std::vector sn = std::vector(18); - std::vector date_of_manufacture = std::vector(16); - std::vector mac = std::vector(6); - std::vector sw_ver = std::vector(16); - std::vector hw_ver = std::vector(16); - std::vector control_fw_ver = std::vector(16); - std::vector sensor_fw_ver = std::vector(16); - int angle_offset; - int model; - int motor_type; - int num_of_lines; - std::vector reserved = std::vector(11); - - HesaiInventory() {} - HesaiInventory(const HesaiInventory & arg) - { - std::copy(arg.sn.begin(), arg.sn.end(), sn.begin()); - std::copy( - arg.date_of_manufacture.begin(), arg.date_of_manufacture.end(), date_of_manufacture.begin()); - std::copy(arg.mac.begin(), arg.mac.end(), mac.begin()); - std::copy(arg.sw_ver.begin(), arg.sw_ver.end(), sw_ver.begin()); - std::copy(arg.hw_ver.begin(), arg.hw_ver.end(), hw_ver.begin()); - std::copy(arg.control_fw_ver.begin(), arg.control_fw_ver.end(), control_fw_ver.begin()); - std::copy(arg.sensor_fw_ver.begin(), arg.sensor_fw_ver.end(), sensor_fw_ver.begin()); - angle_offset = arg.angle_offset; - model = arg.model; - motor_type = arg.motor_type; - num_of_lines = arg.num_of_lines; - std::copy(arg.reserved.begin(), arg.reserved.end(), reserved.begin()); - } + char sn[18]; + char date_of_manufacture[16]; + uint8_t mac[6]; + char sw_ver[16]; + char hw_ver[16]; + char control_fw_ver[16]; + char sensor_fw_ver[16]; + big_uint16_buf_t angle_offset; + uint8_t model; + uint8_t motor_type; + uint8_t num_of_lines; + uint8_t reserved[11]; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiInventory const & arg) { - os << "sn: " << std::string(arg.sn.begin(), arg.sn.end()); + std::ios initial_format(nullptr); + initial_format.copyfmt(os); + + os << "sn: " << std::string(arg.sn, strnlen(arg.sn, sizeof(arg.sn))); os << ", "; os << "date_of_manufacture: " - << std::string(arg.date_of_manufacture.begin(), arg.date_of_manufacture.end()); + << std::string( + arg.date_of_manufacture, + strnlen(arg.date_of_manufacture, sizeof(arg.date_of_manufacture))); os << ", "; os << "mac: "; - std::stringstream ss; - for (long unsigned int i = 0; i < arg.mac.size() - 1; i++) { - ss << std::hex << std::setfill('0') << std::setw(2) << (static_cast(arg.mac[i]) & 0xff) - << ":"; + + for (size_t i = 0; i < sizeof(arg.mac); i++) { + if (i != 0) { + os << ':'; + } + os << std::hex << std::setfill('0') << std::setw(2) << (+arg.mac[i]); } - ss << std::hex << std::setfill('0') << std::setw(2) - << (static_cast(arg.mac[arg.mac.size() - 1]) & 0xff); - os << ss.str(); + os.copyfmt(initial_format); + os << ", "; - os << "sw_ver: " << std::string(arg.sw_ver.begin(), arg.sw_ver.end()); + os << "sw_ver: " << std::string(arg.sw_ver, strnlen(arg.sw_ver, sizeof(arg.sw_ver))); os << ", "; - os << "hw_ver: " << std::string(arg.hw_ver.begin(), arg.hw_ver.end()); + os << "hw_ver: " << std::string(arg.hw_ver, strnlen(arg.hw_ver, sizeof(arg.hw_ver))); os << ", "; - os << "control_fw_ver: " << std::string(arg.control_fw_ver.begin(), arg.control_fw_ver.end()); + os << "control_fw_ver: " + << std::string(arg.control_fw_ver, strnlen(arg.control_fw_ver, sizeof(arg.control_fw_ver))); os << ", "; - os << "sensor_fw_ver: " << std::string(arg.sensor_fw_ver.begin(), arg.sensor_fw_ver.end()); + os << "sensor_fw_ver: " + << std::string(arg.sensor_fw_ver, strnlen(arg.sensor_fw_ver, sizeof(arg.sensor_fw_ver))); os << ", "; os << "angle_offset: " << arg.angle_offset; os << ", "; - os << "model: " << arg.model; + os << "model: " << +arg.model; os << ", "; - os << "motor_type: " << arg.motor_type; + os << "motor_type: " << +arg.motor_type; os << ", "; - os << "num_of_lines: " << arg.num_of_lines; + os << "num_of_lines: " << +arg.num_of_lines; os << ", "; - // os << "reserved: " << boost::algorithm::join(arg.reserved, ","); - os << "reserved: "; - for (long unsigned int i = 0; i < arg.reserved.size() - 1; i++) { - os << arg.reserved[i] << ","; + + for (size_t i = 0; i < sizeof(arg.reserved); i++) { + if (i != 0) { + os << ' '; + } + os << std::hex << std::setfill('0') << std::setw(2) << (+arg.reserved[i]); } - os << arg.reserved[arg.reserved.size() - 1]; + os.copyfmt(initial_format); return os; } - std::string get_str_model() + std::string get_str_model() const { switch (model) { case 0: @@ -259,7 +246,7 @@ struct HesaiInventory case 48: return "PandarAT128"; default: - return "Unknown(" + std::to_string(model) + ")"; + return "Unknown(" + std::to_string(static_cast(model)) + ")"; } } }; @@ -267,42 +254,46 @@ struct HesaiInventory /// @brief struct of PTC_COMMAND_GET_CONFIG_INFO struct HesaiConfig { - int ipaddr[4]; - int mask[4]; - int gateway[4]; - int dest_ipaddr[4]; - int dest_LiDAR_udp_port; - int dest_gps_udp_port; - int spin_rate; - int sync; - int sync_angle; - int start_angle; - int stop_angle; - int clock_source; - int udp_seq; - int trigger_method; - int return_mode; - int standby_mode; - int motor_status; - int vlan_flag; - int vlan_id; - int clock_data_fmt; - int noise_filtering; - int reflectivity_mapping; - unsigned char reserved[6]; + uint8_t ipaddr[4]; + uint8_t mask[4]; + uint8_t gateway[4]; + uint8_t dest_ipaddr[4]; + big_uint16_buf_t dest_LiDAR_udp_port; + big_uint16_buf_t dest_gps_udp_port; + big_uint16_buf_t spin_rate; + uint8_t sync; + big_uint16_buf_t sync_angle; + big_uint16_buf_t start_angle; + big_uint16_buf_t stop_angle; + uint8_t clock_source; + uint8_t udp_seq; + uint8_t trigger_method; + uint8_t return_mode; + uint8_t standby_mode; + uint8_t motor_status; + uint8_t vlan_flag; + big_uint16_buf_t vlan_id; + uint8_t clock_data_fmt; // FIXME: labeled as gps_nmea_sentence in AT128, OT128 datasheets + uint8_t noise_filtering; + uint8_t reflectivity_mapping; + uint8_t reserved[6]; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiConfig const & arg) { - os << "ipaddr: " << arg.ipaddr[0] << "." << arg.ipaddr[1] << "." << arg.ipaddr[2] << "." - << arg.ipaddr[3]; + std::ios initial_format(nullptr); + initial_format.copyfmt(os); + + os << "ipaddr: " << +arg.ipaddr[0] << "." << +arg.ipaddr[1] << "." << +arg.ipaddr[2] << "." + << +arg.ipaddr[3]; os << ", "; - os << "mask: " << arg.mask[0] << "." << arg.mask[1] << "." << arg.mask[2] << "." << arg.mask[3]; + os << "mask: " << +arg.mask[0] << "." << +arg.mask[1] << "." << +arg.mask[2] << "." + << +arg.mask[3]; os << ", "; - os << "gateway: " << arg.gateway[0] << "." << arg.gateway[1] << "." << arg.gateway[2] << "." - << arg.gateway[3]; + os << "gateway: " << +arg.gateway[0] << "." << +arg.gateway[1] << "." << +arg.gateway[2] << "." + << +arg.gateway[3]; os << ", "; - os << "dest_ipaddr: " << arg.dest_ipaddr[0] << "." << arg.dest_ipaddr[1] << "." - << arg.dest_ipaddr[2] << "." << arg.dest_ipaddr[3]; + os << "dest_ipaddr: " << +arg.dest_ipaddr[0] << "." << +arg.dest_ipaddr[1] << "." + << +arg.dest_ipaddr[2] << "." << +arg.dest_ipaddr[3]; os << ", "; os << "dest_LiDAR_udp_port: " << arg.dest_LiDAR_udp_port; os << ", "; @@ -310,7 +301,7 @@ struct HesaiConfig os << ", "; os << "spin_rate: " << arg.spin_rate; os << ", "; - os << "sync: " << arg.sync; + os << "sync: " << +arg.sync; os << ", "; os << "sync_angle: " << arg.sync_angle; os << ", "; @@ -318,30 +309,37 @@ struct HesaiConfig os << ", "; os << "stop_angle: " << arg.stop_angle; os << ", "; - os << "clock_source: " << arg.clock_source; + os << "clock_source: " << +arg.clock_source; os << ", "; - os << "udp_seq: " << arg.udp_seq; + os << "udp_seq: " << +arg.udp_seq; os << ", "; - os << "trigger_method: " << arg.trigger_method; + os << "trigger_method: " << +arg.trigger_method; os << ", "; - os << "return_mode: " << arg.return_mode; + os << "return_mode: " << +arg.return_mode; os << ", "; - os << "standby_mode: " << arg.standby_mode; + os << "standby_mode: " << +arg.standby_mode; os << ", "; - os << "motor_status: " << arg.motor_status; + os << "motor_status: " << +arg.motor_status; os << ", "; - os << "vlan_flag: " << arg.vlan_flag; + os << "vlan_flag: " << +arg.vlan_flag; os << ", "; os << "vlan_id: " << arg.vlan_id; os << ", "; - os << "clock_data_fmt: " << arg.clock_data_fmt; + os << "clock_data_fmt: " << +arg.clock_data_fmt; os << ", "; - os << "noise_filtering: " << arg.noise_filtering; + os << "noise_filtering: " << +arg.noise_filtering; os << ", "; - os << "reflectivity_mapping: " << arg.reflectivity_mapping; + os << "reflectivity_mapping: " << +arg.reflectivity_mapping; os << ", "; - os << "reserved: " << arg.reserved[0] << "," << arg.reserved[1] << "," << arg.reserved[2] << "," - << arg.reserved[3] << "," << arg.reserved[4] << "," << arg.reserved[5]; + os << "reserved: "; + + for (size_t i = 0; i < sizeof(arg.reserved); i++) { + if (i != 0) { + os << ' '; + } + os << std::hex << std::setfill('0') << std::setw(2) << (+arg.reserved[i]); + } + os.copyfmt(initial_format); return os; } @@ -350,108 +348,93 @@ struct HesaiConfig /// @brief struct of PTC_COMMAND_GET_LIDAR_STATUS struct HesaiLidarStatus { - int system_uptime; - int motor_speed; - // int temperature[8]; - std::vector temperature = std::vector(8); - int gps_pps_lock; - int gps_gprmc_status; - int startup_times; - int total_operation_time; - int ptp_clock_status; - std::vector reserved = std::vector(5); - - HesaiLidarStatus() {} - HesaiLidarStatus(const HesaiLidarStatus & arg) - { - system_uptime = arg.system_uptime; - motor_speed = arg.motor_speed; - std::copy(arg.temperature.begin(), arg.temperature.end(), temperature.begin()); - gps_pps_lock = arg.gps_pps_lock; - gps_gprmc_status = arg.gps_gprmc_status; - startup_times = arg.startup_times; - total_operation_time = arg.total_operation_time; - ptp_clock_status = arg.ptp_clock_status; - std::copy(arg.reserved.begin(), arg.reserved.end(), reserved.begin()); - } + big_uint32_buf_t system_uptime; + big_uint16_buf_t motor_speed; + big_int32_buf_t temperature[8]; + uint8_t gps_pps_lock; + uint8_t gps_gprmc_status; + big_uint32_buf_t startup_times; + big_uint32_buf_t total_operation_time; + uint8_t ptp_clock_status; + uint8_t reserved[5]; // FIXME: 4 bytes labeled as humidity in OT128 datasheet friend std::ostream & operator<<(std::ostream & os, nebula::HesaiLidarStatus const & arg) { + std::ios initial_format(nullptr); + initial_format.copyfmt(os); + os << "system_uptime: " << arg.system_uptime; os << ", "; os << "motor_speed: " << arg.motor_speed; os << ", "; os << "temperature: "; - for (long unsigned int i = 0; i < arg.temperature.size() - 1; i++) { - os << arg.temperature[i] << ","; + + for (size_t i = 0; i < sizeof(arg.temperature); i++) { + if (i != 0) { + os << ','; + } + os << arg.temperature[i]; } - os << arg.temperature[arg.temperature.size() - 1]; + os << ", "; - os << "gps_pps_lock: " << arg.gps_pps_lock; + os << "gps_pps_lock: " << static_cast(arg.gps_pps_lock); os << ", "; - os << "gps_gprmc_status: " << arg.gps_gprmc_status; + os << "gps_gprmc_status: " << static_cast(arg.gps_gprmc_status); os << ", "; os << "startup_times: " << arg.startup_times; os << ", "; os << "total_operation_time: " << arg.total_operation_time; os << ", "; - os << "ptp_clock_status: " << arg.ptp_clock_status; + os << "ptp_clock_status: " << static_cast(arg.ptp_clock_status); os << ", "; os << "reserved: "; - for (long unsigned int i = 0; i < arg.reserved.size() - 1; i++) { - os << arg.reserved[i] << ","; + + for (size_t i = 0; i < sizeof(arg.reserved); i++) { + if (i != 0) { + os << ' '; + } + os << std::hex << std::setfill('0') << std::setw(2) << (static_cast(arg.reserved[i])); } - os << arg.reserved[arg.reserved.size() - 1]; + os.copyfmt(initial_format); return os; } - std::string get_str_gps_pps_lock() + [[nodiscard]] std::string get_str_gps_pps_lock() const { switch (gps_pps_lock) { case 1: return "Lock"; - break; case 0: return "Unlock"; - break; default: return "Unknown"; - break; } } - std::string get_str_gps_gprmc_status() + [[nodiscard]] std::string get_str_gps_gprmc_status() const { switch (gps_gprmc_status) { case 1: return "Lock"; - break; case 0: return "Unlock"; - break; default: return "Unknown"; - break; } } - std::string get_str_ptp_clock_status() + [[nodiscard]] std::string get_str_ptp_clock_status() const { switch (ptp_clock_status) { case 0: return "free run"; - break; case 1: return "tracking"; - break; case 2: return "locked"; - break; case 3: return "frozen"; - break; default: return "Unknown"; - break; } } }; @@ -459,17 +442,17 @@ struct HesaiLidarStatus /// @brief struct of PTC_COMMAND_GET_LIDAR_RANGE struct HesaiLidarRangeAll { - int method; - int start; - int end; + uint8_t method; + big_uint16_buf_t start; + big_uint16_buf_t end; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiLidarRangeAll const & arg) { - os << "method: " << arg.method; + os << "method: " << static_cast(arg.method); os << ", "; - os << "start: " << arg.start; + os << "start: " << static_cast(arg.start.value()); os << ", "; - os << "end: " << arg.end; + os << "end: " << static_cast(arg.end.value()); return os; } @@ -478,30 +461,31 @@ struct HesaiLidarRangeAll /// @brief struct of PTC_COMMAND_GET_PTP_CONFIG struct HesaiPtpConfig { - int status; - int profile; - int domain; - int network; - int logAnnounceInterval; - int logSyncInterval; - int logMinDelayReqInterval; + int8_t status; + int8_t profile; + int8_t domain; + int8_t network; + int8_t logAnnounceInterval; + int8_t logSyncInterval; + int8_t logMinDelayReqInterval; + // FIXME: this format is not correct for OT128, or for AT128 on 802.1AS friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpConfig const & arg) { - os << "status: " << arg.status; + os << "status: " << static_cast(arg.status); os << ", "; - os << "profile: " << arg.profile; + os << "profile: " << static_cast(arg.profile); os << ", "; - os << "domain: " << arg.domain; + os << "domain: " << static_cast(arg.domain); os << ", "; - os << "network: " << arg.network; + os << "network: " << static_cast(arg.network); if (arg.status == 0) { os << ", "; - os << "logAnnounceInterval: " << arg.logAnnounceInterval; + os << "logAnnounceInterval: " << static_cast(arg.logAnnounceInterval); os << ", "; - os << "logSyncInterval: " << arg.logSyncInterval; + os << "logSyncInterval: " << static_cast(arg.logSyncInterval); os << ", "; - os << "logMinDelayReqInterval: " << arg.logMinDelayReqInterval; + os << "logMinDelayReqInterval: " << static_cast(arg.logMinDelayReqInterval); } return os; } @@ -510,10 +494,11 @@ struct HesaiPtpConfig /// @brief struct of PTC_COMMAND_LIDAR_MONITOR struct HesaiLidarMonitor { - int input_voltage; - int input_current; - int input_power; - std::vector reserved = std::vector(52); + // FIXME: this format is not correct for OT128 + big_int32_buf_t input_voltage; + big_int32_buf_t input_current; + big_int32_buf_t input_power; + uint8_t reserved[52]; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiLidarMonitor const & arg) { @@ -524,14 +509,19 @@ struct HesaiLidarMonitor os << "input_power: " << arg.input_power; os << ", "; os << "reserved: "; - for (long unsigned int i = 0; i < arg.reserved.size() - 1; i++) { - os << arg.reserved[i] << ","; + + for (size_t i = 0; i < sizeof(arg.reserved); i++) { + if (i != 0) { + os << ' '; + } + os << std::hex << std::setfill('0') << std::setw(2) << (static_cast(arg.reserved[i])); } - os << arg.reserved[arg.reserved.size() - 1]; return os; } }; +#pragma pack(pop) + } // namespace nebula #endif // HESAI_CMD_RESPONSE_HPP diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 2f5639373..471057ea3 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -1,9 +1,25 @@ +// 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_HESAI_HW_INTERFACE_H #define NEBULA_HESAI_HW_INTERFACE_H // 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 + +#include #if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 #define BOOST_BIND_GLOBAL_PLACEHOLDERS #endif @@ -15,20 +31,19 @@ #include "boost_udp_driver/udp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/hesai/hesai_status.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" +#include "nebula_common/util/expected.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" #include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" - #include #include #include #include #include +#include +#include namespace nebula { @@ -63,6 +78,20 @@ const uint8_t PTC_COMMAND_RESET = 0x25; const uint8_t PTC_COMMAND_SET_ROTATE_DIRECTION = 0x2a; const uint8_t PTC_COMMAND_LIDAR_MONITOR = 0x27; +const uint8_t PTC_ERROR_CODE_NO_ERROR = 0x00; +const uint8_t PTC_ERROR_CODE_INVALID_INPUT_PARAM = 0x01; +const uint8_t PTC_ERROR_CODE_SERVER_CONN_FAILED = 0x02; +const uint8_t PTC_ERROR_CODE_INVALID_DATA = 0x03; +const uint8_t PTC_ERROR_CODE_OUT_OF_MEMORY = 0x04; +const uint8_t PTC_ERROR_CODE_UNSUPPORTED_CMD = 0x05; +const uint8_t PTC_ERROR_CODE_FPGA_COMM_FAILED = 0x06; +const uint8_t PTC_ERROR_CODE_OTHER = 0x07; + +const uint8_t TCP_ERROR_UNRELATED_RESPONSE = 1; +const uint8_t TCP_ERROR_UNEXPECTED_PAYLOAD = 2; +const uint8_t TCP_ERROR_TIMEOUT = 4; +const uint8_t TCP_ERROR_INCOMPLETE_RESPONSE = 8; + const uint16_t PANDARQT64_PACKET_SIZE = 1072; const uint16_t PANDARQT128_PACKET_SIZE = 1127; const uint16_t PANDARXT32_PACKET_SIZE = 1080; @@ -74,13 +103,18 @@ const uint16_t PANDAR40_PACKET_SIZE = 1262; const uint16_t PANDAR40P_EXTENDED_PACKET_SIZE = 1266; const uint16_t PANDAR128_E4X_PACKET_SIZE = 861; const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117; - const uint16_t MTU_SIZE = 1500; +/// @brief The kernel buffer size in bytes to use for receiving UDP packets. If the buffer is too +/// small to bridge scheduling and processing delays, packets will be dropped. This corresponds to +/// the net.core.rmem_default setting in Linux. The current value is hardcoded to accommodate one +/// pointcloud worth of OT128 packets (currently the highest data rate sensor supported). +const size_t UDP_SOCKET_BUFFER_SIZE = MTU_SIZE * 3600; + // Time interval between Announce messages, in units of log seconds (default: 1) -const int PTP_LOG_ANNOUNCE_INTERVAL = 1; +const int PTP_LOG_ANNOUNCE_INTERVAL = 1; // Time interval between Sync messages, in units of log seconds (default: 1) -const int PTP_SYNC_INTERVAL = 1; +const int PTP_SYNC_INTERVAL = 1; // Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0) const int PTP_LOG_MIN_DELAY_INTERVAL = 0; @@ -88,26 +122,29 @@ const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; /// @brief Hardware interface of hesai driver -class HesaiHwInterface : NebulaHwInterfaceBase +class HesaiHwInterface { private: + struct ptc_error_t + { + uint8_t error_flags = 0; + uint8_t ptc_error_code = 0; + + [[nodiscard]] bool ok() const { return !error_flags && !ptc_error_code; } + }; + + using ptc_cmd_result_t = nebula::util::expected, ptc_error_t>; + std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; std::shared_ptr m_owned_ctx; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_; - std::shared_ptr sensor_configuration_; - std::shared_ptr calibration_configuration_; - size_t azimuth_index_{}; - size_t mtu_size_{}; - std::unique_ptr scan_cloud_ptr_; - std::function - is_valid_packet_; /*Lambda Function Array to verify proper packet size*/ - std::function buffer)> - scan_reception_callback_; /**This function pointer is called when the scan is complete*/ - - int prev_phase_{}; - - bool is_solid_state = false; + std::shared_ptr sensor_configuration_; + std::function & buffer)> + cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ + + std::mutex mtx_inflight_tcp_request_; + int target_model_no; /// @brief Get a one-off HTTP client to communicate with the hardware @@ -141,13 +178,24 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param bytes Target byte vector void PrintDebug(const std::vector & bytes); + /// @brief Convert an error code to a human-readable string + /// @param error_code The error code, containing the sensor's error code (if any), along with + /// flags such as TCP_ERROR_UNRELATED_RESPONSE etc. + /// @return A string description of all errors in this code + std::string PrettyPrintPTCError(ptc_error_t error_code); + + /// @brief Checks if the data size matches that of the struct to be parsed, and parses the struct. + /// If data is too small, a std::runtime_error is thrown. If data is too large, a warning is + /// printed and the struct is parsed with the first sizeof(T) bytes. + template + T CheckSizeAndParse(const std::vector & data); + /// @brief Send a PTC request with an optional payload, and return the full response payload. /// Blocking. /// @param command_id PTC command number. /// @param payload Payload bytes of the PTC command. Not including the 8-byte PTC header. /// @return The returned payload, if successful, or nullptr. - std::shared_ptr> SendReceive( - const uint8_t command_id, const std::vector & payload = {}); + ptc_cmd_result_t SendReceive(const uint8_t command_id, const std::vector & payload = {}); public: /// @brief Constructor @@ -168,17 +216,17 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart() final; + Status SensorInterfaceStart(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop() final; + Status SensorInterfaceStop(); /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + Status GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration); /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status @@ -187,12 +235,11 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; + std::shared_ptr sensor_configuration); /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback( - std::function)> scan_callback); + Status RegisterScanCallback(std::function &)> scan_callback); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status std::string GetLidarCalibrationString(); @@ -284,6 +331,16 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return Resulting status HesaiLidarRangeAll GetLidarRange(); + /** + * @brief Given the HW interface's sensor configuration and a given calibration, set the sensor + * FoV (min and max angles) with appropriate padding around the FoV set in the configuration. This + * compensates for the points lost due to the sensor filtering FoV by raw encoder angle. + * + * @param calibration The calibration file of the sensor + * @return Status Resulting status of setting the FoV + */ + [[nodiscard]] Status checkAndSetLidarRange(const HesaiCalibrationConfigurationBase & calibration); + Status SetClockSource(int clock_source); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG @@ -358,13 +415,13 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param hesai_config Current HesaiConfig /// @return Resulting status HesaiStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, HesaiConfig hesai_config); + std::shared_ptr sensor_configuration, HesaiConfig hesai_config); /// @brief Checking the current settings and changing the difference point /// @param sensor_configuration Current SensorConfiguration /// @param hesai_lidar_range_all Current HesaiLidarRangeAll /// @return Resulting status HesaiStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all); /// @brief Checking the current settings and changing the difference point /// @return Resulting status diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp index 75cb21e65..7ae25bf21 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp @@ -1,3 +1,17 @@ +// 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. + #pragma once // Have to define macros to silence warnings about deprecated headers being used by @@ -11,15 +25,13 @@ #define BOOST_ALLOW_DEPRECATED_HEADERS #endif -#include "boost_udp_driver/udp_driver.hpp" -#include "nebula_common/robosense/robosense_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" - +#include +#include #include -#include "robosense_msgs/msg/robosense_info_packet.hpp" -#include "robosense_msgs/msg/robosense_packet.hpp" -#include "robosense_msgs/msg/robosense_scan.hpp" +#include +#include +#include namespace nebula { @@ -32,27 +44,17 @@ constexpr uint16_t BPEARL_PACKET_SIZE = 1248; constexpr uint16_t BPEARL_INFO_PACKET_SIZE = 1248; /// @brief Hardware interface of Robosense driver -class RobosenseHwInterface : NebulaHwInterfaceBase +class RobosenseHwInterface { private: std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; std::unique_ptr<::drivers::common::IoContext> info_io_context_; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::unique_ptr<::drivers::udp_driver::UdpDriver> info_udp_driver_; - std::shared_ptr sensor_configuration_; - std::unique_ptr scan_cloud_ptr_; - size_t azimuth_index_{44}; // For Helios and Bpearl 42 byte header + 2 byte flag - int prev_phase_{}; - std::atomic is_info_received{false}; // To check if DIFOP is received - std::optional> info_buffer_; // To hold DIFOP data - std::optional sensor_model_; // To hold sensor model - std::function - is_valid_packet_; /*Lambda Function Array to verify proper packet size for data*/ - std::function - is_valid_info_packet_; /*Lambda Function Array to verify proper packet size for info*/ - std::function buffer)> + std::shared_ptr sensor_configuration_; + std::function & buffer)> scan_reception_callback_; /**This function pointer is called when the scan is complete*/ - std::function buffer)> + std::function & buffer)> info_reception_callback_; /**This function pointer is called when DIFOP packet is received*/ std::shared_ptr parent_node_logger_; @@ -70,52 +72,35 @@ class RobosenseHwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(std::vector & buffer); /// @brief Callback function to receive the Info Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveInfoPacketCallback(const std::vector & buffer); + void ReceiveInfoPacketCallback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams for MSOP packets /// @return Resulting status - Status SensorInterfaceStart() final; + Status SensorInterfaceStart(); /// @brief Starting the interface that handles UDP streams for DIFOP packets /// @return Resulting status Status InfoInterfaceStart(); - /// @brief Function for stopping the interface that handles UDP streams - /// @return Resulting status - Status SensorInterfaceStop() final; - /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; - - /// @brief Printing sensor configuration - /// @param sensor_configuration SensorConfiguration for this interface - /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; - - /// @brief Printing calibration configuration - /// @param calibration_configuration CalibrationConfiguration for the checking - /// @return Resulting status - Status GetCalibrationConfiguration( - CalibrationConfigurationBase & calibration_configuration) override; + std::shared_ptr sensor_configuration); /// @brief Registering callback for RobosenseScan /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback( - std::function)> scan_callback); + Status RegisterScanCallback(std::function &)> scan_callback); /// @brief Registering callback for RobosensePacket /// @param scan_callback Callback function /// @return Resulting status - Status RegisterInfoCallback( - std::function)> info_callback); + Status RegisterInfoCallback(std::function &)> info_callback); /// @brief Setting rclcpp::Logger /// @param node Logger diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp index fcb907a15..3911b37d3 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp @@ -1,3 +1,17 @@ +// 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_VELODYNE_HW_INTERFACE_H #define NEBULA_VELODYNE_HW_INTERFACE_H @@ -11,51 +25,41 @@ #if (BOOST_VERSION / 100 == 1074) // Boost 1.74 #define BOOST_ALLOW_DEPRECATED_HEADERS #endif -#include "boost_tcp_driver/http_client_driver.hpp" -#include "boost_udp_driver/udp_driver.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_common/velodyne/velodyne_status.hpp" + #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" +#include +#include +#include +#include #include -#include -#include - #include #include #include +#include +#include namespace nebula { namespace drivers { /// @brief Hardware interface of velodyne driver -class VelodyneHwInterface : NebulaHwInterfaceBase +class VelodyneHwInterface { private: std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; - std::shared_ptr sensor_configuration_; - std::shared_ptr calibration_configuration_; - std::unique_ptr scan_cloud_ptr_; - std::function - is_valid_packet_; /*Lambda Function Array to verify proper packet size*/ - std::function buffer)> - scan_reception_callback_; /**This function pointer is called when the scan is complete*/ - - uint16_t packet_first_azm_ = 0; - uint16_t packet_first_azm_phased_ = 0; - uint16_t packet_last_azm_ = 0; - uint16_t packet_last_azm_phased_ = 0; - uint16_t prev_packet_first_azm_phased_ = 0; - uint16_t phase_ = 0; - uint processed_packets_ = 0; + std::shared_ptr sensor_configuration_; + std::function &)> + cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ std::shared_ptr boost_ctx_; std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> http_client_driver_; + std::mutex mtx_inflight_request_; + std::string TARGET_STATUS{"/cgi/status.json"}; std::string TARGET_DIAG{"/cgi/diag.json"}; std::string TARGET_SNAPSHOT{"/cgi/snapshot.hdl"}; @@ -67,6 +71,9 @@ class VelodyneHwInterface : NebulaHwInterfaceBase std::string TARGET_RESET{"/cgi/reset"}; void StringCallback(const std::string & str); + std::string HttpGetRequest(const std::string & endpoint); + std::string HttpPostRequest(const std::string & endpoint, const std::string & body); + /// @brief Get a one-off HTTP client to communicate with the hardware /// @param ctx IO Context /// @param hcd Got http client driver @@ -86,7 +93,7 @@ class VelodyneHwInterface : NebulaHwInterfaceBase /// @param tree Current settings (property_tree) /// @return Resulting status VelodyneStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, boost::property_tree::ptree tree); std::shared_ptr parent_node_logger; @@ -106,38 +113,36 @@ class VelodyneHwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart() final; + Status SensorInterfaceStart(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop() final; + Status SensorInterfaceStop(); /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration); /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status - Status GetCalibrationConfiguration( - CalibrationConfigurationBase & calibration_configuration) final; + Status GetCalibrationConfiguration(CalibrationConfigurationBase & calibration_configuration); /// @brief Initializing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status InitializeSensorConfiguration( - std::shared_ptr sensor_configuration); + std::shared_ptr sensor_configuration); /// @brief Setting sensor configuration with InitializeSensorConfiguration & /// CheckAndSetConfigBySnapshotAsync /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; + std::shared_ptr sensor_configuration); /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback( - std::function)> scan_callback); + Status RegisterScanCallback(std::function & packet)> scan_callback); /// @brief Parsing JSON string to property_tree /// @param str JSON string @@ -217,95 +222,6 @@ class VelodyneHwInterface : NebulaHwInterfaceBase /// @return Resulting status VelodyneStatus SetNetDhcp(bool use_dhcp); - /// @brief Initializing HTTP client (async) - /// @return Resulting status - VelodyneStatus InitHttpClientAsync(); - /// @brief Getting the current operational state and parameters of the sensor (async) - /// @param str_callback Callback function for received JSON string - /// @return Resulting status - VelodyneStatus GetStatusAsync(std::function str_callback); - /// @brief Getting the current operational state and parameters of the sensor (async) - /// @return Resulting status - VelodyneStatus GetStatusAsync(); - /// @brief Getting diagnostic information from the sensor (async) - /// @param str_callback Callback function for received JSON string - /// @return Resulting status - VelodyneStatus GetDiagAsync(std::function str_callback); - /// @brief Getting diagnostic information from the sensor (async) - /// @return Resulting status - VelodyneStatus GetDiagAsync(); - /// @brief Getting current sensor configuration and status data (async) - /// @param str_callback Callback function for received JSON string - /// @return Resulting status - VelodyneStatus GetSnapshotAsync(std::function str_callback); - /// @brief Getting current sensor configuration and status data (async) - /// @return Resulting status - VelodyneStatus GetSnapshotAsync(); - /// @brief Checking the current settings and changing the difference point - /// @return Resulting status - VelodyneStatus CheckAndSetConfigBySnapshotAsync( - std::shared_ptr sensor_configuration); - /// @brief Setting Motor RPM (async) - /// @param rpm the RPM of the motor - /// @return Resulting status - VelodyneStatus SetRpmAsync(uint16_t rpm); - /// @brief Setting Field of View Start (async) - /// @param fov_start FOV start - /// @return Resulting status - VelodyneStatus SetFovStartAsync(uint16_t fov_start); - /// @brief Setting Field of View End (async) - /// @param fov_end FOV end - /// @return Resulting status - VelodyneStatus SetFovEndAsync(uint16_t fov_end); - /// @brief Setting Return Type (async) - /// @param return_mode ReturnMode - /// @return Resulting status - VelodyneStatus SetReturnTypeAsync(ReturnMode return_mode); - /// @brief Save Configuration to the LiDAR memory (async) - /// @return Resulting status - VelodyneStatus SaveConfigAsync(); - /// @brief Resets the sensor (async) - /// @return Resulting status - VelodyneStatus ResetSystemAsync(); - /// @brief Turn laser state on (async) - /// @return Resulting status - VelodyneStatus LaserOnAsync(); - /// @brief Turn laser state off (async) - /// @return Resulting status - VelodyneStatus LaserOffAsync(); - /// @brief Turn laser state on/off (async) - /// @param on is ON - /// @return Resulting status - VelodyneStatus LaserOnOffAsync(bool on); - /// @brief Setting host (destination) IP address (async) - /// @param addr destination IP address - /// @return Resulting status - VelodyneStatus SetHostAddrAsync(std::string addr); - /// @brief Setting host (destination) data port (async) - /// @param dport destination data port - /// @return Resulting status - VelodyneStatus SetHostDportAsync(uint16_t dport); - /// @brief Setting host (destination) telemetry port (async) - /// @param tport destination telemetry port - /// @return Resulting status - VelodyneStatus SetHostTportAsync(uint16_t tport); - /// @brief Setting network (sensor) IP address (async) - /// @param addr sensor IP address - /// @return Resulting status - VelodyneStatus SetNetAddrAsync(std::string addr); - /// @brief Setting the network mask of the sensor (async) - /// @param mask Network mask - /// @return Resulting status - VelodyneStatus SetNetMaskAsync(std::string mask); - /// @brief Setting the gateway address of the sensor (async) - /// @param gateway Gateway address - /// @return Resulting status - VelodyneStatus SetNetGatewayAsync(std::string gateway); - /// @brief This determines if the sensor is to rely on a DHCP server for its IP address (async) - /// @param use_dhcp DHCP on - /// @return Resulting status - VelodyneStatus SetNetDhcpAsync(bool use_dhcp); - /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr node); diff --git a/nebula_hw_interfaces/package.xml b/nebula_hw_interfaces/package.xml index b526108c9..f77e88765 100644 --- a/nebula_hw_interfaces/package.xml +++ b/nebula_hw_interfaces/package.xml @@ -2,7 +2,7 @@ nebula_hw_interfaces - 0.1.0 + 0.2.0 Nebula HW Interfaces MAP IV @@ -13,13 +13,11 @@ boost_tcp_driver boost_udp_driver - libpcl-all-dev nebula_common nebula_msgs pandar_msgs - rclcpp robosense_msgs - sensor_msgs + ros2_socketcan velodyne_msgs ament_cmake_gtest diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index e9a4b5031..cd1053731 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -13,11 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp" + #include -#include -#include -#include namespace nebula { namespace drivers @@ -25,150 +24,80 @@ namespace drivers namespace continental_ars548 { ContinentalARS548HwInterface::ContinentalARS548HwInterface() -: sensor_io_context_{new ::drivers::common::IoContext(1)}, - sensor_udp_driver_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_)}, - nebula_packets_ptr_{std::make_unique()} +: sensor_io_context_ptr_{new ::drivers::common::IoContext(1)}, + sensor_udp_driver_ptr_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_ptr_)} { } Status ContinentalARS548HwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr new_config_ptr) { - Status status = Status::OK; - - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - } catch (const std::exception & ex) { - status = Status::SENSOR_CONFIG_ERROR; - std::cerr << status << std::endl; - return status; - } - + config_ptr_ = new_config_ptr; return Status::OK; } Status ContinentalARS548HwInterface::SensorInterfaceStart() { try { - sensor_udp_driver_->init_receiver( - sensor_configuration_->multicast_ip, sensor_configuration_->data_port, - sensor_configuration_->host_ip, sensor_configuration_->data_port, 2 << 16); - sensor_udp_driver_->receiver()->setMulticast(true); - sensor_udp_driver_->receiver()->open(); - sensor_udp_driver_->receiver()->bind(); - sensor_udp_driver_->receiver()->asyncReceiveWithSender(std::bind( + sensor_udp_driver_ptr_->init_receiver( + config_ptr_->multicast_ip, config_ptr_->data_port, config_ptr_->host_ip, + config_ptr_->data_port, 2 << 16); + sensor_udp_driver_ptr_->receiver()->setMulticast(true); + sensor_udp_driver_ptr_->receiver()->open(); + sensor_udp_driver_ptr_->receiver()->bind(); + sensor_udp_driver_ptr_->receiver()->asyncReceiveWithSender(std::bind( &ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender, this, std::placeholders::_1, std::placeholders::_2)); - sensor_udp_driver_->init_sender( - sensor_configuration_->sensor_ip, sensor_configuration_->configuration_sensor_port, - sensor_configuration_->host_ip, sensor_configuration_->configuration_host_port); + sensor_udp_driver_ptr_->init_sender( + config_ptr_->sensor_ip, config_ptr_->configuration_sensor_port, config_ptr_->host_ip, + config_ptr_->configuration_host_port); - sensor_udp_driver_->sender()->open(); - sensor_udp_driver_->sender()->bind(); + sensor_udp_driver_ptr_->sender()->open(); + sensor_udp_driver_ptr_->sender()->bind(); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," - << sensor_configuration_->data_port << std::endl; + std::cerr << status << config_ptr_->sensor_ip << "," << config_ptr_->data_port << std::endl; return status; } return Status::OK; } -Status ContinentalARS548HwInterface::RegisterScanCallback( - std::function)> callback) +Status ContinentalARS548HwInterface::RegisterPacketCallback( + std::function)> packet_callback) { - nebula_packets_reception_callback_ = std::move(callback); + packet_callback_ = std::move(packet_callback); return Status::OK; } void ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender( - const std::vector & buffer, const std::string & sender_ip) + std::vector & buffer, const std::string & sender_ip) { - if (sender_ip == sensor_configuration_->sensor_ip) { + if (sender_ip == config_ptr_->sensor_ip) { ReceiveSensorPacketCallback(buffer); } } -void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { if (buffer.size() < sizeof(HeaderPacket)) { PrintError("Unrecognized packet. Too short"); return; } - HeaderPacket header_packet{}; - std::memcpy(&header_packet, buffer.data(), sizeof(HeaderPacket)); - - if (header_packet.service_id.value() != 0) { - PrintError("Invalid service id"); - return; - } else if (header_packet.method_id.value() == SENSOR_STATUS_METHOD_ID) { - if ( - buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || - header_packet.length.value() != SENSOR_STATUS_PDU_LENGTH) { - PrintError("SensorStatus message with invalid size"); - return; - } - ProcessDataPacket(buffer); - } else if (header_packet.method_id.value() == FILTER_STATUS_METHOD_ID) { - if ( - buffer.size() != FILTER_STATUS_UDP_PAYLOAD || - header_packet.length.value() != FILTER_STATUS_PDU_LENGTH) { - PrintError("FilterStatus message with invalid size"); - return; - } - - ProcessFilterStatusPacket(buffer); - } else if (header_packet.method_id.value() == DETECTION_LIST_METHOD_ID) { - if ( - buffer.size() != DETECTION_LIST_UDP_PAYLOAD || - header_packet.length.value() != DETECTION_LIST_PDU_LENGTH) { - PrintError("DetectionList message with invalid size"); - return; - } - - ProcessDataPacket(buffer); - } else if (header_packet.method_id.value() == OBJECT_LIST_METHOD_ID) { - if ( - buffer.size() != OBJECT_LIST_UDP_PAYLOAD || - header_packet.length.value() != OBJECT_LIST_PDU_LENGTH) { - PrintError("ObjectList message with invalid size"); - return; - } - - ProcessDataPacket(buffer); - } -} - -void ContinentalARS548HwInterface::ProcessFilterStatusPacket(const std::vector & buffer) -{ - assert(buffer.size() == sizeof(FilterStatusPacket)); - std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); -} - -void ContinentalARS548HwInterface::ProcessDataPacket(const std::vector & buffer) -{ - nebula_msgs::msg::NebulaPacket nebula_packet; - nebula_packet.data = buffer; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = + const auto now = std::chrono::high_resolution_clock::now(); + const auto timestamp_ns = std::chrono::duration_cast(now.time_since_epoch()).count(); - nebula_packet.stamp.sec = static_cast(now_secs); - nebula_packet.stamp.nanosec = - static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); - nebula_packets_ptr_->packets.emplace_back(nebula_packet); - nebula_packets_ptr_->header.stamp = nebula_packets_ptr_->packets.front().stamp; - nebula_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(buffer); - nebula_packets_reception_callback_(std::move(nebula_packets_ptr_)); - nebula_packets_ptr_ = std::make_unique(); + packet_callback_(std::move(msg_ptr)); } Status ContinentalARS548HwInterface::SensorInterfaceStop() @@ -176,15 +105,6 @@ Status ContinentalARS548HwInterface::SensorInterfaceStop() return Status::ERROR_1; } -Status ContinentalARS548HwInterface::GetSensorConfiguration( - SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - PrintDebug(ss.str()); - return Status::ERROR_1; -} - Status ContinentalARS548HwInterface::SetSensorMounting( float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float pitch_autosar, uint8_t plug_orientation) @@ -220,11 +140,11 @@ Status ContinentalARS548HwInterface::SetSensorMounting( PrintInfo("pitch_autosar = " + std::to_string(pitch_autosar)); PrintInfo("plug_orientation = " + std::to_string(plug_orientation)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -259,11 +179,11 @@ Status ContinentalARS548HwInterface::SetVehicleParameters( PrintInfo("height_autosar = " + std::to_string(height_autosar)); PrintInfo("wheel_base_autosar = " + std::to_string(wheel_base_autosar)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -303,11 +223,11 @@ Status ContinentalARS548HwInterface::SetRadarParameters( PrintInfo("hcc = " + std::to_string(hcc)); PrintInfo("power_save_standstill = " + std::to_string(power_save_standstill)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -344,11 +264,11 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens std::vector send_vector(sizeof(ConfigurationPacket)); std::memcpy(send_vector.data(), &configuration, sizeof(ConfigurationPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -375,11 +295,11 @@ Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acc std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); std::memcpy(send_vector.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoGPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -408,11 +328,11 @@ Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longit std::memcpy( send_vector.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoGPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -439,11 +359,11 @@ Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic std::vector send_vector(sizeof(CharacteristicSpeedPacket)); std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharacteristicSpeedPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -475,11 +395,11 @@ Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) std::vector send_vector(sizeof(DrivingDirectionPacket)); std::memcpy(send_vector.data(), &driving_direction_packet, sizeof(DrivingDirectionPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -507,11 +427,11 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) std::memcpy( send_vector.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxlePacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -538,11 +458,11 @@ Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) std::vector send_vector(sizeof(VelocityVehiclePacket)); std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -569,32 +489,24 @@ Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) std::vector send_vector(sizeof(YawRatePacket)); std::memcpy(send_vector.data(), &yaw_rate_packet, sizeof(YawRatePacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } -Status ContinentalARS548HwInterface::CheckAndSetConfig() -{ - RCLCPP_ERROR( - *parent_node_logger, - "This functionality is not yet implemented. Sensor is probably out of sync with config now."); - return Status::ERROR_1; -} - void ContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) { - parent_node_logger = logger; + parent_node_logger_ptr_ = logger; } void ContinentalARS548HwInterface::PrintInfo(std::string info) { - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); + if (parent_node_logger_ptr_) { + RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); } else { std::cout << info << std::endl; } @@ -602,8 +514,8 @@ void ContinentalARS548HwInterface::PrintInfo(std::string info) void ContinentalARS548HwInterface::PrintError(std::string error) { - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); + if (parent_node_logger_ptr_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); } else { std::cerr << error << std::endl; } @@ -611,23 +523,13 @@ void ContinentalARS548HwInterface::PrintError(std::string error) void ContinentalARS548HwInterface::PrintDebug(std::string debug) { - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + if (parent_node_logger_ptr_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); } else { std::cout << debug << std::endl; } } -void ContinentalARS548HwInterface::PrintDebug(const std::vector & bytes) -{ - std::stringstream ss; - for (const auto & b : bytes) { - ss << static_cast(b) << ", "; - } - ss << std::endl; - PrintDebug(ss.str()); -} - } // namespace continental_ars548 } // namespace drivers } // namespace nebula diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp new file mode 100644 index 000000000..bf7985bf0 --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp @@ -0,0 +1,405 @@ +// 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_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp" + +#include +#include + +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +ContinentalSRR520HwInterface::ContinentalSRR520HwInterface() +{ +} + +Status ContinentalSRR520HwInterface::SetSensorConfiguration( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> + new_config_ptr) +{ + config_ptr_ = new_config_ptr; + + return Status::OK; +} + +Status ContinentalSRR520HwInterface::SensorInterfaceStart() +{ + std::lock_guard lock(receiver_mutex_); + + try { + can_sender_ptr_ = + std::make_unique<::drivers::socketcan::SocketCanSender>(config_ptr_->interface, true); + can_receiver_ptr_ = + std::make_unique<::drivers::socketcan::SocketCanReceiver>(config_ptr_->interface, true); + + can_receiver_ptr_->SetCanFilters( + ::drivers::socketcan::SocketCanReceiver::CanFilterList(config_ptr_->filters)); + PrintInfo(std::string("applied filters: ") + config_ptr_->filters); + + sensor_interface_active_ = true; + receiver_thread_ptr_ = + std::make_unique(&ContinentalSRR520HwInterface::ReceiveLoop, this); + } catch (const std::exception & ex) { + Status status = Status::CAN_CONNECTION_ERROR; + std::cerr << status << config_ptr_->interface << std::endl; + return status; + } + return Status::OK; +} + +template +bool ContinentalSRR520HwInterface::SendFrame(const std::array & data, int can_frame_id) +{ + ::drivers::socketcan::CanId send_id( + can_frame_id, 0, ::drivers::socketcan::FrameType::DATA, ::drivers::socketcan::StandardFrame); + + try { + can_sender_ptr_->send_fd( + data.data(), data.size(), send_id, + std::chrono::duration_cast( + std::chrono::duration(config_ptr_->sender_timeout_sec))); + return true; + } catch (const std::exception & ex) { + PrintError(std::string("Error sending CAN message: ") + ex.what()); + return false; + } +} + +void ContinentalSRR520HwInterface::ReceiveLoop() +{ + ::drivers::socketcan::CanId receive_id{}; + std::chrono::nanoseconds receiver_timeout_nsec; + bool use_bus_time; + + while (true) { + auto packet_msg_ptr = std::make_unique(); + + { + std::lock_guard lock(receiver_mutex_); + receiver_timeout_nsec = std::chrono::duration_cast( + std::chrono::duration(config_ptr_->receiver_timeout_sec)); + use_bus_time = config_ptr_->use_bus_time; + + if (!sensor_interface_active_) { + break; + } + } + + try { + packet_msg_ptr->data.resize(68); // 64 bytes of data + 4 bytes of ID + receive_id = can_receiver_ptr_->receive_fd( + packet_msg_ptr->data.data() + 4 * sizeof(uint8_t), receiver_timeout_nsec); + } catch (const std::exception & ex) { + PrintError(std::string("Error receiving CAN FD message: ") + ex.what()); + continue; + } + + packet_msg_ptr->data.resize(receive_id.length() + 4); + + uint32_t id = receive_id.identifier(); + packet_msg_ptr->data[0] = (id & 0xFF000000) >> 24; + packet_msg_ptr->data[1] = (id & 0x00FF0000) >> 16; + packet_msg_ptr->data[2] = (id & 0x0000FF00) >> 8; + packet_msg_ptr->data[3] = (id & 0x000000FF) >> 0; + + int64_t stamp = use_bus_time + ? static_cast(receive_id.get_bus_time() * 1000U) + : static_cast(std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count()); + + packet_msg_ptr->stamp.sec = stamp / 1'000'000'000; + packet_msg_ptr->stamp.nanosec = stamp % 1'000'000'000; + + if (receive_id.frame_type() == ::drivers::socketcan::FrameType::ERROR) { + PrintError("CAN FD message is an error frame"); + continue; + } + + nebula_packet_callback_(std::move(packet_msg_ptr)); + } +} + +Status ContinentalSRR520HwInterface::RegisterPacketCallback( + std::function)> callback) +{ + nebula_packet_callback_ = std::move(callback); + return Status::OK; +} + +void ContinentalSRR520HwInterface::SensorSyncFollowUp(builtin_interfaces::msg::Time stamp) +{ + if (!can_sender_ptr_) { + PrintError("Can sender is invalid so can not do follow up"); + } + + if (!config_ptr_->sync_use_bus_time || sync_follow_up_sent_) { + return; + } + + auto t0s = last_sync_stamp_; + t0s.nanosec = 0; + const auto & t1r = stamp; + + builtin_interfaces::msg::Time t4r = + rclcpp::Time(rclcpp::Time() + (rclcpp::Time(t1r) - rclcpp::Time(t0s))); + uint8_t t4r_seconds = static_cast(t4r.sec); + uint32_t t4r_nanoseconds = t4r.nanosec; + std::array data; + data[0] = 0x28; // mode 0x18 is without CRC + data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + (sync_counter_ & 0x0F); // Domain and counter + data[3] = t4r_seconds & 0x3; // SGW and OVS + data[4] = (t4r_nanoseconds & 0xFF000000) >> 24; + data[5] = (t4r_nanoseconds & 0x00FF0000) >> 16; + data[6] = (t4r_nanoseconds & 0x0000FF00) >> 8; + data[7] = (t4r_nanoseconds & 0x000000FF) >> 0; + + std::array follow_up_crc_array{data[2], data[3], data[4], data[5], + data[6], data[7], 0x00}; + uint8_t follow_up_crc = crc8h2f(follow_up_crc_array.begin(), follow_up_crc_array.end()); + data[1] = follow_up_crc; + + SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); + + sync_follow_up_sent_ = true; + sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; +} + +void ContinentalSRR520HwInterface::SensorSync() +{ + if (!can_sender_ptr_) { + PrintError("Can sender is invalid so can not do sync up"); + return; + } + + if (!sync_follow_up_sent_) { + PrintError("We will send a SYNC message without having sent a FollowUp message first!"); + } + + auto now = std::chrono::system_clock::now(); + auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); + auto now_nanosecs = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + builtin_interfaces::msg::Time stamp; + stamp.sec = static_cast(now_secs); + stamp.nanosec = static_cast(now_nanosecs % 1'000'000'000); + last_sync_stamp_ = stamp; + + std::array data; + data[0] = 0x20; // mode 0x10 is without CRC + data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + (sync_counter_ & 0x0F); // Domain and counter + data[3] = 0; // use data + data[4] = (stamp.sec & 0xFF000000) >> 24; + data[5] = (stamp.sec & 0x00FF0000) >> 16; + data[6] = (stamp.sec & 0x0000FF00) >> 8; + data[7] = (stamp.sec & 0x000000FF) >> 0; + + std::array sync_crc_array{data[2], data[3], data[4], data[5], data[6], data[7], 0x00}; + uint8_t sync_crc = crc8h2f(sync_crc_array.begin(), sync_crc_array.end()); + data[1] = sync_crc; + + SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); + + if (config_ptr_->sync_use_bus_time) { + sync_follow_up_sent_ = false; + return; + } + + data[0] = 0x28; // mode 0x18 is without CRC + data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + (sync_counter_ & 0x0F); // Domain and counter + data[3] = 0; // SGW and OVS + data[4] = (stamp.nanosec & 0xFF000000) >> 24; + data[5] = (stamp.nanosec & 0x00FF0000) >> 16; + data[6] = (stamp.nanosec & 0x0000FF00) >> 8; + data[7] = (stamp.nanosec & 0x000000FF) >> 0; + + std::array follow_up_crc_array{data[2], data[3], data[4], data[5], + data[6], data[7], 0x00}; + uint8_t follow_up_crc = crc8h2f(follow_up_crc_array.begin(), follow_up_crc_array.end()); + data[1] = follow_up_crc; + + SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); + + sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; + sync_follow_up_sent_ = true; +} + +Status ContinentalSRR520HwInterface::SensorInterfaceStop() +{ + { + std::lock_guard l(receiver_mutex_); + sensor_interface_active_ = false; + } + + receiver_thread_ptr_->join(); + return Status::ERROR_1; +} + +Status ContinentalSRR520HwInterface::ConfigureSensor( + uint8_t sensor_id, float longitudinal_autosar, float lateral_autosar, float vertical_autosar, + float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, bool plug_bottom, + bool reset) +{ + PrintInfo("longitudinal_autosar=" + std::to_string(longitudinal_autosar)); + PrintInfo("lateral_autosar=" + std::to_string(lateral_autosar)); + PrintInfo("vertical_autosar=" + std::to_string(vertical_autosar)); + PrintInfo("longitudinal_cog=" + std::to_string(longitudinal_cog)); + PrintInfo("wheelbase=" + std::to_string(wheelbase)); + PrintInfo("yaw_autosar=" + std::to_string(yaw_autosar)); + PrintInfo("sensor_id=" + std::to_string(static_cast(sensor_id))); + PrintInfo("plug_bottom=" + std::to_string(plug_bottom)); + + if ( + longitudinal_autosar < -32.767f || longitudinal_autosar > 32.767f || + lateral_autosar < -32.767f || lateral_autosar > 32.767f || vertical_autosar < -32.767f || + vertical_autosar > 32.767f || longitudinal_cog < -32.767f || longitudinal_cog > 32.767f || + wheelbase < 0.f || wheelbase > 65.534f || yaw_autosar < -3.14159f || yaw_autosar > 3.14159f || + cover_damping < -32.767f || cover_damping > 32.767f) { + PrintError("Sensor configuration values out of range!"); + return Status::SENSOR_CONFIG_ERROR; + } + + const uint16_t u_long_pos = static_cast((longitudinal_autosar + 32.767f) / 0.001f); + const uint16_t u_lat_pos = static_cast((lateral_autosar + 32.767f) / 0.001f); + const uint16_t u_vert_pos = static_cast((vertical_autosar + 32.767f) / 0.001f); + const uint16_t u_long_pos_cog = static_cast((longitudinal_cog + 32.767f) / 0.001f); + const uint16_t u_wheelbase = static_cast(wheelbase / 0.001f); + const uint16_t u_yaw_angle = static_cast((yaw_autosar + 3.14159f) / 9.5877e-05); + const uint16_t u_cover_damping = static_cast((cover_damping + 32.767f) / 0.001f); + + std::array data; + data[0] = sensor_id; + data[1] = static_cast((u_long_pos & 0xff00) >> 8); + data[2] = static_cast((u_long_pos & 0x00ff)); + + data[3] = static_cast((u_lat_pos & 0xff00) >> 8); + data[4] = static_cast((u_lat_pos & 0x00ff)); + + data[5] = static_cast((u_vert_pos & 0xff00) >> 8); + data[6] = static_cast((u_vert_pos & 0x00ff)); + + data[7] = static_cast((u_long_pos_cog & 0xff00) >> 8); + data[8] = static_cast((u_long_pos_cog & 0x00ff)); + + data[9] = static_cast((u_wheelbase & 0xff00) >> 8); + data[10] = static_cast((u_wheelbase & 0x00ff)); + + data[11] = static_cast((u_yaw_angle & 0xff00) >> 8); + data[12] = static_cast((u_yaw_angle & 0x00ff)); + + data[13] = static_cast((u_cover_damping & 0xff00) >> 8); + data[14] = static_cast((u_cover_damping & 0x00ff)); + + uint8_t plug_value = plug_bottom ? 0x00 : 0x01; + uint8_t reset_value = reset ? 0x80 : 0x00; + data[15] = plug_value | reset_value; + + if (SendFrame(data, SENSOR_CONFIG_CAN_MESSAGE_ID)) { + return Status::OK; + } else { + return Status::CAN_CONNECTION_ERROR; + } +} + +Status ContinentalSRR520HwInterface::SetVehicleDynamics( + float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, + float longitudinal_velocity, bool standstill) +{ + if ( + longitudinal_acceleration < -12.7 || longitudinal_acceleration > 12.7 || + lateral_acceleration < -12.7 || lateral_acceleration > 12.7 || yaw_rate < -3.14159 || + yaw_rate > 3.14159 || abs(longitudinal_velocity) > 100.0) { + PrintError("Vehicle dynamics out of range!"); + return Status::SENSOR_CONFIG_ERROR; + } + + const uint8_t u_long_accel = static_cast((longitudinal_acceleration + 12.7) / 0.1); + const uint8_t u_lat_accel = static_cast((lateral_acceleration + 12.7) / 0.1); + const uint16_t u_yaw_rate = static_cast((yaw_rate + 3.14159) / 0.001534729); + const uint16_t u_long_vel = static_cast(std::abs(longitudinal_velocity) / 0.024425989); + uint8_t u_long_dir; + + if (standstill) { + u_long_dir = 0; + } else if (longitudinal_velocity > 0) { + u_long_dir = 1; + } else { + u_long_dir = 2; + } + + std::array data; + data[0] = u_long_accel; + data[1] = u_lat_accel; + data[2] = static_cast((u_yaw_rate & 0xff0) >> 4); + data[3] = (static_cast(u_yaw_rate & 0x0f) << 4) | + static_cast((u_long_vel & 0xf00) >> 8); + data[4] = static_cast(u_long_vel & 0xff); + data[5] = u_long_dir; + data[6] = 0x00; + data[7] = 0x00; + + if (SendFrame(data, VEH_DYN_CAN_MESSAGE_ID)) { + return Status::OK; + } else { + return Status::CAN_CONNECTION_ERROR; + } +} + +void ContinentalSRR520HwInterface::SetLogger(std::shared_ptr logger) +{ + parent_node_logger_ptr_ = logger; +} + +void ContinentalSRR520HwInterface::PrintInfo(std::string info) +{ + if (parent_node_logger_ptr_) { + RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); + } else { + std::cout << info << std::endl; + } +} + +void ContinentalSRR520HwInterface::PrintError(std::string error) +{ + if (parent_node_logger_ptr_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); + } else { + std::cerr << error << std::endl; + } +} + +void ContinentalSRR520HwInterface::PrintDebug(std::string debug) +{ + if (parent_node_logger_ptr_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); + } else { + std::cout << debug << std::endl; + } +} + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp deleted file mode 100644 index 8120a871b..000000000 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp +++ /dev/null @@ -1,461 +0,0 @@ -// 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 -#include - -#include -#include -namespace nebula -{ -namespace drivers -{ -namespace continental_ars548 -{ -MultiContinentalARS548HwInterface::MultiContinentalARS548HwInterface() -: sensor_io_context_{new ::drivers::common::IoContext(1)}, - nebula_packets_ptr_{std::make_unique()} -{ -} - -Status MultiContinentalARS548HwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) -{ - Status status = Status::OK; - - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - } catch (const std::exception & ex) { - status = Status::SENSOR_CONFIG_ERROR; - std::cerr << status << std::endl; - return status; - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SensorInterfaceStart() -{ - for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_->sensor_ips.size(); - sensor_id++) { - auto udp_driver = std::make_unique<::drivers::udp_driver::UdpDriver>(*sensor_io_context_); - sensor_ip_to_frame_[sensor_configuration_->sensor_ips[sensor_id]] = - sensor_configuration_->frame_ids[sensor_id]; - - try { - if (sensor_id == 0) { - udp_driver->init_receiver( - sensor_configuration_->multicast_ip, sensor_configuration_->data_port, - sensor_configuration_->host_ip, sensor_configuration_->data_port, 2 << 16); - udp_driver->receiver()->setMulticast(true); - udp_driver->receiver()->open(); - udp_driver->receiver()->bind(); - udp_driver->receiver()->asyncReceiveWithSender(std::bind( - &MultiContinentalARS548HwInterface::ReceiveSensorPacketCallback, this, - std::placeholders::_1, std::placeholders::_2)); - } - - udp_driver->init_sender( - sensor_configuration_->sensor_ips[sensor_id], - sensor_configuration_->configuration_sensor_port, sensor_configuration_->host_ip, - sensor_configuration_->configuration_host_port); - - udp_driver->sender()->open(); - udp_driver->sender()->bind(); - - if (!udp_driver->sender()->isOpen()) { - return Status::ERROR_1; - } - } catch (const std::exception & ex) { - Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," - << sensor_configuration_->data_port << std::endl; - return status; - } - - sensor_udp_drivers_.emplace_back(std::move(udp_driver)); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::RegisterScanCallback( - std::function, const std::string &)> - callback) -{ - nebula_packets_reception_callback_ = std::move(callback); - return Status::OK; -} - -void MultiContinentalARS548HwInterface::ReceiveSensorPacketCallback( - const std::vector & buffer, const std::string & sender_ip) -{ - if (buffer.size() < sizeof(HeaderPacket)) { - PrintError("Unrecognized packet. Too short"); - return; - } - - HeaderPacket header_packet{}; - std::memcpy(&header_packet, buffer.data(), sizeof(HeaderPacket)); - - if (header_packet.service_id.value() != 0) { - PrintError("Invalid service id"); - return; - } else if (header_packet.method_id.value() == SENSOR_STATUS_METHOD_ID) { - if ( - buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || - header_packet.length.value() != SENSOR_STATUS_PDU_LENGTH) { - PrintError("SensorStatus message with invalid size"); - return; - } - ProcessDataPacket(buffer, sender_ip); - } else if (header_packet.method_id.value() == FILTER_STATUS_METHOD_ID) { - if ( - buffer.size() != FILTER_STATUS_UDP_PAYLOAD || - header_packet.length.value() != FILTER_STATUS_PDU_LENGTH) { - PrintError("FilterStatus message with invalid size"); - return; - } - - ProcessFilterStatusPacket(buffer); - } else if (header_packet.method_id.value() == DETECTION_LIST_METHOD_ID) { - if ( - buffer.size() != DETECTION_LIST_UDP_PAYLOAD || - header_packet.length.value() != DETECTION_LIST_PDU_LENGTH) { - PrintError("DetectionList message with invalid size"); - return; - } - - ProcessDataPacket(buffer, sender_ip); - } else if (header_packet.method_id.value() == OBJECT_LIST_METHOD_ID) { - if ( - buffer.size() != OBJECT_LIST_UDP_PAYLOAD || - header_packet.length.value() != OBJECT_LIST_PDU_LENGTH) { - PrintError("ObjectList message with invalid size"); - return; - } - - ProcessDataPacket(buffer, sender_ip); - } -} - -void MultiContinentalARS548HwInterface::ProcessFilterStatusPacket( - const std::vector & buffer) -{ - assert(buffer.size() == sizeof(FilterStatusPacket)); - std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); -} - -void MultiContinentalARS548HwInterface::ProcessDataPacket( - const std::vector & buffer, const std::string & sensor_ip) -{ - nebula_msgs::msg::NebulaPacket nebula_packet; - nebula_packet.data = buffer; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - nebula_packet.stamp.sec = static_cast(now_secs); - nebula_packet.stamp.nanosec = - static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); - nebula_packets_ptr_->packets.emplace_back(nebula_packet); - - nebula_packets_ptr_->header.stamp = nebula_packets_ptr_->packets.front().stamp; - nebula_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; - - nebula_packets_reception_callback_(std::move(nebula_packets_ptr_), sensor_ip); - nebula_packets_ptr_ = std::make_unique(); -} - -Status MultiContinentalARS548HwInterface::SensorInterfaceStop() -{ - return Status::ERROR_1; -} - -Status MultiContinentalARS548HwInterface::GetSensorConfiguration( - SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - PrintDebug(ss.str()); - return Status::ERROR_1; -} - -Status MultiContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acceleration) -{ - constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; - constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; - constexpr uint8_t ACCELERATION_LATERAL_COG_LENGTH = 32; - const int ACCELERATION_LATERAL_COG_UDP_LENGTH = 40; - - if (lateral_acceleration < -65.f || lateral_acceleration > 65.f) { - PrintError("Invalid lateral_acceleration value"); - return Status::ERROR_1; - } - - AccelerationLateralCoGPacket acceleration_lateral_cog{}; - static_assert(sizeof(AccelerationLateralCoGPacket) == ACCELERATION_LATERAL_COG_UDP_LENGTH); - acceleration_lateral_cog.header.service_id = ACCELERATION_LATERAL_COG_SERVICE_ID; - acceleration_lateral_cog.header.method_id = ACCELERATION_LATERAL_COG_METHOD_ID; - acceleration_lateral_cog.header.length = ACCELERATION_LATERAL_COG_LENGTH; - acceleration_lateral_cog.acceleration_lateral = lateral_acceleration; - - std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); - std::memcpy(send_vector.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoGPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetAccelerationLongitudinalCog( - float longitudinal_acceleration) -{ - constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; - constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; - constexpr uint8_t ACCELERATION_LONGITUDINAL_COG_LENGTH = 32; - const int ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH = 40; - - if (longitudinal_acceleration < -65.f || longitudinal_acceleration > 65.f) { - PrintError("Invalid longitudinal_acceleration value"); - return Status::ERROR_1; - } - - AccelerationLongitudinalCoGPacket acceleration_longitudinal_cog{}; - static_assert( - sizeof(AccelerationLongitudinalCoGPacket) == ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH); - acceleration_longitudinal_cog.header.service_id = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; - acceleration_longitudinal_cog.header.method_id = ACCELERATION_LONGITUDINAL_COG_METHOD_ID; - acceleration_longitudinal_cog.header.length = ACCELERATION_LONGITUDINAL_COG_LENGTH; - acceleration_longitudinal_cog.acceleration_lateral = longitudinal_acceleration; - - std::vector send_vector(sizeof(AccelerationLongitudinalCoGPacket)); - std::memcpy( - send_vector.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoGPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic_speed) -{ - constexpr uint16_t CHARACTERISTIC_SPEED_SERVICE_ID = 0; - constexpr uint16_t CHARACTERISTIC_SPEED_METHOD_ID = 328; - constexpr uint8_t CHARACTERISTIC_SPEED_LENGTH = 11; - const int CHARACTERISTIC_SPEED_UDP_LENGTH = 19; - - if (characteristic_speed < 0.f || characteristic_speed > 255.f) { - PrintError("Invalid characteristic_speed value"); - return Status::ERROR_1; - } - - CharacteristicSpeedPacket characteristic_speed_packet{}; - static_assert(sizeof(CharacteristicSpeedPacket) == CHARACTERISTIC_SPEED_UDP_LENGTH); - characteristic_speed_packet.header.service_id = CHARACTERISTIC_SPEED_SERVICE_ID; - characteristic_speed_packet.header.method_id = CHARACTERISTIC_SPEED_METHOD_ID; - characteristic_speed_packet.header.length = CHARACTERISTIC_SPEED_LENGTH; - characteristic_speed_packet.characteristic_speed = characteristic_speed; - - std::vector send_vector(sizeof(CharacteristicSpeedPacket)); - std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharacteristicSpeedPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetDrivingDirection(int direction) -{ - constexpr uint16_t DRIVING_DIRECTION_SERVICE_ID = 0; - constexpr uint16_t DRIVING_DIRECTION_METHOD_ID = 325; - constexpr uint8_t DRIVING_DIRECTION_LENGTH = 22; - constexpr uint8_t DRIVING_DIRECTION_STANDSTILL = 0; - constexpr uint8_t DRIVING_DIRECTION_FORWARD = 1; - constexpr uint8_t DRIVING_DIRECTION_BACKWARDS = 2; - const int DRIVING_DIRECTION_UDP_LENGTH = 30; - - DrivingDirectionPacket driving_direction_packet{}; - static_assert(sizeof(DrivingDirectionPacket) == DRIVING_DIRECTION_UDP_LENGTH); - driving_direction_packet.header.service_id = DRIVING_DIRECTION_SERVICE_ID; - driving_direction_packet.header.method_id = DRIVING_DIRECTION_METHOD_ID; - driving_direction_packet.header.length = DRIVING_DIRECTION_LENGTH; - - if (direction == 0) { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_STANDSTILL; - } else if (direction > 0) { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_FORWARD; - } else { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_BACKWARDS; - } - - std::vector send_vector(sizeof(DrivingDirectionPacket)); - std::memcpy(send_vector.data(), &driving_direction_packet, sizeof(DrivingDirectionPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) -{ - constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; - constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; - constexpr uint8_t STEERING_ANGLE_LENGTH = 32; - const int STEERING_ANGLE_UDP_LENGTH = 40; - - if (angle_rad < -90.f || angle_rad > 90.f) { - PrintError("Invalid angle_rad value"); - return Status::ERROR_1; - } - - SteeringAngleFrontAxlePacket steering_angle_front_axle_packet{}; - static_assert(sizeof(SteeringAngleFrontAxlePacket) == STEERING_ANGLE_UDP_LENGTH); - steering_angle_front_axle_packet.header.service_id = STEERING_ANGLE_SERVICE_ID; - steering_angle_front_axle_packet.header.method_id = STEERING_ANGLE_METHOD_ID; - steering_angle_front_axle_packet.header.length = STEERING_ANGLE_LENGTH; - steering_angle_front_axle_packet.steering_angle_front_axle = angle_rad; - - std::vector send_vector(sizeof(SteeringAngleFrontAxlePacket)); - std::memcpy( - send_vector.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxlePacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) -{ - if (velocity_kmh < 0.f || velocity_kmh > 350.f) { - PrintError("Invalid velocity value"); - return Status::ERROR_1; - } - - constexpr uint16_t VELOCITY_VEHICLE_SERVICE_ID = 0; - constexpr uint16_t VELOCITY_VEHICLE_METHOD_ID = 323; - constexpr uint8_t VELOCITY_VEHICLE_LENGTH = 28; - const int VELOCITY_VEHICLE_UDP_SIZE = 36; - - VelocityVehiclePacket steering_angle_front_axle_packet{}; - static_assert(sizeof(VelocityVehiclePacket) == VELOCITY_VEHICLE_UDP_SIZE); - steering_angle_front_axle_packet.header.service_id = VELOCITY_VEHICLE_SERVICE_ID; - steering_angle_front_axle_packet.header.method_id = VELOCITY_VEHICLE_METHOD_ID; - steering_angle_front_axle_packet.header.length = VELOCITY_VEHICLE_LENGTH; - steering_angle_front_axle_packet.velocity_vehicle = velocity_kmh; - - std::vector send_vector(sizeof(VelocityVehiclePacket)); - std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetYawRate(float yaw_rate) -{ - if (yaw_rate < -163.83 || yaw_rate > 163.83) { - PrintError("Invalid yaw rate value"); - return Status::ERROR_1; - } - - constexpr uint16_t YAW_RATE_SERVICE_ID = 0; - constexpr uint16_t YAW_RATE_METHOD_ID = 326; - constexpr uint8_t YAW_RATE_LENGTH = 32; - const int YAW_RATE_UDP_SIZE = 40; - - YawRatePacket yaw_rate_packet{}; - static_assert(sizeof(YawRatePacket) == YAW_RATE_UDP_SIZE); - yaw_rate_packet.header.service_id = YAW_RATE_SERVICE_ID; - yaw_rate_packet.header.method_id = YAW_RATE_METHOD_ID; - yaw_rate_packet.header.length = YAW_RATE_LENGTH; - yaw_rate_packet.yaw_rate = yaw_rate; - - std::vector send_vector(sizeof(YawRatePacket)); - std::memcpy(send_vector.data(), &yaw_rate_packet, sizeof(YawRatePacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::CheckAndSetConfig() -{ - RCLCPP_ERROR( - *parent_node_logger, - "This functionality is not yet implemented. Sensor is probably out of sync with config now."); - return Status::ERROR_1; -} - -void MultiContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) -{ - parent_node_logger = logger; -} - -void MultiContinentalARS548HwInterface::PrintInfo(std::string info) -{ - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); - } else { - std::cout << info << std::endl; - } -} - -void MultiContinentalARS548HwInterface::PrintError(std::string error) -{ - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); - } else { - std::cerr << error << std::endl; - } -} - -void MultiContinentalARS548HwInterface::PrintDebug(std::string debug) -{ - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); - } else { - std::cout << debug << std::endl; - } -} - -void MultiContinentalARS548HwInterface::PrintDebug(const std::vector & bytes) -{ - std::stringstream ss; - for (const auto & b : bytes) { - ss << static_cast(b) << ", "; - } - ss << std::endl; - PrintDebug(ss.str()); -} - -} // namespace continental_ars548 -} // namespace drivers -} // namespace nebula diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 13626debb..e349e3f3b 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -1,5 +1,14 @@ +// Copyright 2024 TIER IV, Inc. + #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" + +#include "nebula_common/nebula_status.hpp" + +#include + #include +#include +#include // #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -10,48 +19,26 @@ #include -namespace nebula -{ -namespace drivers +namespace nebula::drivers { HesaiHwInterface::HesaiHwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, m_owned_ctx{new boost::asio::io_context(1)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, - tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)}, - scan_cloud_ptr_{std::make_unique()} + tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)} { } + HesaiHwInterface::~HesaiHwInterface() { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................st: HesaiHwInterface::~HesaiHwInterface()" << std::endl; -#endif - if (tcp_driver_) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................tcp_driver_ is available" << std::endl; -#endif - if (tcp_driver_ && tcp_driver_->isOpen()) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................st: tcp_driver_->close();" << std::endl; -#endif - tcp_driver_->close(); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: tcp_driver_->close();" << std::endl; -#endif - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: if(tcp_driver_)" << std::endl; -#endif - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: HesaiHwInterface::~HesaiHwInterface()" << std::endl; -#endif + FinalizeTcpDriver(); } -std::shared_ptr> HesaiHwInterface::SendReceive( +HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( const uint8_t command_id, const std::vector & payload) { + std::lock_guard lock(mtx_inflight_tcp_request_); + uint32_t len = payload.size(); std::vector send_buf; @@ -65,11 +52,16 @@ std::shared_ptr> HesaiHwInterface::SendReceive( send_buf.emplace_back(len & 0xff); send_buf.insert(send_buf.end(), payload.begin(), payload.end()); + // These are shared_ptrs so that in case of request timeout, the callback (if ever called) can + // access valid memory auto recv_buf = std::make_shared>(); - bool success = false; + auto response_complete = std::make_shared(false); + + auto error_code = std::make_shared(); std::stringstream ss; - ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) << " (" << len << ") "; + ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) + << " (" << len << ") "; std::string log_tag = ss.str(); PrintDebug(log_tag + "Entering lock"); @@ -85,104 +77,71 @@ std::shared_ptr> HesaiHwInterface::SendReceive( PrintDebug(log_tag + "Sending payload"); tcp_driver_->asyncSendReceiveHeaderPayload( send_buf, - [this, log_tag, &success](const std::vector & header_bytes) { - size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7]; - PrintDebug(log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); - if (payload_len == 0) { success = true; } + [this, log_tag, command_id, response_complete, + error_code](const std::vector & header_bytes) { + error_code->ptc_error_code = header_bytes[3]; + + size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | + (header_bytes[6] << 8) | header_bytes[7]; + PrintDebug( + log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); + // If command_id in the response does not match, we got a response for another command (or + // rubbish), probably as a result of too many simultaneous TCP connections to the sensor (e.g. + // from GUI, Web UI, another nebula instance, etc.) + if (header_bytes[2] != command_id) { + error_code->error_flags |= TCP_ERROR_UNRELATED_RESPONSE; + } + if (payload_len == 0) { + *response_complete = true; + } }, - [this, log_tag, &recv_buf, &success](const std::vector & payload_bytes) { + [this, log_tag, recv_buf, response_complete, + error_code](const std::vector & payload_bytes) { PrintDebug(log_tag + "Received payload"); // Header had payload length 0 (thus, header callback processed request successfully already), // but we still received a payload: invalid state - if (success == true) { - throw std::runtime_error("Received payload despite payload length 0 in header"); + if (*response_complete == true) { + error_code->error_flags |= TCP_ERROR_UNEXPECTED_PAYLOAD; + return; } // Skip 8 header bytes recv_buf->insert(recv_buf->end(), std::next(payload_bytes.begin(), 8), payload_bytes.end()); - success = true; + *response_complete = true; }, [this, log_tag, &tm]() { PrintDebug(log_tag + "Unlocking mutex"); - tm.unlock(); + tm.unlock(); PrintDebug(log_tag + "Unlocked mutex"); }); this->IOContextRun(); if (!tm.try_lock_for(std::chrono::seconds(1))) { PrintError(log_tag + "Request did not finish within 1s"); - return nullptr; + error_code->error_flags |= TCP_ERROR_TIMEOUT; + return *error_code; } - if (!success) { + if (!response_complete) { PrintError(log_tag + "Did not receive response"); - return nullptr; + error_code->error_flags |= TCP_ERROR_INCOMPLETE_RESPONSE; + return *error_code; + } + + if (!error_code->ok()) { + return *error_code; } PrintDebug(log_tag + "Received response"); - return recv_buf; + + return *recv_buf; } Status HesaiHwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr sensor_configuration) { - HesaiStatus status = Status::OK; - mtu_size_ = MTU_SIZE; - is_solid_state = false; - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - if ( - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR40P || - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR40P) { - azimuth_index_ = 2; - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR40_PACKET_SIZE || packet_size == PANDAR40P_EXTENDED_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT64) { - azimuth_index_ = 12; // 12 + 258 * [0-3] - is_valid_packet_ = [](size_t packet_size) { return (packet_size == PANDARQT64_PACKET_SIZE); }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT128) { - azimuth_index_ = 12; // 12 + 514 * [0-1] - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARQT128_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARXT32) { - azimuth_index_ = 12; // 12 + 130 * [0-7] - is_valid_packet_ = [](size_t packet_size) { return (packet_size == PANDARXT32_PACKET_SIZE); }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARXT32M) { - azimuth_index_ = 12; // 12 + 130 * [0-7] - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARXT32M_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { - azimuth_index_ = 12; // 12 + 4 * 128 * [0-1] - is_solid_state = true; - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARAT128_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { - azimuth_index_ = 8; // 8 + 192 * [0-5] - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR64_PACKET_SIZE || packet_size == PANDAR64_EXTENDED_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { - azimuth_index_ = 12; // 12 - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR128_E4X_EXTENDED_PACKET_SIZE || - packet_size == PANDAR128_E4X_PACKET_SIZE); - }; - } else { - status = Status::INVALID_SENSOR_MODEL; - } - } catch (const std::exception & ex) { - status = Status::SENSOR_CONFIG_ERROR; - std::cerr << status << std::endl; - return status; - } + sensor_configuration_ = + std::static_pointer_cast(sensor_configuration); return Status::OK; } @@ -190,8 +149,15 @@ Status HesaiHwInterface::SensorInterfaceStart() { try { std::cout << "Starting UDP server on: " << *sensor_configuration_ << std::endl; - cloud_udp_driver_->init_receiver( - sensor_configuration_->host_ip, sensor_configuration_->data_port); + if (sensor_configuration_->multicast_ip.empty()) { + cloud_udp_driver_->init_receiver( + sensor_configuration_->host_ip, sensor_configuration_->data_port); + } else { + cloud_udp_driver_->init_receiver( + sensor_configuration_->multicast_ip, sensor_configuration_->data_port, + sensor_configuration_->host_ip, sensor_configuration_->data_port); + cloud_udp_driver_->receiver()->setMulticast(true); + } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE PrintError("init ok"); #endif @@ -199,6 +165,15 @@ Status HesaiHwInterface::SensorInterfaceStart() #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE PrintError("open ok"); #endif + + bool success = cloud_udp_driver_->receiver()->setKernelBufferSize(UDP_SOCKET_BUFFER_SIZE); + if (!success) { + PrintError( + "Could not set receive buffer size. Try increasing net.core.rmem_max to " + + std::to_string(UDP_SOCKET_BUFFER_SIZE) + " B."); + return Status::ERROR_1; + } + cloud_udp_driver_->receiver()->bind(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE PrintError("bind ok"); @@ -211,76 +186,31 @@ Status HesaiHwInterface::SensorInterfaceStart() #endif } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," - << sensor_configuration_->data_port << std::endl; + std::cerr << status << " for " << sensor_configuration_->sensor_ip << ":" + << sensor_configuration_->data_port << " - " << ex.what() << std::endl; return status; } return Status::OK; } Status HesaiHwInterface::RegisterScanCallback( - std::function)> scan_callback) + std::function &)> scan_callback) { - scan_reception_callback_ = std::move(scan_callback); + cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } -void HesaiHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +void HesaiHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { - int scan_phase = static_cast(sensor_configuration_->scan_phase * 100.0); - if (!is_valid_packet_(buffer.size())) { - PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); - return; - } - const uint32_t buffer_size = buffer.size(); - pandar_msgs::msg::PandarPacket pandar_packet; - std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, pandar_packet.data.begin()); - pandar_packet.size = buffer_size; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - pandar_packet.stamp.sec = static_cast(now_secs); - pandar_packet.stamp.nanosec = static_cast(now_nanosecs % 1'000'000'000); - scan_cloud_ptr_->packets.emplace_back(pandar_packet); - - int current_phase = 0; - bool comp_flg = false; - - const auto & data = scan_cloud_ptr_->packets.back().data; - current_phase = (data[azimuth_index_] & 0xff) + ((data[azimuth_index_ + 1] & 0xff) << 8); - if (is_solid_state) { - current_phase = (static_cast(current_phase) + 36000 - 0) % 12000; - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; - } else { - comp_flg = true; - } - } else { - current_phase = (static_cast(current_phase) + 36000 - scan_phase) % 36000; - - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; - } else { - comp_flg = true; - } - } - - if (comp_flg) { // Scan complete - if (scan_reception_callback_) { - scan_cloud_ptr_->header.stamp = scan_cloud_ptr_->packets.front().stamp; - // Callback - scan_reception_callback_(std::move(scan_cloud_ptr_)); - scan_cloud_ptr_ = std::make_unique(); - } - } + cloud_packet_callback_(buffer); } Status HesaiHwInterface::SensorInterfaceStop() { return Status::ERROR_1; } -Status HesaiHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) +Status HesaiHwInterface::GetSensorConfiguration( + const SensorConfigurationBase & sensor_configuration) { std::stringstream ss; ss << sensor_configuration; @@ -324,7 +254,9 @@ Status HesaiHwInterface::InitializeTcpDriver() Status HesaiHwInterface::FinalizeTcpDriver() { try { - tcp_driver_->close(); + if (tcp_driver_) { + tcp_driver_->close(); + } } catch (std::exception & e) { PrintError("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; @@ -347,349 +279,93 @@ boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str) std::vector HesaiHwInterface::GetLidarCalibrationBytes() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); - return std::vector(*response_ptr); + auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); + return response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); } std::string HesaiHwInterface::GetLidarCalibrationString() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); - std::string calib_string(response_ptr->begin(), response_ptr->end()); + auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); + auto calib_data = + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + std::string calib_string(calib_data.begin(), calib_data.end()); return calib_string; } HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() { - auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); - auto & response = *response_ptr; - - HesaiPtpDiagStatus hesai_ptp_diag_status{}; - int payload_pos = 0; - hesai_ptp_diag_status.master_offset = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]); - hesai_ptp_diag_status.ptp_state = response[payload_pos++] << 24; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] << 16; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] << 8; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++]; - hesai_ptp_diag_status.elapsed_millisec = response[payload_pos++] << 24; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 16; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 8; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++]; + auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto diag_status = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagStatus: " << hesai_ptp_diag_status; + ss << "HesaiHwInterface::GetPtpDiagStatus: " << diag_status; PrintInfo(ss.str()); - return hesai_ptp_diag_status; + return diag_status; } HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() { - auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); - auto & response = *response_ptr; - - HesaiPtpDiagPort hesai_ptp_diag_port; - int payload_pos = 0; - - for (size_t i = 0; i < hesai_ptp_diag_port.portIdentity.size(); i++) { - hesai_ptp_diag_port.portIdentity[i] = response[payload_pos++]; - } - hesai_ptp_diag_port.portState = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logMinDelayReqInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.peerMeanPathDelay = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logAnnounceInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.announceReceiptTimeout = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logSyncInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.delayMechanism = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logMinPdelayReqInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.versionNumber = static_cast(response[payload_pos++]); + auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto diag_port = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagPort: " << hesai_ptp_diag_port; + ss << "HesaiHwInterface::GetPtpDiagPort: " << diag_port; PrintInfo(ss.str()); - return hesai_ptp_diag_port; + return diag_port; } HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() { - auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); - auto & response = *response_ptr; - - HesaiPtpDiagTime hesai_ptp_diag_time; - int payload_pos = 0; - hesai_ptp_diag_time.master_offset = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]); - hesai_ptp_diag_time.ingress_time = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]); - hesai_ptp_diag_time.cumulativeScaledRateOffset = response[payload_pos++] << 24; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 16; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 8; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++]; - hesai_ptp_diag_time.scaledLastGmPhaseChange = response[payload_pos++] << 24; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 16; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 8; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++]; - hesai_ptp_diag_time.gmTimeBaseIndicator = response[payload_pos++] << 8; - hesai_ptp_diag_time.gmTimeBaseIndicator = - hesai_ptp_diag_time.gmTimeBaseIndicator | response[payload_pos++]; - for (size_t i = 0; i < hesai_ptp_diag_time.lastGmPhaseChange.size(); i++) { - hesai_ptp_diag_time.lastGmPhaseChange[i] = response[payload_pos++]; - } - hesai_ptp_diag_time.gmPresent = response[payload_pos++] << 24; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] << 16; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] << 8; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++]; - hesai_ptp_diag_time.gmIdentity = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]); + auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto diag_time = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagTime: " << hesai_ptp_diag_time; + ss << "HesaiHwInterface::GetPtpDiagTime: " << diag_time; PrintInfo(ss.str()); - return hesai_ptp_diag_time; + return diag_time; } HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() { - auto response_ptr = + auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); - auto & response = *response_ptr; - - HesaiPtpDiagGrandmaster hesai_ptp_diag_grandmaster; - int payload_pos = 0; - - hesai_ptp_diag_grandmaster.clockQuality = response[payload_pos++] << 24; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 16; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 8; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++]; - hesai_ptp_diag_grandmaster.utc_offset = response[payload_pos++] << 8; - hesai_ptp_diag_grandmaster.utc_offset = - hesai_ptp_diag_grandmaster.utc_offset | response[payload_pos++]; - hesai_ptp_diag_grandmaster.time_flags = static_cast(response[payload_pos++]); - hesai_ptp_diag_grandmaster.time_source = static_cast(response[payload_pos++]); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto diag_grandmaster = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << hesai_ptp_diag_grandmaster; + ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << diag_grandmaster; PrintInfo(ss.str()); - return hesai_ptp_diag_grandmaster; + return diag_grandmaster; } HesaiInventory HesaiHwInterface::GetInventory() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); - auto & response = *response_ptr; - - HesaiInventory hesai_inventory; - int payload_pos = 0; - for (size_t i = 0; i < hesai_inventory.sn.size(); i++) { - hesai_inventory.sn[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.date_of_manufacture.size(); i++) { - hesai_inventory.date_of_manufacture[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.mac.size(); i++) { - hesai_inventory.mac[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sw_ver.size(); i++) { - hesai_inventory.sw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.hw_ver.size(); i++) { - hesai_inventory.hw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.control_fw_ver.size(); i++) { - hesai_inventory.control_fw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sensor_fw_ver.size(); i++) { - hesai_inventory.sensor_fw_ver[i] = response[payload_pos++]; - } - hesai_inventory.angle_offset = response[payload_pos++] << 8; - hesai_inventory.angle_offset = hesai_inventory.angle_offset | response[payload_pos++]; - hesai_inventory.model = static_cast(response[payload_pos++]); - hesai_inventory.motor_type = static_cast(response[payload_pos++]); - hesai_inventory.num_of_lines = static_cast(response[payload_pos++]); - for (size_t i = 0; i < hesai_inventory.reserved.size(); i++) { - hesai_inventory.reserved[i] = static_cast(response[payload_pos++]); - } - - return hesai_inventory; + auto response_or_err = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + return CheckSizeAndParse(response); } HesaiConfig HesaiHwInterface::GetConfig() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); - auto & response = *response_ptr; - - HesaiConfig hesai_config{}; - int payload_pos = 0; - hesai_config.ipaddr[0] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[1] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[2] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[3] = static_cast(response[payload_pos++]); - hesai_config.mask[0] = static_cast(response[payload_pos++]); - hesai_config.mask[1] = static_cast(response[payload_pos++]); - hesai_config.mask[2] = static_cast(response[payload_pos++]); - hesai_config.mask[3] = static_cast(response[payload_pos++]); - hesai_config.gateway[0] = static_cast(response[payload_pos++]); - hesai_config.gateway[1] = static_cast(response[payload_pos++]); - hesai_config.gateway[2] = static_cast(response[payload_pos++]); - hesai_config.gateway[3] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[0] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[1] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[2] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[3] = static_cast(response[payload_pos++]); - hesai_config.dest_LiDAR_udp_port = response[payload_pos++] << 8; - hesai_config.dest_LiDAR_udp_port = hesai_config.dest_LiDAR_udp_port | response[payload_pos++]; - hesai_config.dest_gps_udp_port = response[payload_pos++] << 8; - hesai_config.dest_gps_udp_port = hesai_config.dest_gps_udp_port | response[payload_pos++]; - hesai_config.spin_rate = response[payload_pos++] << 8; - hesai_config.spin_rate = hesai_config.spin_rate | response[payload_pos++]; - hesai_config.sync = static_cast(response[payload_pos++]); - hesai_config.sync_angle = response[payload_pos++] << 8; - hesai_config.sync_angle = hesai_config.sync_angle | response[payload_pos++]; - hesai_config.start_angle = response[payload_pos++] << 8; - hesai_config.start_angle = hesai_config.start_angle | response[payload_pos++]; - hesai_config.stop_angle = response[payload_pos++] << 8; - hesai_config.stop_angle = hesai_config.stop_angle | response[payload_pos++]; - hesai_config.clock_source = static_cast(response[payload_pos++]); - hesai_config.udp_seq = static_cast(response[payload_pos++]); - hesai_config.trigger_method = static_cast(response[payload_pos++]); - hesai_config.return_mode = static_cast(response[payload_pos++]); - hesai_config.standby_mode = static_cast(response[payload_pos++]); - hesai_config.motor_status = static_cast(response[payload_pos++]); - hesai_config.vlan_flag = static_cast(response[payload_pos++]); - hesai_config.vlan_id = response[payload_pos++] << 8; - hesai_config.vlan_id = hesai_config.vlan_id | response[payload_pos++]; - hesai_config.clock_data_fmt = static_cast(response[payload_pos++]); - hesai_config.noise_filtering = static_cast(response[payload_pos++]); - hesai_config.reflectivity_mapping = static_cast(response[payload_pos++]); - hesai_config.reserved[0] = static_cast(response[payload_pos++]); - hesai_config.reserved[1] = static_cast(response[payload_pos++]); - hesai_config.reserved[2] = static_cast(response[payload_pos++]); - hesai_config.reserved[3] = static_cast(response[payload_pos++]); - hesai_config.reserved[4] = static_cast(response[payload_pos++]); - hesai_config.reserved[5] = static_cast(response[payload_pos++]); - + auto response_or_err = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto hesai_config = CheckSizeAndParse(response); std::cout << "Config: " << hesai_config << std::endl; return hesai_config; } HesaiLidarStatus HesaiHwInterface::GetLidarStatus() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); - auto & response = *response_ptr; - - HesaiLidarStatus hesai_status; - int payload_pos = 0; - hesai_status.system_uptime = response[payload_pos++] << 24; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 16; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 8; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++]; - hesai_status.motor_speed = response[payload_pos++] << 8; - hesai_status.motor_speed = hesai_status.motor_speed | response[payload_pos++]; - for (size_t i = 0; i < hesai_status.temperature.size(); i++) { - hesai_status.temperature[i] = response[payload_pos++] << 24; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 16; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 8; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++]; - } - hesai_status.gps_pps_lock = static_cast(response[payload_pos++]); - hesai_status.gps_gprmc_status = static_cast(response[payload_pos++]); - hesai_status.startup_times = response[payload_pos++] << 24; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 16; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 8; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++]; - hesai_status.total_operation_time = response[payload_pos++] << 24; - hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++] - << 16; - hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++] - << 8; - hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++]; - hesai_status.ptp_clock_status = static_cast(response[payload_pos++]); - for (size_t i = 0; i < hesai_status.reserved.size(); i++) { - hesai_status.reserved[i] = static_cast(response[payload_pos++]); - } - - return hesai_status; + auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + return CheckSizeAndParse(response); } Status HesaiHwInterface::SetSpinRate(uint16_t rpm) @@ -698,18 +374,25 @@ Status HesaiHwInterface::SetSpinRate(uint16_t rpm) request_payload.emplace_back((rpm >> 8) & 0xff); request_payload.emplace_back(rpm & 0xff); - SendReceive(PTC_COMMAND_SET_SPIN_RATE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_SPIN_RATE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle) { + if (sync_angle < 0 || sync_angle > 360) { + return Status::SENSOR_CONFIG_ERROR; + } + std::vector request_payload; - request_payload.emplace_back(sync_angle & 0xff); + // 360 is converted to 0 + request_payload.emplace_back((sync_angle % 360) & 0xff); request_payload.emplace_back((angle >> 8) & 0xff); request_payload.emplace_back(angle & 0xff); - SendReceive(PTC_COMMAND_SET_SYNC_ANGLE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_SYNC_ANGLE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -718,7 +401,8 @@ Status HesaiHwInterface::SetTriggerMethod(int trigger_method) std::vector request_payload; request_payload.emplace_back(trigger_method & 0xff); - SendReceive(PTC_COMMAND_SET_TRIGGER_METHOD, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_TRIGGER_METHOD, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -727,7 +411,8 @@ Status HesaiHwInterface::SetStandbyMode(int standby_mode) std::vector request_payload; request_payload.emplace_back(standby_mode & 0xff); - SendReceive(PTC_COMMAND_SET_STANDBY_MODE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_STANDBY_MODE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -736,7 +421,8 @@ Status HesaiHwInterface::SetReturnMode(int return_mode) std::vector request_payload; request_payload.emplace_back(return_mode & 0xff); - SendReceive(PTC_COMMAND_SET_RETURN_MODE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_RETURN_MODE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -753,7 +439,8 @@ Status HesaiHwInterface::SetDestinationIp( request_payload.emplace_back((gps_port >> 8) & 0xff); request_payload.emplace_back(gps_port & 0xff); - SendReceive(PTC_COMMAND_SET_DESTINATION_IP, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_DESTINATION_IP, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -778,12 +465,16 @@ Status HesaiHwInterface::SetControlPort( request_payload.emplace_back((vlan_id >> 8) & 0xff); request_payload.emplace_back(vlan_id & 0xff); - SendReceive(PTC_COMMAND_SET_CONTROL_PORT, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_CONTROL_PORT, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } Status HesaiHwInterface::SetLidarRange(int method, std::vector data) { + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + return Status::SENSOR_CONFIG_ERROR; + } // 0 - for all channels : 5-1 bytes // 1 - for each channel : 323-1 bytes // 2 - multi-section FOV : 1347-1 bytes @@ -791,63 +482,99 @@ Status HesaiHwInterface::SetLidarRange(int method, std::vector da request_payload.emplace_back(method & 0xff); request_payload.insert(request_payload.end(), data.begin(), data.end()); - SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } -Status HesaiHwInterface::SetLidarRange(int start, int end) +Status HesaiHwInterface::SetLidarRange(int start_ddeg, int end_ddeg) { + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + return Status::SENSOR_CONFIG_ERROR; + } // 0 - for all channels : 5-1 bytes // 1 - for each channel : 323-1 bytes // 2 - multi-section FOV : 1347-1 bytes std::vector request_payload; int method = 0; request_payload.emplace_back(method & 0xff); - request_payload.emplace_back((start >> 8) & 0xff); - request_payload.emplace_back(start & 0xff); - request_payload.emplace_back((end >> 8) & 0xff); - request_payload.emplace_back(end & 0xff); + request_payload.emplace_back((start_ddeg >> 8) & 0xff); + request_payload.emplace_back(start_ddeg & 0xff); + request_payload.emplace_back((end_ddeg >> 8) & 0xff); + request_payload.emplace_back(end_ddeg & 0xff); - SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() { - HesaiLidarRangeAll hesai_range_all{}; - auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE); - if (!response_ptr) return hesai_range_all; + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + throw std::runtime_error("Not supported on this sensor"); + } + auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - auto & response = *response_ptr; - if (response.empty()) return hesai_range_all; + if (response.size() < 1) { + throw std::runtime_error("Response payload too short"); + } - int payload_pos = 0; - int method = static_cast(response[payload_pos++]); - switch (method) { + HesaiLidarRangeAll hesai_range_all{}; + hesai_range_all.method = response[0]; + switch (hesai_range_all.method) { case 0: // for all channels - hesai_range_all.method = method; - hesai_range_all.start = response[payload_pos++] << 8; - hesai_range_all.start = hesai_range_all.start | response[payload_pos++]; - hesai_range_all.end = response[payload_pos++] << 8; - hesai_range_all.end = hesai_range_all.end | response[payload_pos++]; + if (response.size() != 5) { + throw std::runtime_error("Unexpected response payload"); + } + + memcpy(&hesai_range_all.start, &response[1], 2); + memcpy(&hesai_range_all.end, &response[3], 2); break; case 1: // for each channel - hesai_range_all.method = method; + // TODO(yukkysaito) break; case 2: // multi-section FOV - hesai_range_all.method = method; + // TODO(yukkysaito) break; } return hesai_range_all; } +Status HesaiHwInterface::checkAndSetLidarRange( + const HesaiCalibrationConfigurationBase & calibration) +{ + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + return Status::SENSOR_CONFIG_ERROR; + } + + int cloud_min_ddeg = sensor_configuration_->cloud_min_angle * 10; + int cloud_max_ddeg = sensor_configuration_->cloud_max_angle * 10; + + // Only oversize the FoV if it is not already the full 360deg + if (cloud_min_ddeg != 0 || cloud_max_ddeg != 3600) { + auto padding_deg = calibration.getFovPadding(); + cloud_min_ddeg += floor(std::get<0>(padding_deg) * 10); + cloud_max_ddeg += ceil(std::get<1>(padding_deg) * 10); + } + + auto clamp = [](int angle_ddeg) { + while (angle_ddeg < 0) angle_ddeg += 3600; + while (angle_ddeg > 3600) angle_ddeg -= 3600; + return angle_ddeg; + }; + + return SetLidarRange(clamp(cloud_min_ddeg), clamp(cloud_max_ddeg)); +} + Status HesaiHwInterface::SetClockSource(int clock_source) { std::vector request_payload; request_payload.emplace_back(clock_source & 0xff); - SendReceive(PTC_COMMAND_SET_CLOCK_SOURCE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_CLOCK_SOURCE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -880,28 +607,28 @@ Status HesaiHwInterface::SetPtpConfig( request_payload.emplace_back(switch_type & 0xff); } - SendReceive(PTC_COMMAND_SET_PTP_CONFIG, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_PTP_CONFIG, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } HesaiPtpConfig HesaiHwInterface::GetPtpConfig() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_PTP_CONFIG); - auto & response = *response_ptr; + auto response_or_err = SendReceive(PTC_COMMAND_GET_PTP_CONFIG); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - HesaiPtpConfig hesai_ptp_config{}; - - int payload_pos = 0; - hesai_ptp_config.status = static_cast(response[payload_pos++]); - hesai_ptp_config.profile = static_cast(response[payload_pos++]); - hesai_ptp_config.domain = static_cast(response[payload_pos++]); - hesai_ptp_config.network = static_cast(response[payload_pos++]); - if (hesai_ptp_config.status == 0) { - hesai_ptp_config.logAnnounceInterval = static_cast(response[payload_pos++]); - hesai_ptp_config.logSyncInterval = static_cast(response[payload_pos++]); - hesai_ptp_config.logMinDelayReqInterval = static_cast(response[payload_pos++]); + if (response.size() < sizeof(HesaiPtpConfig)) { + throw std::runtime_error("HesaiPtpConfig has unexpected payload size"); + } else if (response.size() > sizeof(HesaiPtpConfig)) { + PrintError("HesaiPtpConfig from Sensor has unknown format. Will parse anyway."); } + HesaiPtpConfig hesai_ptp_config; + memcpy(&hesai_ptp_config.status, response.data(), 1); + + size_t bytes_to_parse = (hesai_ptp_config.status == 0) ? sizeof(HesaiPtpConfig) : 4; + memcpy(&hesai_ptp_config, response.data(), bytes_to_parse); + std::stringstream ss; ss << "HesaiHwInterface::GetPtpConfig: " << hesai_ptp_config; PrintInfo(ss.str()); @@ -911,7 +638,8 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig() Status HesaiHwInterface::SendReset() { - SendReceive(PTC_COMMAND_RESET); + auto response_or_err = SendReceive(PTC_COMMAND_RESET); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -920,42 +648,26 @@ Status HesaiHwInterface::SetRotDir(int mode) std::vector request_payload; request_payload.emplace_back(mode & 0xff); - SendReceive(PTC_COMMAND_SET_ROTATE_DIRECTION, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_ROTATE_DIRECTION, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() { - HesaiLidarMonitor hesai_lidar_monitor{}; - auto response_ptr = SendReceive(PTC_COMMAND_LIDAR_MONITOR); - if (!response_ptr) return hesai_lidar_monitor; - - auto & response = *response_ptr; - if (response.empty()) return hesai_lidar_monitor; - - int payload_pos = 0; - hesai_lidar_monitor.input_voltage = response[payload_pos++] << 24; - hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++] - << 16; - hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++] - << 8; - hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++]; - hesai_lidar_monitor.input_current = response[payload_pos++] << 24; - hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++] - << 16; - hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++] - << 8; - hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++]; - hesai_lidar_monitor.input_power = response[payload_pos++] << 24; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] << 16; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] << 8; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++]; - - for (size_t i = 0; i < hesai_lidar_monitor.reserved.size(); i++) { - hesai_lidar_monitor.reserved[i] = static_cast(response[payload_pos++]); + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + throw std::runtime_error("Not supported on this sensor"); } - return hesai_lidar_monitor; + auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); + + // FIXME(mojomex): this is a hotfix for sensors that do not support this command + if (!response_or_err.has_value()) { + return HesaiLidarMonitor{}; + } + + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + return CheckSizeAndParse(response); } void HesaiHwInterface::IOContextRun() @@ -1112,7 +824,7 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( } hcd->asyncGet( - [this, str_callback](const std::string & str) { str_callback(str); }, + [str_callback](const std::string & str) { str_callback(str); }, "/pandar.cgi?action=get&object=lidar_monitor"); boost::system::error_code ec; ctx->run(ec); @@ -1129,9 +841,9 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( } HesaiStatus HesaiHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, HesaiConfig hesai_config) + std::shared_ptr sensor_configuration, HesaiConfig hesai_config) { - using namespace std::chrono_literals; + using namespace std::chrono_literals; // NOLINT(build/namespaces) #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif @@ -1162,8 +874,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( } auto current_rotation_speed = hesai_config.spin_rate; - if (sensor_configuration->rotation_speed != current_rotation_speed) { - PrintInfo("current lidar rotation_speed: " + std::to_string(current_rotation_speed)); + if (sensor_configuration->rotation_speed != current_rotation_speed.value()) { + PrintInfo( + "current lidar rotation_speed: " + + std::to_string(static_cast(current_rotation_speed.value()))); PrintInfo( "current configuration rotation_speed: " + std::to_string(sensor_configuration->rotation_speed)); @@ -1181,34 +895,43 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( bool set_flg = false; std::stringstream ss; - ss << hesai_config.dest_ipaddr[0] << "." << hesai_config.dest_ipaddr[1] << "." - << hesai_config.dest_ipaddr[2] << "." << hesai_config.dest_ipaddr[3]; + ss << static_cast(hesai_config.dest_ipaddr[0]) << "." + << static_cast(hesai_config.dest_ipaddr[1]) << "." + << static_cast(hesai_config.dest_ipaddr[2]) << "." + << static_cast(hesai_config.dest_ipaddr[3]); auto current_host_addr = ss.str(); - if (sensor_configuration->host_ip != current_host_addr) { + auto desired_host_addr = sensor_configuration->multicast_ip.empty() + ? sensor_configuration->host_ip + : sensor_configuration->multicast_ip; + if (desired_host_addr != current_host_addr) { set_flg = true; PrintInfo("current lidar dest_ipaddr: " + current_host_addr); - PrintInfo("current configuration host_ip: " + sensor_configuration->host_ip); + PrintInfo("current configuration host_ip: " + desired_host_addr); } auto current_host_dport = hesai_config.dest_LiDAR_udp_port; - if (sensor_configuration->data_port != current_host_dport) { + if (sensor_configuration->data_port != current_host_dport.value()) { set_flg = true; - PrintInfo("current lidar dest_LiDAR_udp_port: " + std::to_string(current_host_dport)); + PrintInfo( + "current lidar dest_LiDAR_udp_port: " + + std::to_string(static_cast(current_host_dport.value()))); PrintInfo( "current configuration data_port: " + std::to_string(sensor_configuration->data_port)); } auto current_host_tport = hesai_config.dest_gps_udp_port; - if (sensor_configuration->gnss_port != current_host_tport) { + if (sensor_configuration->gnss_port != current_host_tport.value()) { set_flg = true; - PrintInfo("current lidar dest_gps_udp_port: " + std::to_string(current_host_tport)); + PrintInfo( + "current lidar dest_gps_udp_port: " + + std::to_string(static_cast(current_host_tport.value()))); PrintInfo( "current configuration gnss_port: " + std::to_string(sensor_configuration->gnss_port)); } if (set_flg) { std::vector list_string; - boost::split(list_string, sensor_configuration->host_ip, boost::is_any_of(".")); + boost::split(list_string, desired_host_addr, boost::is_any_of(".")); std::thread t([this, sensor_configuration, list_string] { SetDestinationIp( std::stoi(list_string[0]), std::stoi(list_string[1]), std::stoi(list_string[2]), @@ -1221,17 +944,18 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128) { set_flg = true; - auto sync_angle = static_cast(hesai_config.sync_angle / 100); - auto scan_phase = static_cast(sensor_configuration->scan_phase); + auto sensor_sync_angle = static_cast(hesai_config.sync_angle.value() / 100); + auto config_sync_angle = sensor_configuration->sync_angle; int sync_flg = 1; - if (scan_phase != sync_angle) { + if (config_sync_angle != sensor_sync_angle) { set_flg = true; } if (sync_flg && set_flg) { PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); - PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle)); - PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase)); - std::thread t([this, sync_flg, scan_phase] { SetSyncAngle(sync_flg, scan_phase); }); + PrintInfo("current lidar sync_angle: " + std::to_string(sensor_sync_angle)); + PrintInfo("current configuration sync_angle: " + std::to_string(config_sync_angle)); + std::thread t( + [this, sync_flg, config_sync_angle] { SetSyncAngle(sync_flg, config_sync_angle); }); t.join(); std::this_thread::sleep_for(wait_time); } @@ -1267,7 +991,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::this_thread::sleep_for(wait_time); } else { // AT128 only supports PTP setup via HTTP PrintInfo("Trying to set SyncAngle via HTTP"); - SetSyncAngleSyncHttp(1, static_cast(sensor_configuration->scan_phase)); + SetSyncAngleSyncHttp(1, sensor_configuration->sync_angle); std::ostringstream tmp_ostringstream; tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile << ", Domain: " << sensor_configuration->ptp_domain @@ -1288,7 +1012,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( } HesaiStatus HesaiHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -1304,19 +1028,27 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( #endif set_flg = true; } else { - auto current_cloud_min_angle = hesai_lidar_range_all.start; - if (static_cast(sensor_configuration->cloud_min_angle * 10) != current_cloud_min_angle) { + auto current_cloud_min_angle_ddeg = hesai_lidar_range_all.start; + if ( + static_cast(sensor_configuration->cloud_min_angle * 10) != + current_cloud_min_angle_ddeg.value()) { set_flg = true; - PrintInfo("current lidar range.start: " + std::to_string(current_cloud_min_angle)); + PrintInfo( + "current lidar range.start: " + + std::to_string(static_cast(current_cloud_min_angle_ddeg.value()))); PrintInfo( "current configuration cloud_min_angle: " + std::to_string(sensor_configuration->cloud_min_angle)); } - auto current_cloud_max_angle = hesai_lidar_range_all.end; - if (static_cast(sensor_configuration->cloud_max_angle * 10) != current_cloud_max_angle) { + auto current_cloud_max_angle_ddeg = hesai_lidar_range_all.end; + if ( + static_cast(sensor_configuration->cloud_max_angle * 10) != + current_cloud_max_angle_ddeg.value()) { set_flg = true; - PrintInfo("current lidar range.end: " + std::to_string(current_cloud_max_angle)); + PrintInfo( + "current lidar range.end: " + + std::to_string(static_cast(current_cloud_max_angle_ddeg.value()))); PrintInfo( "current configuration cloud_max_angle: " + std::to_string(sensor_configuration->cloud_max_angle)); @@ -1327,9 +1059,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::thread t([this, sensor_configuration] { SetLidarRange( static_cast(sensor_configuration->cloud_min_angle * 10), - static_cast(sensor_configuration->cloud_max_angle * 10) //, - // false - ); + static_cast(sensor_configuration->cloud_max_angle * 10)); }); t.join(); } @@ -1352,17 +1082,21 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() ss << result; PrintInfo(ss.str()); CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); + std::static_pointer_cast(sensor_configuration_), result); }); t.join(); + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + return Status::OK; + } + std::thread t2([this] { auto result = GetLidarRange(); std::stringstream ss; ss << result; PrintInfo(ss.str()); CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); + std::static_pointer_cast(sensor_configuration_), result); }); t2.join(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -1556,5 +1290,91 @@ void HesaiHwInterface::PrintDebug(const std::vector & bytes) PrintDebug(ss.str()); } -} // namespace drivers -} // namespace nebula +std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) +{ + if (error_code.ok()) { + return "No error"; + } + + auto ptc_error = error_code.ptc_error_code; + auto error_flags = error_code.error_flags; + std::stringstream ss; + + if (ptc_error) { + ss << "Sensor error: 0x" << std::setfill('0') << std::setw(2) << std::hex + << static_cast(ptc_error) << ' '; + } + + switch (ptc_error) { + case PTC_ERROR_CODE_NO_ERROR: + break; + case PTC_ERROR_CODE_INVALID_INPUT_PARAM: + ss << "Invalid input parameter"; + break; + case PTC_ERROR_CODE_SERVER_CONN_FAILED: + ss << "Failure to connect to server"; + break; + case PTC_ERROR_CODE_INVALID_DATA: + ss << "No valid data returned"; + break; + case PTC_ERROR_CODE_OUT_OF_MEMORY: + ss << "Server does not have enough memory"; + break; + case PTC_ERROR_CODE_UNSUPPORTED_CMD: + ss << "Server does not support this command yet"; + break; + case PTC_ERROR_CODE_FPGA_COMM_FAILED: + ss << "Server failed to communicate with FPGA"; + break; + case PTC_ERROR_CODE_OTHER: + ss << "Unspecified internal error"; + break; + default: + ss << "Unknown error"; + break; + } + + if (!error_flags) { + return ss.str(); + } + + if (ptc_error) { + ss << ", "; + } + + ss << "Communication error: "; + std::vector nebula_errors; + + if (error_flags & TCP_ERROR_INCOMPLETE_RESPONSE) { + nebula_errors.emplace_back("Incomplete response payload"); + } + if (error_flags & TCP_ERROR_TIMEOUT) { + nebula_errors.emplace_back("Request timeout"); + } + if (error_flags & TCP_ERROR_UNEXPECTED_PAYLOAD) { + nebula_errors.emplace_back("Received payload but expected payload length 0"); + } + if (error_flags & TCP_ERROR_UNRELATED_RESPONSE) { + nebula_errors.emplace_back("Received unrelated response"); + } + + ss << boost::algorithm::join(nebula_errors, ", "); + + return ss.str(); +} + +template +T HesaiHwInterface::CheckSizeAndParse(const std::vector & data) +{ + if (data.size() < sizeof(T)) { + throw std::runtime_error("Attempted to parse too-small payload"); + } else if (data.size() > sizeof(T)) { + PrintError("Sensor returned longer payload than expected. Will parse anyway."); + } + + T parsed; + memcpy(&parsed, data.data(), sizeof(T)); + return parsed; +} + +} // namespace nebula::drivers diff --git a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp index d5e3e08bb..d8d972488 100644 --- a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp @@ -1,102 +1,34 @@ +// Copyright 2024 TIER IV, Inc. + #include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" namespace nebula { namespace drivers { RobosenseHwInterface::RobosenseHwInterface() -: cloud_io_context_{new ::drivers::common::IoContext(1)}, - info_io_context_{new ::drivers::common::IoContext(1)}, - cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, - info_udp_driver_{new ::drivers::udp_driver::UdpDriver(*info_io_context_)}, - scan_cloud_ptr_{std::make_unique()} +: cloud_io_context_(new ::drivers::common::IoContext(1)), + info_io_context_(new ::drivers::common::IoContext(1)), + cloud_udp_driver_(new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)), + info_udp_driver_(new ::drivers::udp_driver::UdpDriver(*info_io_context_)) { } -void RobosenseHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +void RobosenseHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { - int scan_phase = static_cast(sensor_configuration_->scan_phase * 100.0); - if (!is_valid_packet_(buffer.size())) { - PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); + if (!scan_reception_callback_) { return; } - // Copy data - uint32_t buffer_size = buffer.size(); - std::array packet_data{}; - std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, packet_data.begin()); - robosense_msgs::msg::RobosensePacket msop_packet; - msop_packet.data = packet_data; - - // Add timestamp (Sensor timestamp will be handled by decoder) - const auto now = std::chrono::system_clock::now(); - const auto timestamp_ns = - std::chrono::duration_cast(now.time_since_epoch()).count(); - - constexpr int nanosec_per_sec = 1000000000; - msop_packet.stamp.sec = static_cast(timestamp_ns / nanosec_per_sec); - msop_packet.stamp.nanosec = static_cast(timestamp_ns % nanosec_per_sec); - - if ( - !sensor_model_.has_value() && - sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL) { - if (buffer[32] == BPEARL_V4_FLAG) { - sensor_model_.emplace(drivers::SensorModel::ROBOSENSE_BPEARL_V4); - PrintInfo("Bpearl V4 detected."); - } else { - sensor_model_.emplace(drivers::SensorModel::ROBOSENSE_BPEARL_V3); - PrintInfo("Bpearl V3 detected."); - } - } - - scan_cloud_ptr_->packets.emplace_back(msop_packet); - - int current_phase{}; - bool comp_flg = false; - - const auto & data = scan_cloud_ptr_->packets.back().data; - current_phase = (data[azimuth_index_ + 1] & 0xff) + ((data[azimuth_index_] & 0xff) << 8); - - current_phase = (static_cast(current_phase) + 36000 - scan_phase) % 36000; - - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; - } else { - comp_flg = true; - } - if (comp_flg) { // Scan complete - if (scan_reception_callback_) { - scan_cloud_ptr_->header.stamp = scan_cloud_ptr_->packets.front().stamp; - // Callback - scan_reception_callback_(std::move(scan_cloud_ptr_)); - scan_cloud_ptr_ = std::make_unique(); - } - } + scan_reception_callback_(buffer); } -void RobosenseHwInterface::ReceiveInfoPacketCallback(const std::vector & buffer) +void RobosenseHwInterface::ReceiveInfoPacketCallback(std::vector & buffer) { - if (!is_valid_info_packet_(buffer.size())) { - PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); + if (!info_reception_callback_) { return; } - info_buffer_.emplace(buffer); ////// - is_info_received = true; //////// - - if (info_reception_callback_) { - std::unique_ptr difop_packet = - std::make_unique(); - std::copy_n( - std::make_move_iterator(buffer.begin()), buffer.size(), difop_packet->packet.data.begin()); - - if (sensor_model_.has_value()) { - difop_packet->lidar_model = SensorModelToString(sensor_model_.value()); - } else { - difop_packet->lidar_model = SensorModelToString(sensor_configuration_->sensor_model); - } - - info_reception_callback_(std::move(difop_packet)); - } + info_reception_callback_(buffer); } Status RobosenseHwInterface::SensorInterfaceStart() @@ -124,7 +56,7 @@ Status RobosenseHwInterface::InfoInterfaceStart() try { std::cout << "Starting UDP server for info packets on: " << *sensor_configuration_ << std::endl; PrintInfo( - "Starting UDP server for info packets on: " + sensor_configuration_->sensor_ip + ":" + + "Starting UDP server for info packets on: " + sensor_configuration_->sensor_ip + ": " + std::to_string(sensor_configuration_->gnss_port)); info_udp_driver_->init_receiver( sensor_configuration_->host_ip, sensor_configuration_->gnss_port); @@ -133,7 +65,6 @@ Status RobosenseHwInterface::InfoInterfaceStart() info_udp_driver_->receiver()->asyncReceive( std::bind(&RobosenseHwInterface::ReceiveInfoPacketCallback, this, std::placeholders::_1)); - } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; std::cerr << status << sensor_configuration_->sensor_ip << "," @@ -141,74 +72,32 @@ Status RobosenseHwInterface::InfoInterfaceStart() return status; } - std::this_thread::sleep_for(std::chrono::seconds(1)); return Status::OK; } -Status RobosenseHwInterface::SensorInterfaceStop() -{ - return Status::ERROR_1; -} - Status RobosenseHwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr sensor_configuration) { - Status status = Status::OK; - - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - - if ( - sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL || - sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL_V3 || - sensor_configuration_->sensor_model == SensorModel::ROBOSENSE_BPEARL_V4) { - azimuth_index_ = 44; - is_valid_packet_ = [](size_t packet_size) { return (packet_size == BPEARL_PACKET_SIZE); }; - is_valid_info_packet_ = [](size_t packet_size) { - return (packet_size == BPEARL_INFO_PACKET_SIZE); - }; - } else if (sensor_configuration->sensor_model == SensorModel::ROBOSENSE_HELIOS) { - azimuth_index_ = 44; - is_valid_packet_ = [](size_t packet_size) { return (packet_size == HELIOS_PACKET_SIZE); }; - is_valid_info_packet_ = [](size_t packet_size) { - return (packet_size == HELIOS_INFO_PACKET_SIZE); - }; - } else { - status = Status::INVALID_SENSOR_MODEL; - } - } catch (const std::exception & ex) { - status = Status::SENSOR_CONFIG_ERROR; - std::cerr << status << std::endl; - return status; + if (!(sensor_configuration->sensor_model == SensorModel::ROBOSENSE_BPEARL_V3 || + sensor_configuration->sensor_model == SensorModel::ROBOSENSE_BPEARL_V4 || + sensor_configuration->sensor_model == SensorModel::ROBOSENSE_HELIOS)) { + return Status::INVALID_SENSOR_MODEL; } - return status; -} -Status RobosenseHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - PrintDebug(ss.str()); - return Status::ERROR_1; -} + sensor_configuration_ = sensor_configuration; -Status RobosenseHwInterface::GetCalibrationConfiguration( - CalibrationConfigurationBase & calibration_configuration) -{ - PrintDebug(calibration_configuration.calibration_file); - return Status::ERROR_1; + return Status::OK; } Status RobosenseHwInterface::RegisterScanCallback( - std::function)> scan_callback) + std::function &)> scan_callback) { scan_reception_callback_ = std::move(scan_callback); return Status::OK; } Status RobosenseHwInterface::RegisterInfoCallback( - std::function)> info_callback) + std::function &)> info_callback) { info_reception_callback_ = std::move(info_callback); return Status::OK; diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index 8a8bfefbe..40ba0f754 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -1,3 +1,5 @@ +// Copyright 2024 TIER IV, Inc. + #include "nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp" namespace nebula @@ -7,33 +9,51 @@ namespace drivers VelodyneHwInterface::VelodyneHwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, - scan_cloud_ptr_{std::make_unique()}, boost_ctx_{new boost::asio::io_context()}, http_client_driver_{new ::drivers::tcp_driver::HttpClientDriver(boost_ctx_)} { } -Status VelodyneHwInterface::InitializeSensorConfiguration( - std::shared_ptr sensor_configuration) +std::string VelodyneHwInterface::HttpGetRequest(const std::string & endpoint) { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100); + std::lock_guard lock(mtx_inflight_request_); + if (!http_client_driver_->client()->isOpen()) { + http_client_driver_->client()->open(); + } - GetDiagAsync(); - GetStatusAsync(); - Status status = Status::OK; - return status; + std::string response = http_client_driver_->get(endpoint); + http_client_driver_->client()->close(); + return response; +} + +std::string VelodyneHwInterface::HttpPostRequest( + const std::string & endpoint, const std::string & body) +{ + std::lock_guard lock(mtx_inflight_request_); + if (!http_client_driver_->client()->isOpen()) { + http_client_driver_->client()->open(); + } + + std::string response = http_client_driver_->post(endpoint, body); + http_client_driver_->client()->close(); + return response; +} + +Status VelodyneHwInterface::InitializeSensorConfiguration( + std::shared_ptr sensor_configuration) +{ + sensor_configuration_ = sensor_configuration; + return Status::OK; } Status VelodyneHwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr sensor_configuration) { - auto velodyne_sensor_configuration = - std::static_pointer_cast(sensor_configuration); - VelodyneStatus status = CheckAndSetConfigBySnapshotAsync(velodyne_sensor_configuration); - Status rt = status; - return rt; + auto snapshot = GetSnapshot(); + auto tree = ParseJson(snapshot); + VelodyneStatus status = CheckAndSetConfig(sensor_configuration, tree); + + return status; } Status VelodyneHwInterface::SensorInterfaceStart() @@ -55,49 +75,19 @@ Status VelodyneHwInterface::SensorInterfaceStart() } Status VelodyneHwInterface::RegisterScanCallback( - std::function)> scan_callback) + std::function & packet)> scan_callback) { - scan_reception_callback_ = std::move(scan_callback); + cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } -void VelodyneHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) -{ - // Process current packet - const uint32_t buffer_size = buffer.size(); - velodyne_msgs::msg::VelodynePacket velodyne_packet; - std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, velodyne_packet.data.begin()); - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - velodyne_packet.stamp.sec = static_cast(now_secs); - velodyne_packet.stamp.nanosec = static_cast(now_nanosecs % 1'000'000'000); - scan_cloud_ptr_->packets.emplace_back(velodyne_packet); - processed_packets_++; - - // Check if scan is complete - packet_first_azm_ = scan_cloud_ptr_->packets.back().data[2]; // lower word of azimuth block 0 - packet_first_azm_ |= scan_cloud_ptr_->packets.back().data[3] - << 8; // higher word of azimuth block 0 - - packet_last_azm_ = scan_cloud_ptr_->packets.back().data[1102]; - packet_last_azm_ |= scan_cloud_ptr_->packets.back().data[1103] << 8; - - packet_first_azm_phased_ = (36000 + packet_first_azm_ - phase_) % 36000; - packet_last_azm_phased_ = (36000 + packet_last_azm_ - phase_) % 36000; - - if (processed_packets_ > 1) { - if ( - packet_last_azm_phased_ < packet_first_azm_phased_ || - packet_first_azm_phased_ < prev_packet_first_azm_phased_) { - // Callback - scan_reception_callback_(std::move(scan_cloud_ptr_)); - scan_cloud_ptr_ = std::make_unique(); - processed_packets_ = 0; - } +void VelodyneHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) +{ + if (!cloud_packet_callback_) { + return; } - prev_packet_first_azm_phased_ = packet_first_azm_phased_; + + cloud_packet_callback_(buffer); } Status VelodyneHwInterface::SensorInterfaceStop() { @@ -112,13 +102,6 @@ Status VelodyneHwInterface::GetSensorConfiguration(SensorConfigurationBase & sen return Status::ERROR_1; } -Status VelodyneHwInterface::GetCalibrationConfiguration( - CalibrationConfigurationBase & calibration_configuration) -{ - PrintDebug(calibration_configuration.calibration_file); - return Status::ERROR_1; -} - VelodyneStatus VelodyneHwInterface::InitHttpClient() { try { @@ -133,42 +116,6 @@ VelodyneStatus VelodyneHwInterface::InitHttpClient() return Status::OK; } -VelodyneStatus VelodyneHwInterface::InitHttpClientAsync() -{ - try { - http_client_driver_->init_client(sensor_configuration_->sensor_ip, 80); - } catch (const std::exception & ex) { - VelodyneStatus status = Status::HTTP_CONNECTION_ERROR; - return status; - } - return Status::OK; -} - -VelodyneStatus VelodyneHwInterface::GetHttpClientDriverOnce( - std::shared_ptr ctx, - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd) -{ - hcd = std::unique_ptr<::drivers::tcp_driver::HttpClientDriver>( - new ::drivers::tcp_driver::HttpClientDriver(ctx)); - try { - hcd->init_client(sensor_configuration_->sensor_ip, 80); - } catch (const std::exception & ex) { - Status status = Status::HTTP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," << 80 << std::endl; - return Status::HTTP_CONNECTION_ERROR; - } - return Status::OK; -} - -VelodyneStatus VelodyneHwInterface::GetHttpClientDriverOnce( - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd) -{ - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd_tmp; - auto st = GetHttpClientDriverOnce(std::make_shared(), hcd_tmp); - hcd = std::move(hcd_tmp); - return st; -} - void VelodyneHwInterface::StringCallback(const std::string & str) { std::cout << "VelodyneHwInterface::StringCallback: " << str << std::endl; @@ -182,21 +129,26 @@ boost::property_tree::ptree VelodyneHwInterface::ParseJson(const std::string & s ss << str; boost::property_tree::read_json(ss, tree); } catch (boost::property_tree::json_parser_error & e) { - std::cerr << "Error on ParseJson:" << e.what() << std::endl; + std::cerr << "Error on ParseJson: " << e.what() << std::endl; } return tree; } VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, boost::property_tree::ptree tree) { + VelodyneStatus status; + const auto & OK = VelodyneStatus::OK; + std::string target_key = "config.returns"; auto current_return_mode_str = tree.get(target_key); auto current_return_mode = nebula::drivers::ReturnModeFromStringVelodyne(tree.get(target_key)); if (sensor_configuration->return_mode != current_return_mode) { - SetReturnTypeAsync(sensor_configuration->return_mode); + status = SetReturnType(sensor_configuration->return_mode); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_return_mode_str << std::endl; std::cout << "current_return_mode: " << current_return_mode << std::endl; @@ -207,7 +159,9 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.rpm"; auto current_rotation_speed = tree.get(target_key); if (sensor_configuration->rotation_speed != current_rotation_speed) { - SetRpmAsync(sensor_configuration->rotation_speed); + status = SetRpm(sensor_configuration->rotation_speed); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_rotation_speed << std::endl; std::cout << "sensor_configuration->rotation_speed: " << sensor_configuration->rotation_speed @@ -222,7 +176,9 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( setting_cloud_min_angle = 359; } if (setting_cloud_min_angle != current_cloud_min_angle) { - SetFovStartAsync(setting_cloud_min_angle); + status = SetFovStart(setting_cloud_min_angle); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_cloud_min_angle << std::endl; std::cout << "sensor_configuration->cloud_min_angle: " << setting_cloud_min_angle << std::endl; @@ -236,7 +192,9 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( setting_cloud_max_angle = 359; } if (setting_cloud_max_angle != current_cloud_max_angle) { - SetFovEndAsync(setting_cloud_max_angle); + status = SetFovEnd(setting_cloud_max_angle); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_cloud_max_angle << std::endl; std::cout << "sensor_configuration->cloud_max_angle: " << setting_cloud_max_angle << std::endl; @@ -245,7 +203,9 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.addr"; auto current_host_addr = tree.get(target_key); if (sensor_configuration->host_ip != current_host_addr) { - SetHostAddrAsync(sensor_configuration->host_ip); + status = SetHostAddr(sensor_configuration->host_ip); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_addr << std::endl; std::cout << "sensor_configuration->host_ip: " << sensor_configuration->host_ip << std::endl; @@ -254,7 +214,9 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.dport"; auto current_host_dport = tree.get(target_key); if (sensor_configuration->data_port != current_host_dport) { - SetHostDportAsync(sensor_configuration->data_port); + status = SetHostDport(sensor_configuration->data_port); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_dport << std::endl; std::cout << "sensor_configuration->data_port: " << sensor_configuration->data_port @@ -264,46 +226,35 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.tport"; auto current_host_tport = tree.get(target_key); if (sensor_configuration->gnss_port != current_host_tport) { - SetHostTportAsync(sensor_configuration->gnss_port); + status = SetHostTport(sensor_configuration->gnss_port); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_tport << std::endl; std::cout << "sensor_configuration->gnss_port: " << sensor_configuration->gnss_port << std::endl; } - return Status::WAITING_FOR_SENSOR_RESPONSE; + return OK; } // sync std::string VelodyneHwInterface::GetStatus() { - auto rt = http_client_driver_->get(TARGET_STATUS); - http_client_driver_->client()->close(); - // str_cb(rt); - // return Status::OK; - return rt; + return HttpGetRequest(TARGET_STATUS); } std::string VelodyneHwInterface::GetDiag() { - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - std::cout << "GetHttpClientDriverOnce" << std::endl; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return ""; - } - auto rt = hcd->get(TARGET_DIAG); + auto rt = HttpGetRequest(TARGET_DIAG); std::cout << "read_response: " << rt << std::endl; return rt; } std::string VelodyneHwInterface::GetSnapshot() { - auto rt = http_client_driver_->get(TARGET_SNAPSHOT); - http_client_driver_->client()->close(); - return rt; + return HttpGetRequest(TARGET_SNAPSHOT); } VelodyneStatus VelodyneHwInterface::SetRpm(uint16_t rpm) @@ -311,8 +262,7 @@ VelodyneStatus VelodyneHwInterface::SetRpm(uint16_t rpm) if (rpm < 300 || 1200 < rpm || rpm % 60 != 0) { return VelodyneStatus::INVALID_RPM_ERROR; } - auto rt = http_client_driver_->post(TARGET_SETTING, (boost::format("rpm=%d") % rpm).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SETTING, (boost::format("rpm=%d") % rpm).str()); StringCallback(rt); return Status::OK; } @@ -322,8 +272,7 @@ VelodyneStatus VelodyneHwInterface::SetFovStart(uint16_t fov_start) if (359 < fov_start) { return VelodyneStatus::INVALID_FOV_ERROR; } - auto rt = http_client_driver_->post(TARGET_FOV, (boost::format("start=%d") % fov_start).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_FOV, (boost::format("start=%d") % fov_start).str()); StringCallback(rt); return Status::OK; } @@ -333,8 +282,7 @@ VelodyneStatus VelodyneHwInterface::SetFovEnd(uint16_t fov_end) if (359 < fov_end) { return VelodyneStatus::INVALID_FOV_ERROR; } - auto rt = http_client_driver_->post(TARGET_FOV, (boost::format("end=%d") % fov_end).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_FOV, (boost::format("end=%d") % fov_end).str()); StringCallback(rt); return Status::OK; } @@ -355,8 +303,7 @@ VelodyneStatus VelodyneHwInterface::SetReturnType(nebula::drivers::ReturnMode re default: return VelodyneStatus::INVALID_RETURN_MODE_ERROR; } - auto rt = http_client_driver_->post(TARGET_SETTING, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SETTING, body_str); StringCallback(rt); return Status::OK; } @@ -364,8 +311,7 @@ VelodyneStatus VelodyneHwInterface::SetReturnType(nebula::drivers::ReturnMode re VelodyneStatus VelodyneHwInterface::SaveConfig() { std::string body_str = "submit"; - auto rt = http_client_driver_->post(TARGET_SAVE, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SAVE, body_str); StringCallback(rt); return Status::OK; } @@ -373,8 +319,7 @@ VelodyneStatus VelodyneHwInterface::SaveConfig() VelodyneStatus VelodyneHwInterface::ResetSystem() { std::string body_str = "reset_system"; - auto rt = http_client_driver_->post(TARGET_RESET, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_RESET, body_str); StringCallback(rt); return Status::OK; } @@ -382,8 +327,7 @@ VelodyneStatus VelodyneHwInterface::ResetSystem() VelodyneStatus VelodyneHwInterface::LaserOn() { std::string body_str = "laser=on"; - auto rt = http_client_driver_->post(TARGET_SETTING, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SETTING, body_str); StringCallback(rt); return Status::OK; } @@ -391,8 +335,7 @@ VelodyneStatus VelodyneHwInterface::LaserOn() VelodyneStatus VelodyneHwInterface::LaserOff() { std::string body_str = "laser=off"; - auto rt = http_client_driver_->post(TARGET_SETTING, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SETTING, body_str); StringCallback(rt); return Status::OK; } @@ -400,417 +343,61 @@ VelodyneStatus VelodyneHwInterface::LaserOff() VelodyneStatus VelodyneHwInterface::LaserOnOff(bool on) { std::string body_str = (boost::format("laser=%s") % (on ? "on" : "off")).str(); - auto rt = http_client_driver_->post(TARGET_SETTING, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SETTING, body_str); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetHostAddr(std::string addr) { - auto rt = http_client_driver_->post(TARGET_HOST, (boost::format("addr=%s") % addr).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_HOST, (boost::format("addr=%s") % addr).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetHostDport(uint16_t dport) { - auto rt = http_client_driver_->post(TARGET_HOST, (boost::format("dport=%d") % dport).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_HOST, (boost::format("dport=%d") % dport).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetHostTport(uint16_t tport) { - auto rt = http_client_driver_->post(TARGET_HOST, (boost::format("tport=%d") % tport).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_HOST, (boost::format("tport=%d") % tport).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetAddr(std::string addr) { - auto rt = http_client_driver_->post(TARGET_NET, (boost::format("addr=%s") % addr).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_NET, (boost::format("addr=%s") % addr).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetMask(std::string mask) { - auto rt = http_client_driver_->post(TARGET_NET, (boost::format("mask=%s") % mask).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_NET, (boost::format("mask=%s") % mask).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetGateway(std::string gateway) { - auto rt = http_client_driver_->post(TARGET_NET, (boost::format("gateway=%s") % gateway).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_NET, (boost::format("gateway=%s") % gateway).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetDhcp(bool use_dhcp) { - auto rt = http_client_driver_->post( - TARGET_NET, (boost::format("dhcp=%s") % (use_dhcp ? "on" : "off")).str()); - http_client_driver_->client()->close(); + auto rt = + HttpPostRequest(TARGET_NET, (boost::format("dhcp=%s") % (use_dhcp ? "on" : "off")).str()); StringCallback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::GetStatusAsync( - std::function str_callback) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - hcd->asyncGet(str_callback, TARGET_STATUS); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::GetStatusAsync() -{ - return GetStatusAsync([this](const std::string & str) { StringCallback(str); }); -} - -VelodyneStatus VelodyneHwInterface::GetDiagAsync( - std::function str_callback) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - hcd->asyncGet(str_callback, TARGET_DIAG); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::GetDiagAsync() -{ - return GetDiagAsync([this](const std::string & str) { StringCallback(str); }); -} - -VelodyneStatus VelodyneHwInterface::GetSnapshotAsync( - std::function str_callback) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - hcd->asyncGet(str_callback, TARGET_SNAPSHOT); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::GetSnapshotAsync() -{ - return GetSnapshotAsync([this](const std::string & str) { ParseJson(str); }); -} - -VelodyneStatus VelodyneHwInterface::CheckAndSetConfigBySnapshotAsync( - std::shared_ptr sensor_configuration) -{ - sensor_configuration_ = sensor_configuration; - - return GetSnapshotAsync([this](const std::string & str) { - auto tree = ParseJson(str); - std::cout << "ParseJson OK\n"; - CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), tree); - }); -} - -VelodyneStatus VelodyneHwInterface::SetRpmAsync(uint16_t rpm) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - if (rpm < 300 || 1200 < rpm || rpm % 60 != 0) { - return VelodyneStatus::INVALID_RPM_ERROR; - } - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_SETTING, - (boost::format("rpm=%d") % rpm).str()); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SetFovStartAsync(uint16_t fov_start) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - if (359 < fov_start) { - return VelodyneStatus::INVALID_FOV_ERROR; - } - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_FOV, - (boost::format("start=%d") % fov_start).str()); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SetFovEndAsync(uint16_t fov_end) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - if (359 < fov_end) { - return VelodyneStatus::INVALID_FOV_ERROR; - } - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_FOV, - (boost::format("end=%d") % fov_end).str()); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SetReturnTypeAsync(nebula::drivers::ReturnMode return_mode) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - std::string body_str = ""; - switch (return_mode) { - case nebula::drivers::ReturnMode::SINGLE_STRONGEST: - body_str = "returns=Strongest"; - break; - case nebula::drivers::ReturnMode::SINGLE_LAST: - body_str = "returns=Last"; - break; - case nebula::drivers::ReturnMode::DUAL_ONLY: - body_str = "returns=Dual"; - break; - default: - return VelodyneStatus::INVALID_RETURN_MODE_ERROR; - } - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_SETTING, body_str); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SaveConfigAsync() -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - std::string body_str = "submit"; - hcd->asyncPost([this](const std::string & str) { StringCallback(str); }, TARGET_SAVE, body_str); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::ResetSystemAsync() -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - std::string body_str = "reset_system"; - hcd->asyncPost([this](const std::string & str) { StringCallback(str); }, TARGET_RESET, body_str); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::LaserOnAsync() -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - std::string body_str = "laser=on"; - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_SETTING, body_str); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::LaserOffAsync() -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - std::string body_str = "laser=off"; - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_SETTING, body_str); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::LaserOnOffAsync(bool on) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - std::string body_str = (boost::format("laser=%s") % (on ? "on" : "off")).str(); - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_SETTING, body_str); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SetHostAddrAsync(std::string addr) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_HOST, - (boost::format("addr=%s") % addr).str()); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SetHostDportAsync(uint16_t dport) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_HOST, - (boost::format("dport=%d") % dport).str()); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SetHostTportAsync(uint16_t tport) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_HOST, - (boost::format("tport=%d") % tport).str()); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SetNetAddrAsync(std::string addr) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_NET, - (boost::format("addr=%s") % addr).str()); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SetNetMaskAsync(std::string mask) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_NET, - (boost::format("mask=%s") % mask).str()); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SetNetGatewayAsync(std::string gateway) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_NET, - (boost::format("gateway=%s") % gateway).str()); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - -VelodyneStatus VelodyneHwInterface::SetNetDhcpAsync(bool use_dhcp) -{ - auto ctx = std::make_shared(); - std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; - auto st = GetHttpClientDriverOnce(ctx, hcd); - if (st != Status::OK) { - return st; - } - - hcd->asyncPost( - [this](const std::string & str) { StringCallback(str); }, TARGET_NET, - (boost::format("dhcp=%s") % (use_dhcp ? "on" : "off")).str()); - ctx->run(); - return Status::WAITING_FOR_SENSOR_RESPONSE; -} - void VelodyneHwInterface::SetLogger(std::shared_ptr logger) { parent_node_logger = logger; diff --git a/nebula_messages/continental_msgs/CMakeLists.txt b/nebula_messages/continental_msgs/CMakeLists.txt index c7b7978f6..724189b70 100644 --- a/nebula_messages/continental_msgs/CMakeLists.txt +++ b/nebula_messages/continental_msgs/CMakeLists.txt @@ -19,6 +19,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ContinentalArs548DetectionList.msg" "msg/ContinentalArs548Object.msg" "msg/ContinentalArs548ObjectList.msg" + "msg/ContinentalSrr520Detection.msg" + "msg/ContinentalSrr520DetectionList.msg" + "msg/ContinentalSrr520Object.msg" + "msg/ContinentalSrr520ObjectList.msg" DEPENDENCIES std_msgs geometry_msgs diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg new file mode 100644 index 000000000..9536ffa12 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg @@ -0,0 +1,11 @@ +float32 range +float32 azimuth_angle +float32 range_rate +float32 rcs +uint8 pdh00 +uint8 pdh01 +uint8 pdh02 +uint8 pdh03 +uint8 pdh04 +uint8 pdh05 +float32 snr diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg new file mode 100644 index 000000000..2bd8f85be --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg @@ -0,0 +1,10 @@ +std_msgs/Header header +uint32 internal_time_stamp_usec +uint8 global_time_stamp_sync_status +uint8 signal_status +uint8 sequence_counter +uint32 cycle_counter +float32 v_ambiguous +float32 max_range + +ContinentalSrr520Detection[] detections diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg new file mode 100644 index 000000000..a12787fa9 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg @@ -0,0 +1,21 @@ +uint8 object_id +float32 dist_x +float32 dist_y +float32 v_abs_x +float32 v_abs_y +float32 a_abs_x +float32 a_abs_y +float32 dist_x_std +float32 dist_y_std +float32 v_abs_x_std +float32 v_abs_y_std +float32 a_abs_x_std +float32 a_abs_y_std +float32 box_length +float32 box_width +float32 orientation +float32 rcs +float32 score +uint16 life_cycles +uint8 box_valid +uint8 object_status diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg new file mode 100644 index 000000000..f873eb05a --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg @@ -0,0 +1,12 @@ +std_msgs/Header header + +uint32 internal_time_stamp_usec +uint8 global_time_stamp_sync_status +uint8 signal_status +uint8 sequence_counter +uint32 cycle_counter +float32 ego_vx +float32 ego_yaw_rate +uint8 motion_type + +ContinentalSrr520Object[] objects diff --git a/nebula_messages/continental_msgs/package.xml b/nebula_messages/continental_msgs/package.xml index 5bb32629e..a849e1509 100644 --- a/nebula_messages/continental_msgs/package.xml +++ b/nebula_messages/continental_msgs/package.xml @@ -2,7 +2,7 @@ continental_msgs - 0.1.0 + 0.2.0 Messages for Continental sensors Kenzo Lobos-Tsunekawa diff --git a/nebula_messages/continental_srvs/CMakeLists.txt b/nebula_messages/continental_srvs/CMakeLists.txt index ffa839825..b45d309f2 100644 --- a/nebula_messages/continental_srvs/CMakeLists.txt +++ b/nebula_messages/continental_srvs/CMakeLists.txt @@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ContinentalArs548SetVehicleParameters.srv" "srv/ContinentalArs548SetRadarParameters.srv" "srv/ContinentalArs548SetNetworkConfiguration.srv" + "srv/ContinentalSrr520SetRadarParameters.srv" DEPENDENCIES std_msgs ) diff --git a/nebula_messages/continental_srvs/package.xml b/nebula_messages/continental_srvs/package.xml index 503143c73..160b3178d 100644 --- a/nebula_messages/continental_srvs/package.xml +++ b/nebula_messages/continental_srvs/package.xml @@ -2,7 +2,7 @@ continental_srvs - 0.1.0 + 0.2.0 Services for Continental sensors Kenzo Lobos-Tsunekawa diff --git a/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv b/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv new file mode 100644 index 000000000..4d0d3b297 --- /dev/null +++ b/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv @@ -0,0 +1,15 @@ +bool autoconfigure_extrinsics 1 +float32 longitudinal +float32 lateral +float32 vertical +float32 yaw +float32 pitch +float32 vehicle_wheelbase -1.0 + +uint8 sensor_id # The new sensor id +float32 cover_damping # Cover damping in dB +bool plug_bottom # Whether the plug is in the bottom +bool reset_sensor_configuration +--- +bool success +string message # status messages diff --git a/nebula_messages/nebula_msgs/package.xml b/nebula_messages/nebula_msgs/package.xml index cdd135c37..48d4c2daa 100644 --- a/nebula_messages/nebula_msgs/package.xml +++ b/nebula_messages/nebula_msgs/package.xml @@ -2,7 +2,7 @@ nebula_msgs - 0.1.0 + 0.2.0 Generic sensor raw data messages for Nebula Kenzo Lobos-Tsunekawa diff --git a/nebula_messages/pandar_msgs/package.xml b/nebula_messages/pandar_msgs/package.xml index 071ab79bc..289a903b8 100644 --- a/nebula_messages/pandar_msgs/package.xml +++ b/nebula_messages/pandar_msgs/package.xml @@ -1,7 +1,7 @@ pandar_msgs - 0.0.0 + 0.2.0 ROS message definition for the Hesai PandarQT/Pandar64/Pandar40P/Pandar20A/Pandar20B/Pandar40M LiDAR sensor. diff --git a/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg b/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg index acf5751bd..944d41abf 100644 --- a/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg +++ b/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg @@ -1,2 +1,2 @@ string lidar_model -RobosensePacket packet \ No newline at end of file +RobosensePacket packet diff --git a/nebula_messages/robosense_msgs/msg/RobosensePacket.msg b/nebula_messages/robosense_msgs/msg/RobosensePacket.msg index 713682931..40b73bd17 100644 --- a/nebula_messages/robosense_msgs/msg/RobosensePacket.msg +++ b/nebula_messages/robosense_msgs/msg/RobosensePacket.msg @@ -1,2 +1,2 @@ builtin_interfaces/Time stamp -uint8[1248] data \ No newline at end of file +uint8[1248] data diff --git a/nebula_messages/robosense_msgs/msg/RobosenseScan.msg b/nebula_messages/robosense_msgs/msg/RobosenseScan.msg index 0c2b5518b..25bd36a37 100644 --- a/nebula_messages/robosense_msgs/msg/RobosenseScan.msg +++ b/nebula_messages/robosense_msgs/msg/RobosenseScan.msg @@ -1,2 +1,2 @@ std_msgs/Header header -RobosensePacket[] packets \ No newline at end of file +RobosensePacket[] packets diff --git a/nebula_messages/robosense_msgs/package.xml b/nebula_messages/robosense_msgs/package.xml index 29077edb2..9b12e070e 100644 --- a/nebula_messages/robosense_msgs/package.xml +++ b/nebula_messages/robosense_msgs/package.xml @@ -1,22 +1,22 @@ - robosense_msgs - 0.0.0 - Robosense message types for Nebula - Mehmet Emin BASOGLU - TODO: License declaration + robosense_msgs + 0.2.0 + Robosense message types for Nebula + Mehmet Emin BASOGLU + TODO: License declaration - ament_cmake_auto - rosidl_default_generators + ament_cmake_auto + rosidl_default_generators - builtin_interfaces - std_msgs + builtin_interfaces + std_msgs - rosidl_default_runtime - rosidl_interface_packages + rosidl_default_runtime + rosidl_interface_packages - - ament_cmake - + + ament_cmake + diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index dd81e1ac0..709de2025 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -1,10 +1,6 @@ cmake_minimum_required(VERSION 3.14) project(nebula_ros) -find_package(ament_cmake_auto REQUIRED) - -ament_auto_find_build_dependencies() - # Default to C++17 if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -14,153 +10,258 @@ if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function) endif () -find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) +find_package(ament_cmake_auto REQUIRED) +find_package(PCL REQUIRED components common) +find_package(continental_msgs REQUIRED) +find_package(continental_srvs REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(diagnostic_updater REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nebula_common REQUIRED) find_package(nebula_decoders REQUIRED) find_package(nebula_hw_interfaces REQUIRED) -find_package(yaml-cpp REQUIRED) +find_package(nebula_msgs REQUIRED) +find_package(pandar_msgs REQUIRED) +find_package(radar_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(robosense_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(yaml-cpp REQUIRED) include_directories( - include - SYSTEM - ${YAML_CPP_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${PCL_COMMON_INCLUDE_DIRS} + include + SYSTEM + ${nebula_common_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${rclcpp_components_INCLUDE_DIRS} ) -link_libraries(${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES}) + +link_libraries( + ${nebula_common_TARGETS} + ${YAML_CPP_LIBRARIES} + ${PCL_LIBRARIES} +) + ## Hesai -# Hw Interface -ament_auto_add_library(hesai_hw_ros_wrapper SHARED - src/hesai/hesai_hw_interface_ros_wrapper.cpp - ) - -rclcpp_components_register_node(hesai_hw_ros_wrapper - PLUGIN "HesaiHwInterfaceRosWrapper" - EXECUTABLE hesai_hw_interface_ros_wrapper_node - ) - -# Monitor -ament_auto_add_library(hesai_hw_monitor_ros_wrapper SHARED - src/hesai/hesai_hw_monitor_ros_wrapper.cpp - ) - -rclcpp_components_register_node(hesai_hw_monitor_ros_wrapper - PLUGIN "HesaiHwMonitorRosWrapper" - EXECUTABLE hesai_hw_monitor_ros_wrapper_node - ) - -# DriverDecoder -ament_auto_add_library(hesai_driver_ros_wrapper SHARED - src/hesai/hesai_decoder_ros_wrapper.cpp - ) - -rclcpp_components_register_node(hesai_driver_ros_wrapper - PLUGIN "HesaiDriverRosWrapper" - EXECUTABLE hesai_driver_ros_wrapper_node - ) +add_library(hesai_ros_wrapper SHARED + src/hesai/hesai_ros_wrapper.cpp + src/hesai/decoder_wrapper.cpp + src/hesai/hw_interface_wrapper.cpp + src/hesai/hw_monitor_wrapper.cpp + src/common/parameter_descriptors.cpp +) + +target_include_directories(hesai_ros_wrapper PUBLIC + ${diagnostic_updater_INCLUDE_DIRS} + ${nebula_decoders_INCLUDE_DIRS} + ${nebula_hw_interfaces_INCLUDE_DIRS} + ${pandar_msgs_INCLUDE_DIRS} +) + +target_link_libraries(hesai_ros_wrapper PUBLIC + ${diagnostic_msgs_TARGETS} + ${diagnostic_updater_TARGETS} + ${pandar_msgs_TARGETS} + nebula_decoders::nebula_decoders_hesai + nebula_hw_interfaces::nebula_hw_interfaces_hesai +) + + +rclcpp_components_register_node(hesai_ros_wrapper + PLUGIN "HesaiRosWrapper" + EXECUTABLE hesai_ros_wrapper_node +) ## Velodyne -# Hw Interface -ament_auto_add_library(velodyne_hw_ros_wrapper SHARED - src/velodyne/velodyne_hw_interface_ros_wrapper.cpp - ) -rclcpp_components_register_node(velodyne_hw_ros_wrapper - PLUGIN "VelodyneHwInterfaceRosWrapper" - EXECUTABLE velodyne_hw_ros_wrapper_node - ) - - -# Monitor -ament_auto_add_library(velodyne_hw_monitor_ros_wrapper SHARED - src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp - ) -rclcpp_components_register_node(velodyne_hw_monitor_ros_wrapper - PLUGIN "VelodyneHwMonitorRosWrapper" - EXECUTABLE velodyne_hw_monitor_ros_wrapper_node - ) - -# DriverDecoder -ament_auto_add_library(velodyne_driver_ros_wrapper SHARED - src/velodyne/velodyne_decoder_ros_wrapper.cpp - ) -rclcpp_components_register_node(velodyne_driver_ros_wrapper - PLUGIN "VelodyneDriverRosWrapper" - EXECUTABLE velodyne_driver_ros_wrapper_node - ) +add_library(velodyne_ros_wrapper SHARED + src/velodyne/velodyne_ros_wrapper.cpp + src/velodyne/decoder_wrapper.cpp + src/velodyne/hw_interface_wrapper.cpp + src/velodyne/hw_monitor_wrapper.cpp + src/common/parameter_descriptors.cpp +) + +target_include_directories(velodyne_ros_wrapper PUBLIC + ${diagnostic_updater_INCLUDE_DIRS} + ${nebula_decoders_INCLUDE_DIRS} + ${nebula_hw_interfaces_INCLUDE_DIRS} + ${velodyne_msgs_INCLUDE_DIRS} +) + +target_link_libraries(velodyne_ros_wrapper PUBLIC + ${diagnostic_updater_TARGETS} + ${diagnostic_msgs_TARGETS} + ${velodyne_msgs_TARGETS} + nebula_decoders::nebula_decoders_velodyne + nebula_hw_interfaces::nebula_hw_interfaces_velodyne +) + +rclcpp_components_register_node(velodyne_ros_wrapper + PLUGIN "VelodyneRosWrapper" + EXECUTABLE velodyne_ros_wrapper_node +) ## Robosense -# Hw Interface -ament_auto_add_library(robosense_hw_ros_wrapper SHARED - src/robosense/robosense_hw_interface_ros_wrapper.cpp - ) - -rclcpp_components_register_node(robosense_hw_ros_wrapper - PLUGIN "RobosenseHwInterfaceRosWrapper" - EXECUTABLE robosense_hw_interface_ros_wrapper_node - ) - -# DriverDecoder -ament_auto_add_library(robosense_driver_ros_wrapper SHARED - src/robosense/robosense_decoder_ros_wrapper.cpp - ) - -rclcpp_components_register_node(robosense_driver_ros_wrapper - PLUGIN "RobosenseDriverRosWrapper" - EXECUTABLE robosense_driver_ros_wrapper_node - ) - -# Monitor -ament_auto_add_library(robosense_hw_monitor_ros_wrapper SHARED - src/robosense/robosense_hw_monitor_ros_wrapper.cpp - ) - -rclcpp_components_register_node(robosense_hw_monitor_ros_wrapper - PLUGIN "RobosenseHwMonitorRosWrapper" - EXECUTABLE robosense_hw_monitor_ros_wrapper_node - ) +add_library(robosense_ros_wrapper SHARED + src/robosense/robosense_ros_wrapper.cpp + src/robosense/decoder_wrapper.cpp + src/robosense/hw_interface_wrapper.cpp + src/robosense/hw_monitor_wrapper.cpp + src/common/parameter_descriptors.cpp +) + +target_include_directories(robosense_ros_wrapper PUBLIC + ${diagnostic_updater_INCLUDE_DIRS} + ${nebula_decoders_INCLUDE_DIRS} + ${nebula_hw_interfaces_INCLUDE_DIRS} + ${robosense_msgs_INCLUDE_DIRS} +) + +target_link_libraries(robosense_ros_wrapper PUBLIC + ${diagnostic_updater_TARGETS} + ${diagnostic_msgs_TARGETS} + ${robosense_msgs_TARGETS} + nebula_decoders::nebula_decoders_robosense + nebula_decoders::nebula_decoders_robosense_info + nebula_hw_interfaces::nebula_hw_interfaces_robosense +) + +rclcpp_components_register_node(robosense_ros_wrapper + PLUGIN "RobosenseRosWrapper" + EXECUTABLE robosense_ros_wrapper_node +) ## Continental -# Hw Interface -ament_auto_add_library(continental_ars548_hw_ros_wrapper SHARED - src/continental/continental_ars548_hw_interface_ros_wrapper.cpp - ) -rclcpp_components_register_node(continental_ars548_hw_ros_wrapper - PLUGIN "ContinentalARS548HwInterfaceRosWrapper" - EXECUTABLE continental_ars548_hw_interface_ros_wrapper_node - ) - -ament_auto_add_library(multi_continental_ars548_hw_ros_wrapper SHARED - src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp - ) -rclcpp_components_register_node(multi_continental_ars548_hw_ros_wrapper - PLUGIN "MultiContinentalARS548HwInterfaceRosWrapper" - EXECUTABLE multi_continental_ars548_hw_interface_ros_wrapper_node - ) - -# DriverDecoder -ament_auto_add_library(continental_ars548_driver_ros_wrapper SHARED - src/continental/continental_ars548_decoder_ros_wrapper.cpp - ) -rclcpp_components_register_node(continental_ars548_driver_ros_wrapper - PLUGIN "ContinentalARS548DriverRosWrapper" - EXECUTABLE continental_ars548_driver_ros_wrapper_node - ) + +# ARS548 +add_library(continental_ars548_ros_wrapper SHARED + src/continental/continental_ars548_ros_wrapper.cpp + src/continental/continental_ars548_decoder_wrapper.cpp + src/continental/continental_ars548_hw_interface_wrapper.cpp + src/common/parameter_descriptors.cpp +) + +target_include_directories(continental_ars548_ros_wrapper PUBLIC + ${continental_msgs_INCLUDE_DIRS} + ${continental_srvs_INCLUDE_DIRS} + ${diagnostic_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${nebula_msgs_INCLUDE_DIRS} + ${nebula_decoders_INCLUDE_DIRS} + ${nebula_hw_interfaces_INCLUDE_DIRS} + ${radar_msgs_INCLUDE_DIRS} + ${rclcpp_components_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} +) + +target_link_libraries(continental_ars548_ros_wrapper PUBLIC + ${continental_msgs_TARGETS} + ${continental_srvs_TARGETS} + ${diagnostic_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${nebula_msgs_TARGETS} + ${radar_msgs_TARGETS} + ${tf2_ros_TARGETS} + ${visualization_msgs_TARGETS} + nebula_decoders::nebula_decoders_continental + nebula_hw_interfaces::nebula_hw_interfaces_continental +) + +rclcpp_components_register_node(continental_ars548_ros_wrapper + PLUGIN "ContinentalARS548RosWrapper" + EXECUTABLE continental_ars548_ros_wrapper_node +) + +# SRR520 +add_library(continental_srr520_ros_wrapper SHARED + src/continental/continental_srr520_ros_wrapper.cpp + src/continental/continental_srr520_decoder_wrapper.cpp + src/continental/continental_srr520_hw_interface_wrapper.cpp + src/common/parameter_descriptors.cpp +) + +target_include_directories(continental_srr520_ros_wrapper PUBLIC + ${continental_msgs_INCLUDE_DIRS} + ${continental_srvs_INCLUDE_DIRS} + ${diagnostic_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${nebula_msgs_INCLUDE_DIRS} + ${nebula_decoders_INCLUDE_DIRS} + ${nebula_hw_interfaces_INCLUDE_DIRS} + ${radar_msgs_INCLUDE_DIRS} + ${rclcpp_components_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} +) + +target_link_libraries(continental_srr520_ros_wrapper PUBLIC + ${continental_msgs_TARGETS} + ${continental_srvs_TARGETS} + ${diagnostic_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${nebula_msgs_TARGETS} + ${radar_msgs_TARGETS} + ${tf2_ros_TARGETS} + ${visualization_msgs_TARGETS} + nebula_decoders::nebula_decoders_continental + nebula_hw_interfaces::nebula_hw_interfaces_continental +) + +rclcpp_components_register_node(continental_srr520_ros_wrapper + PLUGIN "ContinentalSRR520RosWrapper" + EXECUTABLE continental_srr520_ros_wrapper_node +) + +install(TARGETS hesai_ros_wrapper EXPORT export_hesai_ros_wrapper) +install(TARGETS velodyne_ros_wrapper EXPORT export_velodyne_ros_wrapper) +install(TARGETS robosense_ros_wrapper EXPORT export_robosense_ros_wrapper) +install(TARGETS continental_ars548_ros_wrapper EXPORT export_continental_ars548_ros_wrapper) +install(TARGETS continental_srr520_ros_wrapper EXPORT export_continental_srr520_ros_wrapper) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() endif() -ament_auto_package( - INSTALL_TO_SHARE - config - launch +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_targets(export_hesai_ros_wrapper) +ament_export_targets(export_velodyne_ros_wrapper) +ament_export_targets(export_robosense_ros_wrapper) +ament_export_targets(export_continental_ars548_ros_wrapper) + +install( + DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} ) +ament_export_dependencies( + PCL + continental_msgs + continental_srvs + diagnostic_msgs + diagnostic_updater + geometry_msgs + nebula_common + nebula_decoders + nebula_hw_interfaces + nebula_msgs + pandar_msgs + radar_msgs + rclcpp_components + robosense_msgs + sensor_msgs + tf2_ros + velodyne_msgs + visualization_msgs + yaml-cpp +) + +ament_package() + set(ROS_DISTRO $ENV{ROS_DISTRO}) if(${ROS_DISTRO} STREQUAL "rolling") add_compile_definitions(ROS_DISTRO_ROLLING) diff --git a/nebula_ros/config/BaseParams.yaml b/nebula_ros/config/BaseParams.yaml deleted file mode 100644 index 2aa17176a..000000000 --- a/nebula_ros/config/BaseParams.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - device_ip: "192.168.1.201" - return_mode: "SingleStrongest" - host_ip: "255.255.255.255" - frame_id: "lidar" - data_port: 2368 - gnss_port: 2369 - setup_sensor: True - online: True diff --git a/nebula_ros/config/hesai/Pandar128E4X.yaml b/nebula_ros/config/hesai/Pandar128E4X.yaml deleted file mode 100644 index b4f5df758..000000000 --- a/nebula_ros/config/hesai/Pandar128E4X.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/**: - ros__parameters: - sensor_model: "Pandar128E4X" # See readme for supported models - sensor_ip: "192.168.1.201" # Lidar Sensor IP - host_ip: "255.255.255.255" # Broadcast IP from Sensor - frame_id: "hesai" - data_port: 2368 # LiDAR Data Port - gnss_port: 10110 # LiDAR GNSS Port - return_mode: "Dual" # See readme for supported return modes - scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] - packet_mtu_size: 1500 # Packet MTU size - rotation_speed: 600 # Motor RPM, the sensor's internal spin rate. - cloud_min_angle: 0 # Field of View, start degrees. - cloud_max_angle: 360 # Field of View, end degrees. - diag_span: 1000 # milliseconds - calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/Pandar128E4X.csv" - setup_sensor: True - - online: True \ No newline at end of file diff --git a/nebula_ros/config/hesai/Pandar40P.yaml b/nebula_ros/config/hesai/Pandar40P.yaml deleted file mode 100644 index 3b457f573..000000000 --- a/nebula_ros/config/hesai/Pandar40P.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/**: - ros__parameters: - sensor_model: "Pandar40P" # See readme for supported models - sensor_ip: "192.168.1.201" # Lidar Sensor IP - host_ip: "255.255.255.255" # Broadcast IP from Sensor - frame_id: "hesai" - data_port: 2368 # LiDAR Data Port - gnss_port: 10110 # LiDAR GNSS Port - return_mode: "Dual" # See readme for supported return modes - scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] - packet_mtu_size: 1500 # Packet MTU size - rotation_speed: 600 # Motor RPM, the sensor's internal spin rate. - cloud_min_angle: 0 # Field of View, start degrees. - cloud_max_angle: 360 # Field of View, end degrees. - diag_span: 1000 # milliseconds - calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/Pandar40P.csv" - setup_sensor: True - - online: True diff --git a/nebula_ros/config/hesai/Pandar64.yaml b/nebula_ros/config/hesai/Pandar64.yaml deleted file mode 100644 index 53ebf0ad3..000000000 --- a/nebula_ros/config/hesai/Pandar64.yaml +++ /dev/null @@ -1,17 +0,0 @@ -/**: - ros__parameters: - sensor_model: "Pandar64" - frame_id: "hesai" - return_mode: "Dual" - sensor_ip: "192.168.1.201" - host_ip: "255.255.255.255" - data_port: 2368 - gnss_port: 2369 - scan_phase: 0.0 - packet_mtu_size: 1500 - rotation_speed: 600 - cloud_min_angle: 0 - cloud_max_angle: 360 - diag_span: 1000 - setup_sensor: True - online: True diff --git a/nebula_ros/config/hesai/PandarAT128.yaml b/nebula_ros/config/hesai/PandarAT128.yaml deleted file mode 100644 index 5ad0027bb..000000000 --- a/nebula_ros/config/hesai/PandarAT128.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - sensor_model: "PandarAT128" # See readme for supported models - sensor_ip: "192.168.1.201" # Lidar Sensor IP - host_ip: "255.255.255.255" # Broadcast IP from Sensor - frame_id: "hesai" - data_port: 2368 # LiDAR Data Port - gnss_port: 10110 # LiDAR GNSS Port - return_mode: "LastStrongest" # See readme for supported return modes - scan_phase: 30.0 # Angle where scans begin (degrees, [30.,150.]) - packet_mtu_size: 1500 # Packet MTU size - rotation_speed: 200 # Motor RPM, the sensor's internal spin rate. - cloud_min_angle: 0 # Field of View, start degrees. - cloud_max_angle: 360 # Field of View, end degrees. - diag_span: 1000 # milliseconds - calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarAT128.csv" - correction_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarAT128.dat" - setup_sensor: True - - online: True diff --git a/nebula_ros/config/hesai/PandarQT128.yaml b/nebula_ros/config/hesai/PandarQT128.yaml deleted file mode 100644 index 6ab2e81a5..000000000 --- a/nebula_ros/config/hesai/PandarQT128.yaml +++ /dev/null @@ -1,18 +0,0 @@ -/**: - ros__parameters: - sensor_model: "PandarQT128" # See readme for supported models - sensor_ip: "192.168.1.201" # Lidar Sensor IP - host_ip: "255.255.255.255" # Broadcast IP from Sensor - frame_id: "hesai" - data_port: 2368 # LiDAR Data Port - gnss_port: 10110 # LiDAR GNSS Port - return_mode: "LastStrongest" # See readme for supported return modes - scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] - packet_mtu_size: 1500 # Packet MTU size - rotation_speed: 200 # Motor RPM, the sensor's internal spin rate. - cloud_min_angle: 0 # Field of View, start degrees. - cloud_max_angle: 360 # Field of View, end degrees. - diag_span: 1000 # milliseconds - calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarQT128.csv" - - online: True diff --git a/nebula_ros/config/hesai/PandarQT64.yaml b/nebula_ros/config/hesai/PandarQT64.yaml deleted file mode 100644 index b2caccca8..000000000 --- a/nebula_ros/config/hesai/PandarQT64.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/**: - ros__parameters: - sensor_model: "PandarQT64" # See readme for supported models - sensor_ip: "192.168.1.201" # Lidar Sensor IP - host_ip: "255.255.255.255" # Broadcast IP from Sensor - frame_id: "hesai" - data_port: 2368 # LiDAR Data Port - gnss_port: 10110 # LiDAR GNSS Port - return_mode: "Dual" # See readme for supported return modes - scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] - packet_mtu_size: 1500 # Packet MTU size - rotation_speed: 600 # Motor RPM, the sensor's internal spin rate. - cloud_min_angle: 0 # Field of View, start degrees. - cloud_max_angle: 360 # Field of View, end degrees. - diag_span: 1000 # milliseconds - calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarQT64.csv" - setup_sensor: True - - online: True \ No newline at end of file diff --git a/nebula_ros/config/hesai/PandarXT32.yaml b/nebula_ros/config/hesai/PandarXT32.yaml deleted file mode 100644 index 245848408..000000000 --- a/nebula_ros/config/hesai/PandarXT32.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/**: - ros__parameters: - sensor_model: "PandarXT32" # See readme for supported models - sensor_ip: "192.168.1.201" # Lidar Sensor IP - host_ip: "255.255.255.255" # Broadcast IP from Sensor - frame_id: "hesai" - data_port: 2368 # LiDAR Data Port - gnss_port: 10110 # LiDAR GNSS Port - return_mode: "Dual" # See readme for supported return modes - scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] - packet_mtu_size: 1500 # Packet MTU size - rotation_speed: 600 # Motor RPM, the sensor's internal spin rate. - cloud_min_angle: 0 # Field of View, start degrees. - cloud_max_angle: 360 # Field of View, end degrees. - diag_span: 1000 # milliseconds - calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarXT32.csv" - setup_sensor: True - - online: True diff --git a/nebula_ros/config/hesai/PandarXT32M.yaml b/nebula_ros/config/hesai/PandarXT32M.yaml deleted file mode 100644 index 994045eae..000000000 --- a/nebula_ros/config/hesai/PandarXT32M.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/**: - ros__parameters: - sensor_model: "PandarXT32M" # See readme for supported models - sensor_ip: "192.168.50.201" # Lidar Sensor IP - host_ip: "255.255.255.255" # Broadcast IP from Sensor - frame_id: "hesai" - data_port: 2369 # LiDAR Data Port - gnss_port: 10110 # LiDAR GNSS Port - return_mode: "LastStrongest" # See readme for supported return modes - scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] - packet_mtu_size: 1500 # Packet MTU size - rotation_speed: 1200 # Motor RPM, the sensor's internal spin rate. - cloud_min_angle: 0 # Field of View, start degrees. - cloud_max_angle: 360 # Field of View, end degrees. - diag_span: 1000 # milliseconds - calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarXT32M.csv" - setup_sensor: True - - online: True diff --git a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml new file mode 100644 index 000000000..85361864c --- /dev/null +++ b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml @@ -0,0 +1,28 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + multicast_ip: "" + data_port: 2368 + gnss_port: 10110 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: hesai + diag_span: 1000 + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 0 + cloud_max_angle: 360 + sync_angle: 0 + cut_angle: 0.0 + sensor_model: Pandar128E4X + calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv + rotation_speed: 600 + return_mode: Dual + ptp_profile: automotive + ptp_domain: 0 + ptp_transport_type: L2 + ptp_switch_type: TSN + retry_hw: true + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml new file mode 100644 index 000000000..1f6b79ff6 --- /dev/null +++ b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml @@ -0,0 +1,28 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + multicast_ip: "" + data_port: 2368 + gnss_port: 10110 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: hesai + diag_span: 1000 + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 0 + cloud_max_angle: 360 + sync_angle: 0 + cut_angle: 0.0 + sensor_model: Pandar40P + calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv + rotation_speed: 600 + return_mode: Dual + ptp_profile: 1588v2 + ptp_domain: 0 + ptp_transport_type: UDP + ptp_switch_type: TSN + retry_hw: true + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml new file mode 100644 index 000000000..31c695643 --- /dev/null +++ b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml @@ -0,0 +1,27 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + multicast_ip: "" + data_port: 2368 + gnss_port: 10110 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: hesai + diag_span: 1000 + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 0 + cloud_max_angle: 360 + sync_angle: 0 + cut_angle: 0.0 + sensor_model: Pandar64 + rotation_speed: 600 + return_mode: Dual + ptp_profile: 1588v2 + ptp_domain: 0 + ptp_transport_type: UDP + ptp_switch_type: TSN + retry_hw: true + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml new file mode 100644 index 000000000..5a2a8858b --- /dev/null +++ b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml @@ -0,0 +1,29 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + multicast_ip: "" + data_port: 2368 + gnss_port: 10110 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: hesai + diag_span: 1000 + correction_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).dat + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 30 + cloud_max_angle: 150 + sync_angle: 30 + cut_angle: 150.0 + sensor_model: PandarAT128 + calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv + rotation_speed: 200 + return_mode: Dual + ptp_profile: 1588v2 + ptp_domain: 0 + ptp_transport_type: UDP + ptp_switch_type: TSN + retry_hw: true + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml new file mode 100644 index 000000000..31e046be2 --- /dev/null +++ b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml @@ -0,0 +1,28 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + multicast_ip: "" + data_port: 2368 + gnss_port: 10110 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: hesai + diag_span: 1000 + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 0 + cloud_max_angle: 360 + sync_angle: 0 + cut_angle: 0.0 + sensor_model: PandarQT128 + calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv + rotation_speed: 600 + return_mode: LastStrongest + ptp_profile: 1588v2 + ptp_domain: 0 + ptp_transport_type: UDP + ptp_switch_type: TSN + retry_hw: true + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml new file mode 100644 index 000000000..4d3d119a8 --- /dev/null +++ b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml @@ -0,0 +1,28 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + multicast_ip: "" + data_port: 2368 + gnss_port: 10110 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: hesai + diag_span: 1000 + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 0 + cloud_max_angle: 360 + sync_angle: 0 + cut_angle: 0.0 + sensor_model: PandarQT64 + calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv + rotation_speed: 600 + return_mode: Dual + ptp_profile: 1588v2 + ptp_domain: 0 + ptp_transport_type: UDP + ptp_switch_type: TSN + retry_hw: true + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml new file mode 100644 index 000000000..09eaef375 --- /dev/null +++ b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml @@ -0,0 +1,28 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + multicast_ip: "" + data_port: 2368 + gnss_port: 10110 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: hesai + diag_span: 1000 + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 0 + cloud_max_angle: 360 + sync_angle: 0 + cut_angle: 0.0 + sensor_model: PandarXT32 + calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv + rotation_speed: 600 + return_mode: Dual + ptp_profile: 1588v2 + ptp_domain: 0 + ptp_transport_type: UDP + ptp_switch_type: TSN + retry_hw: true + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml new file mode 100644 index 000000000..6ad0e6566 --- /dev/null +++ b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml @@ -0,0 +1,28 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + multicast_ip: "" + data_port: 2368 + gnss_port: 10110 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: hesai + diag_span: 1000 + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 0 + cloud_max_angle: 360 + sync_angle: 0 + cut_angle: 0.0 + sensor_model: PandarXT32M + calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv + rotation_speed: 600 + return_mode: LastStrongest + ptp_profile: 1588v2 + ptp_domain: 0 + ptp_transport_type: UDP + ptp_switch_type: TSN + retry_hw: true + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/robosense/Bpearl.param.yaml b/nebula_ros/config/lidar/robosense/Bpearl.param.yaml new file mode 100644 index 000000000..74a12080d --- /dev/null +++ b/nebula_ros/config/lidar/robosense/Bpearl.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + data_port: 2368 + gnss_port: 2369 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: robosense + diag_span: 1000 + cloud_min_angle: 0 + cloud_max_angle: 360 + scan_phase: 0.0 + dual_return_distance_threshold: 0.1 + sensor_model: Bpearl + return_mode: Dual diff --git a/nebula_ros/config/lidar/robosense/Helios.param.yaml b/nebula_ros/config/lidar/robosense/Helios.param.yaml new file mode 100644 index 000000000..5694362fc --- /dev/null +++ b/nebula_ros/config/lidar/robosense/Helios.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + data_port: 2368 + gnss_port: 2369 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: robosense + diag_span: 1000 + cloud_min_angle: 0 + cloud_max_angle: 360 + scan_phase: 0.0 + dual_return_distance_threshold: 0.1 + sensor_model: Helios + return_mode: Dual diff --git a/nebula_ros/config/lidar/velodyne/VLP16.param.yaml b/nebula_ros/config/lidar/velodyne/VLP16.param.yaml new file mode 100644 index 000000000..b46e980f0 --- /dev/null +++ b/nebula_ros/config/lidar/velodyne/VLP16.param.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + data_port: 2368 + gnss_port: 2369 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: velodyne + advanced_diagnostics: false + diag_span: 1000 + calibration_file: $(find-pkg-share nebula_decoders)/calibration/velodyne/VLP16.yaml + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 0 + cloud_max_angle: 360 + scan_phase: 0.0 + sensor_model: VLP16 + rotation_speed: 600 + return_mode: Dual diff --git a/nebula_ros/config/lidar/velodyne/VLP32.param.yaml b/nebula_ros/config/lidar/velodyne/VLP32.param.yaml new file mode 100644 index 000000000..96ce39a32 --- /dev/null +++ b/nebula_ros/config/lidar/velodyne/VLP32.param.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + data_port: 2368 + gnss_port: 2369 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: velodyne + advanced_diagnostics: false + diag_span: 1000 + calibration_file: $(find-pkg-share nebula_decoders)/calibration/velodyne/VLP32.yaml + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 0 + cloud_max_angle: 360 + scan_phase: 0.0 + sensor_model: VLP32 + rotation_speed: 600 + return_mode: Dual diff --git a/nebula_ros/config/lidar/velodyne/VLS128.param.yaml b/nebula_ros/config/lidar/velodyne/VLS128.param.yaml new file mode 100644 index 000000000..c5c0c6313 --- /dev/null +++ b/nebula_ros/config/lidar/velodyne/VLS128.param.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + host_ip: 255.255.255.255 + sensor_ip: 192.168.1.201 + data_port: 2368 + gnss_port: 2369 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + frame_id: velodyne + advanced_diagnostics: false + diag_span: 1000 + calibration_file: $(find-pkg-share nebula_decoders)/calibration/velodyne/VLS128.yaml + min_range: 0.3 + max_range: 300.0 + cloud_min_angle: 0 + cloud_max_angle: 360 + scan_phase: 0.0 + sensor_model: VLS128 + rotation_speed: 600 + return_mode: Dual diff --git a/nebula_ros/config/continental/ARS548.yaml b/nebula_ros/config/radar/continental/ARS548.param.yaml similarity index 59% rename from nebula_ros/config/continental/ARS548.yaml rename to nebula_ros/config/radar/continental/ARS548.param.yaml index 2342cfe93..7f9a76ed7 100644 --- a/nebula_ros/config/continental/ARS548.yaml +++ b/nebula_ros/config/radar/continental/ARS548.param.yaml @@ -1,13 +1,14 @@ /**: ros__parameters: - sensor_model: "ARS548" - frame_id: "continental" - base_frame: "base_link" - object_frame: "base_link" - sensor_ip: "10.13.1.114" - host_ip: "10.13.1.166" - multicast_ip: "224.0.2.2" + host_ip: 10.13.1.166 + sensor_ip: 10.13.1.114 data_port: 42102 + frame_id: continental + base_frame: base_link + object_frame: base_link + launch_hw: true + multicast_ip: 224.0.2.2 + sensor_model: ARS548 configuration_host_port: 42401 configuration_sensor_port: 42101 use_sensor_time: false diff --git a/nebula_ros/config/radar/continental/SRR520.param.yaml b/nebula_ros/config/radar/continental/SRR520.param.yaml new file mode 100644 index 000000000..f3e50f934 --- /dev/null +++ b/nebula_ros/config/radar/continental/SRR520.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + sensor_model: SRR520 + frame_id: continental + base_frame: base_link + interface: can4 + launch_hw: true + receiver_timeout_sec: 0.03 + sender_timeout_sec: 0.01 + filters: 0:0 # candump-like filters + use_bus_time: false + configuration_vehicle_wheelbase: 2.79 diff --git a/nebula_ros/config/robosense/Bpearl.yaml b/nebula_ros/config/robosense/Bpearl.yaml deleted file mode 100644 index 9ccb42e56..000000000 --- a/nebula_ros/config/robosense/Bpearl.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - sensor_model: "Bpearl" # See readme for supported models - sensor_ip: "192.168.1.200" # Lidar Sensor IP - host_ip: "192.168.1.102" # Broadcast IP from Sensor - frame_id: "robosense" - data_port: 6699 # LiDAR Data Port (MSOP) - gnss_port: 7788 # LiDAR GNSS Port (DIFOP) - scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] - diag_span: 1000 # milliseconds - dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/robosense/Helios.yaml b/nebula_ros/config/robosense/Helios.yaml deleted file mode 100644 index 24350822d..000000000 --- a/nebula_ros/config/robosense/Helios.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - sensor_model: "Helios" # See readme for supported models - sensor_ip: "192.168.1.200" # Lidar Sensor IP - host_ip: "192.168.1.102" # Broadcast IP from Sensor - frame_id: "robosense" - data_port: 6699 # LiDAR Data Port (MSOP) - gnss_port: 7788 # LiDAR GNSS Port (DIFOP) - scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] - diag_span: 1000 # milliseconds - dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/velodyne/VLP16.yaml b/nebula_ros/config/velodyne/VLP16.yaml deleted file mode 100644 index 1daf3a4c6..000000000 --- a/nebula_ros/config/velodyne/VLP16.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - sensor_model: "VLP16" - sensor_ip: "192.168.1.201" - host_ip: "255.255.255.255" - frame_id: "velodyne" - data_port: 2368 - gnss_port: 2369 - setup_sensor: True - online: True diff --git a/nebula_ros/config/velodyne/VLP32.yaml b/nebula_ros/config/velodyne/VLP32.yaml deleted file mode 100644 index ef5ae14c1..000000000 --- a/nebula_ros/config/velodyne/VLP32.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - sensor_model: "VLP32" - sensor_ip: "192.168.1.201" - host_ip: "255.255.255.255" - frame_id: "velodyne" - data_port: 2368 - gnss_port: 2369 - setup_sensor: True - online: True diff --git a/nebula_ros/config/velodyne/VLS128.yaml b/nebula_ros/config/velodyne/VLS128.yaml deleted file mode 100644 index db73ecc05..000000000 --- a/nebula_ros/config/velodyne/VLS128.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - sensor_model: "VLS128" - sensor_ip: "192.168.1.201" - host_ip: "255.255.255.255" - frame_id: "velodyne" - data_port: 2368 - gnss_port: 2369 - setup_sensor: True - online: True diff --git a/nebula_ros/include/nebula_ros/common/mt_queue.hpp b/nebula_ros/include/nebula_ros/common/mt_queue.hpp new file mode 100644 index 000000000..c7fe30c0f --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/mt_queue.hpp @@ -0,0 +1,64 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include + +template +class mt_queue +{ +private: + std::mutex mutex_; + std::condition_variable cv_not_empty_, cv_not_full_; + std::deque queue_; + size_t capacity_; + +public: + explicit mt_queue(size_t capacity) : capacity_(capacity) {} + + bool try_push(T && value) + { + std::unique_lock lock(this->mutex_); + bool can_push = queue_.size() < capacity_; + if (can_push) { + queue_.push_front(std::move(value)); + } + this->cv_not_empty_.notify_all(); + return can_push; + } + + void push(T && value) + { + std::unique_lock lock(this->mutex_); + this->cv_not_full_.wait(lock, [=] { return this->queue_.size() < this->capacity_; }); + queue_.push_front(std::move(value)); + this->cv_not_empty_.notify_all(); + } + + T pop() + { + std::unique_lock lock(this->mutex_); + this->cv_not_empty_.wait(lock, [=] { return !this->queue_.empty(); }); + T return_value(std::move(this->queue_.back())); + this->queue_.pop_back(); + this->cv_not_full_.notify_all(); + + return return_value; + } +}; diff --git a/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp deleted file mode 100644 index 8ca27b175..000000000 --- a/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef NEBULA_DRIVER_WRAPPER_BASE_H -#define NEBULA_DRIVER_WRAPPER_BASE_H - -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" - -#include -#include -#include - -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Base class for ros wrapper of each sensor driver -class NebulaDriverRosWrapperBase -{ -public: - NebulaDriverRosWrapperBase() = default; - - NebulaDriverRosWrapperBase(NebulaDriverRosWrapperBase && c) = delete; - NebulaDriverRosWrapperBase & operator=(NebulaDriverRosWrapperBase && c) = delete; - NebulaDriverRosWrapperBase(const NebulaDriverRosWrapperBase & c) = delete; - NebulaDriverRosWrapperBase & operator=(const NebulaDriverRosWrapperBase & c) = delete; - -private: - /// @brief Virtual function for initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - virtual Status InitializeDriver( - [[maybe_unused]] std::shared_ptr sensor_configuration, - [[maybe_unused]] std::shared_ptr - calibration_configuration) - { - return Status::NOT_IMPLEMENTED; - } - - // status ReceiveScanMsgCallback(void * ScanMsg); // ROS message callback for individual packet - // type - - /// @brief Point cloud publisher - rclcpp::Publisher::SharedPtr cloud_pub_; -}; - -} // namespace ros -} // namespace nebula -#endif // NEBULA_DRIVER_WRAPPER_BASE_H diff --git a/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp deleted file mode 100644 index b42948426..000000000 --- a/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef NEBULA_HW_INTERFACE_WRAPPER_BASE_H -#define NEBULA_HW_INTERFACE_WRAPPER_BASE_H - -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" - -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Base class for hardware interface ros wrapper of each LiDAR -class NebulaHwInterfaceWrapperBase -{ -public: - NebulaHwInterfaceWrapperBase() = default; - - NebulaHwInterfaceWrapperBase(NebulaHwInterfaceWrapperBase && c) = delete; - NebulaHwInterfaceWrapperBase & operator=(NebulaHwInterfaceWrapperBase && c) = delete; - NebulaHwInterfaceWrapperBase(const NebulaHwInterfaceWrapperBase & c) = delete; - NebulaHwInterfaceWrapperBase & operator=(const NebulaHwInterfaceWrapperBase & c) = delete; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - virtual Status StreamStart() = 0; - - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - virtual Status StreamStop() = 0; - - /// @brief Shutdown (not used) - /// @return Resulting status - virtual Status Shutdown() = 0; - -protected: - /// @brief Virtual function for initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - virtual Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) = 0; - // void SendDataPacket(const std::vector &buffer); // Ideally this will be - // implemented as specific functions, GetFanStatus, GetEchoMode - - /// @brief Enable sensor setup during initialization and set_parameters_callback - bool setup_sensor; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HW_INTERFACE_WRAPPER_BASE_H diff --git a/nebula_ros/include/nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp deleted file mode 100644 index 29f8440cd..000000000 --- a/nebula_ros/include/nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef NEBULA_HW_MONITOR_WRAPPER_BASE_H -#define NEBULA_HW_MONITOR_WRAPPER_BASE_H - -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" - -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Base class for hardware monitor ros wrapper of each LiDAR -class NebulaHwMonitorWrapperBase -{ -public: - NebulaHwMonitorWrapperBase() = default; - - NebulaHwMonitorWrapperBase(NebulaHwMonitorWrapperBase && c) = delete; - NebulaHwMonitorWrapperBase & operator=(NebulaHwMonitorWrapperBase && c) = delete; - NebulaHwMonitorWrapperBase(const NebulaHwMonitorWrapperBase & c) = delete; - NebulaHwMonitorWrapperBase & operator=(const NebulaHwMonitorWrapperBase & c) = delete; - - /// @brief Start monitoring (not used) - /// @return Resulting status - virtual Status MonitorStart() = 0; - - /// @brief Stop monitoring (not used) - /// @return Resulting status - virtual Status MonitorStop() = 0; - - /// @brief Shutdown (not used) - /// @return Resulting status - virtual Status Shutdown() = 0; - -protected: - /// @brief Virtual function for initializing hardware monitor ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - virtual Status InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) = 0; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HW_MONITOR_WRAPPER_BASE_H diff --git a/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp b/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp new file mode 100644 index 000000000..db37cdb10 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp @@ -0,0 +1,58 @@ +// 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. + +#pragma once + +#include +#include +#include + +#include +#include +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write(); + +rcl_interfaces::msg::ParameterDescriptor param_read_only(); + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step); + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step); + +/// @brief Get a parameter's value from a list of parameters, if that parameter is in the list. +/// @tparam T The parameter's expected value type +/// @param p A vector of parameters +/// @param name Target parameter name +/// @param value (out) Parameter value. Set if parameter is found, left untouched otherwise. +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp new file mode 100644 index 000000000..fc4dd2967 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp @@ -0,0 +1,71 @@ +// 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. + +#pragma once + +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +class WatchdogTimer +{ + using watchdog_cb_t = std::function; + +public: + WatchdogTimer( + rclcpp::Node & node, const std::chrono::microseconds & expected_update_interval, + const watchdog_cb_t & callback) + : node_(node), + callback_(callback), + expected_update_interval_ns_( + std::chrono::duration_cast(expected_update_interval).count()) + { + timer_ = + node_.create_wall_timer(expected_update_interval, std::bind(&WatchdogTimer::onTimer, this)); + } + + void update() { last_update_ns_ = node_.get_clock()->now().nanoseconds(); } + +private: + void onTimer() + { + uint64_t now_ns = node_.get_clock()->now().nanoseconds(); + + // As the clock is not steady, the update timestamp can be newer than clock.now(). + // Define that edge-case as not being late too. + bool is_late = (last_update_ns_ > now_ns) + ? false + : (now_ns - last_update_ns_) > expected_update_interval_ns_; + + callback_(!is_late); + } + + rclcpp::Node & node_; + rclcpp::TimerBase::SharedPtr timer_; + std::atomic last_update_ns_; + const watchdog_cb_t callback_; + + const uint64_t expected_update_interval_ns_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp similarity index 68% rename from nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index 059388639..3c1eaa905 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -12,18 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NEBULA_ContinentalARS548DriverRosWrapper_H -#define NEBULA_ContinentalARS548DriverRosWrapper_H +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" -#include #include #include -#include +#include #include -#include -#include #include -#include #include #include @@ -34,75 +32,38 @@ #include #include #include +#include #include -#include #include +#include #include +#include namespace nebula { namespace ros { -/// @brief Ros wrapper of continental radar ethernet driver -class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase +class ContinentalARS548DecoderWrapper { - std::shared_ptr decoder_ptr_; - Status wrapper_status_; - - rclcpp::Subscription::SharedPtr packets_sub_; - - rclcpp::Publisher::SharedPtr - detection_list_pub_; - rclcpp::Publisher::SharedPtr object_list_pub_; - rclcpp::Publisher::SharedPtr object_pointcloud_pub_; - rclcpp::Publisher::SharedPtr detection_pointcloud_pub_; - rclcpp::Publisher::SharedPtr scan_raw_pub_; - rclcpp::Publisher::SharedPtr objects_raw_pub_; - rclcpp::Publisher::SharedPtr objects_markers_pub_; - rclcpp::Publisher::SharedPtr diagnostics_pub_; - - std::unordered_set previous_ids_; - - constexpr static int REFERENCE_POINTS_NUM = 9; - constexpr static std::array, REFERENCE_POINTS_NUM> reference_to_center_ = { - {{{-1.0, -1.0}}, - {{-1.0, 0.0}}, - {{-1.0, 1.0}}, - {{0.0, 1.0}}, - {{1.0, 1.0}}, - {{1.0, 0.0}}, - {{1.0, -1.0}}, - {{0.0, -1.0}}, - {{0.0, 0.0}}}}; +public: + ContinentalARS548DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config, + bool launch_hw); - std::shared_ptr - sensor_cfg_ptr_; + void ProcessPacket(std::unique_ptr packet_msg); - drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; + void OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & + new_config); - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver(std::shared_ptr sensor_configuration); + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } + nebula::Status Status(); /// @brief Callback to process new ContinentalArs548DetectionList from the driver /// @param msg The new ContinentalArs548DetectionList from the driver @@ -118,16 +79,14 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive void SensorStatusCallback( const drivers::continental_ars548::ContinentalARS548Status & sensor_status); -public: - explicit ContinentalARS548DriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for NebulaPackets subscriber - /// @param scan_msg Received NebulaPackets - void ReceivePacketsMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); + /// @brief Callback to process new ContinentalARS548Status from the driver + /// @param msg The new ContinentalArs548ObjectList from the driver + void PacketsCallback(std::unique_ptr msg); - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); +private: + nebula::Status InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config); /// @brief Convert ARS548 detections to a pointcloud /// @param msg The ARS548 detection list msg @@ -158,9 +117,53 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive /// @return Resulting MarkerArray msg visualization_msgs::msg::MarkerArray ConvertToMarkers( const continental_msgs::msg::ContinentalArs548ObjectList & msg); -}; + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + nebula::Status status_; + rclcpp::Logger logger_; + + std::shared_ptr + config_ptr_{}; + + std::shared_ptr driver_ptr_{}; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + + rclcpp::Publisher::SharedPtr + detection_list_pub_{}; + rclcpp::Publisher::SharedPtr + object_list_pub_{}; + rclcpp::Publisher::SharedPtr object_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr detection_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr scan_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_markers_pub_{}; + rclcpp::Publisher::SharedPtr diagnostics_pub_{}; + + std::unordered_set previous_ids_{}; + + constexpr static int REFERENCE_POINTS_NUM = 9; + constexpr static std::array, REFERENCE_POINTS_NUM> reference_to_center_ = { + {{{-1.0, -1.0}}, + {{-1.0, 0.0}}, + {{-1.0, 1.0}}, + {{0.0, 1.0}}, + {{1.0, 1.0}}, + {{1.0, 0.0}}, + {{1.0, -1.0}}, + {{0.0, -1.0}}, + {{0.0, 0.0}}}}; + + std::shared_ptr watchdog_; +}; } // namespace ros } // namespace nebula - -#endif // NEBULA_ContinentalARS548DriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp similarity index 52% rename from nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index f9b66e951..2931ca5c1 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -12,16 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NEBULA_ContinentalARS548HwInterfaceRosWrapper_H -#define NEBULA_ContinentalARS548HwInterfaceRosWrapper_H +#pragma once -#include -#include -#include +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include #include -#include #include -#include #include #include @@ -29,77 +26,36 @@ #include #include #include -#include -#include -#include #include -#include - #include -#include -#include -#include -#include namespace nebula { namespace ros { -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of continental radar ethernet driver -class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase +class ContinentalARS548HwInterfaceWrapper { - drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; - Status interface_status_; +public: + ContinentalARS548HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config); - drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration_; + /// @brief Starts the hw interface and subscribes to the input topics + void SensorInterfaceStart(); - /// @brief Received Continental Radar message publisher - rclcpp::Publisher::SharedPtr packets_pub_; - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr acceleration_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; + void OnConfigChange( + const std::shared_ptr & + new_config_ptr); - bool standstill_{true}; + /// @brief Get current status of the hw interface + /// @return Current status + nebula::Status Status(); - rclcpp::Service::SharedPtr - set_network_configuration_service_server_; - rclcpp::Service::SharedPtr - set_sensor_mounting_service_server_; - rclcpp::Service::SharedPtr - set_vehicle_parameters_service_server_; - rclcpp::Service::SharedPtr - set_radar_parameters_service_server_; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving NebulaPackets - /// @param packets_buffer Received NebulaPackets - void ReceivePacketsDataCallback(std::unique_ptr packets_buffer); + std::shared_ptr HwInterface() const; +private: /// @brief Callback to send the odometry information to the radar device /// @param msg The odometry message void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); @@ -148,41 +104,28 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, const std::shared_ptr response); -public: - explicit ContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~ContinentalARS548HwInterfaceRosWrapper() noexcept override; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); + rclcpp::Node * const parent_node_; + std::shared_ptr hw_interface_{}; + rclcpp::Logger logger_; + nebula::Status status_{}; + std::shared_ptr + config_ptr_{}; -private: - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; + rclcpp::Subscription::SharedPtr odometry_sub_{}; + rclcpp::Subscription::SharedPtr + acceleration_sub_{}; + rclcpp::Subscription::SharedPtr steering_angle_sub_{}; + bool standstill_{true}; + + rclcpp::Service::SharedPtr + set_network_configuration_service_server_{}; + rclcpp::Service::SharedPtr + set_sensor_mounting_service_server_{}; + rclcpp::Service::SharedPtr + set_vehicle_parameters_service_server_{}; + rclcpp::Service::SharedPtr + set_radar_parameters_service_server_{}; +}; } // namespace ros } // namespace nebula - -#endif // NEBULA_ContinentalARS548HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp new file mode 100644 index 000000000..9de45b042 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -0,0 +1,103 @@ +// 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. + +#pragma once + +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/continental/continental_ars548_decoder_wrapper.hpp" +#include "nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of continental ars548 driver +class ContinentalARS548RosWrapper final : public rclcpp::Node +{ +public: + explicit ContinentalARS548RosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalARS548RosWrapper() noexcept override = default; + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Start data streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart(); + +private: + /// @brief Callback from the hw interface's raw data + void ReceivePacketCallback(std::unique_ptr packet_msg_ptr); + + /// @brief Callback from replayed NebulaPackets + void ReceivePacketsCallback(std::unique_ptr packets_msg_ptr); + + /// @brief Retrieve the parameters from ROS and set the driver and hw interface + /// @return Resulting status + Status DeclareAndGetSensorConfigParams(); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + Status ValidateAndSetConfig( + std::shared_ptr & + new_config); + + Status wrapper_status_; + + std::shared_ptr + config_ptr_{}; + + /// @brief Stores received packets that have not been processed yet by the decoder thread + mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_{}; + + std::optional hw_interface_wrapper_{}; + std::optional decoder_wrapper_{}; + + std::mutex mtx_config_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_{}; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp new file mode 100644 index 000000000..2dbfa3447 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp @@ -0,0 +1,162 @@ +// 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. + +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +class ContinentalSRR520DecoderWrapper +{ +public: + ContinentalSRR520DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config, + std::shared_ptr hw_interface_ptr); + + void ProcessPacket(std::unique_ptr packet_msg); + + void OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & + new_config_ptr); + + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + nebula::Status Status(); + + /// @brief Callback to process a new Near ContinentalSrr520DetectionList from the driver + /// @param msg The new ContinentalSrr520DetectionList from the driver + void NearDetectionListCallback( + std::unique_ptr msg); + + /// @brief Callback to process a new HRR ContinentalSrr520DetectionList from the driver + /// @param msg The new ContinentalSrr520DetectionList from the driver + void HRRDetectionListCallback( + std::unique_ptr msg); + + /// @brief Callback to process a new ContinentalSrr520ObjectList from the driver + /// @param msg The new ContinentalSrr520ObjectList from the driver + void ObjectListCallback(std::unique_ptr msg); + + /// @brief Callback to process a new DiagnosticArray from the driver + /// @param msg The new DiagnosticArray from the driver + void StatusCallback(std::unique_ptr msg); + + /// @brief Callback to process a new SyncFollowUp message from the driver + void SyncFollowUpCallback(builtin_interfaces::msg::Time stamp); + + /// @brief Callback to process a new NebulaPackets message from the driver + /// @param msg The new NebulaPackets from the driver + void PacketsCallback(std::unique_ptr msg); + +private: + nebula::Status InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & config); + + /// @brief Convert SRR520 detections to a pointcloud + /// @param msg The SRR520 detection list msg + /// @return Resulting detection pointcloud + pcl::PointCloud::Ptr + ConvertToPointcloud(const continental_msgs::msg::ContinentalSrr520DetectionList & msg); + + /// @brief Convert SRR520 objects to a pointcloud + /// @param msg The SRR520 object list msg + /// @return Resulting object pointcloud + pcl::PointCloud::Ptr ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + /// @brief Convert SRR520 detections to a standard RadarScan msg + /// @param msg The SRR520 detection list msg + /// @return Resulting RadarScan msg + radar_msgs::msg::RadarScan ConvertToRadarScan( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg); + + /// @brief Convert SRR520 objects to a standard RadarTracks msg + /// @param msg The SRR520 object list msg + /// @return Resulting RadarTracks msg + radar_msgs::msg::RadarTracks ConvertToRadarTracks( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + /// @brief Convert SRR520 objects to a standard MarkerArray msg + /// @param msg The SRR520 object list msg + /// @return Resulting MarkerArray msg + visualization_msgs::msg::MarkerArray ConvertToMarkers( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + const rclcpp::Node * const parent_node_; + nebula::Status status_; + rclcpp::Logger logger_; + + std::shared_ptr + sensor_cfg_{}; + + std::shared_ptr driver_ptr_{}; + std::shared_ptr hw_interface_ptr_{}; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + + rclcpp::Publisher::SharedPtr + near_detection_list_pub_{}; + rclcpp::Publisher::SharedPtr + hrr_detection_list_pub_{}; + rclcpp::Publisher::SharedPtr + object_list_pub_{}; + rclcpp::Publisher::SharedPtr status_pub_{}; + rclcpp::Publisher::SharedPtr near_detection_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr hrr_detection_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr object_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr near_scan_raw_pub_{}; + rclcpp::Publisher::SharedPtr hrr_scan_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_markers_pub_{}; + + std::unordered_set previous_ids_{}; + + std::shared_ptr watchdog_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp new file mode 100644 index 000000000..3da76cab6 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp @@ -0,0 +1,100 @@ +// 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. + +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +namespace nebula +{ +namespace ros +{ +class ContinentalSRR520HwInterfaceWrapper +{ +public: + ContinentalSRR520HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config); + + /// @brief Starts the hw interface and subscribes to the input topics + void SensorInterfaceStart(); + + void OnConfigChange( + const std::shared_ptr & + new_config); + + /// @brief Get current status of the hw interface + /// @return Current status + nebula::Status Status(); + + std::shared_ptr HwInterface() const; + +private: + /// @brief Callback to send the odometry information to the radar device + /// @param odometry_msg The odometry message + /// @param acceleration_msg The acceleration message + void dynamicsCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg); + + /// @brief Service callback to set the new sensor mounting position + /// @param request Empty service request + /// @param response Empty service response + void ConfigureSensorRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); + + /// @brief Method periodically called to initiate the sensor synchronization mechanism + void syncTimerCallback(); + + rclcpp::Node * const parent_node_; + std::shared_ptr hw_interface_{}; + rclcpp::Logger logger_; + nebula::Status status_{}; + std::shared_ptr + config_ptr_{}; + + message_filters::Subscriber odometry_sub_; + message_filters::Subscriber acceleration_sub_; + + using ExactTimeSyncPolicy = message_filters::sync_policies::ExactTime< + geometry_msgs::msg::TwistWithCovarianceStamped, geometry_msgs::msg::AccelWithCovarianceStamped>; + using ExactTimeSync = message_filters::Synchronizer; + std::shared_ptr sync_ptr_{}; + rclcpp::TimerBase::SharedPtr sync_timer_; + + bool standstill_{true}; + + rclcpp::Service::SharedPtr + configure_sensor_service_server_{}; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp new file mode 100644 index 000000000..6df11e9b6 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp @@ -0,0 +1,101 @@ +// 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. + +#pragma once + +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/continental/continental_srr520_decoder_wrapper.hpp" +#include "nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of continental srr520 driver +class ContinentalSRR520RosWrapper final : public rclcpp::Node +{ +public: + explicit ContinentalSRR520RosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalSRR520RosWrapper() noexcept {}; + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Start data streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart(); + +private: + /// @brief Callback from the hw interface's raw data + void ReceivePacketCallback(std::unique_ptr packet_msg_ptr); + + /// @brief Callback from replayed NebulaPackets + void ReceivePacketsCallback(std::unique_ptr packet_packets_msg); + + /// @brief Retrieve the parameters from ROS and set the driver and hw interface + /// @return Resulting status + Status DeclareAndGetSensorConfigParams(); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + Status ValidateAndSetConfig( + std::shared_ptr & + new_config); + + Status wrapper_status_; + + std::shared_ptr + config_ptr_{}; + + /// @brief Stores received packets that have not been processed yet by the decoder thread + mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_{}; + + std::optional hw_interface_wrapper_{}; + std::optional decoder_wrapper_{}; + + std::mutex mtx_config_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 19429b99c..000000000 --- a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,146 +0,0 @@ -// 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_ContinentalARS548HwInterfaceRosWrapper_H -#define NEBULA_ContinentalARS548HwInterfaceRosWrapper_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of continental radar ethernet driver -/// NOTE: this is a temporary class that acts as a single hw interface for multiple devices -/// The reason behind this is a not-so-efficient multicasting and package processing when having N -/// interfaces for N devices If we end up having problems because of that we may switch to this -/// implementation. Otherwise this implementation will be removed at a later date -class MultiContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase -{ - drivers::continental_ars548::MultiContinentalARS548HwInterface hw_interface_; - Status interface_status_; - - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration sensor_configuration_; - - /// @brief Received Continental Radar message publisher - std::unordered_map::SharedPtr> - packets_pub_map_; - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr acceleration_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; - - bool standstill_{true}; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving NebulaPackets - /// @param packets_buffer Received NebulaPackets - void ReceivePacketsDataCallback( - std::unique_ptr packets_buffer, const std::string & sensor_ip); - - /// @brief Callback to send the odometry information to the radar device - /// @param msg The odometry message - void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - - /// @brief Callback to send the acceleration information to the radar device - /// @param msg The acceleration message - void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); - - /// @brief Callback to send the steering angle information to the radar device - /// @param msg The steering angle message - void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); - -public: - explicit MultiContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~MultiContinentalARS548HwInterfaceRosWrapper() noexcept override; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration & sensor_configuration); - -private: - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_ContinentalARS548HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp new file mode 100644 index 000000000..cd77ed937 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -0,0 +1,89 @@ +// 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. + +#pragma once + +#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#include +#include + +#include +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +class HesaiDecoderWrapper +{ +public: + HesaiDecoderWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & config, + const std::shared_ptr & calibration, + bool publish_packets); + + void ProcessCloudPacket(std::unique_ptr packet_msg); + + void OnConfigChange( + const std::shared_ptr & new_config); + + void OnCalibrationChange( + const std::shared_ptr & + new_calibration); + + nebula::Status Status(); + +private: + void PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher); + + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + nebula::Status status_; + rclcpp::Logger logger_; + rclcpp::Node & parent_node_; + + std::shared_ptr sensor_cfg_; + std::shared_ptr calibration_cfg_ptr_{}; + + std::shared_ptr driver_ptr_{}; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_{}; + + rclcpp::Publisher::SharedPtr nebula_points_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_ex_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_base_pub_{}; + + std::shared_ptr cloud_watchdog_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp deleted file mode 100644 index 52518fcd0..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp +++ /dev/null @@ -1,106 +0,0 @@ -#ifndef NEBULA_HesaiDriverRosWrapper_H -#define NEBULA_HesaiDriverRosWrapper_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include -#include - -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" - -namespace nebula -{ -namespace ros -{ -/// @brief Ros wrapper of hesai driver -class HesaiDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase -{ - std::shared_ptr driver_ptr_; - Status wrapper_status_; - rclcpp::Subscription::SharedPtr pandar_scan_sub_; - rclcpp::Publisher::SharedPtr nebula_points_pub_; - rclcpp::Publisher::SharedPtr aw_points_ex_pub_; - rclcpp::Publisher::SharedPtr aw_points_base_pub_; - - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; - std::shared_ptr correction_cfg_ptr_; - - drivers::HesaiHwInterface hw_interface_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration); - - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - - /*** - * Publishes a sensor_msgs::msg::PointCloud2 to the specified publisher - * @param pointcloud unique pointer containing the point cloud to publish - * @param publisher - */ - void PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher); - -public: - explicit HesaiDriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for PandarScan subscriber - /// @param scan_msg Received PandarScan - void ReceiveScanMsgCallback(const pandar_msgs::msg::PandarScan::SharedPtr scan_msg); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - -private: - /// @brief File path of Correction data (Only required only for AT) - std::string correction_file_path; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiDriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 1802f87b2..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef NEBULA_HesaiHwInterfaceRosWrapper_H -#define NEBULA_HesaiHwInterfaceRosWrapper_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" - -#include -#include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" - -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of hesai driver -class HesaiHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceWrapperBase -{ - drivers::HesaiHwInterface hw_interface_; - Status interface_status_; - - drivers::HesaiSensorConfiguration sensor_configuration_; - - /// @brief Received Hesai message publisher - rclcpp::Publisher::SharedPtr pandar_scan_pub_; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving PandarScan - /// @param scan_buffer Received PandarScan - void ReceiveScanDataCallback(std::unique_ptr scan_buffer); - -public: - explicit HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiHwInterfaceRosWrapper() noexcept override; - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration); - -private: - uint16_t delay_hw_ms_; - bool retry_hw_; - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiHwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp deleted file mode 100644 index c23b76c55..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp +++ /dev/null @@ -1,165 +0,0 @@ -#ifndef NEBULA_HesaiHwMonitorRosWrapper_H -#define NEBULA_HesaiHwMonitorRosWrapper_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware monitor ros wrapper of hesai driver -class HesaiHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapperBase -{ - drivers::HesaiHwInterface hw_interface_; - Status interface_status_; - - drivers::HesaiSensorConfiguration sensor_configuration_; - - /// @brief Initializing hardware monitor ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) override; - -public: - explicit HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiHwMonitorRosWrapper() noexcept override; - /// @brief Not used - /// @return Current status - Status MonitorStart() override; - /// @brief Not used - /// @return Status::OK - Status MonitorStop() override; - /// @brief Not used - /// @return Status::OK - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration); - -private: - diagnostic_updater::Updater diagnostics_updater_; - /// @brief Initializing diagnostics - void InitializeHesaiDiagnostics(); - /// @brief Callback of the timer for getting the current lidar status - void OnHesaiStatusTimer(); - /// @brief Callback of the timer for getting the current lidar monitor via http - void OnHesaiLidarMonitorTimerHttp(); - /// @brief Callback of the timer for getting the current lidar monitor via tcp - void OnHesaiLidarMonitorTimer(); - // void OnHesaiDiagnosticsTimer(); - // void OnHesaiStatusTimer(); - - /// @brief Check status information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check ptp information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check temperature information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check rpm information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check voltage information from HesaiLidarStatus for diagnostic_updater via http - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check voltage information from HesaiLidarStatus for diagnostic_updater via tcp - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - - rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; - rclcpp::TimerBase::SharedPtr diagnostics_update_monitor_timer_; - rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_; - std::unique_ptr current_status; - std::unique_ptr current_monitor; - std::unique_ptr current_config; - std::unique_ptr current_inventory; - std::unique_ptr current_lidar_monitor_tree; - std::unique_ptr current_status_time; - std::unique_ptr current_config_time; - std::unique_ptr current_inventory_time; - std::unique_ptr current_lidar_monitor_time; - uint8_t current_diag_status; - uint8_t current_monitor_status; - - uint16_t diag_span_; - uint16_t delay_monitor_ms_; - std::mutex mtx_diag; - std::mutex mtx_status; - std::mutex mtx_lidar_monitor; - // std::timed_mutex mtx_lidar_monitor; - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - /// @brief Get value from property_tree - /// @param pt property_tree - /// @param key Pey string - /// @return Value - std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); - /// @brief Making fixed precision string - /// @param val Target value - /// @param pre Precision - /// @return Created string - std::string GetFixedPrecisionString(double val, int pre = 2); - - std::string info_model; - std::string info_serial; - rclcpp::CallbackGroup::SharedPtr cbg_r_; - rclcpp::CallbackGroup::SharedPtr cbg_m_; - rclcpp::CallbackGroup::SharedPtr cbg_m2_; - - const char * not_supported_message; - const char * error_message; - std::string message_sep; - - std::vector temperature_names; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiHwMonitorRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp new file mode 100644 index 000000000..0292c0f8e --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -0,0 +1,122 @@ +// 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. + +#pragma once + +#include "nebula_common/hesai/hesai_common.hpp" +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/hesai/decoder_wrapper.hpp" +#include "nebula_ros/hesai/hw_interface_wrapper.hpp" +#include "nebula_ros/hesai/hw_monitor_wrapper.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of hesai driver +class HesaiRosWrapper final : public rclcpp::Node +{ + using get_calibration_result_t = nebula::util::expected< + std::shared_ptr, nebula::Status>; + +public: + explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); + ~HesaiRosWrapper() noexcept override = default; + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart(); + +private: + void ReceiveCloudPacketCallback(std::vector & packet); + + void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + + Status DeclareAndGetSensorConfigParams(); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + Status ValidateAndSetConfig( + std::shared_ptr & new_config); + + /// @brief The ROS 2 parameter holding the calibration file path is called differently depending + /// on the sensor model. This function returns the correct parameter name given a model. + /// @param model The sensor model + /// @return std::string The parameter name + std::string getCalibrationParameterName(drivers::SensorModel model) const; + + /// @brief Load calibration data from the best available source: + /// 1. If sensor connected, download and save from sensor + /// 2. If downloaded file available, load that file + /// 3. Load the file given by `calibration_file_path` + /// @param calibration_file_path The file to use if no better option is available + /// @param ignore_others If true, skip straight so step 3 above, ignoring better calibration + /// options + /// @return The calibration data if successful, or an error code if not + get_calibration_result_t GetCalibrationData( + const std::string & calibration_file_path, bool ignore_others = false); + + Status wrapper_status_; + + std::shared_ptr sensor_cfg_ptr_{}; + + /// @brief Stores received packets that have not been processed yet by the decoder thread + mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_; + + std::optional hw_interface_wrapper_; + std::optional hw_monitor_wrapper_; + std::optional decoder_wrapper_; + + std::mutex mtx_config_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp new file mode 100644 index 000000000..addc4f478 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.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. + +#pragma once + +#include +#include +#include + +#include + +namespace nebula +{ +namespace ros +{ +class HesaiHwInterfaceWrapper +{ +public: + HesaiHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config); + + void OnConfigChange( + const std::shared_ptr & new_config); + + nebula::Status Status(); + + std::shared_ptr HwInterface() const; + +private: + std::shared_ptr hw_interface_; + rclcpp::Logger logger_; + nebula::Status status_; + bool setup_sensor_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp new file mode 100644 index 000000000..87b384d1a --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp @@ -0,0 +1,115 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +class HesaiHwMonitorWrapper +{ +public: + HesaiHwMonitorWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config); + + void OnConfigChange( + const std::shared_ptr & /* new_config */) + { + } + + nebula::Status Status(); + +private: + void InitializeHesaiDiagnostics(); + + std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); + + std::string GetFixedPrecisionString(double val, int pre); + + void OnHesaiStatusTimer(); + + void OnHesaiLidarMonitorTimerHttp(); + + void OnHesaiLidarMonitorTimer(); + + void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + rclcpp::Logger logger_; + diagnostic_updater::Updater diagnostics_updater_; + nebula::Status status_; + + const std::shared_ptr hw_interface_; + rclcpp::Node * const parent_node_; + + uint16_t diag_span_; + rclcpp::TimerBase::SharedPtr diagnostics_update_timer_{}; + rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_{}; + + std::unique_ptr current_status_{}; + std::unique_ptr current_monitor_{}; + std::unique_ptr current_config_{}; + std::unique_ptr current_inventory_{}; + std::unique_ptr current_lidar_monitor_tree_{}; + + std::unique_ptr current_status_time_{}; + std::unique_ptr current_config_time_{}; + std::unique_ptr current_inventory_time_{}; + std::unique_ptr current_lidar_monitor_time_{}; + + uint8_t current_diag_status_; + uint8_t current_monitor_status_; + + std::mutex mtx_lidar_status_; + std::mutex mtx_lidar_monitor_; + + std::string info_model_; + std::string info_serial_; + + std::vector temperature_names_; + + bool supports_monitor_; + + const std::string MSG_NOT_SUPPORTED = "Not supported"; + const std::string MSG_ERROR = "Error"; + const std::string MSG_SEP = ": "; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/decoder_wrapper.hpp new file mode 100644 index 000000000..f0d858e59 --- /dev/null +++ b/nebula_ros/include/nebula_ros/robosense/decoder_wrapper.hpp @@ -0,0 +1,88 @@ +// 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. + +#pragma once + +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Ros wrapper of robosense driver +class RobosenseDecoderWrapper +{ +public: + explicit RobosenseDecoderWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + const std::shared_ptr & config, + const std::shared_ptr & calibration); + + void ProcessCloudPacket(std::unique_ptr packet_msg); + + void OnConfigChange( + const std::shared_ptr & new_config); + + nebula::Status Status(); + +private: + void PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher); + + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + nebula::Status status_; + rclcpp::Logger logger_; + + std::shared_ptr hw_interface_; + std::shared_ptr sensor_cfg_; + std::shared_ptr calibration_cfg_ptr_{}; + + std::shared_ptr driver_ptr_; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + robosense_msgs::msg::RobosenseScan::UniquePtr current_scan_msg_{}; + + rclcpp::Publisher::SharedPtr nebula_points_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_ex_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_base_pub_{}; + + std::shared_ptr cloud_watchdog_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/hw_interface_wrapper.hpp new file mode 100644 index 000000000..29c6efef4 --- /dev/null +++ b/nebula_ros/include/nebula_ros/robosense/hw_interface_wrapper.hpp @@ -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. + +#pragma once + +#include +#include +#include +#include + +#include + +namespace nebula +{ +namespace ros +{ +class RobosenseHwInterfaceWrapper +{ +public: + explicit RobosenseHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config); + + void OnConfigChange( + const std::shared_ptr & new_config); + + nebula::Status Status(); + + std::shared_ptr HwInterface() const; + +private: + std::shared_ptr hw_interface_; + rclcpp::Logger logger_; + nebula::Status status_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/hw_monitor_wrapper.hpp new file mode 100644 index 000000000..a5fe251e6 --- /dev/null +++ b/nebula_ros/include/nebula_ros/robosense/hw_monitor_wrapper.hpp @@ -0,0 +1,81 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Hardware monitor ros wrapper of robosense driver +class RobosenseHwMonitorWrapper +{ +public: + explicit RobosenseHwMonitorWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config); + + void OnConfigChange( + const std::shared_ptr & new_config); + + /// @brief Callback for receiving DIFOP packet + /// @param info_msg Received DIFOP packet + void DiagnosticsCallback(const std::map & diag_info); + +private: + /// @brief Initializing diagnostics + void InitializeRobosenseDiagnostics(); + /// @brief Callback of the timer for getting the current lidar status + void OnRobosenseStatusTimer(); + + /// @brief Check status information from RobosenseLidarStatus for diagnostic_updater + /// @param diagnostics DiagnosticStatusWrapper + void RobosenseCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + rclcpp::Node * parent_; + rclcpp::Logger logger_; + std::optional diagnostics_updater_; + nebula::Status status_; + + std::shared_ptr sensor_cfg_ptr_; + std::mutex mtx_config_; + + rclcpp::TimerBase::SharedPtr diagnostics_status_timer_; + rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; + std::map current_sensor_info_; + std::mutex mtx_current_sensor_info_; + + rclcpp::Time current_info_time_; + uint16_t diag_span_{1000}; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp deleted file mode 100644 index 553d653ab..000000000 --- a/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp +++ /dev/null @@ -1,99 +0,0 @@ -#pragma once - -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_common/robosense/robosense_common.hpp" -#include "nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp" -#include "nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include -#include - -#include "robosense_msgs/msg/robosense_info_packet.hpp" -#include "robosense_msgs/msg/robosense_packet.hpp" -#include "robosense_msgs/msg/robosense_scan.hpp" - -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Ros wrapper of robosense driver -class RobosenseDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase -{ - std::shared_ptr driver_ptr_; - std::shared_ptr info_driver_ptr_; - Status wrapper_status_; - bool is_received_info{false}; - rclcpp::Subscription::SharedPtr robosense_scan_sub_; - rclcpp::Subscription::SharedPtr robosense_info_sub_; - rclcpp::Publisher::SharedPtr nebula_points_pub_; - rclcpp::Publisher::SharedPtr aw_points_ex_pub_; - rclcpp::Publisher::SharedPtr aw_points_base_pub_; - - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing info driver - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeInfoDriver( - std::shared_ptr sensor_configuration); - - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @return Resulting status - Status GetParameters( - drivers::RobosenseSensorConfiguration & sensor_configuration, - drivers::RobosenseCalibrationConfiguration & calibration_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - - /*** - * Publishes a sensor_msgs::msg::PointCloud2 to the specified publisher - * @param pointcloud unique pointer containing the point cloud to publish - * @param publisher - */ - void PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher); - -public: - explicit RobosenseDriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for RobosenseScan subscriber - /// @param scan_msg Received RobosenseScan - void ReceiveScanMsgCallback(const robosense_msgs::msg::RobosenseScan::SharedPtr scan_msg); - - /// @brief Callback for DIFOP packet subscriber - /// @param scan_msg Received RobosensePacket - void ReceiveInfoMsgCallback(const robosense_msgs::msg::RobosenseInfoPacket::SharedPtr info_msg); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); -}; - -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 74e33231e..000000000 --- a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,88 +0,0 @@ -#pragma once - -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/robosense/robosense_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" - -#include -#include - -#include "robosense_msgs/msg/robosense_info_packet.hpp" -#include "robosense_msgs/msg/robosense_packet.hpp" -#include "robosense_msgs/msg/robosense_scan.hpp" - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of Robosense driver -class RobosenseHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceWrapperBase -{ -public: - explicit RobosenseHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::RobosenseSensorConfiguration & sensor_configuration); - -private: - drivers::RobosenseHwInterface hw_interface_; - drivers::RobosenseSensorConfiguration sensor_configuration_; - Status interface_status_; - - /// @brief Received Robosense Scan message publisher - rclcpp::Publisher::SharedPtr robosense_scan_pub_; - - /// @brief Received Robosense Difop message publisher - rclcpp::Publisher::SharedPtr robosense_difop_pub_; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - - /// @brief Callback for receiving RobosenseScan - /// @param scan_buffer Received RobosenseScan - void ReceiveScanDataCallback(std::unique_ptr scan_buffer); - - /// @brief Callback for receiving RobosensePacket - /// @param difop_buffer Received DIFOP packet - void ReceiveInfoDataCallback(std::unique_ptr difop_buffer); -}; - -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp deleted file mode 100644 index 2ecdc0b4f..000000000 --- a/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp +++ /dev/null @@ -1,114 +0,0 @@ -#pragma once - -#include "boost_tcp_driver/tcp_driver.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/robosense/robosense_common.hpp" -#include "nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" - -#include -#include -#include -#include - -#include "robosense_msgs/msg/robosense_info_packet.hpp" - -#include -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware monitor ros wrapper of robosense driver -class RobosenseHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapperBase -{ - drivers::RobosenseHwInterface hw_interface_; - Status interface_status_; - std::unique_ptr info_driver_; - std::vector info_packet_buffer_; - - drivers::RobosenseSensorConfiguration sensor_configuration_; - std::shared_ptr calibration_configuration_; - - rclcpp::Subscription::SharedPtr robosense_info_sub_; - - /// @brief Initializing hardware monitor ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) override; - -public: - explicit RobosenseHwMonitorRosWrapper(const rclcpp::NodeOptions & options); - /// @brief Not used - /// @return Current status - Status MonitorStart() override; - /// @brief Not used - /// @return Status::OK - Status MonitorStop() override; - /// @brief Not used - /// @return Status::OK - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::RobosenseSensorConfiguration & sensor_configuration); - -private: - diagnostic_updater::Updater diagnostics_updater_; - /// @brief Initializing diagnostics - void InitializeRobosenseDiagnostics(); - /// @brief Callback of the timer for getting the current lidar status - void OnRobosenseStatusTimer(); - - /// @brief Check status information from RobosenseLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void RobosenseCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - - /// @brief Callback for receiving DIFOP packet - /// @param info_msg Received DIFOP packet - void ReceiveInfoMsgCallback(const robosense_msgs::msg::RobosenseInfoPacket::SharedPtr info_msg); - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - rclcpp::TimerBase::SharedPtr diagnostics_status_timer_; - std::map current_sensor_info_; - - std::unique_ptr current_info_time_; - uint16_t diag_span_{1000}; - std::optional hardware_id_; -}; - -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp new file mode 100644 index 000000000..0c66e0f9d --- /dev/null +++ b/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp @@ -0,0 +1,109 @@ +// 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. + +#pragma once + +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/robosense/decoder_wrapper.hpp" +#include "nebula_ros/robosense/hw_interface_wrapper.hpp" +#include "nebula_ros/robosense/hw_monitor_wrapper.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of robosense driver +class RobosenseRosWrapper final : public rclcpp::Node +{ +public: + explicit RobosenseRosWrapper(const rclcpp::NodeOptions & options); + ~RobosenseRosWrapper() noexcept {}; + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart(); + +private: + void ReceiveCloudPacketCallback(std::vector & packet); + + void ReceiveInfoPacketCallback(std::vector & packet); + + void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + + nebula::Status DeclareAndGetSensorConfigParams(); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + nebula::Status ValidateAndSetConfig( + std::shared_ptr & new_config); + + nebula::Status wrapper_status_; + + std::shared_ptr sensor_cfg_ptr_{}; + + /// @brief Stores received packets that have not been processed yet by the decoder thread + mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_; + + std::optional hw_interface_wrapper_; + std::optional hw_monitor_wrapper_; + std::optional decoder_wrapper_; + + std::optional info_driver_; + + std::mutex mtx_config_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp new file mode 100644 index 000000000..e78e51e0c --- /dev/null +++ b/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp @@ -0,0 +1,107 @@ +// 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. + +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +class VelodyneDecoderWrapper +{ + using get_calibration_result_t = nebula::util::expected< + std::shared_ptr, nebula::Status>; + +public: + VelodyneDecoderWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config); + + void ProcessCloudPacket(std::unique_ptr packet_msg); + + void OnConfigChange( + const std::shared_ptr & new_config); + + void OnCalibrationChange( + const std::shared_ptr & + new_calibration); + + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + nebula::Status Status(); + +private: + /// @brief Load calibration data from file + /// @param calibration_file_path The file to read from + /// @return The calibration data if successful, or an error code if not + get_calibration_result_t GetCalibrationData(const std::string & calibration_file_path); + + void PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher); + + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + nebula::Status status_; + rclcpp::Logger logger_; + + const std::shared_ptr hw_interface_; + std::shared_ptr sensor_cfg_; + + std::string calibration_file_path_{}; + std::shared_ptr calibration_cfg_ptr_{}; + + std::shared_ptr driver_ptr_{}; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + velodyne_msgs::msg::VelodyneScan::UniquePtr current_scan_msg_{}; + + rclcpp::Publisher::SharedPtr nebula_points_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_ex_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_base_pub_{}; + + std::shared_ptr cloud_watchdog_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp new file mode 100644 index 000000000..e919b4367 --- /dev/null +++ b/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp @@ -0,0 +1,50 @@ +// 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. + +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include + +#include + +namespace nebula +{ +namespace ros +{ +class VelodyneHwInterfaceWrapper +{ +public: + VelodyneHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config); + + void OnConfigChange( + const std::shared_ptr & new_config); + + nebula::Status Status(); + + std::shared_ptr HwInterface() const; + +private: + std::shared_ptr hw_interface_; + rclcpp::Logger logger_; + nebula::Status status_; + bool setup_sensor_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp similarity index 69% rename from nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp index 3f589bbbf..c86bfd917 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp @@ -1,92 +1,80 @@ -#ifndef NEBULA_VelodyneHwMonitorRosWrapper_H -#define NEBULA_VelodyneHwMonitorRosWrapper_H +// 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_common/nebula_common.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" -#include #include +#include +#include #include -#include -#include +#include +#include +#include +#include + +#include +#include +#include +#include namespace nebula { namespace ros { -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) +class VelodyneHwMonitorWrapper { - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; +public: + VelodyneHwMonitorWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config); + + void OnConfigChange( + const std::shared_ptr & /* new_config */) + { } - return false; -} -/// @brief Hardware monitor ros wrapper of velodyne driver -class VelodyneHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapperBase -{ - drivers::VelodyneHwInterface hw_interface_; - Status interface_status_; + nebula::Status Status(); - drivers::VelodyneSensorConfiguration sensor_configuration_; - drivers::VelodyneCalibrationConfiguration calibration_configuration_; +private: + void InitializeVelodyneDiagnostics(); - /// @brief Initializing hardware monitor ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) override; + /// @brief Callback of the timer for getting the current lidar status + void OnVelodyneStatusTimer(); + + /// @brief Callback of the timer for getting the current lidar snapshot + void OnVelodyneSnapshotTimer(); + + /// @brief Callback of the timer for getting the current lidar status & updating the diagnostics + void OnVelodyneDiagnosticsTimer(); -public: - explicit VelodyneHwMonitorRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Not used - /// @return Current status - Status MonitorStart() override; - /// @brief Not used - /// @return Status::OK - Status MonitorStop() override; - /// @brief Not used - /// @return Status::OK - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::VelodyneSensorConfiguration & sensor_configuration); - -private: // ROS Diagnostics - diagnostic_updater::Updater diagnostics_updater_; - /// @brief Initializing diagnostics - void InitializeVelodyneDiagnostics(); /// @brief Get value from property_tree /// @param pt property_tree + /// @param mtx_pt the mutex associated with `pt` /// @param key Pey string /// @return Value std::string GetPtreeValue( - std::shared_ptr pt, const std::string & key); + std::shared_ptr pt, std::mutex & mtx_pt, const std::string & key); + /// @brief Making fixed precision string /// @param val Target value /// @param pre Precision /// @return Created string std::string GetFixedPrecisionString(double val, int pre = 2); - rclcpp::TimerBase::SharedPtr diagnostics_diag_timer_; - std::shared_ptr current_diag_tree; - /// @brief Callback of the timer for getting the current lidar status & updating the diagnostics - void OnVelodyneDiagnosticsTimer(); /// @brief Getting top:hv from the current property_tree /// @return tuple @@ -242,8 +230,6 @@ class VelodyneHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWr void VelodyneCheckAdctpStat(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); rclcpp::TimerBase::SharedPtr diagnostics_status_timer_; std::shared_ptr current_status_tree; - /// @brief Callback of the timer for getting the current lidar status - void OnVelodyneStatusTimer(); /// @brief Check gps:pps_state from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper void VelodyneCheckGpsPpsState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); @@ -286,150 +272,110 @@ class VelodyneHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWr /// @param diagnostics DiagnosticStatusWrapper void VelodyneCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Callback of the timer for getting the current lidar snapshot - void OnVelodyneSnapshotTimer(); + rclcpp::Logger logger_; + diagnostic_updater::Updater diagnostics_updater_; + nebula::Status status_; + + const std::shared_ptr hw_interface_; + rclcpp::Node * const parent_node_; + + std::shared_ptr sensor_configuration_; + + uint16_t diag_span_; + bool show_advanced_diagnostics_; + rclcpp::TimerBase::SharedPtr diagnostics_snapshot_timer_; rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; + rclcpp::TimerBase::SharedPtr diagnostics_diag_timer_; + std::shared_ptr current_snapshot; std::shared_ptr current_snapshot_tree; + std::shared_ptr current_diag_tree; std::shared_ptr current_snapshot_time; - // rclcpp::Time current_snapshot_time; - // std::shared_ptr current_diag_status; uint8_t current_diag_status; - uint16_t diag_span_; - std::mutex mtx_diag; - std::mutex mtx_status; - std::mutex mtx_config_; - - /// @brief Test callback function for getting json with curl - /// @param err Error - /// @param body Received body - void curl_callback(std::string err, std::string body); - - const char * key_volt_temp_top_hv; - const char * key_volt_temp_top_ad_temp; - const char * key_volt_temp_top_lm20_temp; - const char * key_volt_temp_top_pwr_5v; - const char * key_volt_temp_top_pwr_2_5v; - const char * key_volt_temp_top_pwr_3_3v; - const char * key_volt_temp_top_pwr_5v_raw; - const char * key_volt_temp_top_pwr_raw; - const char * key_volt_temp_top_pwr_vccint; - const char * key_volt_temp_bot_i_out; - const char * key_volt_temp_bot_pwr_1_2v; - const char * key_volt_temp_bot_lm20_temp; - const char * key_volt_temp_bot_pwr_5v; - const char * key_volt_temp_bot_pwr_2_5v; - const char * key_volt_temp_bot_pwr_3_3v; - const char * key_volt_temp_bot_pwr_v_in; - const char * key_volt_temp_bot_pwr_1_25v; - const char * key_vhv; - const char * key_adc_nf; - const char * key_adc_stats; - const char * key_ixe; - const char * key_adctp_stat; - const char * key_status_gps_pps_state; - const char * key_status_gps_pps_position; - const char * key_status_motor_state; - const char * key_status_motor_rpm; - const char * key_status_motor_lock; - const char * key_status_motor_phase; - const char * key_status_laser_state; - - /* - const char* name_volt_temp_top_hv; - const char* name_volt_temp_top_ad_temp; - const char* name_volt_temp_top_lm20_temp; - const char* name_volt_temp_top_pwr_5v; - const char* name_volt_temp_top_pwr_2_5v; - const char* name_volt_temp_top_pwr_3_3v; - const char* name_volt_temp_top_pwr_raw; - const char* name_volt_temp_top_pwr_vccint; - const char* name_volt_temp_bot_i_out; - const char* name_volt_temp_bot_pwr_1_2v; - const char* name_volt_temp_bot_lm20_temp; - const char* name_volt_temp_bot_pwr_5v; - const char* name_volt_temp_bot_pwr_2_5v; - const char* name_volt_temp_bot_pwr_3_3v; - const char* name_volt_temp_bot_pwr_v_in; - const char* name_volt_temp_bot_pwr_1_25v; - const char* name_vhv; - const char* name_adc_nf; - const char* name_adc_stats; - const char* name_ixe; - const char* name_adctp_stat; - const char* name_status_gps_pps_state; - const char* name_status_gps_pps_position; - const char* name_status_motor_state; - const char* name_status_motor_rpm; - const char* name_status_motor_lock; - const char* name_status_motor_phase; - const char* name_status_laser_state; - */ - - std::string name_volt_temp_top_hv; - std::string name_volt_temp_top_ad_temp; - std::string name_volt_temp_top_lm20_temp; - std::string name_volt_temp_top_pwr_5v; - std::string name_volt_temp_top_pwr_2_5v; - std::string name_volt_temp_top_pwr_3_3v; - std::string name_volt_temp_top_pwr_5v_raw; - std::string name_volt_temp_top_pwr_raw; - std::string name_volt_temp_top_pwr_vccint; - std::string name_volt_temp_bot_i_out; - std::string name_volt_temp_bot_pwr_1_2v; - std::string name_volt_temp_bot_lm20_temp; - std::string name_volt_temp_bot_pwr_5v; - std::string name_volt_temp_bot_pwr_2_5v; - std::string name_volt_temp_bot_pwr_3_3v; - std::string name_volt_temp_bot_pwr_v_in; - std::string name_volt_temp_bot_pwr_1_25v; - std::string name_vhv; - std::string name_adc_nf; - std::string name_adc_stats; - std::string name_ixe; - std::string name_adctp_stat; - std::string name_status_gps_pps_state; - std::string name_status_gps_pps_position; - std::string name_status_motor_state; - std::string name_status_motor_rpm; - std::string name_status_motor_lock; - std::string name_status_motor_phase; - std::string name_status_laser_state; - - const char * not_supported_message; - const char * error_message; - std::string message_sep; - - const char * key_info_model; - const char * key_info_serial; - - std::string temperature_cold_message; - std::string temperature_hot_message; - std::string voltage_low_message; - std::string voltage_high_message; - std::string ampere_low_message; - std::string ampere_high_message; - - std::string info_model; - std::string info_serial; - - bool use_advanced_diagnostics; - - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - // rclcpp::callback_group::CallbackGroup::SharedPtr cbg_; - rclcpp::CallbackGroup::SharedPtr cbg_r_; - rclcpp::CallbackGroup::SharedPtr cbg_m_; -}; + std::mutex mtx_snapshot_; + std::mutex mtx_status_; + std::mutex mtx_diag_; + + std::string info_model_; + std::string info_serial_; + + static constexpr auto key_volt_temp_top_hv = "volt_temp.top.hv"; + static constexpr auto key_volt_temp_top_ad_temp = "volt_temp.top.ad_temp"; // only32 + static constexpr auto key_volt_temp_top_lm20_temp = "volt_temp.top.lm20_temp"; + static constexpr auto key_volt_temp_top_pwr_5v = "volt_temp.top.pwr_5v"; + static constexpr auto key_volt_temp_top_pwr_2_5v = "volt_temp.top.pwr_2_5v"; + static constexpr auto key_volt_temp_top_pwr_3_3v = "volt_temp.top.pwr_3_3v"; + static constexpr auto key_volt_temp_top_pwr_5v_raw = "volt_temp.top.pwr_5v_raw"; // only16 + static constexpr auto key_volt_temp_top_pwr_raw = "volt_temp.top.pwr_raw"; // only32 + static constexpr auto key_volt_temp_top_pwr_vccint = "volt_temp.top.pwr_vccint"; + static constexpr auto key_volt_temp_bot_i_out = "volt_temp.bot.i_out"; + static constexpr auto key_volt_temp_bot_pwr_1_2v = "volt_temp.bot.pwr_1_2v"; + static constexpr auto key_volt_temp_bot_lm20_temp = "volt_temp.bot.lm20_temp"; + static constexpr auto key_volt_temp_bot_pwr_5v = "volt_temp.bot.pwr_5v"; + static constexpr auto key_volt_temp_bot_pwr_2_5v = "volt_temp.bot.pwr_2_5v"; + static constexpr auto key_volt_temp_bot_pwr_3_3v = "volt_temp.bot.pwr_3_3v"; + static constexpr auto key_volt_temp_bot_pwr_v_in = "volt_temp.bot.pwr_v_in"; + static constexpr auto key_volt_temp_bot_pwr_1_25v = "volt_temp.bot.pwr_1_25v"; + static constexpr auto key_vhv = "vhv"; + static constexpr auto key_adc_nf = "adc_nf"; + static constexpr auto key_adc_stats = "adc_stats"; + static constexpr auto key_ixe = "ixe"; + static constexpr auto key_adctp_stat = "adctp_stat"; + static constexpr auto key_status_gps_pps_state = "gps.pps_state"; + static constexpr auto key_status_gps_pps_position = "gps.position"; + static constexpr auto key_status_motor_state = "motor.state"; + static constexpr auto key_status_motor_rpm = "motor.rpm"; + static constexpr auto key_status_motor_lock = "motor.lock"; + static constexpr auto key_status_motor_phase = "motor.phase"; + static constexpr auto key_status_laser_state = "laser.state"; + + static constexpr auto name_volt_temp_top_hv = "Top HV"; + static constexpr auto name_volt_temp_top_ad_temp = "Top A/D TD"; + static constexpr auto name_volt_temp_top_lm20_temp = "Top Temp"; + static constexpr auto name_volt_temp_top_pwr_5v = "Top 5v"; + static constexpr auto name_volt_temp_top_pwr_2_5v = "Top 2.5v"; + static constexpr auto name_volt_temp_top_pwr_3_3v = "Top 3.3v"; + static constexpr auto name_volt_temp_top_pwr_5v_raw = "Top 5v(RAW)"; + static constexpr auto name_volt_temp_top_pwr_raw = "Top RAW"; + static constexpr auto name_volt_temp_top_pwr_vccint = "Top VCCINT"; + static constexpr auto name_volt_temp_bot_i_out = "Bot I out"; + static constexpr auto name_volt_temp_bot_pwr_1_2v = "Bot 1.2v"; + static constexpr auto name_volt_temp_bot_lm20_temp = "Bot Temp"; + static constexpr auto name_volt_temp_bot_pwr_5v = "Bot 5v"; + static constexpr auto name_volt_temp_bot_pwr_2_5v = "Bot 2.5v"; + static constexpr auto name_volt_temp_bot_pwr_3_3v = "Bot 3.3v"; + static constexpr auto name_volt_temp_bot_pwr_v_in = "Bot V in"; + static constexpr auto name_volt_temp_bot_pwr_1_25v = "Bot 1.25v"; // N/A? + static constexpr auto name_vhv = "VHV"; + static constexpr auto name_adc_nf = "adc_nf"; + static constexpr auto name_adc_stats = "adc_stats"; + static constexpr auto name_ixe = "ixe"; + static constexpr auto name_adctp_stat = "adctp_stat"; + static constexpr auto name_status_gps_pps_state = "GPS PPS"; + static constexpr auto name_status_gps_pps_position = "GPS Position"; + static constexpr auto name_status_motor_state = "Motor State"; + static constexpr auto name_status_motor_rpm = "Motor RPM"; + static constexpr auto name_status_motor_lock = "Motor Lock"; + static constexpr auto name_status_motor_phase = "Motor Phase"; + static constexpr auto name_status_laser_state = "Laser State"; + + const std::string message_sep{": "}; + static constexpr auto not_supported_message = "Not supported"; + static constexpr auto error_message = "Error"; + static constexpr auto key_info_model = "info.model"; + static constexpr auto key_info_serial = "info.serial"; + + static constexpr auto temperature_cold_message = "temperature cold"; + static constexpr auto temperature_hot_message = "temperature hot"; + + static constexpr auto voltage_low_message = "voltage low"; + static constexpr auto voltage_high_message = "voltage high"; + + static constexpr auto ampere_low_message = "ampere low"; + static constexpr auto ampere_high_message = "ampere high"; +}; } // namespace ros } // namespace nebula - -#endif // NEBULA_VelodyneHwMonitorRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp deleted file mode 100644 index 5bc79b6c8..000000000 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp +++ /dev/null @@ -1,83 +0,0 @@ -#ifndef NEBULA_VelodyneDriverRosWrapper_H -#define NEBULA_VelodyneDriverRosWrapper_H - -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Ros wrapper of velodyne driver -class VelodyneDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase -{ - std::shared_ptr driver_ptr_; - Status wrapper_status_; - rclcpp::Subscription::SharedPtr velodyne_scan_sub_; - rclcpp::Publisher::SharedPtr nebula_points_pub_; - rclcpp::Publisher::SharedPtr aw_points_ex_pub_; - rclcpp::Publisher::SharedPtr aw_points_base_pub_; - - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @return Resulting status - Status GetParameters( - drivers::VelodyneSensorConfiguration & sensor_configuration, - drivers::VelodyneCalibrationConfiguration & calibration_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - - /*** - * Publishes a sensor_msgs::msg::PointCloud2 to the specified publisher - * @param pointcloud unique pointer containing the point cloud to publish - * @param publisher - */ - void PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher); - -public: - explicit VelodyneDriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for VelodyneScan subscriber - /// @param scan_msg Received VelodyneScan - void ReceiveScanMsgCallback(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg); - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_VelodyneDriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 5a0e094c3..000000000 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,202 +0,0 @@ -#ifndef NEBULA_VelodyneHwInterfaceRosWrapper_H -#define NEBULA_VelodyneHwInterfaceRosWrapper_H - -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" - -#include -#include -#include - -#include -#include - -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of velodyne driver -class VelodyneHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceWrapperBase -{ - drivers::VelodyneHwInterface hw_interface_; - Status interface_status_; - - drivers::VelodyneSensorConfiguration sensor_configuration_; - drivers::VelodyneCalibrationConfiguration calibration_configuration_; - - /// @brief Received Velodyne message publisher - rclcpp::Publisher::SharedPtr velodyne_scan_pub_; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving VelodyneScan - /// @param scan_buffer Received VelodyneScan - void ReceiveScanDataCallback(std::unique_ptr scan_buffer); - -public: - explicit VelodyneHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::VelodyneSensorConfiguration & sensor_configuration); - -private: // ROS Diagnostics - /* -diagnostic_updater::Updater diagnostics_updater_; -void InitializeVelodyneDiagnostics(); -*/ - - /// @brief Get value from property_tree - /// @param pt property_tree - /// @param key Pey string - /// @return Value - std::string GetPtreeValue( - std::shared_ptr pt, const std::string & key); - /* - rclcpp::TimerBase::SharedPtr diagnostics_diag_timer_; -*/ - std::shared_ptr current_diag_tree; - /* - void OnVelodyneDiagnosticsTimer(); - void VelodyneCheckTopHv(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckTopAdTemp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckTopLm20Temp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckTopPwr5v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckTopPwr25v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckTopPwr33v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckTopPwrRaw(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckTopPwrVccint(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckBotIOut(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckBotPwr12v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckBotLm20Temp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckBotPwr5v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckBotPwr25v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckBotPwr33v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckBotPwrVIn(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckBotPwr125v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckVhv(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckAdcNf(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckAdcStats(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckIxe(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckAdctpStat(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - rclcpp::TimerBase::SharedPtr diagnostics_status_timer_; -*/ - std::shared_ptr current_status_tree; - /* - void OnVelodyneStatusTimer(); - void VelodyneCheckGpsPpsState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckGpsPosition(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckMotorState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckMotorRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckMotorLock(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckMotorPhase(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void VelodyneCheckLaserState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - - void VelodyneCheckSnapshot(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - - void OnVelodyneSnapshotTimer(); - rclcpp::TimerBase::SharedPtr diagnostics_snapshot_timer_; - rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; -*/ - std::shared_ptr current_snapshot; - std::shared_ptr current_snapshot_tree; - std::shared_ptr current_snapshot_time; - // rclcpp::Time current_snapshot_time; - // std::shared_ptr current_diag_status; - uint8_t current_diag_status; - - uint16_t diag_span_; - std::mutex mtx_diag; - std::mutex mtx_status; - std::mutex mtx_config_; - - void curl_callback(std::string err, std::string body); - /* - const char* key_volt_temp_top_hv; - const char* key_volt_temp_top_ad_temp; - const char* key_volt_temp_top_lm20_temp; - const char* key_volt_temp_top_pwr_5v; - const char* key_volt_temp_top_pwr_2_5v; - const char* key_volt_temp_top_pwr_3_3v; - const char* key_volt_temp_top_pwr_raw; - const char* key_volt_temp_top_pwr_vccint; - const char* key_volt_temp_bot_i_out; - const char* key_volt_temp_bot_pwr_1_2v; - const char* key_volt_temp_bot_lm20_temp; - const char* key_volt_temp_bot_pwr_5v; - const char* key_volt_temp_bot_pwr_2_5v; - const char* key_volt_temp_bot_pwr_3_3v; - const char* key_volt_temp_bot_pwr_v_in; - const char* key_volt_temp_bot_pwr_1_25v; - const char* key_vhv; - const char* key_adc_nf; - const char* key_adc_stats; - const char* key_ixe; - const char* key_adctp_stat; - const char* key_status_gps_pps_state; - const char* key_status_gps_pps_position; - const char* key_status_motor_state; - const char* key_status_motor_rpm; - const char* key_status_motor_lock; - const char* key_status_motor_phase; - const char* key_status_laser_state; -*/ - const char * not_supported_message; - - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); - - // rclcpp::callback_group::CallbackGroup::SharedPtr cbg_; - rclcpp::CallbackGroup::SharedPtr cbg_r_; - rclcpp::CallbackGroup::SharedPtr cbg_m_; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_VelodyneHwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp new file mode 100644 index 000000000..2ed7c4038 --- /dev/null +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -0,0 +1,106 @@ +// 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. + +#pragma once + +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/velodyne/decoder_wrapper.hpp" +#include "nebula_ros/velodyne/hw_interface_wrapper.hpp" +#include "nebula_ros/velodyne/hw_monitor_wrapper.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of velodyne driver +class VelodyneRosWrapper final : public rclcpp::Node +{ +public: + explicit VelodyneRosWrapper(const rclcpp::NodeOptions & options); + ~VelodyneRosWrapper() noexcept {}; + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart(); + +private: + void ReceiveCloudPacketCallback(std::vector & packet); + + void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + + Status DeclareAndGetSensorConfigParams(); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + Status ValidateAndSetConfig( + std::shared_ptr & new_config); + + Status wrapper_status_; + + std::shared_ptr sensor_cfg_ptr_{}; + + /// @brief Stores received packets that have not been processed yet by the decoder thread + mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_; + + std::optional hw_interface_wrapper_; + std::optional hw_monitor_wrapper_; + std::optional decoder_wrapper_; + + std::mutex mtx_config_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index d5ed36078..9836bb751 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -1,112 +1,41 @@ - - - - - + + + + + + + + + - - + + - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - + + + - - - - - - - - - - - - - - - - - - - + - - - - - - - - - - - - - - - - - + + + - - - - - - - - - - - - - - + diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index f4a1cd41c..5307d2613 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -1,96 +1,24 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml deleted file mode 100644 index 75c7414da..000000000 --- a/nebula_ros/launch/hesai_launch_component.launch.xml +++ /dev/null @@ -1,115 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index b394498fb..ca0eaaacb 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -1,194 +1,90 @@ +# 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. + import os -import warnings -from ament_index_python.packages import get_package_share_directory import launch from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription from launch.actions import OpaqueFunction -from launch.conditions import LaunchConfigurationEquals -from launch.conditions import LaunchConfigurationNotEquals from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode -import yaml - - -def get_sensor_make(sensor_name): - if sensor_name[:6].lower() == "pandar": - return "Hesai", ".csv" - elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: - return "Velodyne", ".yaml" - elif sensor_name.lower() in ["helios", "bpearl"]: - return "Robosense", None - elif sensor_name.lower() == "ars548": - return "Continental", None - return "unrecognized_sensor_model", None - -def get_plugin_name(sensor_make, sensor_model): - if sensor_make.lower() != "continental": - return sensor_make - elif sensor_model.lower() == "ars548": - return "ContinentalARS548" - else: - return "invalid_plugin" - -def is_hw_monitor_available(sensor_make): - return sensor_make.lower() != "continental" - -def launch_setup(context, *args, **kwargs): - # Model and make +from launch.substitutions import ThisLaunchFileDir +from launch_xml.launch_description_sources import XMLLaunchDescriptionSource + +SENSOR_MODELS_VELODYNE = ["VLP16", "VLP32", "VLS128"] +SENSOR_MODELS_HESAI = [ + "Pandar40P", + "Pandar64", + "Pandar128E4X", + "PandarAT128", + "PandarQT64", + "PandarQT128", + "PandarXT32", + "PandarXT32M", +] +SENSOR_MODELS_ROBOSENSE = ["Bpearl", "Helios"] +SENSOR_MODELS_CONTINENTAL = ["ARS548", "SRR520"] + +SENSOR_MODELS = ( + SENSOR_MODELS_VELODYNE + + SENSOR_MODELS_HESAI + + SENSOR_MODELS_ROBOSENSE + + SENSOR_MODELS_CONTINENTAL +) + + +def launch_setup(context: launch.LaunchContext, *args, **kwargs): sensor_model = LaunchConfiguration("sensor_model").perform(context) - calibration_file = LaunchConfiguration("calibration_file").perform(context) - correction_file = LaunchConfiguration("correction_file").perform(context) - sensor_make, sensor_extension = get_sensor_make(sensor_model) - nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") - nebula_ros_share_dir = get_package_share_directory("nebula_ros") - - # Config - sensor_params_fp = LaunchConfiguration("config_file").perform(context) - if sensor_params_fp == "": - warnings.warn("No config file provided, using sensor model default", RuntimeWarning) - sensor_params_fp = os.path.join(nebula_ros_share_dir, "config", sensor_make.lower(), sensor_model + ".yaml") - - if not os.path.exists(sensor_params_fp): - sensor_params_fp = os.path.join(nebula_ros_share_dir, "config", "BaseParams.yaml") - assert os.path.exists(sensor_params_fp), "Sensor params yaml file under config/ was not found: {}".format(sensor_params_fp) - - sensor_calib_fp = sensor_corr_fp = "" - if sensor_extension is not None: # Velodyne and Hesai - sensor_calib_fp = os.path.join(nebula_decoders_share_dir, "calibration", sensor_make.lower(), sensor_model + sensor_extension) - assert os.path.exists(sensor_calib_fp), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) - - if sensor_model.lower() == "pandarat128": - sensor_corr_fp = os.path.splitext(sensor_calib_fp)[0] + ".dat" - assert os.path.exists(sensor_corr_fp), "Sensor corr file under calibration/ was not found: {}".format(sensor_corr_fp) - - with open(sensor_params_fp, "r") as f: - sensor_params = yaml.safe_load(f)["/**"]["ros__parameters"] - nodes = [] - launch_hw = LaunchConfiguration("launch_hw").perform(context) == "true" - - if launch_hw: - nodes.append( - # HwInterface - ComposableNode( - package="nebula_ros", - plugin=get_plugin_name(sensor_make, sensor_model)+"HwInterfaceRosWrapper", - name=sensor_make.lower()+"_hw_interface_ros_wrapper_node", - parameters=[ - sensor_params, - { - "sensor_model": LaunchConfiguration("sensor_model"), - "sensor_ip": LaunchConfiguration("sensor_ip"), - "return_mode": LaunchConfiguration("return_mode"), - "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, - "setup_sensor": LaunchConfiguration("setup_sensor"), - "ptp_profile": LaunchConfiguration("ptp_profile"), - "ptp_domain": LaunchConfiguration("ptp_domain"), - "ptp_transport_type": LaunchConfiguration("ptp_transport_type"), - "ptp_switch_type": LaunchConfiguration("ptp_switch_type"), - }, - ], - ), - ) - - - if launch_hw and is_hw_monitor_available(sensor_make): - nodes.append( - # HwMonitor - ComposableNode( - package="nebula_ros", - plugin=sensor_make+"HwMonitorRosWrapper", - name=sensor_make.lower()+"_hw_monitor_ros_wrapper_node", - parameters=[ - sensor_params, - { - "sensor_model": sensor_model, - "sensor_ip": LaunchConfiguration("sensor_ip"), - "return_mode": LaunchConfiguration("return_mode"), - "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, - }, - ], - ) + launch_args = { + "sensor_model": sensor_model, + } + + if sensor_model in SENSOR_MODELS_VELODYNE: + vendor_launch_file = "velodyne_launch_all_hw.xml" + elif sensor_model in SENSOR_MODELS_HESAI: + vendor_launch_file = "hesai_launch_all_hw.xml" + elif sensor_model in SENSOR_MODELS_ROBOSENSE: + vendor_launch_file = "robosense_launch_all_hw.xml" + elif sensor_model in SENSOR_MODELS_CONTINENTAL: + vendor_launch_file = "continental_launch_all_hw.xml" + else: + raise KeyError(f"Sensor model {sensor_model} does not have an associated launch file") + + vendor_launch_file = os.path.join(ThisLaunchFileDir().perform(context), vendor_launch_file) + if not os.path.isfile(vendor_launch_file): + raise FileNotFoundError(f"Could not find launch file {vendor_launch_file}") + + # Any launch arguments not specified here are implicitly forwarded to the included launch description. + # Thus, passing arguments like `config_file` still works, it is just not appearing here. + # If it would be here, even if empty and not passed in `launch_arguments`, + # it would cause the default substitution in the included launch file to fail. + return [ + IncludeLaunchDescription( + XMLLaunchDescriptionSource(vendor_launch_file), + launch_arguments=launch_args.items(), ) - nodes.append( - ComposableNode( - package="nebula_ros", - plugin=get_plugin_name(sensor_make, sensor_model)+"DriverRosWrapper", - name=sensor_make.lower()+"_driver_ros_wrapper_node", - parameters=[ - sensor_params, - { - "sensor_model": sensor_model, - "sensor_ip": LaunchConfiguration("sensor_ip"), - "return_mode": LaunchConfiguration("return_mode"), - "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, - "launch_hw": LaunchConfiguration("launch_hw"), - "ptp_profile": LaunchConfiguration("ptp_profile"), - "ptp_domain": LaunchConfiguration("ptp_domain"), - "ptp_transport_type": LaunchConfiguration("ptp_transport_type"), - "ptp_switch_type": LaunchConfiguration("ptp_switch_type"), - }, - ], - ), - ) - - loader = LoadComposableNodes( - condition=LaunchConfigurationNotEquals("container", ""), - composable_node_descriptions=nodes, - target_container=LaunchConfiguration("container"), - ) - - container_kwargs = {} - if LaunchConfiguration("debug_logging").perform(context) == "true": - container_kwargs["ros_arguments"] = ['--log-level', 'debug'] - - container = ComposableNodeContainer( - name="nebula_ros_node", - namespace="", - package="rclcpp_components", - executable="component_container", - composable_node_descriptions=nodes, - output="screen", - condition=LaunchConfigurationEquals("container", ""), - **container_kwargs, - ) - - group = GroupAction( - [ - container, - loader, - ] - ) - - return [group] + ] def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - return launch.LaunchDescription( [ - add_launch_arg("container", ""), - add_launch_arg("config_file", ""), - add_launch_arg("sensor_model", ""), - add_launch_arg("sensor_ip", "192.168.1.201"), - add_launch_arg("calibration_file", ""), - add_launch_arg("correction_file", ""), - add_launch_arg("return_mode", "Dual"), - add_launch_arg("launch_hw", "true"), - add_launch_arg("setup_sensor", "true"), - add_launch_arg("debug_logging", "false"), - add_launch_arg("ptp_profile", "1588v2"), - add_launch_arg("ptp_domain", "0"), - add_launch_arg("ptp_transport_type", "UDP"), - add_launch_arg("ptp_switch_type", "TSN"), + DeclareLaunchArgument( + "sensor_model", + choices=SENSOR_MODELS, + description="The sensor model for which to launch Nebula", + ), + OpaqueFunction(function=launch_setup), ] - + [OpaqueFunction(function=launch_setup)] ) diff --git a/nebula_ros/launch/robosense_launch.all_hw.xml b/nebula_ros/launch/robosense_launch.all_hw.xml deleted file mode 100644 index 6e2ab6dda..000000000 --- a/nebula_ros/launch/robosense_launch.all_hw.xml +++ /dev/null @@ -1,53 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/nebula_ros/launch/robosense_launch_all_hw.xml b/nebula_ros/launch/robosense_launch_all_hw.xml new file mode 100644 index 000000000..66df73d15 --- /dev/null +++ b/nebula_ros/launch/robosense_launch_all_hw.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/launch/velodyne_launch_all_hw.xml b/nebula_ros/launch/velodyne_launch_all_hw.xml index c39437039..293b071dd 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -1,67 +1,19 @@ - - - - + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 87bc000c9..3a97db1f6 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -2,7 +2,7 @@ nebula_ros - 0.1.0 + 0.2.0 Nebula Lidar Drivers MAP IV @@ -21,12 +21,12 @@ nebula_common nebula_decoders nebula_hw_interfaces - pcl_conversions + nebula_msgs + pandar_msgs + radar_msgs rclcpp rclcpp_components robosense_msgs - tf2_eigen - tf2_geometry_msgs tf2_ros velodyne_msgs visualization_msgs diff --git a/nebula_ros/schema/ARS548.schema.json b/nebula_ros/schema/ARS548.schema.json new file mode 100644 index 000000000..1245c1190 --- /dev/null +++ b/nebula_ros/schema/ARS548.schema.json @@ -0,0 +1,92 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Radar Continental ARS548 parameters.", + "type": "object", + "definitions": { + "ARS548": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "multicast_ip": { + "$ref": "sub/communication.json#/definitions/multicast_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "configuration_host_port": { + "$ref": "sub/communication.json#/definitions/configuration_host_port" + }, + "configuration_sensor_port": { + "$ref": "sub/communication.json#/definitions/configuration_sensor_port" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "base_frame": { + "$ref": "sub/topic.json#/definitions/base_frame" + }, + "object_frame": { + "$ref": "sub/topic.json#/definitions/object_frame" + }, + "use_sensor_time": { + "$ref": "sub/topic.json#/definitions/use_sensor_time" + }, + "configuration_vehicle_length": { + "$ref": "sub/misc.json#/definitions/configuration_vehicle_length" + }, + "configuration_vehicle_width": { + "$ref": "sub/misc.json#/definitions/configuration_vehicle_width" + }, + "configuration_vehicle_height": { + "$ref": "sub/misc.json#/definitions/configuration_vehicle_height" + }, + "configuration_vehicle_wheelbase": { + "$ref": "sub/misc.json#/definitions/configuration_vehicle_wheelbase" + }, + "sensor_model": { + "$ref": "sub/radar_continental.json#/definitions/sensor_model" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "configuration_host_port", + "configuration_sensor_port", + "launch_hw", + "configuration_vehicle_length", + "configuration_vehicle_width", + "configuration_vehicle_height", + "configuration_vehicle_wheelbase", + "sensor_model" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ARS548" + } + }, + "required": [ + "ros__parameters" + ], + "additionalProperties": false + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/Bpearl.schema.json b/nebula_ros/schema/Bpearl.schema.json new file mode 100644 index 000000000..17326d0fd --- /dev/null +++ b/nebula_ros/schema/Bpearl.schema.json @@ -0,0 +1,93 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Robosense Bpearl parameters.", + "type": "object", + "definitions": { + "Bpearl": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/communication.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "scan_phase": { + "$ref": "sub/misc.json#/definitions/scan_phase" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "sensor_model": { + "$ref": "sub/lidar_robosense.json#/definitions/sensor_model" + }, + "return_mode": { + "$ref": "sub/lidar_robosense.json#/definitions/return_mode" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "diag_span", + "cloud_min_angle", + "cloud_max_angle", + "scan_phase", + "dual_return_distance_threshold", + "sensor_model", + "return_mode" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/Bpearl" + } + }, + "required": [ + "ros__parameters" + ], + "additionalProperties": false + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/Helios.schema.json b/nebula_ros/schema/Helios.schema.json new file mode 100644 index 000000000..6e4881c1c --- /dev/null +++ b/nebula_ros/schema/Helios.schema.json @@ -0,0 +1,93 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Robosense Helios parameters.", + "type": "object", + "definitions": { + "Helios": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/communication.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "scan_phase": { + "$ref": "sub/misc.json#/definitions/scan_phase" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "sensor_model": { + "$ref": "sub/lidar_robosense.json#/definitions/sensor_model" + }, + "return_mode": { + "$ref": "sub/lidar_robosense.json#/definitions/return_mode" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "diag_span", + "cloud_min_angle", + "cloud_max_angle", + "scan_phase", + "dual_return_distance_threshold", + "sensor_model", + "return_mode" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/Helios" + } + }, + "required": [ + "ros__parameters" + ], + "additionalProperties": false + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json new file mode 100644 index 000000000..656ebcbb8 --- /dev/null +++ b/nebula_ros/schema/Pandar128E4X.schema.json @@ -0,0 +1,150 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Hesai Pandar128E4X parameters.", + "type": "object", + "definitions": { + "Pandar128E4X": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "multicast_ip": { + "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/lidar_hesai.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" + }, + "sensor_model": { + "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" + }, + "calibration_file": { + "$ref": "sub/lidar_hesai.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/lidar_hesai.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/misc.json#/definitions/return_mode", + "enum": [ + "Last", + "Strongest", + "LastStrongest", + "First", + "LastFirst", + "FirstStrongest", + "Dual" + ] + }, + "ptp_profile": { + "$ref": "sub/communication.json#/definitions/ptp_profile", + "enum": [ + "automotive" + ] + }, + "ptp_domain": { + "$ref": "sub/communication.json#/definitions/ptp_domain" + }, + "ptp_transport_type": { + "$ref": "sub/communication.json#/definitions/ptp_transport_type", + "enum": [ + "L2" + ] + }, + "ptp_switch_type": { + "$ref": "sub/communication.json#/definitions/ptp_switch_type" + }, + "retry_hw": { + "$ref": "sub/hardware.json#/definitions/retry_hw" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "diag_span", + "cloud_min_angle", + "cloud_max_angle", + "sync_angle", + "cut_angle", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", + "retry_hw", + "dual_return_distance_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/Pandar128E4X" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json new file mode 100644 index 000000000..d867ea4c4 --- /dev/null +++ b/nebula_ros/schema/Pandar40P.schema.json @@ -0,0 +1,141 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Hesai Pandar40P parameters.", + "type": "object", + "definitions": { + "Pandar40P": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "multicast_ip": { + "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/lidar_hesai.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" + }, + "sensor_model": { + "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" + }, + "calibration_file": { + "$ref": "sub/lidar_hesai.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/lidar_hesai.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/misc.json#/definitions/return_mode", + "enum": [ + "Last", + "Strongest", + "LastStrongest", + "Dual" + ] + }, + "ptp_profile": { + "$ref": "sub/communication.json#/definitions/ptp_profile" + }, + "ptp_domain": { + "$ref": "sub/communication.json#/definitions/ptp_domain" + }, + "ptp_transport_type": { + "$ref": "sub/communication.json#/definitions/ptp_transport_type" + }, + "ptp_switch_type": { + "$ref": "sub/communication.json#/definitions/ptp_switch_type" + }, + "retry_hw": { + "$ref": "sub/hardware.json#/definitions/retry_hw" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "diag_span", + "cloud_min_angle", + "cloud_max_angle", + "sync_angle", + "cut_angle", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", + "retry_hw", + "dual_return_distance_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/Pandar40P" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json new file mode 100644 index 000000000..3559e8764 --- /dev/null +++ b/nebula_ros/schema/Pandar64.schema.json @@ -0,0 +1,137 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Hesai Pandar64 parameters.", + "type": "object", + "definitions": { + "Pandar64": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "multicast_ip": { + "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/lidar_hesai.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" + }, + "sensor_model": { + "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" + }, + "rotation_speed": { + "$ref": "sub/lidar_hesai.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/misc.json#/definitions/return_mode", + "enum": [ + "Last", + "Strongest", + "LastStrongest", + "Dual" + ] + }, + "ptp_profile": { + "$ref": "sub/communication.json#/definitions/ptp_profile" + }, + "ptp_domain": { + "$ref": "sub/communication.json#/definitions/ptp_domain" + }, + "ptp_transport_type": { + "$ref": "sub/communication.json#/definitions/ptp_transport_type" + }, + "ptp_switch_type": { + "$ref": "sub/communication.json#/definitions/ptp_switch_type" + }, + "retry_hw": { + "$ref": "sub/hardware.json#/definitions/retry_hw" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "diag_span", + "cloud_min_angle", + "cloud_max_angle", + "sync_angle", + "cut_angle", + "sensor_model", + "rotation_speed", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", + "retry_hw", + "dual_return_distance_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/Pandar64" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json new file mode 100644 index 000000000..81ea068f8 --- /dev/null +++ b/nebula_ros/schema/PandarAT128.schema.json @@ -0,0 +1,162 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Hesai PandarAT128 parameters.", + "type": "object", + "definitions": { + "PandarAT128": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "multicast_ip": { + "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/lidar_hesai.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "correction_file": { + "$ref": "sub/misc.json#/definitions/correction_file" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle", + "default": "30", + "minimum": 30, + "maximum": 150 + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle", + "default": "150", + "minimum": 30, + "maximum": 150 + }, + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle", + "default": "30", + "minimum": 30, + "maximum": 150 + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle", + "default": "30.0", + "minimum": 30.0, + "maximum": 150.0 + }, + "sensor_model": { + "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" + }, + "calibration_file": { + "$ref": "sub/lidar_hesai.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/hardware.json#/definitions/rotation_speed", + "default": "200", + "enum": [ + 200, + 400 + ] + }, + "return_mode": { + "$ref": "sub/misc.json#/definitions/return_mode", + "enum": [ + "Last", + "Strongest", + "LastStrongest", + "Dual" + ] + }, + "ptp_profile": { + "$ref": "sub/communication.json#/definitions/ptp_profile" + }, + "ptp_domain": { + "$ref": "sub/communication.json#/definitions/ptp_domain" + }, + "ptp_transport_type": { + "$ref": "sub/communication.json#/definitions/ptp_transport_type" + }, + "ptp_switch_type": { + "$ref": "sub/communication.json#/definitions/ptp_switch_type" + }, + "retry_hw": { + "$ref": "sub/hardware.json#/definitions/retry_hw" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "diag_span", + "correction_file", + "cloud_min_angle", + "cloud_max_angle", + "sync_angle", + "cut_angle", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", + "retry_hw", + "dual_return_distance_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/PandarAT128" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/PandarQT128.schema.json b/nebula_ros/schema/PandarQT128.schema.json new file mode 100644 index 000000000..52cbf3ed7 --- /dev/null +++ b/nebula_ros/schema/PandarQT128.schema.json @@ -0,0 +1,144 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Hesai PandarQT128 parameters.", + "type": "object", + "definitions": { + "PandarQT128": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "multicast_ip": { + "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/lidar_hesai.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" + }, + "sensor_model": { + "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" + }, + "calibration_file": { + "$ref": "sub/lidar_hesai.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/lidar_hesai.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/misc.json#/definitions/return_mode", + "enum": [ + "Last", + "Strongest", + "LastStrongest", + "First", + "LastFirst", + "FirstStrongest", + "Dual" + ] + }, + "ptp_profile": { + "$ref": "sub/communication.json#/definitions/ptp_profile" + }, + "ptp_domain": { + "$ref": "sub/communication.json#/definitions/ptp_domain" + }, + "ptp_transport_type": { + "$ref": "sub/communication.json#/definitions/ptp_transport_type" + }, + "ptp_switch_type": { + "$ref": "sub/communication.json#/definitions/ptp_switch_type" + }, + "retry_hw": { + "$ref": "sub/hardware.json#/definitions/retry_hw" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "diag_span", + "cloud_min_angle", + "cloud_max_angle", + "sync_angle", + "cut_angle", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", + "retry_hw", + "dual_return_distance_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/PandarQT128" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/PandarQT64.schema.json b/nebula_ros/schema/PandarQT64.schema.json new file mode 100644 index 000000000..d1680b0fb --- /dev/null +++ b/nebula_ros/schema/PandarQT64.schema.json @@ -0,0 +1,141 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Hesai PandarQT64 parameters.", + "type": "object", + "definitions": { + "PandarQT64": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "multicast_ip": { + "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/lidar_hesai.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" + }, + "sensor_model": { + "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" + }, + "calibration_file": { + "$ref": "sub/lidar_hesai.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/lidar_hesai.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/misc.json#/definitions/return_mode", + "enum": [ + "Last", + "First", + "LastFirst", + "Dual" + ] + }, + "ptp_profile": { + "$ref": "sub/communication.json#/definitions/ptp_profile" + }, + "ptp_domain": { + "$ref": "sub/communication.json#/definitions/ptp_domain" + }, + "ptp_transport_type": { + "$ref": "sub/communication.json#/definitions/ptp_transport_type" + }, + "ptp_switch_type": { + "$ref": "sub/communication.json#/definitions/ptp_switch_type" + }, + "retry_hw": { + "$ref": "sub/hardware.json#/definitions/retry_hw" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "diag_span", + "cloud_min_angle", + "cloud_max_angle", + "sync_angle", + "cut_angle", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", + "retry_hw", + "dual_return_distance_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/PandarQT64" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/PandarXT32.schema.json b/nebula_ros/schema/PandarXT32.schema.json new file mode 100644 index 000000000..aac236d7a --- /dev/null +++ b/nebula_ros/schema/PandarXT32.schema.json @@ -0,0 +1,144 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Hesai PandarXT32 parameters.", + "type": "object", + "definitions": { + "PandarXT32": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "multicast_ip": { + "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/lidar_hesai.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" + }, + "sensor_model": { + "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" + }, + "calibration_file": { + "$ref": "sub/lidar_hesai.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/lidar_hesai.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/misc.json#/definitions/return_mode", + "enum": [ + "Last", + "Strongest", + "LastStrongest", + "First", + "LastFirst", + "FirstStrongest", + "Dual" + ] + }, + "ptp_profile": { + "$ref": "sub/communication.json#/definitions/ptp_profile" + }, + "ptp_domain": { + "$ref": "sub/communication.json#/definitions/ptp_domain" + }, + "ptp_transport_type": { + "$ref": "sub/communication.json#/definitions/ptp_transport_type" + }, + "ptp_switch_type": { + "$ref": "sub/communication.json#/definitions/ptp_switch_type" + }, + "retry_hw": { + "$ref": "sub/hardware.json#/definitions/retry_hw" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "diag_span", + "cloud_min_angle", + "cloud_max_angle", + "sync_angle", + "cut_angle", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", + "retry_hw", + "dual_return_distance_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/PandarXT32" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/PandarXT32M.schema.json b/nebula_ros/schema/PandarXT32M.schema.json new file mode 100644 index 000000000..ea29162fc --- /dev/null +++ b/nebula_ros/schema/PandarXT32M.schema.json @@ -0,0 +1,144 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Hesai PandarXT32M parameters.", + "type": "object", + "definitions": { + "PandarXT32M": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "multicast_ip": { + "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/lidar_hesai.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" + }, + "sensor_model": { + "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" + }, + "calibration_file": { + "$ref": "sub/lidar_hesai.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/lidar_hesai.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/misc.json#/definitions/return_mode", + "enum": [ + "Last", + "Strongest", + "LastStrongest", + "First", + "LastFirst", + "FirstStrongest", + "Dual" + ] + }, + "ptp_profile": { + "$ref": "sub/communication.json#/definitions/ptp_profile" + }, + "ptp_domain": { + "$ref": "sub/communication.json#/definitions/ptp_domain" + }, + "ptp_transport_type": { + "$ref": "sub/communication.json#/definitions/ptp_transport_type" + }, + "ptp_switch_type": { + "$ref": "sub/communication.json#/definitions/ptp_switch_type" + }, + "retry_hw": { + "$ref": "sub/hardware.json#/definitions/retry_hw" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "diag_span", + "cloud_min_angle", + "cloud_max_angle", + "sync_angle", + "cut_angle", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", + "retry_hw", + "dual_return_distance_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/PandarXT32M" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/SRR520.schema.json b/nebula_ros/schema/SRR520.schema.json new file mode 100644 index 000000000..0c3004a64 --- /dev/null +++ b/nebula_ros/schema/SRR520.schema.json @@ -0,0 +1,73 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Radar Continental SRR520 parameters.", + "type": "object", + "definitions": { + "SRR520": { + "type": "object", + "properties": { + "interface": { + "$ref": "sub/communication.json#/definitions/interface" + }, + "receiver_timeout_sec": { + "$ref": "sub/communication.json#/definitions/receiver_timeout_sec" + }, + "sender_timeout_sec": { + "$ref": "sub/communication.json#/definitions/sender_timeout_sec" + }, + "filters": { + "$ref": "sub/communication.json#/definitions/filters" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "base_frame": { + "$ref": "sub/topic.json#/definitions/base_frame" + }, + "object_frame": { + "$ref": "sub/topic.json#/definitions/object_frame" + }, + "use_bus_time": { + "$ref": "sub/topic.json#/definitions/use_bus_time" + }, + "configuration_vehicle_wheelbase": { + "$ref": "sub/misc.json#/definitions/configuration_vehicle_wheelbase" + }, + "sensor_model": { + "$ref": "sub/radar_continental.json#/definitions/sensor_model" + } + }, + "required": [ + "interface", + "receiver_timeout_sec", + "sender_timeout_sec", + "filters", + "use_bus_time", + "launch_hw", + "configuration_vehicle_wheelbase", + "sensor_model" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/SRR520" + } + }, + "required": [ + "ros__parameters" + ], + "additionalProperties": false + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/VLP16.schema.json b/nebula_ros/schema/VLP16.schema.json new file mode 100644 index 000000000..db72f5778 --- /dev/null +++ b/nebula_ros/schema/VLP16.schema.json @@ -0,0 +1,109 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Velodyne VLP16 parameters.", + "type": "object", + "definitions": { + "VLP16": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/communication.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "advanced_diagnostics": { + "$ref": "sub/topic.json#/definitions/advanced_diagnostics" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "scan_phase": { + "$ref": "sub/misc.json#/definitions/scan_phase" + }, + "sensor_model": { + "$ref": "sub/lidar_velodyne.json#/definitions/sensor_model" + }, + "calibration_file": { + "$ref": "sub/lidar_velodyne.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/lidar_velodyne.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/lidar_velodyne.json#/definitions/return_mode" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "advanced_diagnostics", + "diag_span", + "min_range", + "max_range", + "cloud_min_angle", + "cloud_max_angle", + "scan_phase", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/VLP16" + } + }, + "required": [ + "ros__parameters" + ], + "additionalProperties": false + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/VLP32.schema.json b/nebula_ros/schema/VLP32.schema.json new file mode 100644 index 000000000..55cbc546d --- /dev/null +++ b/nebula_ros/schema/VLP32.schema.json @@ -0,0 +1,109 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Velodyne VLP32 parameters.", + "type": "object", + "definitions": { + "VLP32": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/communication.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "advanced_diagnostics": { + "$ref": "sub/topic.json#/definitions/advanced_diagnostics" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "scan_phase": { + "$ref": "sub/misc.json#/definitions/scan_phase" + }, + "sensor_model": { + "$ref": "sub/lidar_velodyne.json#/definitions/sensor_model" + }, + "calibration_file": { + "$ref": "sub/lidar_velodyne.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/lidar_velodyne.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/lidar_velodyne.json#/definitions/return_mode" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "advanced_diagnostics", + "diag_span", + "min_range", + "max_range", + "cloud_min_angle", + "cloud_max_angle", + "scan_phase", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/VLP32" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/VLS128.schema.json b/nebula_ros/schema/VLS128.schema.json new file mode 100644 index 000000000..86ddcb79f --- /dev/null +++ b/nebula_ros/schema/VLS128.schema.json @@ -0,0 +1,109 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Velodyne VLS128 parameters.", + "type": "object", + "definitions": { + "VLS128": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/communication.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "advanced_diagnostics": { + "$ref": "sub/topic.json#/definitions/advanced_diagnostics" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "scan_phase": { + "$ref": "sub/misc.json#/definitions/scan_phase" + }, + "sensor_model": { + "$ref": "sub/lidar_velodyne.json#/definitions/sensor_model" + }, + "calibration_file": { + "$ref": "sub/lidar_velodyne.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/lidar_velodyne.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/lidar_velodyne.json#/definitions/return_mode" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "frame_id", + "advanced_diagnostics", + "diag_span", + "min_range", + "max_range", + "cloud_min_angle", + "cloud_max_angle", + "scan_phase", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/VLS128" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/example.schema.template.json b/nebula_ros/schema/example.schema.template.json new file mode 100644 index 000000000..eef5ca4e2 --- /dev/null +++ b/nebula_ros/schema/example.schema.template.json @@ -0,0 +1,66 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": " parameters.", + "type": "object", + "definitions": { + "": { + "type": "object", + "properties": { + "example_parameter_1": { + "type": "integer", + "default": "42", + "readOnly": true, + "description": "Exclusive parameter only for this sensor model." + }, + "example_parameter_2": { + "$ref": "sub/communication.json#/definitions/example_parameter_2", + "maximum": 100, + "$comment": "Shared parameter, but needs some changes only for this sensor model." + }, + "example_parameter_3": { + "$ref": "sub/communication.json#/definitions/example_parameter_3" + }, + "example_parameter_4": { + "$ref": "sub/hardware.json#/definitions/example_parameter_4" + }, + "example_parameter_5": { + "$ref": "sub/topic.json#/definitions/example_parameter_5" + }, + "example_parameter_6": { + "$ref": "sub/misc.json#/definitions/example_parameter_6" + }, + "example_parameter_7": { + "$ref": "sub/_.json#/definitions/example_parameter_7" + } + }, + "required": [ + "example_parameter_1", + "example_parameter_2", + "example_parameter_3", + "example_parameter_4", + "example_parameter_5", + "example_parameter_6", + "example_parameter_7" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/" + } + }, + "required": [ + "ros__parameters" + ], + "additionalProperties": false + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/sub/communication.json b/nebula_ros/schema/sub/communication.json new file mode 100644 index 000000000..3823e1e97 --- /dev/null +++ b/nebula_ros/schema/sub/communication.json @@ -0,0 +1,120 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Communication parameters.", + "type": "object", + "definitions": { + "configuration_host_port": { + "type": "integer", + "default": "42401", + "minimum": 0, + "readOnly": true, + "description": "Host configuration port." + }, + "configuration_sensor_port": { + "type": "integer", + "default": "42101", + "minimum": 0, + "readOnly": true, + "description": "Sensor configuration port." + }, + "data_port": { + "type": "integer", + "default": "2368", + "minimum": 0, + "readOnly": true, + "description": "Sensor data port." + }, + "filters": { + "type": "string", + "default": "0:0", + "description": "candump-style filters for CAN frames." + }, + "gnss_port": { + "type": "integer", + "default": "2369", + "minimum": 0, + "readOnly": true, + "description": "GNSS port." + }, + "host_ip": { + "type": "string", + "default": "255.255.255.255", + "pattern": "^((25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){3}(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$", + "readOnly": true, + "description": "Host IPv4 address." + }, + "interface": { + "type": "string", + "default": "can", + "description": "CAN interface name." + }, + "multicast_ip": { + "type": "string", + "default": "224.0.0.2", + "pattern": "^((25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){3}(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$", + "readOnly": true, + "description": "Multicast IPv4 address." + }, + "packet_mtu_size": { + "type": "integer", + "default": "1500", + "minimum": 0, + "readOnly": true, + "description": "Packet MTU size." + }, + "sensor_ip": { + "type": "string", + "default": "192.168.1.201", + "pattern": "^((25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){3}(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$", + "readOnly": true, + "description": "Sensor IPv4 address." + }, + "ptp_profile": { + "type": "string", + "default": "1588v2", + "enum": [ + "1588v2", + "802.1as", + "automotive" + ], + "description": "PTP profile." + }, + "ptp_domain": { + "type": "integer", + "default": "0", + "minimum": 0, + "maximum": 127, + "description": "PTP domain (PTP operates within a logical scope)." + }, + "ptp_transport_type": { + "type": "string", + "default": "UDP", + "enum": [ + "UDP", + "L2" + ], + "description": "1588v2 supports 'UDP' or 'L2', other profiles only L2 (HW)." + }, + "ptp_switch_type": { + "type": "string", + "default": "TSN", + "enum": [ + "TSN", + "NON_TSN" + ], + "description": "For automotive profile,'TSN' or 'NON_TSN'." + }, + "receiver_timeout_sec": { + "type": "number", + "default": "0.03", + "minimum": 0.0, + "description": "Timeout for reading data from the CAN bus." + }, + "sender_timeout_sec": { + "type": "number", + "default": "0.01", + "minimum": 0.0, + "description": "Timeout for sending data to the CAN bus." + } + } +} diff --git a/nebula_ros/schema/sub/hardware.json b/nebula_ros/schema/sub/hardware.json new file mode 100644 index 000000000..ea1b17586 --- /dev/null +++ b/nebula_ros/schema/sub/hardware.json @@ -0,0 +1,33 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Hardware parameters.", + "type": "object", + "definitions": { + "launch_hw": { + "type": "boolean", + "default": "true", + "readOnly": true, + "description": "Whether network sockets should be opened or not. If disabled, replay from NebulaPackets messages is enabled automatically." + }, + "rotation_speed": { + "type": "integer", + "description": "Motor RPM, the sensor's internal spin rate." + }, + "sensor_model": { + "type": "string", + "readOnly": true, + "description": "Sensor model." + }, + "setup_sensor": { + "type": "boolean", + "default": "true", + "readOnly": true, + "description": "Enable sensor setup on hardware driver." + }, + "retry_hw": { + "type": "boolean", + "default": "true", + "description": "Whether TCP connections are retried on failure or the driver should instead exit." + } + } +} diff --git a/nebula_ros/schema/sub/lidar_hesai.json b/nebula_ros/schema/sub/lidar_hesai.json new file mode 100644 index 000000000..e4c4d5bcd --- /dev/null +++ b/nebula_ros/schema/sub/lidar_hesai.json @@ -0,0 +1,57 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Lidar Hesai parameters.", + "type": "object", + "definitions": { + "sensor_model": { + "$ref": "hardware.json#/definitions/sensor_model", + "enum": [ + "Pandar64", + "Pandar40P", + "PandarXT32", + "PandarXT32M", + "PandarQT64", + "PandarQT128", + "Pandar128E4X", + "PandarAT128" + ] + }, + "calibration_file": { + "$ref": "misc.json#/definitions/calibration_file", + "default": "$(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv" + }, + "gnss_port": { + "$ref": "communication.json#/definitions/gnss_port", + "default": "10110", + "minimum": 0 + }, + "rotation_speed": { + "$ref": "hardware.json#/definitions/rotation_speed", + "default": "600", + "minimum": 300, + "maximum": 1200, + "multipleOf": 60 + }, + "sync_angle": { + "type": "integer", + "minimum": 0, + "maximum": 359, + "default": "0", + "description": "The angle in whole degrees which should be hit on top-of-second." + }, + "cut_angle": { + "type": "number", + "minimum": 0, + "maximum": 360, + "default": "0.0", + "description": "The angle in degrees at which pointclouds are cut/published." + }, + "multicast_ip": { + "type": "string", + "default": "\"\"", + "pattern": "(^((22[4-9]|23[0-9])\\.)((25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){2}(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$)|", + "readOnly": true, + "description": "Multicast IPv4 address (leave empty to disable multicast)." + } + } +} diff --git a/nebula_ros/schema/sub/lidar_robosense.json b/nebula_ros/schema/sub/lidar_robosense.json new file mode 100644 index 000000000..5fed19f04 --- /dev/null +++ b/nebula_ros/schema/sub/lidar_robosense.json @@ -0,0 +1,23 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Lidar Robosense parameters.", + "type": "object", + "definitions": { + "sensor_model": { + "$ref": "hardware.json#/definitions/sensor_model", + "enum": [ + "Helios", + "Bpearl" + ] + }, + "return_mode": { + "$ref": "misc.json#/definitions/return_mode", + "enum": [ + "Dual", + "Strongest", + "Last", + "First" + ] + } + } +} diff --git a/nebula_ros/schema/sub/lidar_velodyne.json b/nebula_ros/schema/sub/lidar_velodyne.json new file mode 100644 index 000000000..dbb9f2c96 --- /dev/null +++ b/nebula_ros/schema/sub/lidar_velodyne.json @@ -0,0 +1,35 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Lidar Velodyne parameters.", + "type": "object", + "definitions": { + "sensor_model": { + "$ref": "hardware.json#/definitions/sensor_model", + "enum": [ + "VLP16", + "VLP32", + "VLS128" + ] + }, + "calibration_file": { + "$ref": "misc.json#/definitions/calibration_file", + "default": "$(find-pkg-share nebula_decoders)/calibration/velodyne/$(var sensor_model).yaml" + }, + "return_mode": { + "$ref": "misc.json#/definitions/return_mode", + "enum": [ + "SingleStrongest", + "SingleLast", + "Dual", + "SingleFirst" + ] + }, + "rotation_speed": { + "$ref": "hardware.json#/definitions/rotation_speed", + "default": "600", + "minimum": 300, + "maximum": 1200, + "multipleOf": 60 + } + } +} diff --git a/nebula_ros/schema/sub/misc.json b/nebula_ros/schema/sub/misc.json new file mode 100644 index 000000000..8aebfbc65 --- /dev/null +++ b/nebula_ros/schema/sub/misc.json @@ -0,0 +1,90 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Sensors common parameters.", + "type": "object", + "definitions": { + "calibration_file": { + "type": "string", + "default": "$(find-pkg-share nebula_decoders)/calibration/$(var sensor_vendor)/$(var sensor_model).yaml", + "description": "Sensor calibration file." + }, + "correction_file": { + "type": "string", + "default": "$(find-pkg-share nebula_decoders)/calibration/$(var sensor_vendor)/$(var sensor_model).dat", + "pattern": "^.*\\.dat$", + "description": "Sensor correction file." + }, + "cloud_max_angle": { + "type": "integer", + "default": "360", + "minimum": 0, + "maximum": 360, + "description": "Field of View, end degrees." + }, + "cloud_min_angle": { + "type": "integer", + "default": "0", + "minimum": 0, + "maximum": 360, + "description": "Field of View, start degrees." + }, + "configuration_vehicle_height": { + "type": "number", + "default": "2.5", + "minimum": 0.0, + "readOnly": true, + "description": "New vehicle height." + }, + "configuration_vehicle_length": { + "type": "number", + "default": "4.9", + "minimum": 0.0, + "readOnly": true, + "description": "New vehicle length." + }, + "configuration_vehicle_wheelbase": { + "type": "number", + "default": "2.8", + "minimum": 0.0, + "readOnly": true, + "description": "New vehicle wheelbase." + }, + "configuration_vehicle_width": { + "type": "number", + "default": "1.9", + "minimum": 0.0, + "readOnly": true, + "description": "New vehicle width." + }, + "dual_return_distance_threshold": { + "type": "number", + "default": "0.1", + "minimum": 0.01, + "maximum": 0.5, + "description": "Distance threshold between two neighboring points for dual return mode." + }, + "max_range": { + "type": "number", + "default": "300.0", + "minimum": 0.0, + "description": "Sensor maximum single point range." + }, + "min_range": { + "type": "number", + "default": "0.3", + "minimum": 0.0, + "description": "Sensor minimum single point range." + }, + "return_mode": { + "type": "string", + "description": "Sensor return mode." + }, + "scan_phase": { + "type": "number", + "default": "0.0", + "minimum": 0.0, + "maximum": 360.0, + "description": "Sensor scan phase." + } + } +} diff --git a/nebula_ros/schema/sub/radar_continental.json b/nebula_ros/schema/sub/radar_continental.json new file mode 100644 index 000000000..278877917 --- /dev/null +++ b/nebula_ros/schema/sub/radar_continental.json @@ -0,0 +1,14 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Radar Continental parameters.", + "type": "object", + "definitions": { + "sensor_model": { + "$ref": "hardware.json#/definitions/sensor_model", + "enum": [ + "ARS548", + "SRR520" + ] + } + } +} diff --git a/nebula_ros/schema/sub/topic.json b/nebula_ros/schema/sub/topic.json new file mode 100644 index 000000000..47fd7bb44 --- /dev/null +++ b/nebula_ros/schema/sub/topic.json @@ -0,0 +1,50 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Topic parameters.", + "type": "object", + "definitions": { + "advanced_diagnostics": { + "type": "boolean", + "default": "false", + "readOnly": true, + "description": "Enable advanced diagnostics." + }, + "base_frame": { + "type": "string", + "default": "base_link", + "readOnly": true, + "description": "Frame with true sensor pose. The final sensor data is transformed by base_frame -> frame_id." + }, + "diag_span": { + "type": "integer", + "default": "1000", + "minimum": 1, + "readOnly": true, + "description": "Diagnostics rate." + }, + "frame_id": { + "type": "string", + "default": "data_link", + "readOnly": true, + "description": "Sensor data frame_id." + }, + "object_frame": { + "type": "string", + "default": "base_link", + "readOnly": true, + "description": "Tracked objects frame." + }, + "use_bus_time": { + "type": "boolean", + "default": "false", + "readOnly": true, + "description": "Use CAN bus time for published sensor data." + }, + "use_sensor_time": { + "type": "boolean", + "default": "false", + "readOnly": true, + "description": "Use sensor time for published sensor data." + } + } +} diff --git a/nebula_ros/schema/sub/type_vendor.template.json b/nebula_ros/schema/sub/type_vendor.template.json new file mode 100644 index 000000000..5e81c4f33 --- /dev/null +++ b/nebula_ros/schema/sub/type_vendor.template.json @@ -0,0 +1,29 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": " parameters.", + "type": "object", + "definitions": { + "sensor_model": { + "$ref": "hardware.json#/definitions/sensor_model", + "enum": [ + "", + "" + ] + }, + "example_parameter_1": { + "type": "integer", + "default": "42", + "readOnly": true, + "description": "Exclusive parameter only for specific sensor vendor." + }, + "example_parameter_2": { + "$ref": "communication.json#/definitions/example_parameter_2", + "maximum": 100, + "$comment": "Shared parameter, but needs some changes only for this sensor vendor." + }, + "example_parameter_3": { + "$ref": "hardware.json#/definitions/example_parameter_3", + "$comment": "Wrong! If no changes declare parameter in schema/.schema.json." + } + } +} diff --git a/nebula_ros/src/common/parameter_descriptors.cpp b/nebula_ros/src/common/parameter_descriptors.cpp new file mode 100644 index 000000000..7f4cfed8c --- /dev/null +++ b/nebula_ros/src/common/parameter_descriptors.cpp @@ -0,0 +1,36 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/common/parameter_descriptors.hpp" + +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write() +{ + return rcl_interfaces::msg::ParameterDescriptor{}; +}; + +rcl_interfaces::msg::ParameterDescriptor param_read_only() +{ + return rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); +} + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step) +{ + return { + rcl_interfaces::msg::FloatingPointRange().set__from_value(start).set__to_value(stop).set__step( + step)}; +} + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step) +{ + return { + rcl_interfaces::msg::IntegerRange().set__from_value(start).set__to_value(stop).set__step(step)}; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp similarity index 72% rename from nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp rename to nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index ecad7752f..dede30210 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nebula_ros/continental/continental_ars548_decoder_wrapper.hpp" #include @@ -20,222 +20,112 @@ namespace nebula { namespace ros { -ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_ars548_driver_ros_wrapper", options), hw_interface_() +ContinentalARS548DecoderWrapper::ContinentalARS548DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config_ptr, + bool launch_hw) +: status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("ContinentalARS548Decoder")), + config_ptr_(config_ptr) { - drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration; + using std::chrono_literals::operator""us; + if (!config_ptr) { + throw std::runtime_error( + "ContinentalARS548DecoderWrapper cannot be instantiated without a valid config!"); + } - setvbuf(stdout, NULL, _IONBF, BUFSIZ); + RCLCPP_INFO(logger_, "Starting Decoder"); - hw_interface_.SetLogger(std::make_shared(this->get_logger())); + InitializeDriver(config_ptr); + status_ = driver_ptr_->GetStatus(); - wrapper_status_ = GetParameters(sensor_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - sensor_cfg_ptr_ = - std::make_shared( - sensor_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast( - sensor_cfg_ptr_)); + // Publish packets only if HW interface is connected + if (launch_hw) { + packets_pub_ = parent_node->create_publisher( + "nebula_packets", rclcpp::SensorDataQoS()); + } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); - packets_sub_ = create_subscription( - "nebula_packets", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback, this, std::placeholders::_1)); + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); detection_list_pub_ = - this->create_publisher( + parent_node->create_publisher( "continental_detections", rclcpp::SensorDataQoS()); - object_list_pub_ = this->create_publisher( - "continental_objects", rclcpp::SensorDataQoS()); + object_list_pub_ = + parent_node->create_publisher( + "continental_objects", rclcpp::SensorDataQoS()); - detection_pointcloud_pub_ = this->create_publisher( + detection_pointcloud_pub_ = parent_node->create_publisher( "detection_points", rclcpp::SensorDataQoS()); object_pointcloud_pub_ = - this->create_publisher("object_points", rclcpp::SensorDataQoS()); + parent_node->create_publisher("object_points", pointcloud_qos); scan_raw_pub_ = - this->create_publisher("scan_raw", rclcpp::SensorDataQoS()); + parent_node->create_publisher("scan_raw", pointcloud_qos); objects_raw_pub_ = - this->create_publisher("objects_raw", rclcpp::SensorDataQoS()); + parent_node->create_publisher("objects_raw", pointcloud_qos); objects_markers_pub_ = - this->create_publisher("marker_array", 10); + parent_node->create_publisher("marker_array", 10); diagnostics_pub_ = - this->create_publisher("diagnostics", 10); -} + parent_node->create_publisher("diagnostics", 10); -void ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback( - const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) -{ - decoder_ptr_->ProcessPackets(*scan_msg); + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE(logger_, *parent_node->get_clock(), 5000, "Missed output deadline"); + }); } -Status ContinentalARS548DriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration) +Status ContinentalARS548DecoderWrapper::InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config) { - decoder_ptr_ = std::make_shared( - std::static_pointer_cast( - sensor_configuration)); - - decoder_ptr_->RegisterDetectionListCallback(std::bind( - &ContinentalARS548DriverRosWrapper::DetectionListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalARS548DriverRosWrapper::ObjectListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterSensorStatusCallback(std::bind( - &ContinentalARS548DriverRosWrapper::SensorStatusCallback, this, std::placeholders::_1)); + driver_ptr_.reset(); + driver_ptr_ = std::make_shared(config); + + driver_ptr_->RegisterDetectionListCallback(std::bind( + &ContinentalARS548DecoderWrapper::DetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalARS548DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterSensorStatusCallback( + std::bind(&ContinentalARS548DecoderWrapper::SensorStatusCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterPacketsCallback( + std::bind(&ContinentalARS548DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); return Status::OK; } -Status ContinentalARS548DriverRosWrapper::GetStatus() +void ContinentalARS548DecoderWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & + new_config_ptr) { - return wrapper_status_; + std::lock_guard lock(mtx_driver_ptr_); + InitializeDriver(new_config_ptr); + config_ptr_ = new_config_ptr; } -Status ContinentalARS548DriverRosWrapper::GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) +void ContinentalARS548DecoderWrapper::ProcessPacket( + std::unique_ptr packet_msg) { - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_length", descriptor); - sensor_configuration.configuration_vehicle_length = - static_cast(this->get_parameter("configuration_vehicle_length").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_width", descriptor); - sensor_configuration.configuration_vehicle_width = - static_cast(this->get_parameter("configuration_vehicle_width").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_height", descriptor); - sensor_configuration.configuration_vehicle_height = - static_cast(this->get_parameter("configuration_vehicle_height").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration); + driver_ptr_->ProcessPacket(std::move(packet_msg)); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; + watchdog_->update(); } -void ContinentalARS548DriverRosWrapper::DetectionListCallback( +void ContinentalARS548DecoderWrapper::DetectionListCallback( std::unique_ptr msg) { if ( @@ -264,7 +154,7 @@ void ContinentalARS548DriverRosWrapper::DetectionListCallback( } } -void ContinentalARS548DriverRosWrapper::ObjectListCallback( +void ContinentalARS548DecoderWrapper::ObjectListCallback( std::unique_ptr msg) { if ( @@ -300,20 +190,20 @@ void ContinentalARS548DriverRosWrapper::ObjectListCallback( } } -void ContinentalARS548DriverRosWrapper::SensorStatusCallback( +void ContinentalARS548DecoderWrapper::SensorStatusCallback( const drivers::continental_ars548::ContinentalARS548Status & sensor_status) { diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; diagnostic_array_msg.header.stamp.sec = sensor_status.timestamp_seconds; diagnostic_array_msg.header.stamp.nanosec = sensor_status.timestamp_nanoseconds; - diagnostic_array_msg.header.frame_id = sensor_cfg_ptr_->frame_id; + diagnostic_array_msg.header.frame_id = config_ptr_->frame_id; diagnostic_array_msg.status.resize(1); auto & status = diagnostic_array_msg.status[0]; status.values.reserve(36); status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - status.hardware_id = sensor_cfg_ptr_->frame_id; - status.name = sensor_cfg_ptr_->frame_id; + status.hardware_id = config_ptr_->frame_id; + status.name = config_ptr_->frame_id; status.message = "Diagnostic messages from ARS548"; auto add_diagnostic = [&status](const std::string & key, const std::string & value) { @@ -414,8 +304,18 @@ void ContinentalARS548DriverRosWrapper::SensorStatusCallback( diagnostics_pub_->publish(diagnostic_array_msg); } +void ContinentalARS548DecoderWrapper::PacketsCallback( + std::unique_ptr msg) +{ + if ( + packets_pub_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + packets_pub_->publish(std::move(msg)); + } +} + pcl::PointCloud::Ptr -ContinentalARS548DriverRosWrapper::ConvertToPointcloud( +ContinentalARS548DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { pcl::PointCloud::Ptr output_pointcloud( @@ -455,7 +355,7 @@ ContinentalARS548DriverRosWrapper::ConvertToPointcloud( } pcl::PointCloud::Ptr -ContinentalARS548DriverRosWrapper::ConvertToPointcloud( +ContinentalARS548DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { pcl::PointCloud::Ptr output_pointcloud( @@ -494,7 +394,7 @@ ContinentalARS548DriverRosWrapper::ConvertToPointcloud( return output_pointcloud; } -radar_msgs::msg::RadarScan ContinentalARS548DriverRosWrapper::ConvertToRadarScan( +radar_msgs::msg::RadarScan ContinentalARS548DecoderWrapper::ConvertToRadarScan( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { radar_msgs::msg::RadarScan output_msg; @@ -520,7 +420,7 @@ radar_msgs::msg::RadarScan ContinentalARS548DriverRosWrapper::ConvertToRadarScan return output_msg; } -radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTracks( +radar_msgs::msg::RadarTracks ContinentalARS548DecoderWrapper::ConvertToRadarTracks( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { radar_msgs::msg::RadarTracks output_msg; @@ -629,7 +529,7 @@ radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTr return output_msg; } -visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertToMarkers( +visualization_msgs::msg::MarkerArray ContinentalARS548DecoderWrapper::ConvertToMarkers( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { visualization_msgs::msg::MarkerArray marker_array; @@ -692,7 +592,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT current_ids.emplace(object.object_id); visualization_msgs::msg::Marker box_marker; - box_marker.header.frame_id = sensor_cfg_ptr_->object_frame; + box_marker.header.frame_id = config_ptr_->object_frame; box_marker.header.stamp = msg.header.stamp; box_marker.ns = "boxes"; box_marker.id = object.object_id; @@ -773,7 +673,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT } visualization_msgs::msg::Marker delete_marker; - delete_marker.header.frame_id = sensor_cfg_ptr_->object_frame; + delete_marker.header.frame_id = config_ptr_->object_frame; delete_marker.header.stamp = msg.header.stamp; delete_marker.ns = "boxes"; delete_marker.id = previous_id; @@ -797,6 +697,15 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT return marker_array; } -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548DriverRosWrapper) +nebula::Status ContinentalARS548DecoderWrapper::Status() +{ + std::lock_guard lock(mtx_driver_ptr_); + + if (!driver_ptr_) { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp deleted file mode 100644 index d7baa3f6a..000000000 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,561 +0,0 @@ -// 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 - -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr - sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - hw_interface_.RegisterScanCallback(std::bind( - &ContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, - std::placeholders::_1)); - packets_pub_ = this->create_publisher( - "nebula_packets", rclcpp::SensorDataQoS()); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - - StreamStart(); -} - -ContinentalARS548HwInterfaceRosWrapper::~ContinentalARS548HwInterfaceRosWrapper() -{ -} - -Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - - if (Status::OK == interface_status_) { - odometry_sub_ = this->create_subscription( - "odometry_input", rclcpp::QoS{1}, - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, std::placeholders::_1)); - - acceleration_sub_ = create_subscription( - "acceleration_input", rclcpp::QoS{1}, - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, - std::placeholders::_1)); - - steering_angle_sub_ = create_subscription( - "steering_angle_input", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, - std::placeholders::_1)); - - set_network_configuration_service_server_ = - this->create_service( - "set_network_configuration", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_sensor_mounting_service_server_ = - this->create_service( - "set_sensor_mounting", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_vehicle_parameters_service_server_ = - this->create_service( - "set_vehicle_parameters", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_radar_parameters_service_server_ = - this->create_service( - "set_radar_parameters", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetRadarParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - } - - return interface_status_; -} - -Status ContinentalARS548HwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status ContinentalARS548HwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status ContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("multicast_ip", descriptor); - sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_host_port", descriptor); - sensor_configuration.configuration_host_port = - this->get_parameter("configuration_host_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_sensor_port", descriptor); - sensor_configuration.configuration_sensor_port = - this->get_parameter("configuration_sensor_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_length", descriptor); - sensor_configuration.configuration_vehicle_length = - static_cast(this->get_parameter("configuration_vehicle_length").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_width", descriptor); - sensor_configuration.configuration_vehicle_width = - static_cast(this->get_parameter("configuration_vehicle_width").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_height", descriptor); - sensor_configuration.configuration_vehicle_height = - static_cast(this->get_parameter("configuration_vehicle_height").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void ContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback( - std::unique_ptr scan_buffer) -{ - packets_pub_->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::continental_ars548::ContinentalARS548SensorConfiguration new_param{ - sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - - if ( - get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | - get_param(p, "sensor_ip", new_param.sensor_ip) | get_param(p, "frame_id", new_param.frame_id) | - get_param(p, "data_port", new_param.data_port) | - get_param(p, "multicast_ip", new_param.multicast_ip) | - get_param(p, "base_frame", new_param.base_frame) | - get_param(p, "object_frame", new_param.object_frame) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | - get_param(p, "configuration_vehicle_length", new_param.configuration_vehicle_length) | - get_param(p, "configuration_vehicle_width", new_param.configuration_vehicle_width) | - get_param(p, "configuration_vehicle_height", new_param.configuration_vehicle_height) | - get_param(p, "configuration_vehicle_wheelbase", new_param.configuration_vehicle_wheelbase) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -void ContinentalARS548HwInterfaceRosWrapper::OdometryCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - - constexpr float speed_to_standstill = 0.5f; - constexpr float speed_to_moving = 2.f; - - if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { - standstill_ = false; - } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { - standstill_ = true; - } - - if (standstill_) { - hw_interface_.SetDrivingDirection(0); - } else { - hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); - } - - constexpr float ms_to_kmh = 3.6f; - hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); - - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); -} - -void ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); - hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); -} - -void ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( - const std_msgs::msg::Float32::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetSensorIPAddress(request->sensor_ip.data); - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback( - const std::shared_ptr request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - - auto tf_buffer = std::make_unique(this->get_clock()); - auto tf_listener = std::make_unique(*tf_buffer); - - float longitudinal = request->longitudinal; - float lateral = request->lateral; - float vertical = request->vertical; - float yaw = request->yaw; - float pitch = request->pitch; - - if (request->autoconfigure_extrinsics) { - RCLCPP_INFO( - this->get_logger(), - "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " - "tfs"); - - geometry_msgs::msg::TransformStamped base_to_sensor_tf; - try { - base_to_sensor_tf = tf_buffer->lookupTransform( - sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", - sensor_configuration_.frame_id.c_str(), ex.what()); - response->success = false; - response->message = ex.what(); - return; - } - - const auto & quat = base_to_sensor_tf.transform.rotation; - geometry_msgs::msg::Vector3 rpy; - tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); - - longitudinal = base_to_sensor_tf.transform.translation.x - - sensor_configuration_.configuration_vehicle_wheelbase; - lateral = base_to_sensor_tf.transform.translation.y; - vertical = base_to_sensor_tf.transform.translation.z; - yaw = rpy.z; - pitch = rpy.y; - } - - auto result = hw_interface_.SetSensorMounting( - longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback( - [[maybe_unused]] const std::shared_ptr< - continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> - request, - const std::shared_ptr - response) -{ - float vehicle_length = request->vehicle_length; - float vehicle_width = request->vehicle_width; - float vehicle_height = request->vehicle_height; - float vehicle_wheelbase = request->vehicle_wheelbase; - - if (vehicle_length < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_length); - vehicle_length = sensor_configuration_.configuration_vehicle_length; - } - - if (vehicle_width < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_width); - vehicle_width = sensor_configuration_.configuration_vehicle_width; - } - - if (vehicle_height < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_height); - vehicle_height = sensor_configuration_.configuration_vehicle_height; - } - - if (vehicle_wheelbase < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_wheelbase); - vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase; - } - - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetVehicleParameters( - vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetRadarParametersRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetRadarParameters( - request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, - request->country_code, request->powersave_standstill); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -std::vector -ContinentalARS548HwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), - rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), - rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), - rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), - rclcpp::Parameter( - "configuration_sensor_port", sensor_configuration_.configuration_sensor_port), - rclcpp::Parameter( - "configuration_vehicle_length", sensor_configuration_.configuration_vehicle_length), - rclcpp::Parameter( - "configuration_vehicle_width", sensor_configuration_.configuration_vehicle_width), - rclcpp::Parameter( - "configuration_vehicle_height", sensor_configuration_.configuration_vehicle_height), - rclcpp::Parameter( - "configuration_vehicle_wheelbase", sensor_configuration_.configuration_vehicle_wheelbase)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548HwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp new file mode 100644 index 000000000..9e6d3086f --- /dev/null +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -0,0 +1,287 @@ +// 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_ros/continental/continental_ars548_hw_interface_wrapper.hpp" + +#include +#include + +namespace nebula +{ +namespace ros +{ + +ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config_ptr) +: parent_node_(parent_node), + hw_interface_( + std::make_shared()), + logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), + status_(Status::NOT_INITIALIZED), + config_ptr_(config_ptr) +{ + hw_interface_->SetLogger( + std::make_shared(parent_node->get_logger().get_child("HwInterface"))); + status_ = hw_interface_->SetSensorConfiguration(config_ptr); + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } + + status_ = Status::OK; +} + +void ContinentalARS548HwInterfaceWrapper::SensorInterfaceStart() +{ + if (Status::OK == status_) { + hw_interface_->SensorInterfaceStart(); + } + + if (Status::OK == status_) { + odometry_sub_ = + parent_node_->create_subscription( + "odometry_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceWrapper::OdometryCallback, this, std::placeholders::_1)); + + acceleration_sub_ = + parent_node_->create_subscription( + "acceleration_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceWrapper::AccelerationCallback, this, std::placeholders::_1)); + + steering_angle_sub_ = parent_node_->create_subscription( + "steering_angle_input", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalARS548HwInterfaceWrapper::SteeringAngleCallback, this, std::placeholders::_1)); + + set_network_configuration_service_server_ = + parent_node_->create_service( + "set_network_configuration", + std::bind( + &ContinentalARS548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_sensor_mounting_service_server_ = + parent_node_->create_service( + "set_sensor_mounting", + std::bind( + &ContinentalARS548HwInterfaceWrapper::SetSensorMountingRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_vehicle_parameters_service_server_ = + parent_node_->create_service( + "set_vehicle_parameters", + std::bind( + &ContinentalARS548HwInterfaceWrapper::SetVehicleParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_radar_parameters_service_server_ = + parent_node_->create_service( + "set_radar_parameters", + std::bind( + &ContinentalARS548HwInterfaceWrapper::SetRadarParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + } +} + +void ContinentalARS548HwInterfaceWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & + new_config_ptr_ptr) +{ + hw_interface_->SetSensorConfiguration(new_config_ptr_ptr); + config_ptr_ = new_config_ptr_ptr; +} + +Status ContinentalARS548HwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr +ContinentalARS548HwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +void ContinentalARS548HwInterfaceWrapper::OdometryCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) +{ + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + if (standstill_) { + hw_interface_->SetDrivingDirection(0); + } else { + hw_interface_->SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); + } + + constexpr float ms_to_kmh = 3.6f; + hw_interface_->SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); + + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_->SetYawRate(rad_to_deg * msg->twist.twist.angular.z); +} + +void ContinentalARS548HwInterfaceWrapper::AccelerationCallback( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + hw_interface_->SetAccelerationLateralCog(msg->accel.accel.linear.y); + hw_interface_->SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); +} + +void ContinentalARS548HwInterfaceWrapper::SteeringAngleCallback( + const std_msgs::msg::Float32::SharedPtr msg) +{ + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_->SetSteeringAngleFrontAxle(rad_to_deg * msg->data); +} + +void ContinentalARS548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + auto result = hw_interface_->SetSensorIPAddress(request->sensor_ip.data); + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalARS548HwInterfaceWrapper::SetSensorMountingRequestCallback( + const std::shared_ptr request, + const std::shared_ptr + response) +{ + auto tf_buffer = std::make_unique(parent_node_->get_clock()); + auto tf_listener = std::make_unique(*tf_buffer); + + float longitudinal = request->longitudinal; + float lateral = request->lateral; + float vertical = request->vertical; + float yaw = request->yaw; + float pitch = request->pitch; + + if (request->autoconfigure_extrinsics) { + RCLCPP_INFO( + logger_, + "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " + "tfs"); + + geometry_msgs::msg::TransformStamped base_to_sensor_tf; + try { + base_to_sensor_tf = tf_buffer->lookupTransform( + config_ptr_->base_frame, config_ptr_->frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, "Could not obtain the transform from the base frame to %s (%s)", + config_ptr_->frame_id.c_str(), ex.what()); + response->success = false; + response->message = ex.what(); + return; + } + + const auto & quat = base_to_sensor_tf.transform.rotation; + geometry_msgs::msg::Vector3 rpy; + tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); + + longitudinal = + base_to_sensor_tf.transform.translation.x - config_ptr_->configuration_vehicle_wheelbase; + lateral = base_to_sensor_tf.transform.translation.y; + vertical = base_to_sensor_tf.transform.translation.z; + yaw = rpy.z; + pitch = rpy.y; + } + + auto result = hw_interface_->SetSensorMounting( + longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalARS548HwInterfaceWrapper::SetVehicleParametersRequestCallback( + [[maybe_unused]] const std::shared_ptr< + continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> + request, + const std::shared_ptr + response) +{ + float vehicle_length = request->vehicle_length; + float vehicle_width = request->vehicle_width; + float vehicle_height = request->vehicle_height; + float vehicle_wheelbase = request->vehicle_wheelbase; + + if (vehicle_length < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", + config_ptr_->configuration_vehicle_length); + vehicle_length = config_ptr_->configuration_vehicle_length; + } + + if (vehicle_width < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", + config_ptr_->configuration_vehicle_width); + vehicle_width = config_ptr_->configuration_vehicle_width; + } + + if (vehicle_height < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", + config_ptr_->configuration_vehicle_height); + vehicle_height = config_ptr_->configuration_vehicle_height; + } + + if (vehicle_wheelbase < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", + config_ptr_->configuration_vehicle_wheelbase); + vehicle_wheelbase = config_ptr_->configuration_vehicle_wheelbase; + } + + auto result = hw_interface_->SetVehicleParameters( + vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalARS548HwInterfaceWrapper::SetRadarParametersRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + auto result = hw_interface_->SetRadarParameters( + request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, + request->country_code, request->powersave_standstill); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp new file mode 100644 index 000000000..7815a8742 --- /dev/null +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -0,0 +1,235 @@ +// 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_ros/continental/continental_ars548_ros_wrapper.hpp" + +#pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" + +namespace nebula +{ +namespace ros +{ +ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node( + "continental_ars548_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + packet_queue_(3000), + hw_interface_wrapper_(), + decoder_wrapper_() +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *config_ptr_); + + launch_hw_ = declare_parameter("launch_hw", param_read_only()); + + if (launch_hw_) { + hw_interface_wrapper_.emplace(this, config_ptr_); + } + + decoder_wrapper_.emplace(this, config_ptr_, launch_hw_); + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessPacket(packet_queue_.pop()); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterPacketCallback( + std::bind(&ContinentalARS548RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind(&ContinentalARS548RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&ContinentalARS548RosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status ContinentalARS548RosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration config; + + config.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", param_read_only())); + config.host_ip = declare_parameter("host_ip", param_read_only()); + config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); + config.multicast_ip = declare_parameter("multicast_ip", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_write()); + config.base_frame = declare_parameter("base_frame", param_read_write()); + config.object_frame = declare_parameter("object_frame", param_read_write()); + config.data_port = static_cast(declare_parameter("data_port", param_read_only())); + config.configuration_host_port = + static_cast(declare_parameter("configuration_host_port", param_read_only())); + config.configuration_sensor_port = + static_cast(declare_parameter("configuration_sensor_port", param_read_only())); + config.use_sensor_time = declare_parameter("use_sensor_time", param_read_write()); + config.configuration_vehicle_length = static_cast( + declare_parameter("configuration_vehicle_length", param_read_write())); + config.configuration_vehicle_width = static_cast( + declare_parameter("configuration_vehicle_width", param_read_write())); + config.configuration_vehicle_height = static_cast( + declare_parameter("configuration_vehicle_height", param_read_write())); + config.configuration_vehicle_wheelbase = static_cast( + declare_parameter("configuration_vehicle_wheelbase", param_read_write())); + + if (config.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration>(config); + return ValidateAndSetConfig(new_config_ptr); +} + +Status ContinentalARS548RosWrapper::ValidateAndSetConfig( + std::shared_ptr & + new_config_ptr) +{ + if (new_config_ptr->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (new_config_ptr->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config_ptr); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config_ptr); + } + + config_ptr_ = new_config_ptr; + return Status::OK; +} + +void ContinentalARS548RosWrapper::ReceivePacketsCallback( + std::unique_ptr packets_msg_ptr) +{ + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring NebulaPackets. Launch with launch_hw:=false to enable NebulaPackets " + "replay."); + return; + } + + for (auto & packet : packets_msg_ptr->packets) { + auto nebula_packet_ptr = std::make_unique(); + nebula_packet_ptr->stamp = packet.stamp; + nebula_packet_ptr->data = std::move(packet.data); + + packet_queue_.push(std::move(nebula_packet_ptr)); + } +} + +void ContinentalARS548RosWrapper::ReceivePacketCallback( + std::unique_ptr msg_ptr) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +Status ContinentalARS548RosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalARS548RosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) { + return hw_interface_wrapper_->Status(); + } + + hw_interface_wrapper_->SensorInterfaceStart(); + + return hw_interface_wrapper_->Status(); +} + +rcl_interfaces::msg::SetParametersResult ContinentalARS548RosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + std::scoped_lock lock(mtx_config_); + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::continental_ars548::ContinentalARS548SensorConfiguration new_config(*config_ptr_); + + bool got_any = + get_param(p, "frame_id", new_config.frame_id) | + get_param(p, "base_frame", new_config.base_frame) | + get_param(p, "object_frame", new_config.object_frame) | + get_param(p, "configuration_vehicle_length", new_config.configuration_vehicle_length) | + get_param(p, "configuration_vehicle_width", new_config.configuration_vehicle_width) | + get_param(p, "configuration_vehicle_height", new_config.configuration_vehicle_height) | + get_param(p, "configuration_vehicle_wheelbase", new_config.configuration_vehicle_wheelbase) | + get_param(p, "configuration_host_port", new_config.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_config.configuration_sensor_port); + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration>(new_config); + auto status = ValidateAndSetConfig(new_config_ptr); + + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548RosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp new file mode 100644 index 000000000..e85e6cdca --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp @@ -0,0 +1,603 @@ +// 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_ros/continental/continental_srr520_decoder_wrapper.hpp" + +#include + +namespace nebula +{ +namespace ros +{ +ContinentalSRR520DecoderWrapper::ContinentalSRR520DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config, + std::shared_ptr hw_interface_ptr) +: parent_node_(parent_node), + status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("ContinentalSRR520Decoder")), + sensor_cfg_(config), + hw_interface_ptr_(hw_interface_ptr) +{ + using std::chrono_literals::operator""us; + if (!config) { + throw std::runtime_error( + "ContinentalSRR520DecoderWrapper cannot be instantiated without a valid config!"); + } + + RCLCPP_INFO(logger_, "Starting Decoder"); + + InitializeDriver(config); + status_ = driver_ptr_->GetStatus(); + + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); + } + + // Publish packets only if HW interface is connected + if (hw_interface_ptr_) { + packets_pub_ = parent_node->create_publisher( + "nebula_packets", rclcpp::SensorDataQoS()); + } + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + near_detection_list_pub_ = + parent_node->create_publisher( + "near_continental_detections", pointcloud_qos); + hrr_detection_list_pub_ = + parent_node->create_publisher( + "hrr_continental_detections", pointcloud_qos); + object_list_pub_ = + parent_node->create_publisher( + "continental_objects", pointcloud_qos); + status_pub_ = + parent_node->create_publisher("diagnostics", 10); + + near_detection_pointcloud_pub_ = parent_node->create_publisher( + "near_detection_points", pointcloud_qos); + hrr_detection_pointcloud_pub_ = parent_node->create_publisher( + "hrr_detection_points", pointcloud_qos); + object_pointcloud_pub_ = + parent_node->create_publisher("object_points", pointcloud_qos); + + near_scan_raw_pub_ = + parent_node->create_publisher("near_scan_raw", pointcloud_qos); + hrr_scan_raw_pub_ = + parent_node->create_publisher("hrr_scan_raw", pointcloud_qos); + + objects_raw_pub_ = + parent_node->create_publisher("objects_raw", pointcloud_qos); + + objects_markers_pub_ = + parent_node->create_publisher("marker_array", 10); + + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE( + logger_, *parent_node->get_clock(), 5000, "Missed sensor output deadline"); + }); +} + +Status ContinentalSRR520DecoderWrapper::InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & config) +{ + driver_ptr_.reset(); + driver_ptr_ = std::make_shared(config); + + driver_ptr_->RegisterNearDetectionListCallback(std::bind( + &ContinentalSRR520DecoderWrapper::NearDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterHRRDetectionListCallback(std::bind( + &ContinentalSRR520DecoderWrapper::HRRDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalSRR520DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterStatusCallback( + std::bind(&ContinentalSRR520DecoderWrapper::StatusCallback, this, std::placeholders::_1)); + + if (hw_interface_ptr_) { + driver_ptr_->RegisterSyncFollowUpCallback(std::bind( + &ContinentalSRR520DecoderWrapper::SyncFollowUpCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterPacketsCallback( + std::bind(&ContinentalSRR520DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); + } + + driver_ptr_->SetLogger( + std::make_shared(parent_node_->get_logger().get_child("Driver"))); + + return Status::OK; +} + +void ContinentalSRR520DecoderWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & new_config) +{ + std::lock_guard lock(mtx_driver_ptr_); + InitializeDriver(new_config); + sensor_cfg_ = new_config; +} + +void ContinentalSRR520DecoderWrapper::ProcessPacket( + std::unique_ptr packet_msg) +{ + driver_ptr_->ProcessPacket(std::move(packet_msg)); + + watchdog_->update(); +} + +void ContinentalSRR520DecoderWrapper::NearDetectionListCallback( + std::unique_ptr msg) +{ + if ( + near_detection_pointcloud_pub_->get_subscription_count() > 0 || + near_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + auto detection_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); + + detection_pointcloud_msg_ptr->header = msg->header; + near_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + + if ( + near_scan_raw_pub_->get_subscription_count() > 0 || + near_scan_raw_pub_->get_intra_process_subscription_count() > 0) { + auto radar_scan_msg = ConvertToRadarScan(*msg); + radar_scan_msg.header = msg->header; + near_scan_raw_pub_->publish(std::move(radar_scan_msg)); + } + + if ( + near_detection_list_pub_->get_subscription_count() > 0 || + near_detection_list_pub_->get_intra_process_subscription_count() > 0) { + near_detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSRR520DecoderWrapper::HRRDetectionListCallback( + std::unique_ptr msg) +{ + if ( + hrr_detection_pointcloud_pub_->get_subscription_count() > 0 || + hrr_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + auto detection_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); + + detection_pointcloud_msg_ptr->header = msg->header; + hrr_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + + if ( + hrr_scan_raw_pub_->get_subscription_count() > 0 || + hrr_scan_raw_pub_->get_intra_process_subscription_count() > 0) { + auto radar_scan_msg = ConvertToRadarScan(*msg); + radar_scan_msg.header = msg->header; + hrr_scan_raw_pub_->publish(std::move(radar_scan_msg)); + } + + if ( + hrr_detection_list_pub_->get_subscription_count() > 0 || + hrr_detection_list_pub_->get_intra_process_subscription_count() > 0) { + hrr_detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSRR520DecoderWrapper::ObjectListCallback( + std::unique_ptr msg) +{ + if ( + object_pointcloud_pub_->get_subscription_count() > 0 || + object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto object_pointcloud_ptr = ConvertToPointcloud(*msg); + auto object_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr); + + object_pointcloud_msg_ptr->header = msg->header; + object_pointcloud_pub_->publish(std::move(object_pointcloud_msg_ptr)); + } + + if ( + objects_raw_pub_->get_subscription_count() > 0 || + objects_raw_pub_->get_intra_process_subscription_count() > 0) { + auto objects_raw_msg = ConvertToRadarTracks(*msg); + objects_raw_msg.header = msg->header; + objects_raw_pub_->publish(std::move(objects_raw_msg)); + } + + if ( + objects_markers_pub_->get_subscription_count() > 0 || + objects_markers_pub_->get_intra_process_subscription_count() > 0) { + auto marker_array_msg = ConvertToMarkers(*msg); + objects_markers_pub_->publish(std::move(marker_array_msg)); + } + + if ( + object_list_pub_->get_subscription_count() > 0 || + object_list_pub_->get_intra_process_subscription_count() > 0) { + object_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSRR520DecoderWrapper::StatusCallback( + std::unique_ptr status_msg_ptr) +{ + status_pub_->publish(std::move(status_msg_ptr)); +} + +pcl::PointCloud::Ptr +ContinentalSRR520DecoderWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.detections.size()); + + nebula::drivers::continental_srr520::PointSRR520Detection point{}; + for (const auto & detection : msg.detections) { + point.x = std::cos(detection.azimuth_angle) * detection.range; + point.y = std::sin(detection.azimuth_angle) * detection.range; + point.z = 0.f; + + point.azimuth = detection.azimuth_angle; + point.range = detection.range; + point.range_rate = detection.range_rate; + point.rcs = detection.rcs; + point.snr = detection.snr; + point.pdh00 = detection.pdh00; + point.pdh01 = detection.pdh01; + point.pdh02 = detection.pdh02; + point.pdh03 = detection.pdh03; + point.pdh04 = detection.pdh04; + point.pdh05 = detection.pdh05; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +pcl::PointCloud::Ptr +ContinentalSRR520DecoderWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.objects.size()); + + nebula::drivers::continental_srr520::PointSRR520Object point{}; + for (const auto & object : msg.objects) { + point.x = object.dist_x; + point.y = object.dist_y; + point.z = 0.f; + + point.id = object.object_id; + point.age = object.life_cycles; + + point.box_length = object.box_length; + point.box_width = object.box_width; + + point.object_status = object.object_status; + point.orientation = object.orientation; + point.rcs = object.rcs; + point.score = object.score; + point.dynamics_abs_vel_x = object.v_abs_x; + point.dynamics_abs_vel_y = object.v_abs_y; + point.dynamics_abs_acc_x = object.a_abs_x; + point.dynamics_abs_acc_y = object.a_abs_y; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +radar_msgs::msg::RadarScan ContinentalSRR520DecoderWrapper::ConvertToRadarScan( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg) +{ + radar_msgs::msg::RadarScan output_msg; + output_msg.header = msg.header; + output_msg.returns.reserve(msg.detections.size()); + + radar_msgs::msg::RadarReturn return_msg; + for (const auto & detection : msg.detections) { + if (detection.pdh00 > 0 || detection.pdh00 > 1 || detection.pdh02 > 0) { + continue; + } + + return_msg.range = detection.range; + return_msg.azimuth = detection.azimuth_angle; + return_msg.elevation = 0.f; + return_msg.doppler_velocity = detection.range_rate; + return_msg.amplitude = detection.rcs; + output_msg.returns.emplace_back(return_msg); + } + + return output_msg; +} + +radar_msgs::msg::RadarTracks ContinentalSRR520DecoderWrapper::ConvertToRadarTracks( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + radar_msgs::msg::RadarTracks output_msg; + output_msg.tracks.reserve(msg.objects.size()); + output_msg.header = msg.header; + + constexpr int16_t UNKNOWN_ID = 32000; + constexpr float INVALID_COVARIANCE = 1e6; + + radar_msgs::msg::RadarTrack track_msg; + for (const auto & object : msg.objects) { + if (!object.box_valid || object.object_status == 0) { + continue; + } + + track_msg.uuid.uuid[0] = static_cast(object.object_id & 0xff); + track_msg.uuid.uuid[1] = static_cast((object.object_id >> 8) & 0xff); + track_msg.uuid.uuid[2] = static_cast((object.object_id >> 16) & 0xff); + track_msg.uuid.uuid[3] = static_cast((object.object_id >> 24) & 0xff); + + track_msg.position.x = static_cast(object.dist_x); + track_msg.position.y = static_cast(object.dist_y); + track_msg.position.z = 0.0; + + track_msg.velocity.x = static_cast(object.v_abs_x); + track_msg.velocity.y = static_cast(object.v_abs_y); + track_msg.velocity.z = 0.0; + track_msg.acceleration.x = static_cast(object.a_abs_x); + track_msg.acceleration.y = static_cast(object.a_abs_y); + track_msg.acceleration.z = 0.0; + track_msg.size.x = object.box_length; + track_msg.size.y = object.box_width; + track_msg.size.z = 1.f; + + track_msg.classification = UNKNOWN_ID; + + track_msg.position_covariance[0] = object.dist_x_std * object.dist_x_std; + track_msg.position_covariance[1] = INVALID_COVARIANCE; + track_msg.position_covariance[2] = 0.f; + track_msg.position_covariance[3] = object.dist_y_std * object.dist_y_std; + track_msg.position_covariance[4] = 0.f; + track_msg.position_covariance[5] = INVALID_COVARIANCE; + + track_msg.velocity_covariance[0] = object.v_abs_x_std * object.v_abs_x_std; + track_msg.velocity_covariance[1] = INVALID_COVARIANCE; + track_msg.velocity_covariance[2] = 0.f; + track_msg.velocity_covariance[3] = object.v_abs_y_std * object.v_abs_y_std; + track_msg.velocity_covariance[4] = 0.f; + track_msg.velocity_covariance[5] = INVALID_COVARIANCE; + + track_msg.acceleration_covariance[0] = object.a_abs_x_std * object.a_abs_x_std; + track_msg.acceleration_covariance[1] = INVALID_COVARIANCE; + track_msg.acceleration_covariance[2] = 0.f; + track_msg.acceleration_covariance[3] = object.a_abs_y_std * object.a_abs_y_std; + track_msg.acceleration_covariance[4] = 0.f; + track_msg.acceleration_covariance[5] = INVALID_COVARIANCE; + + track_msg.size_covariance[0] = INVALID_COVARIANCE; + track_msg.size_covariance[1] = 0.f; + track_msg.size_covariance[2] = 0.f; + track_msg.size_covariance[3] = INVALID_COVARIANCE; + track_msg.size_covariance[4] = 0.f; + track_msg.size_covariance[5] = INVALID_COVARIANCE; + + output_msg.tracks.emplace_back(track_msg); + } + + return output_msg; +} + +visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::ConvertToMarkers( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.reserve(4 * msg.objects.size()); + + constexpr int LINE_STRIP_CORNERS_NUM = 17; + constexpr std::array, LINE_STRIP_CORNERS_NUM> cube_corners = { + {{{-1.0, -1.0, -1.0}}, + {{-1.0, -1.0, 1.0}}, + {{-1.0, 1.0, 1.0}}, + {{-1.0, 1.0, -1.0}}, + {{-1.0, -1.0, -1.0}}, + {{1.0, -1.0, -1.0}}, + {{1.0, -1.0, 1.0}}, + {{1.0, 1.0, 1.0}}, + {{1.0, 1.0, -1.0}}, + {{1.0, -1.0, -1.0}}, + {{-1.0, -1.0, -1.0}}, + {{-1.0, -1.0, 1.0}}, + {{1.0, -1.0, 1.0}}, + {{1.0, 1.0, 1.0}}, + {{-1.0, 1.0, 1.0}}, + {{-1.0, 1.0, -1.0}}, + {{1.0, 1.0, -1.0}}}}; + + constexpr int PALETTE_SIZE = 32; + constexpr std::array, PALETTE_SIZE> color_array = {{ + {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, + {{0.0, 0.0, 1.0}}, // Red, Green, Blue + {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, + {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta + {{1.0, 0.647, 0.0}}, {{0.749, 1.0, 0.0}}, + {{0.0, 0.502, 0.502}}, // Orange, Lime, Teal + {{0.502, 0.0, 0.502}}, {{1.0, 0.753, 0.796}}, + {{0.647, 0.165, 0.165}}, // Purple, Pink, Brown + {{0.502, 0.0, 0.0}}, {{0.502, 0.502, 0.0}}, + {{0.0, 0.0, 0.502}}, // Maroon, Olive, Navy + {{0.502, 0.502, 0.502}}, {{1.0, 0.4, 0.4}}, + {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green + {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, + {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan + {{1.0, 0.4, 1.0}}, {{1.0, 0.698, 0.4}}, + {{0.698, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple + {{1.0, 0.6, 0.8}}, {{0.71, 0.396, 0.114}}, + {{0.545, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red + {{0.0, 0.392, 0.0}}, {{0.0, 0.0, 0.545}}, + {{0.545, 0.545, 0.0}}, // Dark Green, Dark Blue, Dark Yellow + {{0.0, 0.545, 0.545}}, {{0.545, 0.0, 0.545}} // Dark Cyan, Dark Magenta + }}; + + std::unordered_set current_ids; + + for (const auto & object : msg.objects) { + if (!object.box_valid || object.object_status == 0) { + continue; + } + + const double half_length = 0.5 * object.box_length; + const double half_width = 0.5 * object.box_width; + constexpr double DEFAULT_HALF_SIZE = 1.0; + + const double & yaw = object.orientation; + current_ids.emplace(object.object_id); + + visualization_msgs::msg::Marker box_marker; + box_marker.header.frame_id = sensor_cfg_->base_frame; + box_marker.header.stamp = msg.header.stamp; + box_marker.ns = "boxes"; + box_marker.id = object.object_id; + box_marker.action = visualization_msgs::msg::Marker::ADD; + box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + box_marker.lifetime = rclcpp::Duration::from_seconds(0); + box_marker.color.r = color_array[object.object_id % PALETTE_SIZE][0]; + box_marker.color.g = color_array[object.object_id % PALETTE_SIZE][1]; + box_marker.color.b = color_array[object.object_id % PALETTE_SIZE][2]; + box_marker.color.a = 1.0; + box_marker.scale.x = 0.1; + + box_marker.pose.position.x = object.dist_x; + box_marker.pose.position.y = object.dist_y; + box_marker.pose.position.z = DEFAULT_HALF_SIZE; + box_marker.pose.orientation.w = std::cos(0.5 * yaw); + box_marker.pose.orientation.z = std::sin(0.5 * yaw); + + for (const auto & corner : cube_corners) { + geometry_msgs::msg::Point p; + p.x = half_length * corner[0]; + p.y = half_width * corner[1]; + p.z = DEFAULT_HALF_SIZE * corner[2]; + box_marker.points.emplace_back(p); + } + + marker_array.markers.emplace_back(box_marker); + + visualization_msgs::msg::Marker text_marker = box_marker; + text_marker.ns = "object_age"; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + text_marker.scale.x = 0.3; + text_marker.scale.y = 0.3; + text_marker.scale.z = 0.3; + text_marker.pose.position.z += 0.5; + text_marker.text = "ID=" + std::to_string(object.object_id) + + " Age=" + std::to_string(object.life_cycles) + "ms"; + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_status_ss; + object_status_ss << std::fixed << std::setprecision(3) + << "ID=" << static_cast(object.object_id) << "\n" + << static_cast(object.box_length) << "/" + << static_cast(object.object_status); + + text_marker.ns = "object_status"; + text_marker.text = object_status_ss.str(); + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_dynamics_ss; + object_dynamics_ss << std::fixed << std::setprecision(3) + << "ID=" << static_cast(object.object_id) + << "\nyaw=" << object.orientation << "\nvx=" << object.v_abs_x + << "\nvy=" << object.v_abs_y << "\nax=" << object.a_abs_x + << "\nay=" << object.a_abs_y; + + text_marker.ns = "object_dynamics"; + text_marker.text = object_dynamics_ss.str(); + + marker_array.markers.emplace_back(text_marker); + } + + for (const auto & previous_id : previous_ids_) { + if (current_ids.find(previous_id) != current_ids.end()) { + continue; + } + + visualization_msgs::msg::Marker delete_marker; + delete_marker.header.frame_id = sensor_cfg_->base_frame; + delete_marker.header.stamp = msg.header.stamp; + delete_marker.ns = "boxes"; + delete_marker.id = previous_id; + delete_marker.action = visualization_msgs::msg::Marker::DELETE; + + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_age"; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_status"; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_dynamics"; + marker_array.markers.push_back(delete_marker); + } + + previous_ids_.clear(); + previous_ids_ = current_ids; + + return marker_array; +} + +void ContinentalSRR520DecoderWrapper::SyncFollowUpCallback(builtin_interfaces::msg::Time stamp) +{ + hw_interface_ptr_->SensorSyncFollowUp(stamp); +} + +void ContinentalSRR520DecoderWrapper::PacketsCallback( + std::unique_ptr msg) +{ + if ( + packets_pub_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + packets_pub_->publish(std::move(msg)); + } +} + +nebula::Status ContinentalSRR520DecoderWrapper::Status() +{ + std::lock_guard lock(mtx_driver_ptr_); + + if (!driver_ptr_) { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp new file mode 100644 index 000000000..42745fa58 --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp @@ -0,0 +1,183 @@ +// 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_ros/continental/continental_srr520_hw_interface_wrapper.hpp" + +#include +#include + +namespace nebula +{ +namespace ros +{ + +ContinentalSRR520HwInterfaceWrapper::ContinentalSRR520HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config_ptr) +: parent_node_(parent_node), + hw_interface_( + std::make_shared()), + logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), + status_(Status::NOT_INITIALIZED), + config_ptr_(config_ptr) +{ + hw_interface_->SetLogger( + std::make_shared(parent_node->get_logger().get_child("HwInterface"))); + status_ = hw_interface_->SetSensorConfiguration(config_ptr_); + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } + + status_ = Status::OK; +} + +void ContinentalSRR520HwInterfaceWrapper::SensorInterfaceStart() +{ + using std::chrono_literals::operator""ms; + + if (Status::OK == status_) { + hw_interface_->SensorInterfaceStart(); + } + + if (Status::OK == status_) { + odometry_sub_.subscribe(parent_node_, "odometry_input"); + acceleration_sub_.subscribe(parent_node_, "acceleration_input"); + + sync_ptr_ = + std::make_shared(ExactTimeSyncPolicy(10), odometry_sub_, acceleration_sub_); + sync_ptr_->registerCallback(&ContinentalSRR520HwInterfaceWrapper::dynamicsCallback, this); + + configure_sensor_service_server_ = + parent_node_->create_service( + "configure_sensor", std::bind( + &ContinentalSRR520HwInterfaceWrapper::ConfigureSensorRequestCallback, + this, std::placeholders::_1, std::placeholders::_2)); + + sync_timer_ = rclcpp::create_timer( + parent_node_, parent_node_->get_clock(), 100ms, + std::bind(&ContinentalSRR520HwInterfaceWrapper::syncTimerCallback, this)); + } +} + +void ContinentalSRR520HwInterfaceWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & + new_config_ptr) +{ + hw_interface_->SetSensorConfiguration(new_config_ptr); + config_ptr_ = new_config_ptr; +} + +Status ContinentalSRR520HwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr +ContinentalSRR520HwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +void ContinentalSRR520HwInterfaceWrapper::dynamicsCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg) +{ + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(odometry_msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(odometry_msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + hw_interface_->SetVehicleDynamics( + acceleration_msg->accel.accel.linear.x, acceleration_msg->accel.accel.linear.y, + odometry_msg->twist.twist.angular.z, odometry_msg->twist.twist.linear.x, standstill_); +} + +void ContinentalSRR520HwInterfaceWrapper::ConfigureSensorRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + auto tf_buffer = std::make_unique(parent_node_->get_clock()); + auto tf_listener = std::make_unique(*tf_buffer); + + float longitudinal = request->longitudinal; + float lateral = request->lateral; + float vertical = request->vertical; + float yaw = request->yaw; + float vehicle_wheelbase = request->vehicle_wheelbase; + + if (vehicle_wheelbase < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", + config_ptr_->configuration_vehicle_wheelbase); + vehicle_wheelbase = config_ptr_->configuration_vehicle_wheelbase; + } + + if (request->autoconfigure_extrinsics) { + RCLCPP_INFO( + logger_, + "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " + "tfs"); + + geometry_msgs::msg::TransformStamped base_to_sensor_tf; + try { + base_to_sensor_tf = tf_buffer->lookupTransform( + config_ptr_->base_frame, config_ptr_->frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, "Could not obtain the transform from the base frame to %s (%s)", + config_ptr_->frame_id.c_str(), ex.what()); + response->success = false; + response->message = ex.what(); + return; + } + + const auto & quat = base_to_sensor_tf.transform.rotation; + geometry_msgs::msg::Vector3 rpy; + tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); + + longitudinal = base_to_sensor_tf.transform.translation.x - vehicle_wheelbase; + lateral = base_to_sensor_tf.transform.translation.y; + vertical = base_to_sensor_tf.transform.translation.z; + yaw = rpy.z; + } + + yaw = std::min(std::max(yaw, -3.14159f), 3.14159f); + + auto result = hw_interface_->ConfigureSensor( + request->sensor_id, longitudinal, lateral, vertical, yaw, + longitudinal + 0.5 * vehicle_wheelbase, vehicle_wheelbase, request->cover_damping, + request->plug_bottom, request->reset_sensor_configuration); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalSRR520HwInterfaceWrapper::syncTimerCallback() +{ + hw_interface_->SensorSync(); +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp new file mode 100644 index 000000000..e0bed3ca6 --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -0,0 +1,220 @@ +// 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_ros/continental/continental_srr520_ros_wrapper.hpp" + +#pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" + +namespace nebula +{ +namespace ros +{ +ContinentalSRR520RosWrapper::ContinentalSRR520RosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node( + "continental_srr520_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + packet_queue_(3000), + hw_interface_wrapper_(), + decoder_wrapper_() +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *config_ptr_); + + launch_hw_ = declare_parameter("launch_hw", param_read_only()); + + if (launch_hw_) { + hw_interface_wrapper_.emplace(this, config_ptr_); + decoder_wrapper_.emplace(this, config_ptr_, hw_interface_wrapper_->HwInterface()); + } else { + decoder_wrapper_.emplace(this, config_ptr_, nullptr); + } + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessPacket(packet_queue_.pop()); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterPacketCallback( + std::bind(&ContinentalSRR520RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind(&ContinentalSRR520RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&ContinentalSRR520RosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status ContinentalSRR520RosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration config; + + config.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", param_read_only())); + config.interface = declare_parameter("interface", param_read_only()); + config.receiver_timeout_sec = + static_cast(declare_parameter("receiver_timeout_sec", param_read_only())); + config.sender_timeout_sec = + static_cast(declare_parameter("sender_timeout_sec", param_read_only())); + config.filters = declare_parameter("filters", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_write()); + config.base_frame = declare_parameter("base_frame", param_read_write()); + config.use_bus_time = declare_parameter("use_bus_time", param_read_only()); + config.configuration_vehicle_wheelbase = static_cast( + declare_parameter("configuration_vehicle_wheelbase", param_read_only())); + + if (config.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration>(config); + return ValidateAndSetConfig(new_config_ptr); +} + +Status ContinentalSRR520RosWrapper::ValidateAndSetConfig( + std::shared_ptr & + new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config); + } + + config_ptr_ = new_config; + return Status::OK; +} + +void ContinentalSRR520RosWrapper::ReceivePacketsCallback( + std::unique_ptr packets_msg) +{ + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring NebulaPackets. Launch with launch_hw:=false to enable NebulaPackets " + "replay."); + return; + } + + for (auto & packet : packets_msg->packets) { + auto nebula_packet_ptr = std::make_unique(); + nebula_packet_ptr->stamp = packet.stamp; + nebula_packet_ptr->data = std::move(packet.data); + + packet_queue_.push(std::move(nebula_packet_ptr)); + } +} + +void ContinentalSRR520RosWrapper::ReceivePacketCallback( + std::unique_ptr msg_ptr) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +Status ContinentalSRR520RosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalSRR520RosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) { + return hw_interface_wrapper_->Status(); + } + + hw_interface_wrapper_->SensorInterfaceStart(); + + return hw_interface_wrapper_->Status(); +} + +rcl_interfaces::msg::SetParametersResult ContinentalSRR520RosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::continental_srr520::ContinentalSRR520SensorConfiguration new_config(*config_ptr_); + + bool got_any = + get_param(p, "frame_id", new_config.frame_id) | + get_param(p, "base_frame", new_config.base_frame) | + get_param(p, "use_bus_time", new_config.use_bus_time) | + get_param(p, "configuration_vehicle_wheelbase", new_config.configuration_vehicle_wheelbase); + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration>(new_config); + auto status = ValidateAndSetConfig(new_config_ptr); + + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSRR520RosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp deleted file mode 100644 index b8af00fd2..000000000 --- a/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,356 +0,0 @@ -// 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 - -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -MultiContinentalARS548HwInterfaceRosWrapper::MultiContinentalARS548HwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("multi_continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr - sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - assert(sensor_configuration_.sensor_ips.size() == sensor_configuration_.frame_ids.size()); - hw_interface_.RegisterScanCallback(std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_.sensor_ips.size(); - sensor_id++) { - const std::string sensor_ip = sensor_configuration_.sensor_ips[sensor_id]; - const std::string frame_id = sensor_configuration_.frame_ids[sensor_id]; - packets_pub_map_[sensor_ip] = this->create_publisher( - frame_id + "/nebula_packets", rclcpp::SensorDataQoS()); - } - - set_param_res_ = this->add_on_set_parameters_callback(std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - - StreamStart(); -} - -MultiContinentalARS548HwInterfaceRosWrapper::~MultiContinentalARS548HwInterfaceRosWrapper() -{ -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - - if (Status::OK == interface_status_) { - odometry_sub_ = this->create_subscription( - "odometry_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, - std::placeholders::_1)); - - acceleration_sub_ = create_subscription( - "acceleration_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, - std::placeholders::_1)); - - steering_angle_sub_ = create_subscription( - "steering_angle_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, - std::placeholders::_1)); - } - - return interface_status_; -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status MultiContinentalARS548HwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::GetParameters( - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter>("sensor_ips", descriptor); - sensor_configuration.sensor_ips = this->get_parameter("sensor_ips").as_string_array(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("multicast_ip", descriptor); - sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter>("frame_ids", descriptor); - sensor_configuration.frame_ids = this->get_parameter("frame_ids").as_string_array(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_host_port", descriptor); - sensor_configuration.configuration_host_port = - this->get_parameter("configuration_host_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_sensor_port", descriptor); - sensor_configuration.configuration_sensor_port = - this->get_parameter("configuration_sensor_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void MultiContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback( - std::unique_ptr scan_buffer, const std::string & sensor_ip) -{ - packets_pub_map_[sensor_ip]->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult MultiContinentalARS548HwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration new_param{ - sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - - if ( - get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | - get_param(p, "sensor_ips", new_param.sensor_ips) | - get_param(p, "frame_ids", new_param.frame_ids) | - get_param(p, "data_port", new_param.data_port) | - get_param(p, "multicast_ip", new_param.multicast_ip) | - get_param(p, "base_frame", new_param.base_frame) | - get_param(p, "object_frame", new_param.object_frame) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -void MultiContinentalARS548HwInterfaceRosWrapper::OdometryCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - - constexpr float speed_to_standstill = 0.5f; - constexpr float speed_to_moving = 2.f; - - if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { - standstill_ = false; - } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { - standstill_ = true; - } - - if (standstill_) { - hw_interface_.SetDrivingDirection(0); - } else { - hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); - } - - constexpr float ms_to_kmh = 3.6f; - hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); - - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); -} - -void MultiContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); - hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); -} - -void MultiContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( - const std_msgs::msg::Float32::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); -} - -std::vector -MultiContinentalARS548HwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ips", sensor_configuration_.sensor_ips), - rclcpp::Parameter("frame_ids", sensor_configuration_.frame_ids), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), - rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), - rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), - rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), - rclcpp::Parameter( - "configuration_sensor_port", sensor_configuration_.configuration_sensor_port)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(MultiContinentalARS548HwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp new file mode 100644 index 000000000..8ed584eaf --- /dev/null +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -0,0 +1,195 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/hesai/decoder_wrapper.hpp" + +#include +#include +#include + +#include + +#pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" +namespace nebula +{ +namespace ros +{ + +using namespace std::chrono_literals; // NOLINT(build/namespaces) + +HesaiDecoderWrapper::HesaiDecoderWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & config, + const std::shared_ptr & calibration, + bool publish_packets) +: status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("HesaiDecoder")), + parent_node_(*parent_node), + sensor_cfg_(config), + calibration_cfg_ptr_(calibration) +{ + if (!sensor_cfg_) { + throw std::runtime_error("HesaiDecoderWrapper cannot be instantiated without a valid config!"); + } + + if (!calibration_cfg_ptr_) { + throw std::runtime_error("HesaiDecoderWrapper cannot be instantiated without a valid config!"); + } + + RCLCPP_INFO_STREAM( + logger_, "Using calibration data from " << calibration_cfg_ptr_->calibration_file); + + RCLCPP_INFO(logger_, "Starting Decoder"); + + driver_ptr_ = std::make_shared(config, calibration_cfg_ptr_); + status_ = driver_ptr_->GetStatus(); + + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); + } + + // Publish packets only if enabled by the ROS wrapper + if (publish_packets) { + current_scan_msg_ = std::make_unique(); + packets_pub_ = parent_node->create_publisher( + "pandar_packets", rclcpp::SensorDataQoS()); + } + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + nebula_points_pub_ = + parent_node->create_publisher("pandar_points", pointcloud_qos); + aw_points_base_pub_ = + parent_node->create_publisher("aw_points", pointcloud_qos); + aw_points_ex_pub_ = + parent_node->create_publisher("aw_points_ex", pointcloud_qos); + + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + cloud_watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE( + logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + }); +} + +void HesaiDecoderWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(new_config, calibration_cfg_ptr_); + driver_ptr_ = new_driver; + sensor_cfg_ = new_config; +} + +void HesaiDecoderWrapper::OnCalibrationChange( + const std::shared_ptr & new_calibration) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(sensor_cfg_, new_calibration); + driver_ptr_ = new_driver; + calibration_cfg_ptr_ = new_calibration; +} + +void HesaiDecoderWrapper::ProcessCloudPacket( + std::unique_ptr packet_msg) +{ + // Accumulate packets for recording only if someone is subscribed to the topic (for performance) + if ( + packets_pub_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + if (current_scan_msg_->packets.size() == 0) { + current_scan_msg_->header.stamp = packet_msg->stamp; + } + + pandar_msgs::msg::PandarPacket pandar_packet_msg{}; + pandar_packet_msg.stamp = packet_msg->stamp; + pandar_packet_msg.size = packet_msg->data.size(); + std::copy(packet_msg->data.begin(), packet_msg->data.end(), pandar_packet_msg.data.begin()); + current_scan_msg_->packets.emplace_back(std::move(pandar_packet_msg)); + } + + std::tuple pointcloud_ts{}; + nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; + { + std::lock_guard lock(mtx_driver_ptr_); + pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); + pointcloud = std::get<0>(pointcloud_ts); + } + + // A pointcloud is only emitted when a scan completes (e.g. 3599 packets do not emit, the 3600th + // emits one) + if (pointcloud == nullptr) { + // Since this ends the function early, the `cloud_watchdog_` will not be updated. + // Thus, if pointclouds are not emitted for too long (e.g. when decoder settings are wrong or no + // packets come in), the watchdog will log a warning automatically + return; + } + + cloud_watchdog_->update(); + + // Publish scan message only if it has been written to + if (current_scan_msg_ && !current_scan_msg_->packets.empty()) { + packets_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = std::make_unique(); + } + + if ( + nebula_points_pub_->get_subscription_count() > 0 || + nebula_points_pub_->get_intra_process_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(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + } + if ( + aw_points_base_pub_->get_subscription_count() > 0 || + aw_points_base_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_cloud_xyzi = + nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); + } + if ( + aw_points_ex_pub_->get_subscription_count() > 0 || + aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + pointcloud, std::get<1>(pointcloud_ts)); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); + } +} + +void HesaiDecoderWrapper::PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher) +{ + if (pointcloud->header.stamp.sec < 0) { + RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); + } + pointcloud->header.frame_id = sensor_cfg_->frame_id; + publisher->publish(std::move(pointcloud)); +} + +nebula::Status HesaiDecoderWrapper::Status() +{ + std::lock_guard lock(mtx_driver_ptr_); + + if (!driver_ptr_) { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp deleted file mode 100644 index 6f27d547e..000000000 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,488 +0,0 @@ -#include "nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiDriverRosWrapper::HesaiDriverRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_driver_ros_wrapper", options), hw_interface_() -{ - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiSensorConfiguration sensor_configuration; - drivers::HesaiCorrection correction_configuration; - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - - wrapper_status_ = - GetParameters(sensor_configuration, calibration_configuration, correction_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); - - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), - qos_profile); - pandar_scan_sub_ = create_subscription( - "pandar_packets", qos, - std::bind(&HesaiDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); - nebula_points_pub_ = - this->create_publisher("pandar_points", rclcpp::SensorDataQoS()); - aw_points_base_pub_ = - this->create_publisher("aw_points", rclcpp::SensorDataQoS()); - aw_points_ex_pub_ = - this->create_publisher("aw_points_ex", rclcpp::SensorDataQoS()); -} - -void HesaiDriverRosWrapper::ReceiveScanMsgCallback( - const pandar_msgs::msg::PandarScan::SharedPtr scan_msg) -{ - auto t_start = std::chrono::high_resolution_clock::now(); - std::tuple pointcloud_ts = - driver_ptr_->ConvertScanToPointcloud(scan_msg); - nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); - - if (pointcloud == nullptr) { - RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); - return; - }; - if ( - nebula_points_pub_->get_subscription_count() > 0 || - nebula_points_pub_->get_intra_process_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(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); - } - if ( - aw_points_base_pub_->get_subscription_count() > 0 || - aw_points_base_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_cloud_xyzi = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); - } - if ( - aw_points_ex_pub_->get_subscription_count() > 0 || - aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, std::get<1>(pointcloud_ts)); - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); - } - - auto runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG(get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); -} - -void HesaiDriverRosWrapper::PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher) -{ - if (pointcloud->header.stamp.sec < 0) { - RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); - } - pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; - publisher->publish(std::move(pointcloud)); -} - -Status HesaiDriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiDriverRosWrapper::InitializeDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration), - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiDriverRosWrapper::GetStatus() { return wrapper_status_; } - -Status HesaiDriverRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("min_range", 0.3, descriptor); - sensor_configuration.min_range = this->get_parameter("min_range").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("max_range", 300., descriptor); - sensor_configuration.max_range = this->get_parameter("max_range").as_double(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.01).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - bool launch_hw; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("launch_hw", "", descriptor); - launch_hw = this->get_parameter("launch_hw").as_bool(); - } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration); - - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - bool run_local = !launch_hw; - if (sensor_configuration.sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { - std::string calibration_file_path_from_sensor; - if (launch_hw && !calibration_configuration.calibration_file.empty()) { - int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); - calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); - } - if(launch_hw) { - run_local = false; - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - std::future future = std::async(std::launch::async, - [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver() == Status::OK) { - auto str = hw_interface_.GetLidarCalibrationString(); - auto rt = calibration_configuration.SaveFileFromString( - calibration_file_path_from_sensor, str); - RCLCPP_ERROR_STREAM(get_logger(), str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" - << calibration_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" - << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), - "LoadFromString success:" << str << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), - "LoadFromString failed:" << str << "\n"); - } - } else { - run_local = true; - } - }); - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(5000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - RCLCPP_ERROR_STREAM(get_logger(), "GetCalibration Timeout"); - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), "The calibration has been saved in '" - << calibration_file_path_from_sensor << "'"); - } - } - if(run_local) { - RCLCPP_WARN_STREAM(get_logger(), "Running locally"); - bool run_org = false; - if (calibration_file_path_from_sensor.empty()) { - run_org = true; - } else { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_file_path_from_sensor); - auto cal_status = - calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); - - if (cal_status != Status::OK) { - run_org = true; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_file_path_from_sensor << "'"); - } - } - if(run_org) { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_configuration.calibration_file); - if (calibration_configuration.calibration_file.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_configuration.calibration_file << "'"); - } - } - } - } - } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 - std::string correction_file_path_from_sensor; - if (launch_hw && !correction_file_path.empty()) { - int ext_pos = correction_file_path.find_last_of('.'); - correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); - correction_file_path_from_sensor += "_from_sensor"; - correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); - } - std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local, &launch_hw]() { - if (launch_hw && hw_interface_.InitializeTcpDriver() == Status::OK) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor"); - auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); - RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); - run_local = false; - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); - run_local = true; - } - }else{ - RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); - run_local = true; - } - }); - if (!run_local) { - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(8000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired correction data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), "The correction has been saved in '" - << correction_file_path_from_sensor << "'"); - } - } - if(run_local) { - bool run_org = false; - if (correction_file_path_from_sensor.empty()) { - run_org = true; - } else { - auto cal_status = - correction_configuration.LoadFromFile(correction_file_path_from_sensor); - - if (cal_status != Status::OK) { - run_org = true; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path_from_sensor << "'"); - } - } - if(run_org) { - if (correction_file_path.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); - - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path << "'"); - } - } - } - } - } // end AT128 - // Do not use outside of this location - hw_interface_.~HesaiHwInterface(); - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiDriverRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp deleted file mode 100644 index 1ac59b55b..000000000 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,510 +0,0 @@ -#include "nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_hw_ms_)); - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); -#if not defined(TEST_PCAP) - Status rt = hw_interface_.InitializeTcpDriver(); - if(this->retry_hw_) - { - int cnt = 0; - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - while(rt == Status::ERROR_1) - { - cnt++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - rt = hw_interface_.InitializeTcpDriver(); - } - } - - if(rt != Status::ERROR_1){ - try{ - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - RCLCPP_INFO_STREAM(get_logger(), result); - hw_interface_.SetTargetModel(result.model); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - - } - catch (...) - { - std::cout << "catch (...) in parent" << std::endl; - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); - } - if (this->setup_sensor) { - hw_interface_.CheckAndSetConfig(); - updateParameters(); - } - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); - hw_interface_.SetTargetModel(sensor_cfg_ptr->sensor_model); - } -#endif - - hw_interface_.RegisterScanCallback( - std::bind(&HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); - pandar_scan_pub_ = - this->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); - -#if not defined(TEST_PCAP) - if (this->setup_sensor) { - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiHwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - } -#endif - -#ifdef WITH_DEBUG_STDOUT_HesaiHwInterfaceRosWrapper - if (false) { - std::vector thread_pool{}; - thread_pool.emplace_back([this] { hw_interface_.SetStandbyMode(0); }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarCalib(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagStatus(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagPort(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagTime(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagGrandmaster(ios); - }); - // thread_pool.emplace_back([&hw_interface_]{ - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetInventory(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetConfig(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarStatus(ios); - }); - thread_pool.emplace_back([this] { hw_interface_.SetStandbyMode(1); }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpConfig(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarRange(ios); - }); - for (std::thread & th : thread_pool) { - // hw_interface_.IOServiceRun(); - th.join(); - } - } - if (false) { - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - std::cout << "GetLidarCalib" << std::endl; - hw_interface_.GetLidarCalib(ios); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - } -#endif - - RCLCPP_DEBUG(this->get_logger(), "Starting stream"); - StreamStart(); -} - -HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() { - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); -} - -Status HesaiHwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - return interface_status_; -} - -Status HesaiHwInterfaceRosWrapper::StreamStop() { return Status::OK; } -Status HesaiHwInterfaceRosWrapper::Shutdown() { return Status::OK; } - -Status HesaiHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status HesaiHwInterfaceRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 2369, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { - descriptor.additional_constraints = "200, 300, 400, 500"; - range.set__from_value(200).set__to_value(500).set__step(100); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 200, descriptor); - } else { - descriptor.additional_constraints = "300, 600, 1200"; - range.set__from_value(300).set__to_value(1200).set__step(300); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 600, descriptor); - } - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.01).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("setup_sensor", true, descriptor); - this->setup_sensor = this->get_parameter("setup_sensor").as_bool(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("delay_hw_ms", 0, descriptor); - this->delay_hw_ms_ = this->get_parameter("delay_hw_ms").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("retry_hw", true, descriptor); - this->retry_hw_ = this->get_parameter("retry_hw").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("ptp_profile", ""); - sensor_configuration.ptp_profile = - nebula::drivers::PtpProfileFromString(this->get_parameter("ptp_profile").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("ptp_transport_type", ""); - sensor_configuration.ptp_transport_type = - nebula::drivers::PtpTransportTypeFromString(this->get_parameter("ptp_transport_type").as_string()); - if(static_cast(sensor_configuration.ptp_profile) > 0) { - sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("ptp_switch_type", ""); - sensor_configuration.ptp_switch_type = - nebula::drivers::PtpSwitchTypeFromString(this->get_parameter("ptp_switch_type").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(127).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("ptp_domain", 0, descriptor); - sensor_configuration.ptp_domain = this->get_parameter("ptp_domain").as_int(); - } - - if(sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); - return Status::SENSOR_CONFIG_ERROR; - } - if(sensor_configuration.ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when using the '1588v2' PTP Profile"); - return Status::SENSOR_CONFIG_ERROR; - } - if(sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); - return Status::SENSOR_CONFIG_ERROR; - } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { // || - return Status::SENSOR_CONFIG_ERROR; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback( - std::unique_ptr scan_buffer) -{ - // Publish - scan_buffer->header.frame_id = sensor_configuration_.frame_id; - scan_buffer->header.stamp = scan_buffer->packets.front().stamp; - pandar_scan_pub_->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult HesaiHwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::HesaiSensorConfiguration new_param{sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - std::string return_mode_str; - if ( - get_param(p, "sensor_model", sensor_model_str) || - get_param(p, "return_mode", return_mode_str) || get_param(p, "host_ip", new_param.host_ip) || - get_param(p, "sensor_ip", new_param.sensor_ip) || - get_param(p, "frame_id", new_param.frame_id) || - get_param(p, "data_port", new_param.data_port) || - get_param(p, "gnss_port", new_param.gnss_port) || - get_param(p, "scan_phase", new_param.scan_phase) || - get_param(p, "packet_mtu_size", new_param.packet_mtu_size) || - get_param(p, "rotation_speed", new_param.rotation_speed) || - get_param(p, "cloud_min_angle", new_param.cloud_min_angle) || - get_param(p, "cloud_max_angle", new_param.cloud_max_angle) || - get_param(p, "dual_return_distance_threshold", new_param.dual_return_distance_threshold)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - if (0 < return_mode_str.length()) - new_param.return_mode = nebula::drivers::ReturnModeFromString(return_mode_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -std::vector HesaiHwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - std::ostringstream os_return_mode; - os_return_mode << sensor_configuration_.return_mode; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("return_mode", os_return_mode.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("gnss_port", sensor_configuration_.gnss_port), - rclcpp::Parameter("scan_phase", sensor_configuration_.scan_phase), - rclcpp::Parameter("packet_mtu_size", sensor_configuration_.packet_mtu_size), - rclcpp::Parameter("rotation_speed", sensor_configuration_.rotation_speed), - rclcpp::Parameter("cloud_min_angle", sensor_configuration_.cloud_min_angle), - rclcpp::Parameter("cloud_max_angle", sensor_configuration_.cloud_max_angle), - rclcpp::Parameter( - "dual_return_distance_threshold", sensor_configuration_.dual_return_distance_threshold)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp deleted file mode 100644 index 6d130a027..000000000 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ /dev/null @@ -1,641 +0,0 @@ -#include "nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp" - -#include - -namespace nebula -{ -namespace ros -{ -HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_hw_monitor_ros_wrapper", options), hw_interface_(), diagnostics_updater_(this) -{ - cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_monitor_ms_)); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - - message_sep = ": "; - not_supported_message = "Not supported"; - error_message = "Error"; - - switch (sensor_cfg_ptr->sensor_model) { - case nebula::drivers::SensorModel::HESAI_PANDARXT32: - case nebula::drivers::SensorModel::HESAI_PANDARXT32M: - case nebula::drivers::SensorModel::HESAI_PANDARAT128: - temperature_names.emplace_back("Bottom circuit board T1"); - temperature_names.emplace_back("Bottom circuit board T2"); - temperature_names.emplace_back("Laser emitting board RT_L1(Internal)"); - temperature_names.emplace_back("Laser emitting board RT_L2"); - temperature_names.emplace_back("Receiving board RT_R"); - temperature_names.emplace_back("Receiving board RT2"); - temperature_names.emplace_back("Top circuit RT3"); - temperature_names.emplace_back("Not used"); - break; - case nebula::drivers::SensorModel::HESAI_PANDAR64: - case nebula::drivers::SensorModel::HESAI_PANDAR40P: - case nebula::drivers::SensorModel::HESAI_PANDAR40M: - case nebula::drivers::SensorModel::HESAI_PANDARQT64: - case nebula::drivers::SensorModel::HESAI_PANDARQT128: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: - default: - temperature_names.emplace_back("Bottom circuit RT1"); - temperature_names.emplace_back("Bottom circuit RT2"); - temperature_names.emplace_back("Internal Temperature"); - temperature_names.emplace_back("Laser emitting board RT1"); - temperature_names.emplace_back("Laser emitting board RT2"); - temperature_names.emplace_back("Receiving board RT1"); - temperature_names.emplace_back("Top circuit RT1"); - temperature_names.emplace_back("Top circuit RT2"); - break; - } - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - while (hw_interface_.InitializeTcpDriver() == Status::ERROR_1) { - RCLCPP_WARN(this->get_logger(), "Could not initialize TCP driver, retrying in 8s..."); - std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 - } - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - current_inventory.reset(new HesaiInventory(result)); - current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); - RCLCPP_INFO_STREAM(this->get_logger(), "HesaiInventory"); - RCLCPP_INFO_STREAM(this->get_logger(), result); - info_model = result.get_str_model(); - info_serial = std::string(result.sn.begin(), result.sn.end()); - hw_interface_.SetTargetModel(result.model); - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); - InitializeHesaiDiagnostics(); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiHwMonitorRosWrapper::paramCallback, this, std::placeholders::_1)); -} - -Status HesaiHwMonitorRosWrapper::MonitorStart() -{ - return interface_status_; -} - -Status HesaiHwMonitorRosWrapper::MonitorStop() -{ - return Status::OK; -} -Status HesaiHwMonitorRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status HesaiHwMonitorRosWrapper::InitializeHwMonitor( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status HesaiHwMonitorRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 2369, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { - descriptor.additional_constraints = "200, 300, 400, 500"; - range.set__from_value(200).set__to_value(500).set__step(100); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 200, descriptor); - } else { - descriptor.additional_constraints = "300, 600, 1200"; - range.set__from_value(300).set__to_value(1200).set__step(300); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 600, descriptor); - } - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.01).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { // || - return Status::SENSOR_CONFIG_ERROR; - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("diag_span", 1000, descriptor); - this->diag_span_ = this->get_parameter("diag_span").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("delay_monitor_ms", 0, descriptor); - this->delay_monitor_ms_ = this->get_parameter("delay_monitor_ms").as_int(); - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void HesaiHwMonitorRosWrapper::InitializeHesaiDiagnostics() -{ - RCLCPP_INFO_STREAM(this->get_logger(), "InitializeHesaiDiagnostics"); - using std::chrono_literals::operator""s; - std::ostringstream os; - auto hardware_id = info_model + ": " + info_serial; - diagnostics_updater_.setHardwareID(hardware_id); - RCLCPP_INFO_STREAM(this->get_logger(), "hardware_id: " + hardware_id); - - diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorRosWrapper::HesaiCheckStatus); - diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorRosWrapper::HesaiCheckPtp); - diagnostics_updater_.add( - "hesai_temperature", this, &HesaiHwMonitorRosWrapper::HesaiCheckTemperature); - diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorRosWrapper::HesaiCheckRpm); - - current_status.reset(); - current_monitor.reset(); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - - auto fetch_diag_from_sensor = [this]() { - OnHesaiStatusTimer(); - if (hw_interface_.UseHttpGetLidarMonitor()) { - OnHesaiLidarMonitorTimerHttp(); - } else { - OnHesaiLidarMonitorTimer(); - } - }; - - fetch_diagnostics_timer_ = - std::make_shared>( - this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(fetch_diagnostics_timer_, cbg_m_); - - if (hw_interface_.UseHttpGetLidarMonitor()) { - diagnostics_updater_.add( - "hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltageHttp); - } else { - diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltage); - } - - auto on_timer_update = [this] { - RCLCPP_DEBUG_STREAM(get_logger(), "OnUpdateTimer"); - auto now = this->get_clock()->now(); - auto dif = (now - *current_status_time).seconds(); - - RCLCPP_DEBUG_STREAM(get_logger(), "dif(status): " << dif); - - if (diag_span_ * 2.0 < dif * 1000) { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - dif = (now - *current_lidar_monitor_time).seconds(); - RCLCPP_DEBUG_STREAM(get_logger(), "dif(monitor): " << dif); - if (diag_span_ * 2.0 < dif * 1000) { - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - diagnostics_updater_.force_update(); - }; - diagnostics_update_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(1000), std::move(on_timer_update), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(diagnostics_update_timer_, cbg_r_); - - RCLCPP_DEBUG_STREAM(get_logger(), "add_timer"); -} - -std::string HesaiHwMonitorRosWrapper::GetPtreeValue( - boost::property_tree::ptree * pt, const std::string & key) -{ - boost::optional value = pt->get_optional(key); - if (value) { - return value.get(); - } else { - return not_supported_message; - } -} -std::string HesaiHwMonitorRosWrapper::GetFixedPrecisionString(double val, int pre) -{ - std::stringstream ss; - ss << std::fixed << std::setprecision(pre) << val; - return ss.str(); -} - -rcl_interfaces::msg::SetParametersResult HesaiHwMonitorRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(get_logger(), p); - RCLCPP_DEBUG_STREAM(get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::HesaiSensorConfiguration new_param{sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - uint16_t new_diag_span = 0; - if (get_param(p, "diag_span", new_diag_span)) { - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - return *result; -} - -void HesaiHwMonitorRosWrapper::OnHesaiStatusTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer" << std::endl); - try { - auto ios = std::make_shared(); - auto result = hw_interface_.GetLidarStatus(); - std::scoped_lock lock(mtx_status); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_status.reset(new HesaiLidarStatus(result)); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorRosWrapper::OnHesaiStatusTimer(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"), - error.what()); - } - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer END" << std::endl); -} - -void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp"); - try { - hw_interface_.GetLidarMonitorAsyncHttp([this](const std::string & str) { - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_tree = - std::make_unique(hw_interface_.ParseJson(str)); - }); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_error)"), - error.what()); - } - - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp END"); -} - -void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer"); - try { - auto ios = std::make_shared(); - auto result = hw_interface_.GetLidarMonitor(); - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_monitor.reset(new HesaiLidarMonitor(result)); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"), - error.what()); - } - - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer END"); -} - -void HesaiHwMonitorRosWrapper::HesaiCheckStatus( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - diagnostics.add("system_uptime", std::to_string(current_status->system_uptime)); - diagnostics.add("startup_times", std::to_string(current_status->startup_times)); - diagnostics.add("total_operation_time", std::to_string(current_status->total_operation_time)); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckPtp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - auto gps_status = current_status->get_str_gps_pps_lock(); - auto gprmc_status = current_status->get_str_gps_gprmc_status(); - auto ptp_status = current_status->get_str_ptp_clock_status(); - std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); - std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); - std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); - diagnostics.add("gps_pps_lock", gps_status); - diagnostics.add("gps_gprmc_status", gprmc_status); - diagnostics.add("ptp_clock_status", ptp_status); - if (gps_status != "UNKNOWN") { - msg.emplace_back("gps_pps_lock: " + gps_status); - } - if (gprmc_status != "UNKNOWN") { - msg.emplace_back("gprmc_status: " + gprmc_status); - } - if (ptp_status != "UNKNOWN") { - msg.emplace_back("ptp_status: " + ptp_status); - } - if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckTemperature( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - for (size_t i = 0; i < current_status->temperature.size(); i++) { - diagnostics.add( - temperature_names[i], GetFixedPrecisionString(current_status->temperature[i] * 0.01)); - } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckRpm( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - diagnostics.add("motor_speed", std::to_string(current_status->motor_speed)); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckVoltageHttp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_monitor); - if (current_lidar_monitor_tree) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - std::string key = ""; - - std::string mes; - key = "lidarInCur"; - try { - mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); - } catch (boost::bad_lexical_cast & ex) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message + std::string(ex.what()); - } - diagnostics.add(key, mes); - key = "lidarInVol"; - try { - mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); - } catch (boost::bad_lexical_cast & ex) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message + std::string(ex.what()); - } - diagnostics.add(key, mes); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckVoltage( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_monitor); - if (current_monitor) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - if (sensor_configuration_.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - diagnostics.add("input_voltage", not_supported_message); - diagnostics.add("input_current", not_supported_message); - diagnostics.add("input_power", not_supported_message); - diagnostics.summary(level, ""); - return; - } - - diagnostics.add( - "input_voltage", GetFixedPrecisionString(current_monitor->input_voltage * 0.01) + " V"); - diagnostics.add( - "input_current", GetFixedPrecisionString(current_monitor->input_current * 0.01) + " mA"); - diagnostics.add( - "input_power", GetFixedPrecisionString(current_monitor->input_power * 0.01) + " W"); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -HesaiHwMonitorRosWrapper::~HesaiHwMonitorRosWrapper() -{ - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); -} - -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwMonitorRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp new file mode 100644 index 000000000..d4fa54b95 --- /dev/null +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -0,0 +1,510 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/hesai/hesai_ros_wrapper.hpp" + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" + +namespace nebula +{ +namespace ros +{ +HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + sensor_cfg_ptr_(nullptr), + packet_queue_(3000), + hw_interface_wrapper_(), + hw_monitor_wrapper_(), + decoder_wrapper_() +{ + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *sensor_cfg_ptr_); + + launch_hw_ = declare_parameter("launch_hw", param_read_only()); + + if (launch_hw_) { + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); + hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->HwInterface(), sensor_cfg_ptr_); + } + + auto calibration_result = GetCalibrationData(sensor_cfg_ptr_->calibration_path); + if (!calibration_result.has_value()) { + throw std::runtime_error( + (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); + } + + if ( + hw_interface_wrapper_ && + sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { + auto status = + hw_interface_wrapper_->HwInterface()->checkAndSetLidarRange(*calibration_result.value()); + if (status != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not set sensor FoV: " << status).str()); + } + } + + decoder_wrapper_.emplace(this, sensor_cfg_ptr_, calibration_result.value(), launch_hw_); + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessCloudPacket(packet_queue_.pop()); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterScanCallback( + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "pandar_packets", rclcpp::SensorDataQoS(), + std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&HesaiRosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::HesaiSensorConfiguration config; + + auto _sensor_model = declare_parameter("sensor_model", param_read_only()); + config.sensor_model = drivers::SensorModelFromString(_sensor_model); + + auto _return_mode = declare_parameter("return_mode", param_read_write()); + config.return_mode = drivers::ReturnModeFromStringHesai(_return_mode, config.sensor_model); + + config.host_ip = declare_parameter("host_ip", param_read_only()); + config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); + config.multicast_ip = declare_parameter("multicast_ip", param_read_only()); + config.data_port = declare_parameter("data_port", param_read_only()); + config.gnss_port = declare_parameter("gnss_port", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_write()); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.floating_point_range = float_range(0, 360, 0.01); + descriptor.description = + "Angle at which to cut the scan. Cannot be equal to the start angle in a non-360 deg " + "FoV. Choose the end angle instead."; + config.cut_angle = declare_parameter("cut_angle", descriptor); + } + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + if (config.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + descriptor.integer_range = int_range(30, 150, 1); + } else { + descriptor.integer_range = int_range(0, 359, 1); + } + config.sync_angle = declare_parameter("sync_angle", descriptor); + } + + config.min_range = declare_parameter("min_range", param_read_write()); + config.max_range = declare_parameter("max_range", param_read_write()); + config.packet_mtu_size = declare_parameter("packet_mtu_size", param_read_only()); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + RCLCPP_DEBUG_STREAM(get_logger(), config.sensor_model); + if (config.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { + descriptor.additional_constraints = "200, 400"; + descriptor.integer_range = int_range(200, 400, 200); + } else { + descriptor.additional_constraints = "300, 600, 1200"; + descriptor.integer_range = int_range(300, 1200, 300); + } + config.rotation_speed = declare_parameter("rotation_speed", descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_min_angle = declare_parameter("cloud_min_angle", descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_max_angle = declare_parameter("cloud_max_angle", descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; + descriptor.floating_point_range = float_range(0.01, 0.5, 0.01); + config.dual_return_distance_threshold = + declare_parameter("dual_return_distance_threshold", descriptor); + } + + std::string calibration_parameter_name = getCalibrationParameterName(config.sensor_model); + config.calibration_path = + declare_parameter(calibration_parameter_name, param_read_write()); + + auto _ptp_profile = declare_parameter("ptp_profile", param_read_only()); + config.ptp_profile = drivers::PtpProfileFromString(_ptp_profile); + + auto _ptp_transport = declare_parameter("ptp_transport_type", param_read_only()); + config.ptp_transport_type = drivers::PtpTransportTypeFromString(_ptp_transport); + + if ( + config.ptp_transport_type != drivers::PtpTransportType::L2 && + config.ptp_profile != drivers::PtpProfile::IEEE_1588v2 && + config.ptp_profile != drivers::PtpProfile::UNKNOWN_PROFILE) { + RCLCPP_WARN_STREAM( + get_logger(), "PTP transport was set to '" << _ptp_transport << "' but PTP profile '" + << _ptp_profile + << "' only supports 'L2'. Setting it to 'L2'."); + config.ptp_transport_type = drivers::PtpTransportType::L2; + set_parameter(rclcpp::Parameter("ptp_transport_type", "L2")); + } + + auto _ptp_switch = declare_parameter("ptp_switch_type", param_read_only()); + config.ptp_switch_type = drivers::PtpSwitchTypeFromString(_ptp_switch); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.integer_range = int_range(0, 127, 1); + config.ptp_domain = declare_parameter("ptp_domain", descriptor); + } + + auto new_cfg_ptr = std::make_shared(config); + return ValidateAndSetConfig(new_cfg_ptr); +} + +Status HesaiRosWrapper::ValidateAndSetConfig( + std::shared_ptr & new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) { + return Status::INVALID_ECHO_MODE; + } + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) { + RCLCPP_ERROR( + get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { + RCLCPP_ERROR( + get_logger(), + "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " + "using the '1588v2' PTP Profile"); + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { + RCLCPP_ERROR(get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + return Status::SENSOR_CONFIG_ERROR; + } + if (!drivers::angle_is_between( + new_config->cloud_min_angle, new_config->cloud_max_angle, new_config->cut_angle)) { + RCLCPP_ERROR(get_logger(), "Cannot cut scan outside of the FoV."); + return Status::SENSOR_CONFIG_ERROR; + } + + bool fov_is_360 = new_config->cloud_min_angle == 0 && new_config->cloud_max_angle == 360; + if (!fov_is_360 && new_config->cut_angle == new_config->cloud_min_angle) { + RCLCPP_ERROR( + get_logger(), "Cannot cut scan right at the start of the FoV. Cut at the end instead."); + return Status::SENSOR_CONFIG_ERROR; + } + + // Handling cutting at 360deg (as opposed to 0deg) for a full 360deg FoV requires a special case + // in the cutting logic. Thus, require the user to cut at 0deg. + if (fov_is_360 && new_config->cut_angle == 360) { + RCLCPP_ERROR(get_logger(), "Cannot cut a 360deg FoV at 360deg. Cut at 0deg instead."); + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config); + } + if (hw_monitor_wrapper_) { + hw_monitor_wrapper_->OnConfigChange(new_config); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config); + } + + sensor_cfg_ptr_ = new_config; + return Status::OK; +} + +void HesaiRosWrapper::ReceiveScanMessageCallback( + std::unique_ptr scan_msg) +{ + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring received PandarScan. Launch with launch_hw:=false to enable PandarScan replay."); + return; + } + + for (auto & pkt : scan_msg->packets) { + auto nebula_pkt_ptr = std::make_unique(); + nebula_pkt_ptr->stamp = pkt.stamp; + std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); + + packet_queue_.push(std::move(nebula_pkt_ptr)); + } +} + +Status HesaiRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status HesaiRosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) { + return hw_interface_wrapper_->Status(); + } + + return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); +} + +rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + std::scoped_lock lock(mtx_config_); + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::HesaiSensorConfiguration new_cfg(*sensor_cfg_ptr_); + + std::string _return_mode{}; + std::string calibration_parameter_name = + getCalibrationParameterName(sensor_cfg_ptr_->sensor_model); + + bool got_any = + get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | + get_param(p, "sync_angle", new_cfg.sync_angle) | get_param(p, "cut_angle", new_cfg.cut_angle) | + get_param(p, "min_range", new_cfg.min_range) | get_param(p, "max_range", new_cfg.max_range) | + get_param(p, "rotation_speed", new_cfg.rotation_speed) | + get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | + get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | + get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold) | + get_param(p, calibration_parameter_name, new_cfg.calibration_path); + + // Currently, all of the sub-wrappers read-only parameters, so they do not be queried for updates + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + if (_return_mode.length() > 0) + new_cfg.return_mode = + nebula::drivers::ReturnModeFromStringHesai(_return_mode, sensor_cfg_ptr_->sensor_model); + + // //////////////////////////////////////// + // Get and validate new calibration, if any + // //////////////////////////////////////// + + std::shared_ptr new_calibration_ptr{}; + + bool new_calibration_set = new_cfg.calibration_path != sensor_cfg_ptr_->calibration_path; + if (new_calibration_set) { + // Calibration paths set during runtime are always queried from the filesystem, never fetched + // from the sensor. + if (!std::filesystem::exists(new_cfg.calibration_path)) { + auto result = SetParametersResult(); + result.successful = false; + result.reason = + "The given calibration path does not exist, ignoring: '" + new_cfg.calibration_path + "'"; + return result; + } + + // Fail early and do not set the new config if getting calibration data failed. + auto get_calibration_result = GetCalibrationData(new_cfg.calibration_path, true); + if (!get_calibration_result.has_value()) { + auto result = SetParametersResult(); + result.successful = false; + std::stringstream ss{}; + ss << "Could not change calibration file to '" << new_cfg.calibration_path << "': "; + ss << get_calibration_result.error(); + result.reason = ss.str(); + return result; + } + + new_calibration_ptr = get_calibration_result.value(); + } + + auto new_cfg_ptr = std::make_shared(new_cfg); + auto status = ValidateAndSetConfig(new_cfg_ptr); + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + // Set calibration (if any) only once all parameters have been validated + if (new_calibration_ptr) { + decoder_wrapper_->OnCalibrationChange(new_calibration_ptr); + RCLCPP_INFO_STREAM(get_logger(), "Changed calibration to '" << new_cfg.calibration_path << "'"); + } + + if ( + new_calibration_ptr && hw_interface_wrapper_ && + sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { + auto status = hw_interface_wrapper_->HwInterface()->checkAndSetLidarRange(*new_calibration_ptr); + if (status != Status::OK) { + RCLCPP_ERROR_STREAM( + get_logger(), "Sensor configuration updated, but setting hardware FoV failed: " << status); + } + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + const auto now = std::chrono::high_resolution_clock::now(); + const auto timestamp_ns = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(packet); + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +std::string HesaiRosWrapper::getCalibrationParameterName(drivers::SensorModel model) const +{ + if (model == drivers::SensorModel::HESAI_PANDARAT128) { + return "correction_file"; + } + + return "calibration_file"; +} + +HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::GetCalibrationData( + const std::string & calibration_file_path, bool ignore_others) +{ + std::shared_ptr calib; + const auto & logger = get_logger(); + + if (sensor_cfg_ptr_->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + calib = std::make_shared(); + } else { + calib = std::make_shared(); + } + + std::string calibration_file_path_from_sensor; + + { + int ext_pos = calibration_file_path.find_last_of('.'); + calibration_file_path_from_sensor = calibration_file_path.substr(0, ext_pos); + calibration_file_path_from_sensor += "_from_sensor_" + sensor_cfg_ptr_->sensor_ip; + calibration_file_path_from_sensor += + calibration_file_path.substr(ext_pos, calibration_file_path.size() - ext_pos); + } + + // If a sensor is connected, try to download and save its calibration data + if (!ignore_others && launch_hw_) { + try { + auto raw_data = hw_interface_wrapper_->HwInterface()->GetLidarCalibrationBytes(); + RCLCPP_INFO(logger, "Downloaded calibration data from sensor."); + auto status = calib->SaveToFileFromBytes(calibration_file_path_from_sensor, raw_data); + if (status != Status::OK) { + RCLCPP_ERROR_STREAM(logger, "Could not save calibration data: " << status); + } else { + RCLCPP_INFO_STREAM( + logger, "Saved downloaded data to " << calibration_file_path_from_sensor); + } + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger, "Could not download calibration data: " << e.what()); + } + } + + // If saved calibration data from a sensor exists (either just downloaded above, or previously), + // try to load it + if (!ignore_others && std::filesystem::exists(calibration_file_path_from_sensor)) { + auto status = calib->LoadFromFile(calibration_file_path_from_sensor); + if (status == Status::OK) { + calib->calibration_file = calibration_file_path_from_sensor; + return calib; + } + + RCLCPP_ERROR_STREAM(logger, "Could not load downloaded calibration data: " << status); + } else if (!ignore_others) { + RCLCPP_ERROR(logger, "No downloaded calibration data found."); + } + + if (!ignore_others) { + RCLCPP_WARN(logger, "Falling back to generic calibration file."); + } + + // If downloaded data did not exist or could not be loaded, fall back to a generic file. + // If that file does not exist either, return an error code + if (!std::filesystem::exists(calibration_file_path)) { + RCLCPP_ERROR(logger, "No calibration data found."); + return nebula::Status(Status::INVALID_CALIBRATION_FILE); + } + + // Try to load the existing fallback calibration file. Return an error if this fails + auto status = calib->LoadFromFile(calibration_file_path); + if (status != Status::OK) { + RCLCPP_ERROR_STREAM( + logger, "Could not load calibration file at '" << calibration_file_path << "'"); + return status; + } + + // Return the fallback calibration file + calib->calibration_file = calibration_file_path; + return calib; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(HesaiRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp new file mode 100644 index 000000000..f0ba51102 --- /dev/null +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -0,0 +1,86 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/hesai/hw_interface_wrapper.hpp" + +#include "nebula_ros/common/parameter_descriptors.hpp" + +namespace nebula +{ +namespace ros +{ + +HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config) +: hw_interface_(new nebula::drivers::HesaiHwInterface()), + logger_(parent_node->get_logger().get_child("HwInterface")), + status_(Status::NOT_INITIALIZED) +{ + setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); + bool retry_connect = parent_node->declare_parameter("retry_hw", param_read_only()); + + status_ = hw_interface_->SetSensorConfiguration( + std::static_pointer_cast(config)); + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } + + hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); + hw_interface_->SetTargetModel(config->sensor_model); + + int retry_count = 0; + + while (true) { + status_ = hw_interface_->InitializeTcpDriver(); + if (status_ == Status::OK || !retry_connect) { + break; + } + + retry_count++; + std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 + RCLCPP_WARN_STREAM(logger_, status_ << ". Retry #" << retry_count); + } + + if (status_ == Status::OK) { + try { + auto inventory = hw_interface_->GetInventory(); + RCLCPP_INFO_STREAM(logger_, inventory); + hw_interface_->SetTargetModel(inventory.model); + } catch (...) { + RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor..."); + } + if (setup_sensor_) { + hw_interface_->CheckAndSetConfig(); + } + } else { + RCLCPP_ERROR_STREAM( + logger_, "Failed to get model from sensor... Set from config: " << config->sensor_model); + } + + status_ = Status::OK; +} + +void HesaiHwInterfaceWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + hw_interface_->SetSensorConfiguration( + std::static_pointer_cast(new_config)); + if (setup_sensor_) { + hw_interface_->CheckAndSetConfig(); + } +} + +Status HesaiHwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr HesaiHwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp new file mode 100644 index 000000000..1a81cfdcc --- /dev/null +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -0,0 +1,373 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/hesai/hw_monitor_wrapper.hpp" + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include + +namespace nebula +{ +namespace ros +{ +HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config) +: logger_(parent_node->get_logger().get_child("HwMonitor")), + diagnostics_updater_(parent_node), + status_(Status::OK), + hw_interface_(hw_interface), + parent_node_(parent_node) +{ + diag_span_ = parent_node->declare_parameter("diag_span", param_read_only()); + + switch (config->sensor_model) { + case nebula::drivers::SensorModel::HESAI_PANDARXT32: + case nebula::drivers::SensorModel::HESAI_PANDARXT32M: + case nebula::drivers::SensorModel::HESAI_PANDARAT128: + temperature_names_.emplace_back("Bottom circuit board T1"); + temperature_names_.emplace_back("Bottom circuit board T2"); + temperature_names_.emplace_back("Laser emitting board RT_L1 (Internal)"); + temperature_names_.emplace_back("Laser emitting board RT_L2"); + temperature_names_.emplace_back("Receiving board RT_R"); + temperature_names_.emplace_back("Receiving board RT2"); + temperature_names_.emplace_back("Top circuit RT3"); + temperature_names_.emplace_back("Not used"); + break; + case nebula::drivers::SensorModel::HESAI_PANDAR64: + case nebula::drivers::SensorModel::HESAI_PANDAR40P: + case nebula::drivers::SensorModel::HESAI_PANDAR40M: + case nebula::drivers::SensorModel::HESAI_PANDARQT64: + case nebula::drivers::SensorModel::HESAI_PANDARQT128: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: + default: + temperature_names_.emplace_back("Bottom circuit RT1"); + temperature_names_.emplace_back("Bottom circuit RT2"); + temperature_names_.emplace_back("Internal Temperature"); + temperature_names_.emplace_back("Laser emitting board RT1"); + temperature_names_.emplace_back("Laser emitting board RT2"); + temperature_names_.emplace_back("Receiving board RT1"); + temperature_names_.emplace_back("Top circuit RT1"); + temperature_names_.emplace_back("Top circuit RT2"); + break; + } + + supports_monitor_ = config->sensor_model != drivers::SensorModel::HESAI_PANDARAT128; + + auto result = hw_interface->GetInventory(); + current_inventory_.reset(new HesaiInventory(result)); + current_inventory_time_.reset(new rclcpp::Time(parent_node->get_clock()->now())); + std::cout << "HesaiInventory" << std::endl; + std::cout << result << std::endl; + info_model_ = result.get_str_model(); + info_serial_ = std::string(std::begin(result.sn), std::end(result.sn)); + RCLCPP_INFO_STREAM(logger_, "Model: " << info_model_); + RCLCPP_INFO_STREAM(logger_, "Serial: " << info_serial_); + InitializeHesaiDiagnostics(); +} + +void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() +{ + RCLCPP_INFO_STREAM(logger_, "InitializeHesaiDiagnostics"); + using std::chrono_literals::operator""s; + std::ostringstream os; + auto hardware_id = info_model_ + ": " + info_serial_; + diagnostics_updater_.setHardwareID(hardware_id); + RCLCPP_INFO_STREAM(logger_, "Hardware ID: " + hardware_id); + + diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorWrapper::HesaiCheckStatus); + diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorWrapper::HesaiCheckPtp); + diagnostics_updater_.add( + "hesai_temperature", this, &HesaiHwMonitorWrapper::HesaiCheckTemperature); + diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorWrapper::HesaiCheckRpm); + + current_status_.reset(); + current_monitor_.reset(); + current_status_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + + auto fetch_diag_from_sensor = [this]() { + OnHesaiStatusTimer(); + + if (!supports_monitor_) return; + + if (hw_interface_->UseHttpGetLidarMonitor()) { + OnHesaiLidarMonitorTimerHttp(); + } else { + OnHesaiLidarMonitorTimer(); + } + }; + + fetch_diagnostics_timer_ = parent_node_->create_wall_timer( + std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor)); + + if (hw_interface_->UseHttpGetLidarMonitor()) { + diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltageHttp); + } else { + diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltage); + } + + auto on_timer_update = [this] { + RCLCPP_DEBUG_STREAM(logger_, "OnUpdateTimer"); + auto now = parent_node_->get_clock()->now(); + auto dif = (now - *current_status_time_).seconds(); + + RCLCPP_DEBUG_STREAM(logger_, "dif(status): " << dif); + + if (diag_span_ * 2.0 < dif * 1000) { + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(logger_, "STALE"); + } else { + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(logger_, "OK"); + } + dif = (now - *current_lidar_monitor_time_).seconds(); + RCLCPP_DEBUG_STREAM(logger_, "dif(monitor): " << dif); + if (diag_span_ * 2.0 < dif * 1000) { + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(logger_, "STALE"); + } else { + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(logger_, "OK"); + } + diagnostics_updater_.force_update(); + }; + diagnostics_update_timer_ = + parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); + + RCLCPP_DEBUG_STREAM(logger_, "add_timer"); +} + +std::string HesaiHwMonitorWrapper::GetPtreeValue( + boost::property_tree::ptree * pt, const std::string & key) +{ + boost::optional value = pt->get_optional(key); + if (value) { + return value.get(); + } else { + return MSG_NOT_SUPPORTED; + } +} +std::string HesaiHwMonitorWrapper::GetFixedPrecisionString(double val, int pre) +{ + std::stringstream ss; + ss << std::fixed << std::setprecision(pre) << val; + return ss.str(); +} + +void HesaiHwMonitorWrapper::OnHesaiStatusTimer() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer" << std::endl); + try { + auto result = hw_interface_->GetLidarStatus(); + std::scoped_lock lock(mtx_lidar_status_); + current_status_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_status_.reset(new HesaiLidarStatus(result)); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(boost::system::system_error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer END" << std::endl); +} + +void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp"); + try { + hw_interface_->GetLidarMonitorAsyncHttp([this](const std::string & str) { + std::scoped_lock lock(mtx_lidar_monitor_); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_lidar_monitor_tree_ = + std::make_unique(hw_interface_->ParseJson(str)); + }); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_" + "error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp END"); +} + +void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer"); + try { + auto result = hw_interface_->GetLidarMonitor(); + std::scoped_lock lock(mtx_lidar_monitor_); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_monitor_.reset(new HesaiLidarMonitor(result)); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(boost::system::system_" + "error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer END"); +} + +void HesaiHwMonitorWrapper::HesaiCheckStatus( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + + diagnostics.add("system_uptime", std::to_string(current_status_->system_uptime.value())); + diagnostics.add("startup_times", std::to_string(current_status_->startup_times.value())); + diagnostics.add( + "total_operation_time", std::to_string(current_status_->total_operation_time.value())); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + auto gps_status = current_status_->get_str_gps_pps_lock(); + auto gprmc_status = current_status_->get_str_gps_gprmc_status(); + auto ptp_status = current_status_->get_str_ptp_clock_status(); + std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); + std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); + std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); + diagnostics.add("gps_pps_lock", gps_status); + diagnostics.add("gps_gprmc_status", gprmc_status); + diagnostics.add("ptp_clock_status", ptp_status); + if (gps_status != "UNKNOWN") { + msg.emplace_back("gps_pps_lock: " + gps_status); + } + if (gprmc_status != "UNKNOWN") { + msg.emplace_back("gprmc_status: " + gprmc_status); + } + if (ptp_status != "UNKNOWN") { + msg.emplace_back("ptp_status: " + ptp_status); + } + if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckTemperature( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + for (size_t i = 0; i < std::size(current_status_->temperature); i++) { + diagnostics.add( + temperature_names_[i], + GetFixedPrecisionString(current_status_->temperature[i].value() * 0.01, 3)); + } + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + diagnostics.add("motor_speed", std::to_string(current_status_->motor_speed.value())); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckVoltageHttp( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_monitor_); + if (current_lidar_monitor_tree_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + std::string key = ""; + + std::string mes; + key = "lidarInCur"; + try { + mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); + } catch (boost::bad_lexical_cast & ex) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = MSG_ERROR + std::string(ex.what()); + } + diagnostics.add(key, mes); + key = "lidarInVol"; + try { + mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); + } catch (boost::bad_lexical_cast & ex) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = MSG_ERROR + std::string(ex.what()); + } + diagnostics.add(key, mes); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckVoltage( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_monitor_); + if (current_monitor_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + diagnostics.add( + "input_voltage", + GetFixedPrecisionString(current_monitor_->input_voltage.value() * 0.01, 3) + " V"); + diagnostics.add( + "input_current", GetFixedPrecisionString(current_monitor_->input_current.value() * 0.01, 3) + + " m" + "A"); + diagnostics.add( + "input_power", + GetFixedPrecisionString(current_monitor_->input_power.value() * 0.01, 3) + " W"); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +Status HesaiHwMonitorWrapper::Status() +{ + return Status::OK; +} +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/robosense/decoder_wrapper.cpp b/nebula_ros/src/robosense/decoder_wrapper.cpp new file mode 100644 index 000000000..c7cae7d3a --- /dev/null +++ b/nebula_ros/src/robosense/decoder_wrapper.cpp @@ -0,0 +1,161 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/robosense/decoder_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +using namespace std::chrono_literals; // NOLINT(build/namespaces) + +RobosenseDecoderWrapper::RobosenseDecoderWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + const std::shared_ptr & config, + const std::shared_ptr & calibration) +: status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("DecoderWrapper")), + hw_interface_(hw_interface), + sensor_cfg_(config), + calibration_cfg_ptr_(calibration), + driver_ptr_(new drivers::RobosenseDriver(config, calibration)) +{ + status_ = driver_ptr_->GetStatus(); + + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + RCLCPP_INFO(logger_, ". Starting..."); + RCLCPP_INFO(logger_, ". Driver "); + + // Publish packets only if HW interface is connected + if (hw_interface_) { + current_scan_msg_ = std::make_unique(); + packets_pub_ = parent_node->create_publisher( + "robosense_packets", rclcpp::SensorDataQoS()); + } + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + nebula_points_pub_ = parent_node->create_publisher( + "robosense_points", pointcloud_qos); + aw_points_base_pub_ = + parent_node->create_publisher("aw_points", pointcloud_qos); + aw_points_ex_pub_ = + parent_node->create_publisher("aw_points_ex", pointcloud_qos); + + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + cloud_watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE( + logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + }); + + RCLCPP_INFO(logger_, "Initialized decoder wrapper."); +} + +void RobosenseDecoderWrapper::ProcessCloudPacket( + std::unique_ptr packet_msg) +{ + // Accumulate packets for recording only if someone is subscribed to the topic (for performance) + if ( + hw_interface_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + if (current_scan_msg_->packets.size() == 0) { + current_scan_msg_->header.stamp = packet_msg->stamp; + } + + robosense_msgs::msg::RobosensePacket robosense_packet_msg{}; + robosense_packet_msg.stamp = packet_msg->stamp; + std::copy(packet_msg->data.begin(), packet_msg->data.end(), robosense_packet_msg.data.begin()); + current_scan_msg_->packets.emplace_back(std::move(robosense_packet_msg)); + } + + std::tuple pointcloud_ts{}; + nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; + + { + std::lock_guard lock(mtx_driver_ptr_); + pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); + pointcloud = std::get<0>(pointcloud_ts); + } + + if (pointcloud == nullptr) { + return; + } + + cloud_watchdog_->update(); + + // Publish scan message only if it has been written to + if (current_scan_msg_ && !current_scan_msg_->packets.empty()) { + packets_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = std::make_unique(); + } + + if ( + nebula_points_pub_->get_subscription_count() > 0 || + nebula_points_pub_->get_intra_process_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(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + } + if ( + aw_points_base_pub_->get_subscription_count() > 0 || + aw_points_base_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_cloud_xyzi = + nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); + } + if ( + aw_points_ex_pub_->get_subscription_count() > 0 || + aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + pointcloud, std::get<1>(pointcloud_ts)); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); + } +} + +void RobosenseDecoderWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(new_config, calibration_cfg_ptr_); + driver_ptr_ = new_driver; + sensor_cfg_ = new_config; +} + +/// @brief Get current status of this driver +/// @return Current status +nebula::Status RobosenseDecoderWrapper::Status() +{ + return status_; +} + +void RobosenseDecoderWrapper::PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher) +{ + if (pointcloud->header.stamp.sec < 0) { + RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); + return; + } + pointcloud->header.frame_id = sensor_cfg_->frame_id; + publisher->publish(std::move(pointcloud)); +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/robosense/hw_interface_wrapper.cpp b/nebula_ros/src/robosense/hw_interface_wrapper.cpp new file mode 100644 index 000000000..2abddd285 --- /dev/null +++ b/nebula_ros/src/robosense/hw_interface_wrapper.cpp @@ -0,0 +1,43 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/robosense/hw_interface_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +RobosenseHwInterfaceWrapper::RobosenseHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config) +: hw_interface_(new nebula::drivers::RobosenseHwInterface()), + logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), + status_(nebula::Status::NOT_INITIALIZED) +{ + hw_interface_->SetLogger( + std::make_shared(parent_node->get_logger().get_child("HwInterface"))); + status_ = hw_interface_->SetSensorConfiguration(config); + + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << status_).str()); + } +} + +void RobosenseHwInterfaceWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + hw_interface_->SetSensorConfiguration(new_config); +} + +nebula::Status RobosenseHwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr RobosenseHwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/robosense/hw_monitor_wrapper.cpp b/nebula_ros/src/robosense/hw_monitor_wrapper.cpp new file mode 100644 index 000000000..d5c43ea7b --- /dev/null +++ b/nebula_ros/src/robosense/hw_monitor_wrapper.cpp @@ -0,0 +1,113 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/robosense/hw_monitor_wrapper.hpp" + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include + +namespace nebula +{ +namespace ros +{ +RobosenseHwMonitorWrapper::RobosenseHwMonitorWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config) +: parent_(parent_node), + logger_(parent_->get_logger().get_child("HwMonitorWrapper")), + diagnostics_updater_(), + status_(nebula::Status::NOT_INITIALIZED), + sensor_cfg_ptr_(config) +{ + auto descriptor = param_read_only().set__additional_constraints("milliseconds"); + diag_span_ = parent_->declare_parameter("diag_span", descriptor); +} + +void RobosenseHwMonitorWrapper::InitializeRobosenseDiagnostics() +{ + std::scoped_lock lock(mtx_config_, mtx_current_sensor_info_); + + if (current_sensor_info_.empty()) { + RCLCPP_WARN(logger_, "Tried to initialize diagnostics updater without any diagnostics data"); + return; + } + + auto hw_id = nebula::drivers::SensorModelToString(sensor_cfg_ptr_->sensor_model) + ": " + + current_sensor_info_["serial_number"]; + + RCLCPP_INFO(logger_, "InitializeRobosenseDiagnostics"); + diagnostics_updater_.emplace(parent_); + diagnostics_updater_->setHardwareID(hw_id); + RCLCPP_INFO_STREAM(logger_, "hardware_id: " + hw_id); + + diagnostics_updater_->add( + "robosense_status", this, &RobosenseHwMonitorWrapper::RobosenseCheckStatus); + + auto on_timer_update = [this] { + RCLCPP_DEBUG(logger_, "OnUpdateTimer"); + auto now = parent_->get_clock()->now(); + auto dif = (now - current_info_time_).seconds(); + + RCLCPP_DEBUG_STREAM(logger_, "dif(status): " << dif); + + if (diag_span_ * 2.0 < dif * 1000) { + RCLCPP_WARN(logger_, "STALE"); + } else { + RCLCPP_DEBUG(logger_, "OK"); + } + diagnostics_updater_->force_update(); + }; + + diagnostics_update_timer_ = + parent_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); + + RCLCPP_DEBUG_STREAM(logger_, "add_timer"); +} + +void RobosenseHwMonitorWrapper::RobosenseCheckStatus( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::lock_guard lock(mtx_current_sensor_info_); + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + for (const auto & info : current_sensor_info_) { + diagnostics.add(info.first, info.second); + } + + diagnostics.summary(level, "OK"); +} + +void RobosenseHwMonitorWrapper::DiagnosticsCallback( + const std::map & diag_info) +{ + auto current_time = parent_->get_clock()->now(); + + { + std::lock_guard lock(mtx_current_sensor_info_); + current_sensor_info_ = diag_info; + current_info_time_ = current_time; + } + + if (!diagnostics_updater_) { + InitializeRobosenseDiagnostics(); + } +} + +void RobosenseHwMonitorWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + std::lock_guard lock(mtx_config_); + + if (!new_config) { + throw std::invalid_argument("Config is not nullable"); + } + + if (new_config->sensor_model != sensor_cfg_ptr_->sensor_model) { + throw std::invalid_argument("Sensor model is read-only during runtime"); + } + + sensor_cfg_ptr_ = new_config; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp deleted file mode 100644 index 9025ab080..000000000 --- a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,302 +0,0 @@ -#include "nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -RobosenseDriverRosWrapper::RobosenseDriverRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("robosense_driver_ros_wrapper", options) -{ - RCLCPP_WARN_STREAM(this->get_logger(), "RobosenseDriverRosWrapper"); - drivers::RobosenseCalibrationConfiguration calibration_configuration; - drivers::RobosenseSensorConfiguration sensor_configuration; - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); - - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - - robosense_scan_sub_ = create_subscription( - "robosense_packets", rclcpp::SensorDataQoS(), - std::bind(&RobosenseDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); - robosense_info_sub_ = create_subscription( - "robosense_difop_packets", rclcpp::SensorDataQoS(), - std::bind(&RobosenseDriverRosWrapper::ReceiveInfoMsgCallback, this, std::placeholders::_1)); - nebula_points_pub_ = this->create_publisher( - "robosense_points", rclcpp::SensorDataQoS()); - aw_points_base_pub_ = - this->create_publisher("aw_points", rclcpp::SensorDataQoS()); - aw_points_ex_pub_ = - this->create_publisher("aw_points_ex", rclcpp::SensorDataQoS()); - - RCLCPP_WARN_STREAM(this->get_logger(), "Initialized decoder ros wrapper."); -} - -void RobosenseDriverRosWrapper::ReceiveScanMsgCallback( - const robosense_msgs::msg::RobosenseScan::SharedPtr scan_msg) -{ - if (!driver_ptr_) { - if (sensor_cfg_ptr_->sensor_model == drivers::SensorModel::ROBOSENSE_BPEARL) { - if (scan_msg->packets.back().data[32] == drivers::BPEARL_V4_FLAG) { - sensor_cfg_ptr_->sensor_model = drivers::SensorModel::ROBOSENSE_BPEARL_V4; - RCLCPP_INFO_STREAM(this->get_logger(), "Bpearl V4 detected."); - } else { - sensor_cfg_ptr_->sensor_model = drivers::SensorModel::ROBOSENSE_BPEARL_V3; - RCLCPP_INFO_STREAM(this->get_logger(), "Bpearl V3 detected."); - } - } - if (!info_driver_ptr_) { - wrapper_status_ = InitializeInfoDriver( - std::static_pointer_cast(sensor_cfg_ptr_)); - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); - } - } - - if (!is_received_info) { - RCLCPP_WARN_STREAM(this->get_logger(), "Waiting for info packet."); - return; - } - - auto t_start = std::chrono::high_resolution_clock::now(); - - std::tuple pointcloud_ts = - driver_ptr_->ConvertScanToPointcloud(scan_msg); - nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); - - if (pointcloud == nullptr) { - RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); - return; - }; - if ( - nebula_points_pub_->get_subscription_count() > 0 || - nebula_points_pub_->get_intra_process_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(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); - } - if ( - aw_points_base_pub_->get_subscription_count() > 0 || - aw_points_base_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_cloud_xyzi = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); - } - if ( - aw_points_ex_pub_->get_subscription_count() > 0 || - aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( - pointcloud, std::get<1>(pointcloud_ts)); - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); - } - - auto runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG( - get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); -} - -void RobosenseDriverRosWrapper::ReceiveInfoMsgCallback( - const robosense_msgs::msg::RobosenseInfoPacket::SharedPtr info_msg) -{ - if (!sensor_cfg_ptr_) { - RCLCPP_WARN_STREAM(this->get_logger(), "Sensor configuration has not been initialized yet."); - return; - } - - if (!info_driver_ptr_) { - RCLCPP_WARN_STREAM(this->get_logger(), "Info driver has not been initialized yet."); - return; - } - - if (info_msg->packet.data.size() == 0) { - RCLCPP_WARN_STREAM(this->get_logger(), "Empty info packet received."); - return; - } - - std::vector info_data(info_msg->packet.data.begin(), info_msg->packet.data.end()); - const auto decode_status = info_driver_ptr_->DecodeInfoPacket(info_data); - if (decode_status != Status::OK) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to decode DIFOP packet."); - return; - } - - sensor_cfg_ptr_->return_mode = info_driver_ptr_->GetReturnMode(); - sensor_cfg_ptr_->use_sensor_time = info_driver_ptr_->GetSyncStatus(); - *calibration_cfg_ptr_ = info_driver_ptr_->GetSensorCalibration(); - calibration_cfg_ptr_->CreateCorrectedChannels(); - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); - - wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); - - is_received_info = true; - - // Unsubscribe from info topic - robosense_info_sub_.reset(); -} - -void RobosenseDriverRosWrapper::PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher) -{ - if (!sensor_cfg_ptr_->use_sensor_time) { - pointcloud->header.stamp = this->now(); - } - if (pointcloud->header.stamp.sec < 0) { - RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); - return; - } - pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; - publisher->publish(std::move(pointcloud)); -} - -Status RobosenseDriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - RCLCPP_INFO_STREAM(this->get_logger(), "Initializing driver..."); - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration)); - - return driver_ptr_->GetStatus(); -} - -Status RobosenseDriverRosWrapper::InitializeInfoDriver( - std::shared_ptr sensor_configuration) -{ - RCLCPP_INFO_STREAM(this->get_logger(), "Initializing info driver..."); - info_driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration)); - - return info_driver_ptr_->GetStatus(); -} - -Status RobosenseDriverRosWrapper::GetStatus() -{ - return wrapper_status_; -} - -Status RobosenseDriverRosWrapper::GetParameters( - drivers::RobosenseSensorConfiguration & sensor_configuration, - drivers::RobosenseCalibrationConfiguration & calibration_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 7788, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - calibration_configuration.SetChannelSize( - nebula::drivers::GetChannelSize(sensor_configuration.sensor_model)); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.01).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "robosense", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(RobosenseDriverRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp deleted file mode 100644 index 773249813..000000000 --- a/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,169 +0,0 @@ -#include "nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -ros::RobosenseHwInterfaceRosWrapper::RobosenseHwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("robosense_hw_interface_ros_wrapper", options) -{ - interface_status_ = GetParameters(sensor_configuration_); - - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - hw_interface_.RegisterScanCallback(std::bind( - &RobosenseHwInterfaceRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); - - hw_interface_.RegisterInfoCallback(std::bind( - &RobosenseHwInterfaceRosWrapper::ReceiveInfoDataCallback, this, std::placeholders::_1)); - - robosense_scan_pub_ = this->create_publisher( - "robosense_packets", rclcpp::SensorDataQoS()); - - robosense_difop_pub_ = this->create_publisher( - "robosense_difop_packets", rclcpp::SensorDataQoS()); - - StreamStart(); -} - -Status RobosenseHwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - RCLCPP_INFO_STREAM(get_logger(), "Starting interface."); - hw_interface_.SensorInterfaceStart(); - hw_interface_.InfoInterfaceStart(); - } - return interface_status_; -} -Status RobosenseHwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status RobosenseHwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status RobosenseHwInterfaceRosWrapper::InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status RobosenseHwInterfaceRosWrapper::GetParameters( - drivers::RobosenseSensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.200", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "robosense", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 6699, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 7788, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void RobosenseHwInterfaceRosWrapper::ReceiveScanDataCallback( - std::unique_ptr scan_buffer) -{ - // Publish - scan_buffer->header.frame_id = sensor_configuration_.frame_id; - scan_buffer->header.stamp = scan_buffer->packets.front().stamp; - robosense_scan_pub_->publish(*scan_buffer); -} - -void RobosenseHwInterfaceRosWrapper::ReceiveInfoDataCallback( - std::unique_ptr difop_buffer) -{ - // Publish - robosense_difop_pub_->publish(*difop_buffer); -} - -RCLCPP_COMPONENTS_REGISTER_NODE(RobosenseHwInterfaceRosWrapper) - -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp deleted file mode 100644 index 9818d10e5..000000000 --- a/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp +++ /dev/null @@ -1,192 +0,0 @@ -#include "nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp" - -#include - -namespace nebula -{ -namespace ros -{ -RobosenseHwMonitorRosWrapper::RobosenseHwMonitorRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("robosense_hw_monitor_ros_wrapper", options), diagnostics_updater_(this) -{ - interface_status_ = GetParameters(sensor_configuration_); - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - - robosense_info_sub_ = create_subscription( - "robosense_difop_packets", rclcpp::SensorDataQoS(), - std::bind(&RobosenseHwMonitorRosWrapper::ReceiveInfoMsgCallback, this, std::placeholders::_1)); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&RobosenseHwMonitorRosWrapper::paramCallback, this, std::placeholders::_1)); -} - -Status RobosenseHwMonitorRosWrapper::MonitorStart() -{ - return interface_status_; -} - -Status RobosenseHwMonitorRosWrapper::MonitorStop() -{ - return Status::OK; -} -Status RobosenseHwMonitorRosWrapper::Shutdown() -{ - return Status::OK; -} -// -Status RobosenseHwMonitorRosWrapper::InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status RobosenseHwMonitorRosWrapper::GetParameters( - drivers::RobosenseSensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("diag_span", 1000, descriptor); - diag_span_ = this->get_parameter("diag_span").as_int(); - } - - return Status::OK; -} - -void RobosenseHwMonitorRosWrapper::InitializeRobosenseDiagnostics() -{ - RCLCPP_INFO_STREAM(this->get_logger(), "InitializeRobosenseDiagnostics"); - diagnostics_updater_.setHardwareID(*hardware_id_); - RCLCPP_INFO_STREAM(this->get_logger(), "hardware_id: " + *hardware_id_); - - diagnostics_updater_.add( - "robosense_status", this, &RobosenseHwMonitorRosWrapper::RobosenseCheckStatus); - - auto on_timer_status = [this] { OnRobosenseStatusTimer(); }; - diagnostics_status_timer_ = - this->create_wall_timer(std::chrono::milliseconds(diag_span_), std::move(on_timer_status)); - - RCLCPP_DEBUG_STREAM(get_logger(), "add_timer"); -} - -void RobosenseHwMonitorRosWrapper::OnRobosenseStatusTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnRobosenseStatusTimer" << std::endl); - info_driver_->DecodeInfoPacket(info_packet_buffer_); - current_sensor_info_ = info_driver_->GetSensorInfo(); - current_info_time_ = std::make_unique(this->get_clock()->now()); -} - -void RobosenseHwMonitorRosWrapper::RobosenseCheckStatus( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if (!hardware_id_.has_value()) { - return; - } - - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - - for (const auto & info : current_sensor_info_) { - diagnostics.add(info.first, info.second); - } - - diagnostics.summary(level, "OK"); -} - -rcl_interfaces::msg::SetParametersResult RobosenseHwMonitorRosWrapper::paramCallback( - const std::vector & parameters) -{ - RCLCPP_DEBUG_STREAM(get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(get_logger(), parameters); - RCLCPP_DEBUG_STREAM(get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), parameters); - - drivers::RobosenseSensorConfiguration new_param{sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - uint16_t new_diag_span = 0; - if (get_param(parameters, "diag_span", new_diag_span)) { - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - return *result; -} - -void RobosenseHwMonitorRosWrapper::ReceiveInfoMsgCallback( - const robosense_msgs::msg::RobosenseInfoPacket::SharedPtr info_msg) -{ - if (!info_driver_) { - auto sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - - if (sensor_cfg_ptr->sensor_model == drivers::SensorModel::ROBOSENSE_BPEARL) { - if ( - drivers::SensorModelFromString(info_msg->lidar_model) == - drivers::SensorModel::ROBOSENSE_BPEARL_V3) { - sensor_cfg_ptr->sensor_model = drivers::SensorModel::ROBOSENSE_BPEARL_V3; - } else if ( - drivers::SensorModelFromString(info_msg->lidar_model) == - drivers::SensorModel::ROBOSENSE_BPEARL_V4) { - sensor_cfg_ptr->sensor_model = drivers::SensorModel::ROBOSENSE_BPEARL_V4; - } else { - RCLCPP_ERROR_STREAM(this->get_logger(), "No version for Bpearl."); - return; - } - } - - info_driver_ = std::make_unique(sensor_cfg_ptr); - } - - info_packet_buffer_ = - std::vector(info_msg->packet.data.begin(), info_msg->packet.data.end()); - - if (!hardware_id_.has_value()) { - info_driver_->DecodeInfoPacket(info_packet_buffer_); - current_sensor_info_ = info_driver_->GetSensorInfo(); - current_info_time_ = std::make_unique(this->get_clock()->now()); - - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << sensor_configuration_.sensor_model); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << current_sensor_info_["serial_number"]); - - hardware_id_.emplace( - nebula::drivers::SensorModelToString(sensor_configuration_.sensor_model) + ": " + - current_sensor_info_["serial_number"]); - InitializeRobosenseDiagnostics(); - } -} - -RCLCPP_COMPONENTS_REGISTER_NODE(RobosenseHwMonitorRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp new file mode 100644 index 000000000..65c5ec8a9 --- /dev/null +++ b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp @@ -0,0 +1,291 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/robosense/robosense_ros_wrapper.hpp" + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" + +namespace nebula +{ +namespace ros +{ +RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("robosense_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + sensor_cfg_ptr_(nullptr), + packet_queue_(3000), + hw_interface_wrapper_(), + hw_monitor_wrapper_(), + decoder_wrapper_() +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *sensor_cfg_ptr_); + + launch_hw_ = declare_parameter("launch_hw", param_read_only()); + + if (launch_hw_) { + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); + hw_monitor_wrapper_.emplace(this, sensor_cfg_ptr_); + info_driver_.emplace(sensor_cfg_ptr_); + } + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessCloudPacket(packet_queue_.pop()); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterScanCallback( + std::bind(&RobosenseRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + hw_interface_wrapper_->HwInterface()->RegisterInfoCallback( + std::bind(&RobosenseRosWrapper::ReceiveInfoPacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "robosense_packets", rclcpp::SensorDataQoS(), + std::bind(&RobosenseRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&RobosenseRosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status RobosenseRosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::RobosenseSensorConfiguration config; + + auto _sensor_model = declare_parameter("sensor_model", param_read_only()); + config.sensor_model = drivers::SensorModelFromString(_sensor_model); + + auto _return_mode = declare_parameter("return_mode", param_read_write()); + config.return_mode = drivers::ReturnModeFromStringRobosense(_return_mode); + + config.host_ip = declare_parameter("host_ip", param_read_only()); + config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); + config.data_port = declare_parameter("data_port", param_read_only()); + config.gnss_port = declare_parameter("gnss_port", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_write()); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.])"; + descriptor.floating_point_range = float_range(0, 360, 0.01); + config.scan_phase = declare_parameter("scan_phase", descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; + descriptor.floating_point_range = float_range(0.01, 0.5, 0.01); + config.dual_return_distance_threshold = + declare_parameter("dual_return_distance_threshold", descriptor); + } + + auto new_cfg_ptr = std::make_shared(config); + return ValidateAndSetConfig(new_cfg_ptr); +} + +Status RobosenseRosWrapper::ValidateAndSetConfig( + std::shared_ptr & new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) { + return Status::INVALID_ECHO_MODE; + } + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config); + } + if (hw_monitor_wrapper_) { + hw_monitor_wrapper_->OnConfigChange(new_config); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config); + } + + sensor_cfg_ptr_ = new_config; + return Status::OK; +} + +void RobosenseRosWrapper::ReceiveScanMessageCallback( + std::unique_ptr scan_msg) +{ + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring received RobosenseScan. Launch with launch_hw:=false to enable RobosenseScan " + "replay."); + return; + } + + for (auto & pkt : scan_msg->packets) { + auto nebula_pkt_ptr = std::make_unique(); + nebula_pkt_ptr->stamp = pkt.stamp; + std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); + + packet_queue_.push(std::move(nebula_pkt_ptr)); + } +} + +void RobosenseRosWrapper::ReceiveInfoPacketCallback(std::vector & packet) +{ + if (!sensor_cfg_ptr_ || !info_driver_) { + throw std::runtime_error( + "Wrapper already receiving packets despite not being fully initialized yet."); + } + + auto status = info_driver_->DecodeInfoPacket(packet); + + if (status != nebula::Status::OK) { + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), *get_clock(), 1000, "Could not decode info packet: " << status); + return; + } + + if (!decoder_wrapper_) { + auto new_cfg = *sensor_cfg_ptr_; + new_cfg.return_mode = info_driver_->GetReturnMode(); + new_cfg.use_sensor_time = info_driver_->GetSyncStatus(); + auto calib = info_driver_->GetSensorCalibration(); + calib.CreateCorrectedChannels(); + + auto new_cfg_ptr = + std::make_shared(new_cfg); + status = ValidateAndSetConfig(new_cfg_ptr); + + if (status != nebula::Status::OK) { + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), *get_clock(), 1000, + "Invalid config from sensor (" << status << "): " << new_cfg); + return; + } + + auto calib_ptr = + std::make_shared(std::move(calib)); + decoder_wrapper_.emplace( + this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, sensor_cfg_ptr_, + calib_ptr); + RCLCPP_INFO_STREAM( + this->get_logger(), "Initialized decoder wrapper: " << decoder_wrapper_->Status()); + } + + if (!hw_monitor_wrapper_) { + return; + } + + hw_monitor_wrapper_->DiagnosticsCallback(info_driver_->GetSensorInfo()); +} + +Status RobosenseRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status RobosenseRosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) { + return hw_interface_wrapper_->Status(); + } + + auto info_status = hw_interface_wrapper_->HwInterface()->InfoInterfaceStart(); + + if (info_status != Status::OK) { + return info_status; + } + + return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); +} + +rcl_interfaces::msg::SetParametersResult RobosenseRosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + std::scoped_lock lock(mtx_config_); + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::RobosenseSensorConfiguration new_cfg(*sensor_cfg_ptr_); + + std::string _return_mode = ""; + bool got_any = + get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | + get_param(p, "scan_phase", new_cfg.scan_phase) | + get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold); + + // Currently, none of the wrappers have writeable parameters, so their update logic is not + // implemented + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + if (_return_mode.length() > 0) + new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode); + + auto new_cfg_ptr = std::make_shared(new_cfg); + auto status = ValidateAndSetConfig(new_cfg_ptr); + + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +void RobosenseRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + const auto now = std::chrono::high_resolution_clock::now(); + const auto timestamp_ns = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(packet); + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(RobosenseRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/velodyne/decoder_wrapper.cpp b/nebula_ros/src/velodyne/decoder_wrapper.cpp new file mode 100644 index 000000000..f55d7f28e --- /dev/null +++ b/nebula_ros/src/velodyne/decoder_wrapper.cpp @@ -0,0 +1,250 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/velodyne/decoder_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +using namespace std::chrono_literals; // NOLINT(build/namespaces) + +VelodyneDecoderWrapper::VelodyneDecoderWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config) +: status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("VelodyneDecoder")), + hw_interface_(hw_interface), + sensor_cfg_(config) +{ + if (!config) { + throw std::runtime_error( + "VelodyneDecoderWrapper cannot be instantiated without a valid config!"); + } + + calibration_file_path_ = + parent_node->declare_parameter("calibration_file", param_read_write()); + auto calibration_result = GetCalibrationData(calibration_file_path_); + + if (!calibration_result.has_value()) { + throw std::runtime_error( + (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); + } + + calibration_cfg_ptr_ = calibration_result.value(); + RCLCPP_INFO_STREAM( + logger_, "Using calibration data from " << calibration_cfg_ptr_->calibration_file); + + RCLCPP_INFO(logger_, "Starting Decoder"); + + driver_ptr_ = std::make_shared(config, calibration_cfg_ptr_); + status_ = driver_ptr_->GetStatus(); + + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); + } + + // Publish packets only if HW interface is connected + if (hw_interface_) { + current_scan_msg_ = std::make_unique(); + packets_pub_ = parent_node->create_publisher( + "velodyne_packets", rclcpp::SensorDataQoS()); + } + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + nebula_points_pub_ = + parent_node->create_publisher("velodyne_points", pointcloud_qos); + aw_points_base_pub_ = + parent_node->create_publisher("aw_points", pointcloud_qos); + aw_points_ex_pub_ = + parent_node->create_publisher("aw_points_ex", pointcloud_qos); + + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + cloud_watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE( + logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + }); +} + +void VelodyneDecoderWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(new_config, calibration_cfg_ptr_); + driver_ptr_ = new_driver; + sensor_cfg_ = new_config; +} + +void VelodyneDecoderWrapper::OnCalibrationChange( + const std::shared_ptr & new_calibration) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(sensor_cfg_, new_calibration); + driver_ptr_ = new_driver; + calibration_cfg_ptr_ = new_calibration; + calibration_file_path_ = calibration_cfg_ptr_->calibration_file; +} + +rcl_interfaces::msg::SetParametersResult VelodyneDecoderWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + std::string calibration_path = ""; + + bool got_any = get_param(p, "calibration_file", calibration_path); + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + if (!std::filesystem::exists(calibration_path)) { + auto result = SetParametersResult(); + result.successful = false; + result.reason = + "The given calibration path does not exist, ignoring: '" + calibration_path + "'"; + return result; + } + + auto get_calibration_result = GetCalibrationData(calibration_path); + if (!get_calibration_result.has_value()) { + auto result = SetParametersResult(); + result.successful = false; + result.reason = + (std::stringstream() << "Could not change calibration file to '" << calibration_path + << "': " << get_calibration_result.error()) + .str(); + return result; + } + + OnCalibrationChange(get_calibration_result.value()); + RCLCPP_INFO_STREAM( + logger_, "Changed calibration to '" << calibration_cfg_ptr_->calibration_file << "'"); + return rcl_interfaces::build().successful(true).reason(""); +} + +VelodyneDecoderWrapper::get_calibration_result_t VelodyneDecoderWrapper::GetCalibrationData( + const std::string & calibration_file_path) +{ + auto calib = std::make_shared(); + + // If downloaded data did not exist or could not be loaded, fall back to a generic file. + // If that file does not exist either, return an error code + if (!std::filesystem::exists(calibration_file_path)) { + RCLCPP_ERROR(logger_, "No calibration data found."); + return nebula::Status(Status::INVALID_CALIBRATION_FILE); + } + + // Try to load the existing fallback calibration file. Return an error if this fails + auto status = calib->LoadFromFile(calibration_file_path); + if (status != Status::OK) { + RCLCPP_ERROR_STREAM( + logger_, "Could not load calibration file at '" << calibration_file_path << "'"); + return status; + } + + // Return the fallback calibration file + calib->calibration_file = calibration_file_path; + return calib; +} + +void VelodyneDecoderWrapper::ProcessCloudPacket( + std::unique_ptr packet_msg) +{ + // Accumulate packets for recording only if someone is subscribed to the topic (for performance) + if ( + hw_interface_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + if (current_scan_msg_->packets.size() == 0) { + current_scan_msg_->header.stamp = packet_msg->stamp; + } + + velodyne_msgs::msg::VelodynePacket velodyne_packet_msg{}; + velodyne_packet_msg.stamp = packet_msg->stamp; + std::copy(packet_msg->data.begin(), packet_msg->data.end(), velodyne_packet_msg.data.begin()); + current_scan_msg_->packets.emplace_back(std::move(velodyne_packet_msg)); + } + + std::tuple pointcloud_ts{}; + nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; + { + std::lock_guard lock(mtx_driver_ptr_); + pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data, packet_msg->stamp.sec); + pointcloud = std::get<0>(pointcloud_ts); + } + + if (pointcloud == nullptr) { + return; + } + + cloud_watchdog_->update(); + + // Publish scan message only if it has been written to + if (current_scan_msg_ && !current_scan_msg_->packets.empty()) { + packets_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = std::make_unique(); + } + + if ( + nebula_points_pub_->get_subscription_count() > 0 || + nebula_points_pub_->get_intra_process_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(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + } + if ( + aw_points_base_pub_->get_subscription_count() > 0 || + aw_points_base_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_cloud_xyzi = + nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); + } + if ( + aw_points_ex_pub_->get_subscription_count() > 0 || + aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + pointcloud, std::get<1>(pointcloud_ts)); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); + } +} + +void VelodyneDecoderWrapper::PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher) +{ + if (pointcloud->header.stamp.sec < 0) { + RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); + } + pointcloud->header.frame_id = sensor_cfg_->frame_id; + publisher->publish(std::move(pointcloud)); +} + +nebula::Status VelodyneDecoderWrapper::Status() +{ + std::lock_guard lock(mtx_driver_ptr_); + + if (!driver_ptr_) { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp new file mode 100644 index 000000000..dafbf0d7e --- /dev/null +++ b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp @@ -0,0 +1,69 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/velodyne/hw_interface_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config) +: hw_interface_(new nebula::drivers::VelodyneHwInterface()), + logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), + status_(Status::NOT_INITIALIZED) +{ + setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); + + hw_interface_->SetLogger( + std::make_shared(parent_node->get_logger().get_child("HwInterface"))); + status_ = hw_interface_->InitializeSensorConfiguration(config); + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } + + status_ = hw_interface_->InitHttpClient(); + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HTTP client: " << status_).str()); + } + + if (setup_sensor_) { + RCLCPP_INFO_STREAM(logger_, "Setting sensor configuration"); + status_ = hw_interface_->SetSensorConfiguration(config); + } + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not set sensor configuration: " << status_).str()); + } + + status_ = Status::OK; +} + +void VelodyneHwInterfaceWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + hw_interface_->InitializeSensorConfiguration(new_config); + hw_interface_->InitHttpClient(); + if (setup_sensor_) { + hw_interface_->SetSensorConfiguration(new_config); + } +} + +Status VelodyneHwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr VelodyneHwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp b/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp new file mode 100644 index 000000000..94365d025 --- /dev/null +++ b/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp @@ -0,0 +1,1567 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/velodyne/hw_monitor_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +VelodyneHwMonitorWrapper::VelodyneHwMonitorWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config) +: logger_(parent_node->get_logger().get_child("HwMonitor")), + diagnostics_updater_(parent_node), + status_(Status::OK), + hw_interface_(hw_interface), + parent_node_(parent_node), + sensor_configuration_(config) +{ + diag_span_ = parent_node->declare_parameter("diag_span", param_read_only()); + show_advanced_diagnostics_ = + parent_node->declare_parameter("advanced_diagnostics", param_read_only()); + + std::cout << "Get model name and serial." << std::endl; + auto str = hw_interface_->GetSnapshot(); + current_snapshot_time.reset(new rclcpp::Time(parent_node_->now())); + current_snapshot_tree = + std::make_shared(hw_interface_->ParseJson(str)); + current_diag_tree = + std::make_shared(current_snapshot_tree->get_child("diag")); + current_status_tree = + std::make_shared(current_snapshot_tree->get_child("status")); + current_snapshot.reset(new std::string(str)); + + try { + info_model_ = GetPtreeValue(current_snapshot_tree, mtx_snapshot_, key_info_model); + info_serial_ = GetPtreeValue(current_snapshot_tree, mtx_snapshot_, key_info_serial); + RCLCPP_INFO_STREAM(logger_, "Model: " << info_model_); + RCLCPP_INFO_STREAM(logger_, "Serial: " << info_serial_); + } catch (boost::bad_lexical_cast & ex) { + RCLCPP_ERROR(logger_, " Error: Can't get model and serial"); + return; + } + + InitializeVelodyneDiagnostics(); +} + +void VelodyneHwMonitorWrapper::InitializeVelodyneDiagnostics() +{ + RCLCPP_INFO_STREAM(logger_, "InitializeVelodyneDiagnostics"); + using std::chrono_literals::operator""s; + std::ostringstream os; + auto hardware_id = info_model_ + ": " + info_serial_; + diagnostics_updater_.setHardwareID(hardware_id); + RCLCPP_INFO_STREAM(logger_, "Hardware ID: " << hardware_id); + + if (show_advanced_diagnostics_) { + diagnostics_updater_.add( + "velodyne_snapshot-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckSnapshot); + + diagnostics_updater_.add( + "velodyne_volt_temp_top_hv-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopHv); + if (sensor_configuration_->sensor_model != nebula::drivers::SensorModel::VELODYNE_VLP16) { + diagnostics_updater_.add( + "velodyne_volt_temp_top_ad_temp-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopAdTemp); + } + diagnostics_updater_.add( + "velodyne_volt_temp_top_lm20_temp-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopLm20Temp); + diagnostics_updater_.add( + "velodyne_volt_temp_top_pwr_5v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr5v); + diagnostics_updater_.add( + "velodyne_volt_temp_top_pwr_2_5v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr25v); + diagnostics_updater_.add( + "velodyne_volt_temp_top_pwr_3_3v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr33v); + diagnostics_updater_.add( + "velodyne_volt_temp_top_pwr_raw-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopPwrRaw); + diagnostics_updater_.add( + "velodyne_volt_temp_top_pwr_vccint-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopPwrVccint); + diagnostics_updater_.add( + "velodyne_volt_temp_bot_i_out-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotIOut); + diagnostics_updater_.add( + "velodyne_volt_temp_bot_pwr_1_2v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr12v); + diagnostics_updater_.add( + "velodyne_volt_temp_bot_lm20_temp-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotLm20Temp); + diagnostics_updater_.add( + "velodyne_volt_temp_bot_pwr_5v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr5v); + diagnostics_updater_.add( + "velodyne_volt_temp_bot_pwr_2_5v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr25v); + diagnostics_updater_.add( + "velodyne_volt_temp_bot_pwr_3_3v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr33v); + diagnostics_updater_.add( + "velodyne_volt_temp_bot_pwr_v_in-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwrVIn); + diagnostics_updater_.add( + "velodyne_volt_temp_bot_pwr_1_25v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr125v); + diagnostics_updater_.add( + "velodyne_vhv-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckVhv); + diagnostics_updater_.add( + "velodyne_adc_nf-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckAdcNf); + diagnostics_updater_.add( + "velodyne_adc_stats-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckAdcStats); + diagnostics_updater_.add( + "velodyne_ixe-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckIxe); + diagnostics_updater_.add( + "velodyne_adctp_stat-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckAdctpStat); + + diagnostics_updater_.add( + "velodyne_status_gps_pps_state-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckGpsPpsState); + diagnostics_updater_.add( + "velodyne_status_gps_pps_position-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckGpsPosition); + diagnostics_updater_.add( + "velodyne_status_motor_state-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckMotorState); + diagnostics_updater_.add( + "velodyne_status_motor_rpm-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckMotorRpm); + diagnostics_updater_.add( + "velodyne_status_motor_lock-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckMotorLock); + diagnostics_updater_.add( + "velodyne_status_motor_phase-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckMotorPhase); + diagnostics_updater_.add( + "velodyne_status_laser_state-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckLaserState); + } + + diagnostics_updater_.add("velodyne_status", this, &VelodyneHwMonitorWrapper::VelodyneCheckStatus); + diagnostics_updater_.add("velodyne_pps", this, &VelodyneHwMonitorWrapper::VelodyneCheckPps); + diagnostics_updater_.add( + "velodyne_temperature", this, &VelodyneHwMonitorWrapper::VelodyneCheckTemperature); + diagnostics_updater_.add("velodyne_rpm", this, &VelodyneHwMonitorWrapper::VelodyneCheckRpm); + diagnostics_updater_.add( + "velodyne_voltage", this, &VelodyneHwMonitorWrapper::VelodyneCheckVoltage); + + { + std::lock_guard lock(mtx_snapshot_); + current_snapshot.reset(new std::string("")); + current_snapshot_time.reset(new rclcpp::Time(parent_node_->now())); + } + + current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + + auto on_timer_snapshot = [this] { OnVelodyneSnapshotTimer(); }; + diagnostics_snapshot_timer_ = parent_node_->create_wall_timer( + std::chrono::milliseconds(diag_span_), std::move(on_timer_snapshot)); + + auto on_timer_update = [this] { + auto now = parent_node_->now(); + double dif; + { + std::lock_guard lock(mtx_snapshot_); + dif = (now - *current_snapshot_time).seconds(); + } + if (diag_span_ * 2.0 < dif * 1000) { + current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(logger_, "STALE"); + } else { + current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(logger_, "OK"); + } + diagnostics_updater_.force_update(); + }; + diagnostics_update_timer_ = + parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); +} + +void VelodyneHwMonitorWrapper::OnVelodyneSnapshotTimer() +{ + auto str = hw_interface_->GetSnapshot(); + auto ptree = hw_interface_->ParseJson(str); + + { + std::lock_guard lock(mtx_snapshot_); + + current_snapshot_time.reset(new rclcpp::Time(parent_node_->now())); + current_snapshot_tree = std::make_shared(ptree); + current_diag_tree = + std::make_shared(current_snapshot_tree->get_child("diag")); + current_status_tree = + std::make_shared(current_snapshot_tree->get_child("status")); + current_snapshot.reset(new std::string(str)); + } +} + +void VelodyneHwMonitorWrapper::OnVelodyneDiagnosticsTimer() +{ + std::cout << "OnVelodyneDiagnosticsTimer" << std::endl; + auto str = hw_interface_->GetDiag(); + { + std::lock_guard lock(mtx_diag_); + current_diag_tree = + std::make_shared(hw_interface_->ParseJson(str)); + } + diagnostics_updater_.force_update(); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopHv() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_hv)); + val = 101.0 * (val * 5.0 / 4096.0 - 5.0); + if (val < -150.0) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_hv + message_sep + voltage_low_message; + } else if (-132.0 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_hv + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopAdTemp() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_ad_temp)); + val = val * 5.0 / 4096.0; + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple +VelodyneHwMonitorWrapper::VelodyneGetTopLm20Temp() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_lm20_temp)); + val = -1481.96 + std::sqrt(2.1962e6 + ((1.8639 - val * 5.0 / 4096.0) / 3.88e-6)); + if (val < -25.0) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_lm20_temp + message_sep + temperature_cold_message; + } else if (90.0 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_lm20_temp + message_sep + temperature_hot_message; + } + // mes = boost::lexical_cast(val) + " C"; + mes = GetFixedPrecisionString(val) + " C"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopPwr5v() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_5v)); + val = 2.0 * val * 5.0 / 4096.0; + if (val < 4.8) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_5v + message_sep + voltage_low_message; + } else if (5.2 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_5v + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopPwr25v() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_2_5v)); + val = val * 5.0 / 4096.0; + if (val < 2.3) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_2_5v + message_sep + voltage_low_message; + } else if (2.7 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_2_5v + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopPwr33v() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_3_3v)); + val = val * 5.0 / 4096.0; + if (val < 3.1) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_3_3v + message_sep + voltage_low_message; + } else if (3.5 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_3_3v + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple +VelodyneHwMonitorWrapper::VelodyneGetTopPwr5vRaw() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_5v_raw)); + val = 2.0 * val * 5.0 / 4096.0; + if (val < 2.3) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_5v_raw + message_sep + voltage_low_message; + } else if (2.7 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_5v_raw + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopPwrRaw() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_raw)); + val = val * 5.0 / 4096.0; + if (val < 1.6) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_raw + message_sep + voltage_low_message; + } else if (1.9 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_raw + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple +VelodyneHwMonitorWrapper::VelodyneGetTopPwrVccint() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_vccint)); + val = val * 5.0 / 4096.0; + if (val < 1.0) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_vccint + message_sep + voltage_low_message; + } else if (1.4 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_top_pwr_vccint + message_sep + voltage_high_message; + } + // mes = boost::lexical_cast(val) + " V"; + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotIOut() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_i_out)); + val = 10.0 * (val * 5.0 / 4096.0 - 2.5); + if (val < 0.3) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_i_out + message_sep + ampere_low_message; + } else if (1.0 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_i_out + message_sep + ampere_high_message; + } + mes = GetFixedPrecisionString(val) + " A"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotPwr12v() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_1_2v)); + val = val * 5.0 / 4096.0; + if (val < 1.0) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_1_2v + message_sep + voltage_low_message; + } else if (1.4 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_1_2v + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple +VelodyneHwMonitorWrapper::VelodyneGetBotLm20Temp() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_lm20_temp)); + val = -1481.96 + std::sqrt(2.1962e6 + ((1.8639 - val * 5.0 / 4096.0) / 3.88e-6)); + if (val < -25.0) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_lm20_temp + message_sep + temperature_cold_message; + } else if (90.0 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_lm20_temp + message_sep + temperature_hot_message; + } + // mes = boost::lexical_cast(val) + " C"; + mes = GetFixedPrecisionString(val) + " C"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotPwr5v() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_5v)); + val = 2.0 * val * 5.0 / 4096.0; + if (val < 4.8) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_5v + message_sep + voltage_low_message; + } else if (5.2 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_5v + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotPwr25v() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_2_5v)); + val = val * 5.0 / 4096.0; + if (val < 2.3) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_2_5v + message_sep + voltage_low_message; + } else if (2.7 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_2_5v + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotPwr33v() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_3_3v)); + val = val * 5.0 / 4096.0; + if (val < 3.1) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_3_3v + message_sep + voltage_low_message; + } else if (3.5 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_3_3v + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotPwrVIn() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_v_in)); + val = 11.0 * val * 5.0 / 4096.0; + if (val < 8.0) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_v_in + message_sep + voltage_low_message; + } else if (19.0 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_v_in + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple +VelodyneHwMonitorWrapper::VelodyneGetBotPwr125v() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast( + GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_1_25v)); + val = val * 5.0 / 4096.0; + if (val < 1.0) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_1_25v + message_sep + voltage_low_message; + } else if (1.4 < val) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = name_volt_temp_bot_pwr_1_25v + message_sep + voltage_high_message; + } + mes = GetFixedPrecisionString(val) + " V"; + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetVhv() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + double val = 0.0; + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_vhv)); + mes = boost::lexical_cast(val); + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetAdcNf() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + std::ostringstream os; + boost::optional child = + current_diag_tree->get_child_optional(key_adc_nf); + if (child) { + std::ostringstream os; + for (auto v = child->begin(); v != child->end(); ++v) { + os << v->second.get("") << ", "; + } + mes = os.str(); + } else { + mes = not_supported_message; + } + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetAdcStats() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + std::ostringstream os; + boost::optional child = + current_diag_tree->get_child_optional(key_adc_stats); + if (child) { + std::ostringstream os; + for (auto v = child->begin(); v != child->end(); ++v) { + os << "("; + os << "mean: " << v->second.get("mean") << ", "; + os << "stddev: " << v->second.get("stddev") << ", "; + os << "), "; + } + mes = os.str(); + } else { + mes = not_supported_message; + } + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetIxe() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + mes = GetPtreeValue(current_diag_tree, mtx_diag_, key_ixe); + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetAdctpStat() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + std::ostringstream os; + boost::optional child = + current_diag_tree->get_child_optional(key_adctp_stat); + if (child) { + std::ostringstream os; + for (auto v = child->begin(); v != child->end(); ++v) { + os << v->second.get("") << ", "; + } + mes = os.str(); + } else { + mes = not_supported_message; + } + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple +VelodyneHwMonitorWrapper::VelodyneGetGpsPpsState() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_gps_pps_state); + if (mes == "Absent") { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + error_mes = mes; + } else if (mes == "Error") { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + error_mes = mes; + } + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple +VelodyneHwMonitorWrapper::VelodyneGetGpsPosition() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_gps_pps_position); + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple +VelodyneHwMonitorWrapper::VelodyneGetMotorState() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_state); + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetMotorRpm() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_rpm); + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple VelodyneHwMonitorWrapper::VelodyneGetMotorLock() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_lock); + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple +VelodyneHwMonitorWrapper::VelodyneGetMotorPhase() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_phase); + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +std::tuple +VelodyneHwMonitorWrapper::VelodyneGetLaserState() +{ + bool not_ex = true; + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string mes; + std::string error_mes; + try { + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_laser_state); + } catch (boost::bad_lexical_cast & ex) { + not_ex = false; + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = error_message; + } + return std::make_tuple(not_ex, level, mes, error_mes); +} + +void VelodyneHwMonitorWrapper::VelodyneCheckTopHv( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetTopHv(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckTopAdTemp( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetTopAdTemp(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckTopLm20Temp( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetTopLm20Temp(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr5v( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetTopPwr5v(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr25v( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetTopPwr25v(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr33v( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetTopPwr33v(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckTopPwrRaw( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetTopPwrRaw(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckTopPwrVccint( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetTopPwrVccint(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckBotIOut( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetBotIOut(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr12v( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetBotPwr12v(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckBotLm20Temp( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetBotLm20Temp(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr5v( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetBotPwr5v(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr25v( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetBotPwr25v(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr33v( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetBotPwr33v(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwrVIn( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetBotPwrVIn(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr125v( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetBotPwr125v(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckVhv( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetVhv(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckAdcNf( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetAdcNf(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckAdcStats( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetAdcStats(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckIxe( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetIxe(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckAdctpStat( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + auto tpl = VelodyneGetAdctpStat(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::OnVelodyneStatusTimer() +{ + auto str = hw_interface_->GetStatus(); + { + std::lock_guard lock(mtx_status_); + current_status_tree = + std::make_shared(hw_interface_->ParseJson(str)); + } + diagnostics_updater_.force_update(); +} + +void VelodyneHwMonitorWrapper::VelodyneCheckGpsPpsState( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + auto tpl = VelodyneGetGpsPpsState(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckGpsPosition( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + auto tpl = VelodyneGetGpsPosition(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckMotorState( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + auto tpl = VelodyneGetMotorState(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckMotorRpm( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + auto tpl = VelodyneGetMotorRpm(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckMotorLock( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + auto tpl = VelodyneGetMotorLock(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckMotorPhase( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + auto tpl = VelodyneGetMotorPhase(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckLaserState( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + auto tpl = VelodyneGetLaserState(); + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckSnapshot( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + uint8_t level = current_diag_status; + diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.summary(level, *current_snapshot); +} + +void VelodyneHwMonitorWrapper::VelodyneCheckStatus( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + + auto tpl = VelodyneGetMotorState(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_status_motor_state, std::get<2>(tpl)); + + tpl = VelodyneGetLaserState(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_status_laser_state, std::get<2>(tpl)); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckPps( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + + auto tpl = VelodyneGetGpsPpsState(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_status_gps_pps_state, std::get<2>(tpl)); + + tpl = VelodyneGetGpsPosition(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_status_gps_pps_position, std::get<2>(tpl)); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckTemperature( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + + auto tpl = VelodyneGetTopLm20Temp(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_top_lm20_temp, std::get<2>(tpl)); + + tpl = VelodyneGetBotLm20Temp(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_bot_lm20_temp, std::get<2>(tpl)); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckRpm( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + + auto tpl = VelodyneGetMotorRpm(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_status_motor_rpm, std::get<2>(tpl)); + + tpl = VelodyneGetMotorLock(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_status_motor_lock, std::get<2>(tpl)); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } +} + +void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + if ( + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + + auto tpl = VelodyneGetTopHv(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_top_hv, std::get<2>(tpl)); + + tpl = VelodyneGetTopPwr5v(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_top_pwr_5v, std::get<2>(tpl)); + + tpl = VelodyneGetTopPwr25v(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_top_pwr_2_5v, std::get<2>(tpl)); + + tpl = VelodyneGetTopPwr33v(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_top_pwr_3_3v, std::get<2>(tpl)); + + if (sensor_configuration_->sensor_model == nebula::drivers::SensorModel::VELODYNE_VLP16) { + tpl = VelodyneGetTopPwr5vRaw(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_top_pwr_5v_raw, std::get<2>(tpl)); + } else { + tpl = VelodyneGetTopPwrRaw(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_top_pwr_raw, std::get<2>(tpl)); + } + + tpl = VelodyneGetTopPwrVccint(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_top_pwr_vccint, std::get<2>(tpl)); + + tpl = VelodyneGetBotIOut(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_bot_i_out, std::get<2>(tpl)); + + tpl = VelodyneGetBotPwr12v(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_bot_pwr_1_2v, std::get<2>(tpl)); + + tpl = VelodyneGetBotPwr5v(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_bot_pwr_5v, std::get<2>(tpl)); + + tpl = VelodyneGetBotPwr25v(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_bot_pwr_2_5v, std::get<2>(tpl)); + + tpl = VelodyneGetBotPwr33v(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_bot_pwr_3_3v, std::get<2>(tpl)); + + tpl = VelodyneGetBotPwrVIn(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_bot_pwr_v_in, std::get<2>(tpl)); + + tpl = VelodyneGetBotPwr125v(); + if (std::get<0>(tpl)) { + level = std::max(level, std::get<1>(tpl)); + if (0 < std::get<3>(tpl).length()) { + msg.emplace_back(std::get<3>(tpl)); + } + } + diagnostics.add(name_volt_temp_bot_pwr_1_25v, std::get<2>(tpl)); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } +} + +std::string VelodyneHwMonitorWrapper::GetPtreeValue( + std::shared_ptr pt, std::mutex & mtx_pt, const std::string & key) +{ + std::lock_guard lock(mtx_pt); + boost::optional value = pt->get_optional(key); + if (value) { + return value.get(); + } else { + return not_supported_message; + } +} + +std::string VelodyneHwMonitorWrapper::GetFixedPrecisionString(double val, int pre) +{ + std::stringstream ss; + ss << std::fixed << std::setprecision(pre) << val; + return ss.str(); +} + +Status VelodyneHwMonitorWrapper::Status() +{ + return Status::OK; +} +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp deleted file mode 100644 index df484a27a..000000000 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,281 +0,0 @@ -#include "nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -VelodyneDriverRosWrapper::VelodyneDriverRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("velodyne_driver_ros_wrapper", options) -{ - drivers::VelodyneCalibrationConfiguration calibration_configuration; - drivers::VelodyneSensorConfiguration sensor_configuration; - - wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; - } - RCLCPP_DEBUG_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); - - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - - RCLCPP_DEBUG_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - - RCLCPP_DEBUG_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); - - velodyne_scan_sub_ = create_subscription( - "velodyne_packets", rclcpp::SensorDataQoS(), - std::bind(&VelodyneDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); - nebula_points_pub_ = this->create_publisher( - "velodyne_points", rclcpp::SensorDataQoS()); - aw_points_base_pub_ = - this->create_publisher("aw_points", rclcpp::SensorDataQoS()); - aw_points_ex_pub_ = - this->create_publisher("aw_points_ex", rclcpp::SensorDataQoS()); -} - -void VelodyneDriverRosWrapper::ReceiveScanMsgCallback( - const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg) -{ - auto t_start = std::chrono::high_resolution_clock::now(); - - std::tuple pointcloud_ts = - driver_ptr_->ConvertScanToPointcloud(scan_msg); - nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); - double cloud_stamp = std::get<1>(pointcloud_ts); - if (pointcloud == nullptr) { - RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); - return; - }; - if ( - nebula_points_pub_->get_subscription_count() > 0 || - nebula_points_pub_->get_intra_process_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(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); - } - if ( - aw_points_base_pub_->get_subscription_count() > 0 || - aw_points_base_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_cloud_xyzi = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); - } - if ( - aw_points_ex_pub_->get_subscription_count() > 0 || - aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, cloud_stamp); - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); - } - - auto runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG(get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); -} - -void VelodyneDriverRosWrapper::PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher) -{ - if (pointcloud->header.stamp.sec < 0) { - RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); - } - pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; - publisher->publish(std::move(pointcloud)); -} - -Status VelodyneDriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status VelodyneDriverRosWrapper::GetStatus() { return wrapper_status_; } - -Status VelodyneDriverRosWrapper::GetParameters( - drivers::VelodyneSensorConfiguration & sensor_configuration, - drivers::VelodyneCalibrationConfiguration & calibration_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = - nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "velodyne", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("min_range", 0.3, descriptor); - sensor_configuration.min_range = this->get_parameter("min_range").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("max_range", 300., descriptor); - sensor_configuration.max_range = this->get_parameter("max_range").as_double(); - } - double view_direction; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("view_direction", 0., descriptor); - view_direction = this->get_parameter("view_direction").as_double(); - } - double view_width = 2.0 * M_PI; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("view_width", 2.0 * M_PI, descriptor); - view_width = this->get_parameter("view_width").as_double(); - } - - if (sensor_configuration.sensor_model != nebula::drivers::SensorModel::VELODYNE_HDL64) { - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - } else { - double min_angle = fmod(fmod(view_direction + view_width / 2, 2 * M_PI) + 2 * M_PI, 2 * M_PI); - double max_angle = fmod(fmod(view_direction - view_width / 2, 2 * M_PI) + 2 * M_PI, 2 * M_PI); - sensor_configuration.cloud_min_angle = 100 * (2 * M_PI - min_angle) * 180 / M_PI + 0.5; - sensor_configuration.cloud_max_angle = 100 * (2 * M_PI - max_angle) * 180 / M_PI + 0.5; - if (sensor_configuration.cloud_min_angle == sensor_configuration.cloud_max_angle) { - // avoid returning empty cloud if min_angle = max_angle - sensor_configuration.cloud_min_angle = 0; - sensor_configuration.cloud_max_angle = 36000; - } - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - - if (calibration_configuration.calibration_file.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - } - } - - RCLCPP_INFO_STREAM( - this->get_logger(), "Sensor model: " << sensor_configuration.sensor_model - << ", Return mode: " << sensor_configuration.return_mode - << ", Scan Phase: " << sensor_configuration.scan_phase); - return Status::OK; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(VelodyneDriverRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp deleted file mode 100644 index cbbc19e05..000000000 --- a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,326 +0,0 @@ -#include "nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -VelodyneHwInterfaceRosWrapper::VelodyneHwInterfaceRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("velodyne_hw_interface_ros_wrapper", options), hw_interface_() -{ - not_supported_message = "Not supported"; - - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - // Initialize sensor_configuration - RCLCPP_INFO_STREAM(this->get_logger(), "Initialize sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.InitializeSensorConfiguration"); - hw_interface_.InitializeSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - if (this->setup_sensor) { - RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - updateParameters(); - } - - // register scan callback and publisher - hw_interface_.RegisterScanCallback(std::bind( - &VelodyneHwInterfaceRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); - velodyne_scan_pub_ = this->create_publisher( - "velodyne_packets", - rclcpp::SensorDataQoS(rclcpp::KeepLast(10)).best_effort().durability_volatile()); - - if (this->setup_sensor) { - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&VelodyneHwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - } - - auto status = StreamStart(); - if (status == nebula::Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "UDP Driver Started"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), status); - } -} - -Status VelodyneHwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - return interface_status_; -} - -Status VelodyneHwInterfaceRosWrapper::StreamStop() { return Status::OK; } -Status VelodyneHwInterfaceRosWrapper::Shutdown() { return Status::OK; } - -Status VelodyneHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status VelodyneHwInterfaceRosWrapper::GetParameters( - drivers::VelodyneSensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = - nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "velodyne", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 2369, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "range from 300 to 1200, in increments of 60"; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(300).set__to_value(1200).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 600, descriptor); - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("setup_sensor", true, descriptor); - this->setup_sensor = this->get_parameter("setup_sensor").as_bool(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { // || - return Status::SENSOR_CONFIG_ERROR; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void VelodyneHwInterfaceRosWrapper::ReceiveScanDataCallback( - std::unique_ptr scan_buffer) -{ - // Publish - scan_buffer->header.frame_id = sensor_configuration_.frame_id; - scan_buffer->header.stamp = scan_buffer->packets.front().stamp; - velodyne_scan_pub_->publish(*scan_buffer); -} - -std::string VelodyneHwInterfaceRosWrapper::GetPtreeValue( - std::shared_ptr pt, const std::string & key) -{ - boost::optional value = pt->get_optional(key); - if (value) { - return value.get(); - } else { - return not_supported_message; - } -} - -rcl_interfaces::msg::SetParametersResult VelodyneHwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - std::cout << "add_on_set_parameters_callback" << std::endl; - std::cout << p << std::endl; - std::cout << sensor_configuration_ << std::endl; - - drivers::VelodyneSensorConfiguration new_param{sensor_configuration_}; - std::cout << new_param << std::endl; - std::string sensor_model_str; - std::string return_mode_str; - if ( - get_param(p, "sensor_model", sensor_model_str) || - get_param(p, "return_mode", return_mode_str) || get_param(p, "host_ip", new_param.host_ip) || - get_param(p, "sensor_ip", new_param.sensor_ip) || - get_param(p, "frame_id", new_param.frame_id) || - get_param(p, "data_port", new_param.data_port) || - get_param(p, "gnss_port", new_param.gnss_port) || - get_param(p, "scan_phase", new_param.scan_phase) || - get_param(p, "packet_mtu_size", new_param.packet_mtu_size) || - get_param(p, "rotation_speed", new_param.rotation_speed) || - get_param(p, "cloud_min_angle", new_param.cloud_min_angle) || - get_param(p, "cloud_max_angle", new_param.cloud_max_angle)) { // || - - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - if (0 < return_mode_str.length()) - new_param.return_mode = nebula::drivers::ReturnModeFromString(return_mode_str); - - sensor_configuration_ = new_param; - // Update sensor_configuration - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - std::cout << "add_on_set_parameters_callback success" << std::endl; - - return *result; -} - -std::vector -VelodyneHwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - std::cout << "!!!!!!!!!!!updateParameters!!!!!!!!!!!!" << std::endl; - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - std::ostringstream os_return_mode; - os_return_mode << sensor_configuration_.return_mode; - std::cout << "set_parameters start" << std::endl; - auto results = set_parameters({ - rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("return_mode", os_return_mode.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("gnss_port", sensor_configuration_.gnss_port), - rclcpp::Parameter("scan_phase", sensor_configuration_.scan_phase), - rclcpp::Parameter("packet_mtu_size", sensor_configuration_.packet_mtu_size), - rclcpp::Parameter("rotation_speed", sensor_configuration_.rotation_speed), - rclcpp::Parameter("cloud_min_angle", sensor_configuration_.cloud_min_angle), - rclcpp::Parameter("cloud_max_angle", sensor_configuration_.cloud_max_angle) //, - }); - std::cout << "set_parameters fin" << std::endl; - return results; -} -RCLCPP_COMPONENTS_REGISTER_NODE(VelodyneHwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp deleted file mode 100644 index 172e61c83..000000000 --- a/nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp +++ /dev/null @@ -1,1996 +0,0 @@ -#include "nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp" - -#include -#include - -#include -#include - -#include - -namespace nebula -{ -namespace ros -{ -VelodyneHwMonitorRosWrapper::VelodyneHwMonitorRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("velodyne_hw_monitor_ros_wrapper", options), - hw_interface_(), - diagnostics_updater_(this) -{ - cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - // Initialize sensor_configuration - RCLCPP_INFO_STREAM(this->get_logger(), "Initialize sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.InitializeSensorConfiguration"); - hw_interface_.InitializeSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&VelodyneHwMonitorRosWrapper::paramCallback, this, std::placeholders::_1)); - - key_volt_temp_top_hv = "volt_temp.top.hv"; - key_volt_temp_top_ad_temp = "volt_temp.top.ad_temp"; // only32 - key_volt_temp_top_lm20_temp = "volt_temp.top.lm20_temp"; - key_volt_temp_top_pwr_5v = "volt_temp.top.pwr_5v"; - key_volt_temp_top_pwr_2_5v = "volt_temp.top.pwr_2_5v"; - key_volt_temp_top_pwr_3_3v = "volt_temp.top.pwr_3_3v"; - key_volt_temp_top_pwr_5v_raw = "volt_temp.top.pwr_5v_raw"; // only16 - key_volt_temp_top_pwr_raw = "volt_temp.top.pwr_raw"; // only32 - key_volt_temp_top_pwr_vccint = "volt_temp.top.pwr_vccint"; - key_volt_temp_bot_i_out = "volt_temp.bot.i_out"; - key_volt_temp_bot_pwr_1_2v = "volt_temp.bot.pwr_1_2v"; - key_volt_temp_bot_lm20_temp = "volt_temp.bot.lm20_temp"; - key_volt_temp_bot_pwr_5v = "volt_temp.bot.pwr_5v"; - key_volt_temp_bot_pwr_2_5v = "volt_temp.bot.pwr_2_5v"; - key_volt_temp_bot_pwr_3_3v = "volt_temp.bot.pwr_3_3v"; - key_volt_temp_bot_pwr_v_in = "volt_temp.bot.pwr_v_in"; - key_volt_temp_bot_pwr_1_25v = "volt_temp.bot.pwr_1_25v"; - key_vhv = "vhv"; - key_adc_nf = "adc_nf"; - key_adc_stats = "adc_stats"; - key_ixe = "ixe"; - key_adctp_stat = "adctp_stat"; - key_status_gps_pps_state = "gps.pps_state"; - key_status_gps_pps_position = "gps.position"; - key_status_motor_state = "motor.state"; - key_status_motor_rpm = "motor.rpm"; - key_status_motor_lock = "motor.lock"; - key_status_motor_phase = "motor.phase"; - key_status_laser_state = "laser.state"; - - name_volt_temp_top_hv = "Top HV"; - name_volt_temp_top_ad_temp = "Top A/D TD"; - name_volt_temp_top_lm20_temp = "Top Temp"; - name_volt_temp_top_pwr_5v = "Top 5v"; - name_volt_temp_top_pwr_2_5v = "Top 2.5v"; - name_volt_temp_top_pwr_3_3v = "Top 3.3v"; - name_volt_temp_top_pwr_5v_raw = "Top 5v(RAW)"; - name_volt_temp_top_pwr_raw = "Top RAW"; - name_volt_temp_top_pwr_vccint = "Top VCCINT"; - name_volt_temp_bot_i_out = "Bot I out"; - name_volt_temp_bot_pwr_1_2v = "Bot 1.2v"; - name_volt_temp_bot_lm20_temp = "Bot Temp"; - name_volt_temp_bot_pwr_5v = "Bot 5v"; - name_volt_temp_bot_pwr_2_5v = "Bot 2.5v"; - name_volt_temp_bot_pwr_3_3v = "Bot 3.3v"; - name_volt_temp_bot_pwr_v_in = "Bot V in"; - name_volt_temp_bot_pwr_1_25v = "Bot 1.25v"; // N/A? - name_vhv = "VHV"; - name_adc_nf = "adc_nf"; - name_adc_stats = "adc_stats"; - name_ixe = "ixe"; - name_adctp_stat = "adctp_stat"; - name_status_gps_pps_state = "GPS PPS"; - name_status_gps_pps_position = "GPS Position"; - name_status_motor_state = "Motor State"; - name_status_motor_rpm = "Motor RPM"; - name_status_motor_lock = "Motor Lock"; - name_status_motor_phase = "Motor Phase"; - name_status_laser_state = "Laser State"; - - message_sep = ": "; - - not_supported_message = "Not supported"; - error_message = "Error"; - - key_info_model = "info.model"; - key_info_serial = "info.serial"; - - temperature_cold_message = "temperature cold"; - temperature_hot_message = "temperature hot"; - voltage_low_message = "voltage low"; - voltage_high_message = "voltage high"; - ampere_low_message = "ampere low"; - ampere_high_message = "ampere high"; - - std::cout << "Get model name and serial." << std::endl; - hw_interface_.GetSnapshotAsync([this](const std::string & str) { - current_snapshot_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_snapshot_tree = - std::make_shared(hw_interface_.ParseJson(str)); - current_diag_tree = - std::make_shared(current_snapshot_tree->get_child("diag")); - current_status_tree = - std::make_shared(current_snapshot_tree->get_child("status")); - current_snapshot.reset(new std::string(str)); - - try { - info_model = GetPtreeValue(current_snapshot_tree, key_info_model); - info_serial = GetPtreeValue(current_snapshot_tree, key_info_serial); - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); - } catch (boost::bad_lexical_cast & ex) { - RCLCPP_ERROR_STREAM( - this->get_logger(), this->get_name() << " Error:" - << "Can't get model and serial"); - return; - } - - InitializeVelodyneDiagnostics(); - }); -} - -Status VelodyneHwMonitorRosWrapper::MonitorStart() { return interface_status_; } - -Status VelodyneHwMonitorRosWrapper::MonitorStop() { return Status::OK; } -Status VelodyneHwMonitorRosWrapper::Shutdown() { return Status::OK; } - -Status VelodyneHwMonitorRosWrapper::InitializeHwMonitor( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status VelodyneHwMonitorRosWrapper::GetParameters( - drivers::VelodyneSensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = - nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "velodyne", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 2369, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "range from 300 to 1200, in increments of 60"; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(300).set__to_value(1200).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 600, descriptor); - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { // || - return Status::SENSOR_CONFIG_ERROR; - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "It may be safe if it is 5000 milliseconds or more..."; - this->declare_parameter("diag_span", 3000, descriptor); - diag_span_ = this->get_parameter("diag_span").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; // because it affects initialization - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Showing advanced diagnostics"; - this->declare_parameter("advanced_diagnostics", false, descriptor); - use_advanced_diagnostics = this->get_parameter("advanced_diagnostics").as_bool(); - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - - return Status::OK; -} - -void VelodyneHwMonitorRosWrapper::InitializeVelodyneDiagnostics() -{ - RCLCPP_INFO_STREAM(get_logger(), "InitializeVelodyneDiagnostics"); - using std::chrono_literals::operator""s; - std::ostringstream os; - auto hardware_id = info_model + ": " + info_serial; - diagnostics_updater_.setHardwareID(hardware_id); - RCLCPP_INFO_STREAM(get_logger(), "hardware_id" << hardware_id); - - if (use_advanced_diagnostics) { - diagnostics_updater_.add( - "velodyne_snapshot-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckSnapshot); - - diagnostics_updater_.add( - "velodyne_volt_temp_top_hv-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopHv); - if (sensor_configuration_.sensor_model != nebula::drivers::SensorModel::VELODYNE_VLP16) { - diagnostics_updater_.add( - "velodyne_volt_temp_top_ad_temp-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopAdTemp); - } - diagnostics_updater_.add( - "velodyne_volt_temp_top_lm20_temp-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopLm20Temp); - diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_5v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr5v); - diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_2_5v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr25v); - diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_3_3v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr33v); - diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_raw-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrRaw); - diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_vccint-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrVccint); - diagnostics_updater_.add( - "velodyne_volt_temp_bot_i_out-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotIOut); - diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_1_2v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr12v); - diagnostics_updater_.add( - "velodyne_volt_temp_bot_lm20_temp-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotLm20Temp); - diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_5v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr5v); - diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_2_5v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr25v); - diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_3_3v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr33v); - diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_v_in-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwrVIn); - diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_1_25v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr125v); - diagnostics_updater_.add( - "velodyne_vhv-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckVhv); - diagnostics_updater_.add( - "velodyne_adc_nf-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckAdcNf); - diagnostics_updater_.add( - "velodyne_adc_stats-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckAdcStats); - diagnostics_updater_.add( - "velodyne_ixe-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckIxe); - diagnostics_updater_.add( - "velodyne_adctp_stat-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckAdctpStat); - - diagnostics_updater_.add( - "velodyne_status_gps_pps_state-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPpsState); - diagnostics_updater_.add( - "velodyne_status_gps_pps_position-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPosition); - diagnostics_updater_.add( - "velodyne_status_motor_state-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorState); - diagnostics_updater_.add( - "velodyne_status_motor_rpm-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorRpm); - diagnostics_updater_.add( - "velodyne_status_motor_lock-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorLock); - diagnostics_updater_.add( - "velodyne_status_motor_phase-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorPhase); - diagnostics_updater_.add( - "velodyne_status_laser_state-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckLaserState); - } - - diagnostics_updater_.add( - "velodyne_status", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckStatus); - diagnostics_updater_.add("velodyne_pps", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckPps); - diagnostics_updater_.add( - "velodyne_temperature", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckTemperature); - diagnostics_updater_.add("velodyne_rpm", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckRpm); - diagnostics_updater_.add( - "velodyne_voltage", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckVoltage); - - current_snapshot.reset(new std::string("")); - current_snapshot_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - - auto on_timer_snapshot = [this] { OnVelodyneSnapshotTimer(); }; - diagnostics_snapshot_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_snapshot), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(diagnostics_snapshot_timer_, cbg_m_); - - auto on_timer_update = [this] { - auto now = this->get_clock()->now(); - auto dif = (now - *current_snapshot_time).seconds(); - if (diag_span_ * 2.0 < dif * 1000) { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - diagnostics_updater_.force_update(); - }; - diagnostics_update_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(1000), std::move(on_timer_update), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(diagnostics_update_timer_, cbg_r_); -} - -std::string VelodyneHwMonitorRosWrapper::GetPtreeValue( - std::shared_ptr pt, const std::string & key) -{ - boost::optional value = pt->get_optional(key); - if (value) { - return value.get(); - } else { - return not_supported_message; - } -} -std::string VelodyneHwMonitorRosWrapper::GetFixedPrecisionString(double val, int pre) -{ - std::stringstream ss; - ss << std::fixed << std::setprecision(pre) << val; - return ss.str(); -} - -// https://memo.appri.me/programming/cpp-curl-http-client -using namespace std; -typedef void (*CurlCallback)(string err, string body); - -class Curl -{ -private: - /** response body */ - string body; - - // see: https://curl.se/docs/faq.html#Using_C_non_static_functions_f - static size_t invoke_write_data(char * buffer, size_t size, size_t nmemb, void * f) - { - // Call non-static member function. - return static_cast(f)->write_data(buffer, size, nmemb, f); - } - - /** a callback function for libcurl request */ - size_t write_data(char * buffer, size_t size, size_t nmemb, void *) - { - int dataLength = size * nmemb; - this->body.append(buffer, dataLength); - return dataLength; - } - -public: - /** user-agent */ - string useragent = "libcurl-agent/1.0"; - /** timeout */ - int timeout = 30L; // timeout 30 seconds - - /** - * Constructor - */ - Curl() - { - // - } - - /** - * HTTP GET - */ - void get(const string url, const CurlCallback cb) - { - CURL * curl; - CURLcode ret; - - this->body = ""; // init result body. - string err = ""; - - curl_global_init(CURL_GLOBAL_ALL); - curl = curl_easy_init(); - - if (curl == NULL) { - err = "curl_easy_init() failed on " + url; - return cb(err, ""); - } - - curl_easy_setopt(curl, CURLOPT_URL, url.c_str()); - curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, this->invoke_write_data); - curl_easy_setopt(curl, CURLOPT_WRITEDATA, this); - curl_easy_setopt(curl, CURLOPT_USERAGENT, this->useragent.c_str()); // UA - curl_easy_setopt(curl, CURLOPT_TIMEOUT, this->timeout); // timeout - // curl_easy_setopt(curl, CURLOPT_VERBOSE, 1L); // verbose - ret = curl_easy_perform(curl); - curl_easy_cleanup(curl); - curl_global_cleanup(); - - if (ret != CURLE_OK) { - err = "curl_easy_perform() failed on " + url + " (ret:" + to_string(ret) + ")"; - return cb(err, ""); - } - return cb(err, this->body); - } - - /** - * HTTP POST - */ - void post(const string url, const string data, const CurlCallback cb) - { - CURL * curl; - CURLcode ret; - - this->body = ""; // init result body. - string err = ""; - - curl_global_init(CURL_GLOBAL_ALL); - curl = curl_easy_init(); - - if (curl == NULL) { - err = "curl_easy_init() failed on " + url; - return cb(err, ""); - } - - curl_easy_setopt(curl, CURLOPT_URL, url.c_str()); - curl_easy_setopt(curl, CURLOPT_POST, 1); - curl_easy_setopt(curl, CURLOPT_POSTFIELDS, data.c_str()); - curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, this->invoke_write_data); - curl_easy_setopt(curl, CURLOPT_WRITEDATA, this); - curl_easy_setopt(curl, CURLOPT_USERAGENT, this->useragent.c_str()); // UA - curl_easy_setopt(curl, CURLOPT_TIMEOUT, this->timeout); // timeout - // curl_easy_setopt(curl, CURLOPT_VERBOSE, 1L); // verbose - ret = curl_easy_perform(curl); - curl_easy_cleanup(curl); - curl_global_cleanup(); - - if (ret != CURLE_OK) { - err = "curl_easy_perform() failed on " + url + " (ret:" + to_string(ret) + ")"; - return cb(err, ""); - } - return cb(err, this->body); - } -}; - -void VelodyneHwMonitorRosWrapper::curl_callback(std::string err, std::string body) -{ - if (err != "") { - RCLCPP_ERROR_STREAM(get_logger(), "curl_callback:" << err); - } else { - RCLCPP_INFO_STREAM(get_logger(), body); - current_diag_tree = - std::make_shared(hw_interface_.ParseJson(body)); - RCLCPP_DEBUG_STREAM(get_logger(), "diagnostics_updater_.force_update()"); - diagnostics_updater_.force_update(); - } -} - -void VelodyneHwMonitorRosWrapper::OnVelodyneDiagnosticsTimer() -{ - std::cout << "OnVelodyneDiagnosticsTimer" << std::endl; - if (mtx_diag.try_lock() || true) { - std::cout << "mtx_diag lock" << std::endl; - hw_interface_.GetDiagAsync([this](const std::string & str) { - current_diag_tree = - std::make_shared(hw_interface_.ParseJson(str)); - diagnostics_updater_.force_update(); - mtx_diag.unlock(); - std::cout << "mtx_diag unlock" << std::endl; - }); - } else { - std::cout << "mtx_diag is locked..." << std::endl; - } -} - -std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetTopHv() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_hv)); - val = 101.0 * (val * 5.0 / 4096.0 - 5.0); - if (val < -150.0) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_hv + message_sep + voltage_low_message; - } else if (-132.0 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_hv + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopAdTemp() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_ad_temp)); - val = val * 5.0 / 4096.0; - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopLm20Temp() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_lm20_temp)); - val = -1481.96 + std::sqrt(2.1962e6 + ((1.8639 - val * 5.0 / 4096.0) / 3.88e-6)); - if (val < -25.0) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_lm20_temp + message_sep + temperature_cold_message; - } else if (90.0 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_lm20_temp + message_sep + temperature_hot_message; - } - // mes = boost::lexical_cast(val) + " C"; - mes = GetFixedPrecisionString(val) + " C"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr5v() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_5v)); - val = 2.0 * val * 5.0 / 4096.0; - if (val < 4.8) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_5v + message_sep + voltage_low_message; - } else if (5.2 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_5v + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr25v() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_2_5v)); - val = val * 5.0 / 4096.0; - if (val < 2.3) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_2_5v + message_sep + voltage_low_message; - } else if (2.7 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_2_5v + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr33v() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_3_3v)); - val = val * 5.0 / 4096.0; - if (val < 3.1) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_3_3v + message_sep + voltage_low_message; - } else if (3.5 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_3_3v + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr5vRaw() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_5v_raw)); - val = 2.0 * val * 5.0 / 4096.0; - if (val < 2.3) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_5v_raw + message_sep + voltage_low_message; - } else if (2.7 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_5v_raw + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwrRaw() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_raw)); - val = val * 5.0 / 4096.0; - if (val < 1.6) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_raw + message_sep + voltage_low_message; - } else if (1.9 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_raw + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwrVccint() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_vccint)); - val = val * 5.0 / 4096.0; - if (val < 1.0) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_vccint + message_sep + voltage_low_message; - } else if (1.4 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_vccint + message_sep + voltage_high_message; - } - // mes = boost::lexical_cast(val) + " V"; - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotIOut() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_i_out)); - val = 10.0 * (val * 5.0 / 4096.0 - 2.5); - if (val < 0.3) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_i_out + message_sep + ampere_low_message; - } else if (1.0 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_i_out + message_sep + ampere_high_message; - } - mes = GetFixedPrecisionString(val) + " A"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr12v() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_1_2v)); - val = val * 5.0 / 4096.0; - if (val < 1.0) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_1_2v + message_sep + voltage_low_message; - } else if (1.4 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_1_2v + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotLm20Temp() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_lm20_temp)); - val = -1481.96 + std::sqrt(2.1962e6 + ((1.8639 - val * 5.0 / 4096.0) / 3.88e-6)); - if (val < -25.0) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_lm20_temp + message_sep + temperature_cold_message; - } else if (90.0 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_lm20_temp + message_sep + temperature_hot_message; - } - // mes = boost::lexical_cast(val) + " C"; - mes = GetFixedPrecisionString(val) + " C"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr5v() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_5v)); - val = 2.0 * val * 5.0 / 4096.0; - if (val < 4.8) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_5v + message_sep + voltage_low_message; - } else if (5.2 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_5v + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr25v() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_2_5v)); - val = val * 5.0 / 4096.0; - if (val < 2.3) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_2_5v + message_sep + voltage_low_message; - } else if (2.7 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_2_5v + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr33v() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_3_3v)); - val = val * 5.0 / 4096.0; - if (val < 3.1) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_3_3v + message_sep + voltage_low_message; - } else if (3.5 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_3_3v + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwrVIn() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_v_in)); - val = 11.0 * val * 5.0 / 4096.0; - if (val < 8.0) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_v_in + message_sep + voltage_low_message; - } else if (19.0 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_v_in + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr125v() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_1_25v)); - val = val * 5.0 / 4096.0; - if (val < 1.0) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_1_25v + message_sep + voltage_low_message; - } else if (1.4 < val) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_1_25v + message_sep + voltage_high_message; - } - mes = GetFixedPrecisionString(val) + " V"; - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetVhv() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_vhv)); - mes = boost::lexical_cast(val); - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetAdcNf() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - std::ostringstream os; - boost::optional child = - current_diag_tree->get_child_optional(key_adc_nf); - if (child) { - std::ostringstream os; - for (auto v = child->begin(); v != child->end(); ++v) { - os << v->second.get("") << ", "; - } - mes = os.str(); - } else { - mes = not_supported_message; - } - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetAdcStats() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - std::ostringstream os; - boost::optional child = - current_diag_tree->get_child_optional(key_adc_stats); - if (child) { - std::ostringstream os; - for (auto v = child->begin(); v != child->end(); ++v) { - os << "("; - os << "mean:" << v->second.get("mean") << ", "; - os << "stddev:" << v->second.get("stddev") << ", "; - os << "), "; - } - mes = os.str(); - } else { - mes = not_supported_message; - } - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetIxe() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - mes = GetPtreeValue(current_diag_tree, key_ixe); - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetAdctpStat() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - std::ostringstream os; - boost::optional child = - current_diag_tree->get_child_optional(key_adctp_stat); - if (child) { - std::ostringstream os; - for (auto v = child->begin(); v != child->end(); ++v) { - os << v->second.get("") << ", "; - } - mes = os.str(); - } else { - mes = not_supported_message; - } - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetGpsPpsState() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - mes = GetPtreeValue(current_status_tree, key_status_gps_pps_state); - if (mes == "Absent") { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = mes; - } else if (mes == "Error") { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - error_mes = mes; - } - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetGpsPosition() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - mes = GetPtreeValue(current_status_tree, key_status_gps_pps_position); - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetMotorState() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - mes = GetPtreeValue(current_status_tree, key_status_motor_state); - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetMotorRpm() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - mes = GetPtreeValue(current_status_tree, key_status_motor_rpm); - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetMotorLock() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - mes = GetPtreeValue(current_status_tree, key_status_motor_lock); - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetMotorPhase() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - mes = GetPtreeValue(current_status_tree, key_status_motor_phase); - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetLaserState() -{ - bool not_ex = true; - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string mes; - std::string error_mes; - try { - mes = GetPtreeValue(current_status_tree, key_status_laser_state); - } catch (boost::bad_lexical_cast & ex) { - not_ex = false; - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message; - } - return std::make_tuple(not_ex, level, mes, error_mes); -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckTopHv( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopHv(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckTopAdTemp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopAdTemp(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckTopLm20Temp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopLm20Temp(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr5v( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopPwr5v(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr25v( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopPwr25v(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr33v( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopPwr33v(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrRaw( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopPwrRaw(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrVccint( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopPwrVccint(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckBotIOut( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotIOut(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr12v( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwr12v(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckBotLm20Temp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotLm20Temp(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr5v( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwr5v(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr25v( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwr25v(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr33v( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwr33v(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwrVIn( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwrVIn(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr125v( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwr125v(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckVhv( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetVhv(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckAdcNf( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetAdcNf(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckAdcStats( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetAdcStats(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckIxe( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetIxe(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckAdctpStat( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetAdctpStat(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::OnVelodyneStatusTimer() -{ - if (mtx_status.try_lock()) { - hw_interface_.GetStatusAsync([this](const std::string & str) { - current_status_tree = - std::make_shared(hw_interface_.ParseJson(str)); - diagnostics_updater_.force_update(); - mtx_status.unlock(); - }); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPpsState( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetGpsPpsState(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPosition( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetGpsPosition(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckMotorState( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetMotorState(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckMotorRpm( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetMotorRpm(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckMotorLock( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetMotorLock(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckMotorPhase( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetMotorPhase(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckLaserState( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetLaserState(); - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckSnapshot( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - uint8_t level = current_diag_status; - diagnostics.add("sensor", sensor_configuration_.frame_id); - diagnostics.summary(level, *current_snapshot); - // } -} - -void VelodyneHwMonitorRosWrapper::OnVelodyneSnapshotTimer() -{ - hw_interface_.GetSnapshotAsync([this](const std::string & str) { - current_snapshot_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_snapshot_tree = - std::make_shared(hw_interface_.ParseJson(str)); - current_diag_tree = - std::make_shared(current_snapshot_tree->get_child("diag")); - current_status_tree = - std::make_shared(current_snapshot_tree->get_child("status")); - current_snapshot.reset(new std::string(str)); - }); -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckStatus( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - auto tpl = VelodyneGetMotorState(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_status_motor_state, std::get<2>(tpl)); - - tpl = VelodyneGetLaserState(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_status_laser_state, std::get<2>(tpl)); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckPps( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - auto tpl = VelodyneGetGpsPpsState(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_status_gps_pps_state, std::get<2>(tpl)); - - tpl = VelodyneGetGpsPosition(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_status_gps_pps_position, std::get<2>(tpl)); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckTemperature( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - auto tpl = VelodyneGetTopLm20Temp(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_top_lm20_temp, std::get<2>(tpl)); - - tpl = VelodyneGetBotLm20Temp(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_bot_lm20_temp, std::get<2>(tpl)); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckRpm( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - auto tpl = VelodyneGetMotorRpm(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_status_motor_rpm, std::get<2>(tpl)); - - tpl = VelodyneGetMotorLock(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_status_motor_lock, std::get<2>(tpl)); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } -} - -void VelodyneHwMonitorRosWrapper::VelodyneCheckVoltage( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - auto tpl = VelodyneGetTopHv(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_top_hv, std::get<2>(tpl)); - - tpl = VelodyneGetTopPwr5v(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_top_pwr_5v, std::get<2>(tpl)); - - tpl = VelodyneGetTopPwr25v(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_top_pwr_2_5v, std::get<2>(tpl)); - - tpl = VelodyneGetTopPwr33v(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_top_pwr_3_3v, std::get<2>(tpl)); - - if (sensor_configuration_.sensor_model == nebula::drivers::SensorModel::VELODYNE_VLP16) { - tpl = VelodyneGetTopPwr5vRaw(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_top_pwr_5v_raw, std::get<2>(tpl)); - } else { - tpl = VelodyneGetTopPwrRaw(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_top_pwr_raw, std::get<2>(tpl)); - } - - tpl = VelodyneGetTopPwrVccint(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_top_pwr_vccint, std::get<2>(tpl)); - - tpl = VelodyneGetBotIOut(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_bot_i_out, std::get<2>(tpl)); - - tpl = VelodyneGetBotPwr12v(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_bot_pwr_1_2v, std::get<2>(tpl)); - - tpl = VelodyneGetBotPwr5v(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_bot_pwr_5v, std::get<2>(tpl)); - - tpl = VelodyneGetBotPwr25v(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_bot_pwr_2_5v, std::get<2>(tpl)); - - tpl = VelodyneGetBotPwr33v(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_bot_pwr_3_3v, std::get<2>(tpl)); - - tpl = VelodyneGetBotPwrVIn(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_bot_pwr_v_in, std::get<2>(tpl)); - - tpl = VelodyneGetBotPwr125v(); - if (std::get<0>(tpl)) { - level = std::max(level, std::get<1>(tpl)); - if (0 < std::get<3>(tpl).length()) { - msg.emplace_back(std::get<3>(tpl)); - } - } - diagnostics.add(name_volt_temp_bot_pwr_1_25v, std::get<2>(tpl)); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } -} - -rcl_interfaces::msg::SetParametersResult VelodyneHwMonitorRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_INFO_STREAM(get_logger(), p); - RCLCPP_INFO_STREAM(get_logger(), sensor_configuration_); - - drivers::VelodyneSensorConfiguration new_param{sensor_configuration_}; - - std::cout << new_param << std::endl; - std::string sensor_model_str; - std::string return_mode_str; - uint16_t new_diag_span = 0; - if (get_param(p, "diag_span", new_diag_span)) { - sensor_configuration_ = new_param; - // Update sensor_configuration - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - std::cout << "add_on_set_parameters_callback success" << std::endl; - - return *result; -} -RCLCPP_COMPONENTS_REGISTER_NODE(VelodyneHwMonitorRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp new file mode 100644 index 000000000..808467e8c --- /dev/null +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -0,0 +1,257 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/velodyne/velodyne_ros_wrapper.hpp" + +#pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" + +namespace nebula +{ +namespace ros +{ +VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("velodyne_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + sensor_cfg_ptr_(nullptr), + packet_queue_(3000), + hw_interface_wrapper_(), + hw_monitor_wrapper_(), + decoder_wrapper_() +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *sensor_cfg_ptr_); + + launch_hw_ = declare_parameter("launch_hw", param_read_only()); + + if (launch_hw_) { + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); + hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->HwInterface(), sensor_cfg_ptr_); + } + + decoder_wrapper_.emplace( + this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, sensor_cfg_ptr_); + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessCloudPacket(packet_queue_.pop()); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterScanCallback( + std::bind(&VelodyneRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "velodyne_packets", rclcpp::SensorDataQoS(), + std::bind(&VelodyneRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&VelodyneRosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status VelodyneRosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::VelodyneSensorConfiguration config; + + auto _sensor_model = declare_parameter("sensor_model", param_read_only()); + config.sensor_model = drivers::SensorModelFromString(_sensor_model); + + auto _return_mode = declare_parameter("return_mode", param_read_write()); + config.return_mode = drivers::ReturnModeFromString(_return_mode); + + config.host_ip = declare_parameter("host_ip", param_read_only()); + config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); + config.data_port = declare_parameter("data_port", param_read_only()); + config.gnss_port = declare_parameter("gnss_port", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_write()); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.])"; + descriptor.floating_point_range = float_range(0, 360, 0.01); + config.scan_phase = declare_parameter("scan_phase", descriptor); + } + + config.min_range = declare_parameter("min_range", param_read_write()); + config.max_range = declare_parameter("max_range", param_read_write()); + config.packet_mtu_size = declare_parameter("packet_mtu_size", param_read_only()); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "from 300 to 1200, in increments of 60"; + descriptor.integer_range = int_range(300, 1200, 60); + config.rotation_speed = declare_parameter("rotation_speed", descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_min_angle = declare_parameter("cloud_min_angle", descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_max_angle = declare_parameter("cloud_max_angle", descriptor); + } + + auto new_cfg_ptr = std::make_shared(config); + return ValidateAndSetConfig(new_cfg_ptr); +} + +Status VelodyneRosWrapper::ValidateAndSetConfig( + std::shared_ptr & new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) { + return Status::INVALID_ECHO_MODE; + } + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config); + } + if (hw_monitor_wrapper_) { + hw_monitor_wrapper_->OnConfigChange(new_config); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config); + } + + sensor_cfg_ptr_ = new_config; + return Status::OK; +} + +void VelodyneRosWrapper::ReceiveScanMessageCallback( + std::unique_ptr scan_msg) +{ + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring received VelodyneScan. Launch with launch_hw:=false to enable VelodyneScan " + "replay."); + return; + } + + for (auto & pkt : scan_msg->packets) { + auto nebula_pkt_ptr = std::make_unique(); + nebula_pkt_ptr->stamp = pkt.stamp; + std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); + + packet_queue_.push(std::move(nebula_pkt_ptr)); + } +} + +Status VelodyneRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status VelodyneRosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) { + return hw_interface_wrapper_->Status(); + } + + return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); +} + +rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + std::scoped_lock lock(mtx_config_); + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::VelodyneSensorConfiguration new_cfg(*sensor_cfg_ptr_); + + std::string _return_mode = ""; + bool got_any = + get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | + get_param(p, "scan_phase", new_cfg.scan_phase) | get_param(p, "min_range", new_cfg.min_range) | + get_param(p, "max_range", new_cfg.max_range) | + get_param(p, "rotation_speed", new_cfg.rotation_speed) | + get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | + get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle); + + // Currently, HW interface and monitor wrappers have only read-only parameters, so their update + // logic is not implemented + if (decoder_wrapper_) { + auto result = decoder_wrapper_->OnParameterChange(p); + if (!result.successful) { + return result; + } + } + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + if (_return_mode.length() > 0) + new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode); + + auto new_cfg_ptr = std::make_shared(new_cfg); + auto status = ValidateAndSetConfig(new_cfg_ptr); + + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +void VelodyneRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + const auto now = std::chrono::high_resolution_clock::now(); + const auto timestamp_ns = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(packet); + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(VelodyneRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_sensor_driver/package.xml b/nebula_sensor_driver/package.xml index fa656a543..8e058494f 100644 --- a/nebula_sensor_driver/package.xml +++ b/nebula_sensor_driver/package.xml @@ -1,7 +1,7 @@ nebula_sensor_driver - 0.0.1 + 0.2.0 Meta package for nebula sensor drivers related packages MAP IV MAP IV diff --git a/nebula_tests/CMakeLists.txt b/nebula_tests/CMakeLists.txt index ac8943890..da2481a2f 100644 --- a/nebula_tests/CMakeLists.txt +++ b/nebula_tests/CMakeLists.txt @@ -1,10 +1,6 @@ cmake_minimum_required(VERSION 3.14) project(nebula_tests) -find_package(ament_cmake_auto REQUIRED) - -ament_auto_find_build_dependencies() - # Default to C++17 if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -25,15 +21,13 @@ elseif(${ROS_DISTRO} STREQUAL "humble") add_compile_definitions(ROS_DISTRO_HUMBLE) endif() - -find_package(PCL REQUIRED) -find_package(rosbag2_cpp REQUIRED) -find_package(pcl_conversions REQUIRED) -find_package(diagnostic_msgs REQUIRED) -find_package(diagnostic_updater REQUIRED) +find_package(ament_cmake_auto REQUIRED) find_package(nebula_common REQUIRED) find_package(nebula_decoders REQUIRED) -find_package(nebula_hw_interfaces REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(rosbag2_cpp REQUIRED) +find_package(diagnostic_updater REQUIRED) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -45,15 +39,35 @@ if(BUILD_TESTING) # TODO: FIX this path to point to nebula_decoders/calibration add_definitions(-D_SRC_CALIBRATION_DIR_PATH="${PROJECT_SOURCE_DIR}/../nebula_decoders/calibration/") - set(NEBULA_TEST_DEPENDENCIES - rclcpp - rosbag2_cpp - continental_msgs - nebula_msgs - pandar_msgs - velodyne_msgs - ) + set(NEBULA_TEST_INCLUDE_DIRS + ${nebula_common_INCLUDE_DIRS} + ${nebula_decoders_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${rosbag2_cpp_INCLUDE_DIRS} + ${diagnostic_updater_INCLUDE_DIRS} + ) + + set(NEBULA_TEST_LIBRARIES + ${nebula_common_TARGETS} + ${PCL_LIBRARIES} + ${rosbag2_cpp_TARGETS} + ${diagnostic_updater_TARGETS} + ) + + set(CONTINENTAL_TEST_LIBRARIES + ${NEBULA_TEST_LIBRARIES} + nebula_decoders::nebula_decoders_continental + ) + + set(HESAI_TEST_LIBRARIES + ${NEBULA_TEST_LIBRARIES} + nebula_decoders::nebula_decoders_hesai + ) + set(VELODYNE_TEST_LIBRARIES + ${NEBULA_TEST_LIBRARIES} + nebula_decoders::nebula_decoders_velodyne + ) add_subdirectory(continental) add_subdirectory(hesai) @@ -61,4 +75,4 @@ if(BUILD_TESTING) endif() -ament_auto_package() +ament_package() diff --git a/nebula_tests/continental/CMakeLists.txt b/nebula_tests/continental/CMakeLists.txt index 44daf8813..10e01ca1f 100644 --- a/nebula_tests/continental/CMakeLists.txt +++ b/nebula_tests/continental/CMakeLists.txt @@ -1,20 +1,52 @@ # Continental ARS548 -ament_auto_add_library(continental_ros_decoder_test_ars548 SHARED -continental_ros_decoder_test_ars548.cpp - ) +add_library(continental_ros_decoder_test_ars548 SHARED + continental_ros_decoder_test_ars548.cpp + parameter_descriptors.cpp +) + +target_include_directories(continental_ros_decoder_test_ars548 PUBLIC + ${NEBULA_TEST_INCLUDE_DIRS} +) + +target_link_libraries(continental_ros_decoder_test_ars548 + ${CONTINENTAL_TEST_LIBRARIES} +) + ament_add_gtest(continental_ros_decoder_test_main_ars548 -continental_ros_decoder_test_main_ars548.cpp - ) -target_link_libraries(continental_ros_decoder_test_ars548 ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) + continental_ros_decoder_test_main_ars548.cpp +) -ament_target_dependencies(continental_ros_decoder_test_main_ars548 - ${NEBULA_TEST_DEPENDENCIES} - ) target_include_directories(continental_ros_decoder_test_main_ars548 PUBLIC - ${PROJECT_SOURCE_DIR}/src/continental - include - ) + ${PROJECT_SOURCE_DIR}/src/continental + include + ${NEBULA_TEST_INCLUDE_DIRS} +) target_link_libraries(continental_ros_decoder_test_main_ars548 - ${PCL_LIBRARIES} - continental_ros_decoder_test_ars548 - ) + continental_ros_decoder_test_ars548 +) +# Continental SRR520 +add_library(continental_ros_decoder_test_srr520 SHARED + continental_ros_decoder_test_srr520.cpp + parameter_descriptors.cpp +) + +target_include_directories(continental_ros_decoder_test_srr520 PUBLIC + ${NEBULA_TEST_INCLUDE_DIRS} +) + +target_link_libraries(continental_ros_decoder_test_srr520 + ${CONTINENTAL_TEST_LIBRARIES} +) + +ament_add_gtest(continental_ros_decoder_test_main_srr520 + continental_ros_decoder_test_main_srr520.cpp +) + +target_include_directories(continental_ros_decoder_test_main_srr520 PUBLIC + ${PROJECT_SOURCE_DIR}/src/continental + include + ${NEBULA_TEST_INCLUDE_DIRS} +) +target_link_libraries(continental_ros_decoder_test_main_srr520 + continental_ros_decoder_test_srr520 +) diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index d31398c5b..f94989a3f 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -14,22 +14,25 @@ #include "continental_ros_decoder_test_ars548.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" -#include "rosbag2_storage/storage_options.hpp" +#include "parameter_descriptors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include #include +#include #include #include #include #include +#include namespace nebula { @@ -43,7 +46,7 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -87,88 +90,28 @@ Status ContinentalRosDecoderTest::GetParameters( std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; bag_root_dir /= "continental"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", "ARS548"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", "some_base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", "some_object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "some_sensor_frame", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / "ars548" / "1708578204").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "target_topic", "/sensing/radar/front_center/nebula_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); - } + + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", "ARS548", param_read_only())); + sensor_configuration.frame_id = + declare_parameter("frame_id", "some_sensor_frame", param_read_only()); + sensor_configuration.base_frame = + declare_parameter("base_frame", "some_base_frame", param_read_only()); + sensor_configuration.object_frame = + declare_parameter("object_frame", "some_object_frame", param_read_only()); + + bag_path_ = declare_parameter( + "bag_path", (bag_root_dir / "ars548" / "1708578204").string(), param_read_only()); + storage_id_ = declare_parameter("storage_id", "sqlite3", param_read_only()); + format_ = declare_parameter("format", "cdr", param_read_only()); + target_topic_ = declare_parameter( + "target_topic", "/sensing/radar/front_center/nebula_packets", param_read_only()); if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "Sensor Configuration: " << sensor_configuration); return Status::OK; } @@ -216,7 +159,7 @@ void ContinentalRosDecoderTest::DetectionListCallback( std::stringstream detection_path; detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_detection.yaml"; - auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); CheckResult(msg_as_string, gt_path.string()); @@ -231,7 +174,7 @@ void ContinentalRosDecoderTest::ObjectListCallback( std::stringstream detection_path; detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_object.yaml"; - auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); CheckResult(msg_as_string, gt_path.string()); @@ -242,22 +185,22 @@ void ContinentalRosDecoderTest::ReadBag() rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; + std::cout << bag_path_ << std::endl; + std::cout << storage_id_ << std::endl; + std::cout << format_ << std::endl; + std::cout << target_topic_ << std::endl; - auto target_topic_name = target_topic; + auto target_topic_name = target_topic_; if (target_topic_name.substr(0, 1) == "/") { target_topic_name = target_topic_name.substr(1); } target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - rcpputils::fs::path bag_dir(bag_path); + rcpputils::fs::path bag_dir(bag_path_); - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; + storage_options.uri = bag_path_; + storage_options.storage_id = storage_id_; + converter_options.output_serialization_format = format_; // "cdr"; rclcpp::Serialization serialization; { @@ -268,7 +211,7 @@ void ContinentalRosDecoderTest::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name == target_topic) { + if (bag_message->topic_name == target_topic_) { nebula_msgs::msg::NebulaPackets extracted_msg; rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); @@ -278,8 +221,9 @@ void ContinentalRosDecoderTest::ReadBag() ASSERT_EQ(1, extracted_msg.packets.size()); - auto extracted_msg_ptr = std::make_shared(extracted_msg); - driver_ptr_->ProcessPackets(extracted_msg); + auto extracted_msg_ptr = + std::make_unique(extracted_msg.packets[0]); + driver_ptr_->ProcessPacket(std::move(extracted_msg_ptr)); } } } diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp index aab813cb2..b509de9aa 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -15,21 +15,18 @@ #ifndef NEBULA_ContinentalRosDecoderTestArs548_H #define NEBULA_ContinentalRosDecoderTestArs548_H -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include +#include +#include +#include #include -#include #include #include #include #include +#include #include #include @@ -38,8 +35,7 @@ namespace nebula { namespace ros { -class ContinentalRosDecoderTest final : public rclcpp::Node, - NebulaDriverRosWrapperBase //, testing::Test +class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -78,10 +74,10 @@ class ContinentalRosDecoderTest final : public rclcpp::Node, void ReadBag(); private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; + std::string bag_path_{}; + std::string storage_id_{}; + std::string format_{}; + std::string target_topic_{}; }; } // namespace ros diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp index a3203da89..9935e446f 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp new file mode 100644 index 000000000..56b072750 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp @@ -0,0 +1,60 @@ +// 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 "continental_ros_decoder_test_srr520.hpp" + +#include + +#include + +#include + +std::shared_ptr continental_driver; + +TEST(TestDecoder, TestBag) +{ + std::cout << "TEST(TestDecoder, TestBag)" << std::endl; + continental_driver->ReadBag(); +} + +int main(int argc, char * argv[]) +{ + std::cout << "continental_ros_decoder_test_main_srr520.cpp" << std::endl; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + std::string node_name = "nebula_continental_decoder_test"; + + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + + continental_driver = std::make_shared(options, node_name); + exec.add_node(continental_driver->get_node_base_interface()); + + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); + nebula::Status driver_status = continental_driver->GetStatus(); + int result = 0; + if (driver_status == nebula::Status::OK) { + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); + result = RUN_ALL_TESTS(); + } else { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); + } + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); + continental_driver.reset(); + rclcpp::shutdown(); + + return result; +} diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp new file mode 100644 index 000000000..347bdf182 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -0,0 +1,259 @@ +// 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 "continental_ros_decoder_test_srr520.hpp" + +#include "parameter_descriptors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +ContinentalRosDecoderTest::ContinentalRosDecoderTest( + const rclcpp::NodeOptions & options, const std::string & node_name) +: rclcpp::Node(node_name, options) +{ + drivers::continental_srr520::ContinentalSRR520SensorConfiguration sensor_configuration; + + wrapper_status_ = GetParameters(sensor_configuration); + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); + + sensor_cfg_ptr_ = + std::make_shared( + sensor_configuration); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); + wrapper_status_ = InitializeDriver( + std::const_pointer_cast( + sensor_cfg_ptr_)); + + driver_ptr_->RegisterHRRDetectionListCallback( + std::bind(&ContinentalRosDecoderTest::HRRDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterNearDetectionListCallback( + std::bind(&ContinentalRosDecoderTest::NearDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalRosDecoderTest::ObjectListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterStatusCallback( + std::bind(&ContinentalRosDecoderTest::StatusCallback, this, std::placeholders::_1)); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); +} + +Status ContinentalRosDecoderTest::InitializeDriver( + std::shared_ptr + sensor_configuration) +{ + // driver should be initialized here with proper decoder + driver_ptr_ = std::make_shared( + std::static_pointer_cast( + sensor_configuration)); + return Status::OK; +} + +Status ContinentalRosDecoderTest::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalRosDecoderTest::GetParameters( + drivers::continental_srr520::ContinentalSRR520SensorConfiguration & sensor_configuration) +{ + std::filesystem::path bag_root_dir = + _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; + bag_root_dir /= "continental"; + + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", "SRR520", param_read_only())); + sensor_configuration.frame_id = + declare_parameter("frame_id", "some_sensor_frame", param_read_only()); + sensor_configuration.base_frame = + declare_parameter("base_frame", "some_base_frame", param_read_only()); + + bag_path_ = declare_parameter( + "bag_path", (bag_root_dir / "srr520" / "1708578209").string(), param_read_only()); + storage_id_ = declare_parameter("storage_id", "sqlite3", param_read_only()); + format_ = declare_parameter("format", "cdr", param_read_only()); + target_topic_ = declare_parameter( + "target_topic", "/sensing/radar/front_left/nebula_packets", param_read_only()); + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "Sensor Configuration: " << sensor_configuration); + return Status::OK; +} + +void ContinentalRosDecoderTest::CompareNodes(const YAML::Node & node1, const YAML::Node & node2) +{ + ASSERT_EQ(node1.IsDefined(), node2.IsDefined()); + ASSERT_EQ(node1.IsMap(), node2.IsMap()); + ASSERT_EQ(node1.IsNull(), node2.IsNull()); + ASSERT_EQ(node1.IsScalar(), node2.IsScalar()); + ASSERT_EQ(node1.IsSequence(), node2.IsSequence()); + + if (node1.IsMap()) { + for (YAML::const_iterator it = node1.begin(); it != node1.end(); ++it) { + CompareNodes(it->second, node2[it->first.as()]); + } + } else if (node1.IsScalar()) { + ASSERT_EQ(node1.as(), node2.as()); + } else if (node1.IsSequence()) { + ASSERT_EQ(node1.size(), node2.size()); + for (std::size_t i = 0; i < node1.size(); i++) { + CompareNodes(node1[i], node2[i]); + } + } +} + +void ContinentalRosDecoderTest::CheckResult( + const std::string msg_as_string, const std::string & gt_path) +{ + YAML::Node current_node = YAML::Load(msg_as_string); + YAML::Node gt_node = YAML::LoadFile(gt_path); + CompareNodes(gt_node, current_node); + + // To generate the gt + // std::cout << gt_path << std::endl; + // std::ofstream ostream(gt_path); + // ostream << msg_as_string; + // ostream.close(); +} + +void ContinentalRosDecoderTest::HRRDetectionListCallback( + std::unique_ptr msg) +{ + hrr_detection_list_count_++; + EXPECT_EQ(sensor_cfg_ptr_->frame_id, msg->header.frame_id); + std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); + + std::stringstream detection_path; + detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec + << "_hrr_detection.yaml"; + + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); + ASSERT_TRUE(gt_path.exists()); + + CheckResult(msg_as_string, gt_path.string()); +} + +void ContinentalRosDecoderTest::NearDetectionListCallback( + std::unique_ptr msg) +{ + near_detection_list_count_++; + EXPECT_EQ(sensor_cfg_ptr_->frame_id, msg->header.frame_id); + std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); + + std::stringstream detection_path; + detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec + << "_near_detection.yaml"; + + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); + ASSERT_TRUE(gt_path.exists()); + + CheckResult(msg_as_string, gt_path.string()); +} + +void ContinentalRosDecoderTest::ObjectListCallback( + std::unique_ptr msg) +{ + object_list_count_++; + EXPECT_EQ(sensor_cfg_ptr_->base_frame, msg->header.frame_id); + std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); + + std::stringstream detection_path; + detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_object.yaml"; + + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); + ASSERT_TRUE(gt_path.exists()); + + CheckResult(msg_as_string, gt_path.string()); +} + +void ContinentalRosDecoderTest::ReadBag() +{ + rosbag2_storage::StorageOptions storage_options; + rosbag2_cpp::ConverterOptions converter_options; + + std::cout << bag_path_ << std::endl; + std::cout << storage_id_ << std::endl; + std::cout << format_ << std::endl; + std::cout << target_topic_ << std::endl; + + auto target_topic_name = target_topic_; + if (target_topic_name.substr(0, 1) == "/") { + target_topic_name = target_topic_name.substr(1); + } + target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); + + rcpputils::fs::path bag_dir(bag_path_); + + storage_options.uri = bag_path_; + storage_options.storage_id = storage_id_; + converter_options.output_serialization_format = format_; // "cdr"; + rclcpp::Serialization serialization; + + { + rosbag2_cpp::Reader bag_reader(std::make_unique()); + bag_reader.open(storage_options, converter_options); + while (bag_reader.has_next()) { + auto bag_message = bag_reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name == target_topic_) { + nebula_msgs::msg::NebulaPackets extracted_msg; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + for (auto & packet_msg : extracted_msg.packets) { + auto extracted_msg_ptr = std::make_unique(packet_msg); + driver_ptr_->ProcessPacket(std::move(extracted_msg_ptr)); + } + } + } + } + + EXPECT_EQ(1, near_detection_list_count_); + EXPECT_EQ(1, hrr_detection_list_count_); + EXPECT_EQ(1, object_list_count_); +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp new file mode 100644 index 000000000..525360e03 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp @@ -0,0 +1,96 @@ +// 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_ContinentalRosDecoderTestsrr520_H +#define NEBULA_ContinentalRosDecoderTestsrr520_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test +{ + std::shared_ptr driver_ptr_; + Status wrapper_status_; + + std::shared_ptr + sensor_cfg_ptr_; + + Status InitializeDriver( + std::shared_ptr + sensor_configuration); + + Status GetParameters( + drivers::continental_srr520::ContinentalSRR520SensorConfiguration & sensor_configuration); + + void CheckResult(const std::string msg_as_string, const std::string & gt_path); + + void HRRDetectionListCallback( + std::unique_ptr msg); + + void NearDetectionListCallback( + std::unique_ptr msg); + + void StatusCallback([[maybe_unused]] std::unique_ptr msg) + { + } + + void ObjectListCallback(std::unique_ptr msg); + + void CompareNodes(const YAML::Node & node1, const YAML::Node & node2); + + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + +public: + explicit ContinentalRosDecoderTest( + const rclcpp::NodeOptions & options, const std::string & node_name); + + void ReceiveScanMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); + Status GetStatus(); + void ReadBag(); + +private: + std::string bag_path_{}; + std::string storage_id_{}; + std::string format_{}; + std::string target_topic_{}; + std::size_t near_detection_list_count_{}; + std::size_t hrr_detection_list_count_{}; + std::size_t object_list_count_{}; +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalRosDecoderTestsrr520_H diff --git a/nebula_tests/continental/parameter_descriptors.cpp b/nebula_tests/continental/parameter_descriptors.cpp new file mode 100644 index 000000000..3f26adf10 --- /dev/null +++ b/nebula_tests/continental/parameter_descriptors.cpp @@ -0,0 +1,36 @@ +// Copyright 2024 TIER IV, Inc. + +#include "parameter_descriptors.hpp" + +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write() +{ + return rcl_interfaces::msg::ParameterDescriptor{}; +}; + +rcl_interfaces::msg::ParameterDescriptor param_read_only() +{ + return rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); +} + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step) +{ + return { + rcl_interfaces::msg::FloatingPointRange().set__from_value(start).set__to_value(stop).set__step( + step)}; +} + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step) +{ + return { + rcl_interfaces::msg::IntegerRange().set__from_value(start).set__to_value(stop).set__step(step)}; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/continental/parameter_descriptors.hpp b/nebula_tests/continental/parameter_descriptors.hpp new file mode 100644 index 000000000..db37cdb10 --- /dev/null +++ b/nebula_tests/continental/parameter_descriptors.hpp @@ -0,0 +1,58 @@ +// 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. + +#pragma once + +#include +#include +#include + +#include +#include +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write(); + +rcl_interfaces::msg::ParameterDescriptor param_read_only(); + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step); + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step); + +/// @brief Get a parameter's value from a list of parameters, if that parameter is in the list. +/// @tparam T The parameter's expected value type +/// @param p A vector of parameters +/// @param name Target parameter name +/// @param value (out) Parameter value. Set if parameter is found, left untouched otherwise. +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml b/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml new file mode 100644 index 000000000..f9d7cfa56 --- /dev/null +++ b/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml @@ -0,0 +1,854 @@ +header: + stamp: + sec: 1708578208 + nanosec: 992410284 + frame_id: some_base_frame +internal_time_stamp_usec: 468502487 +global_time_stamp_sync_status: 1 +signal_status: 1 +sequence_counter: 198 +cycle_counter: 451 +ego_vx: 6.78121 +ego_yaw_rate: -0.00632930 +motion_type: 1 +objects: + - object_id: 8 + dist_x: 26.9082 + dist_y: 1.72125 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -3.12653 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 0 + dist_x: 9.18302 + dist_y: 4.48623 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0714155 + dist_y_std: 0.104376 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.417582 + box_width: 1.00366 + orientation: 2.63000e-07 + rcs: 0.439667 + score: 100.000 + life_cycles: 78 + box_valid: 1 + object_status: 2 + - object_id: 4 + dist_x: 0.503561 + dist_y: 5.72223 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 2.02736 + score: 100.000 + life_cycles: 9 + box_valid: 0 + object_status: 2 + - object_id: 11 + dist_x: 43.1043 + dist_y: -0.119017 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.203259 + dist_y_std: 1.38986 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00732601 + box_width: 0.146520 + orientation: 2.63000e-07 + rcs: 21.8857 + score: 86.6667 + life_cycles: 3 + box_valid: 1 + object_status: 2 + - object_id: 5 + dist_x: 5.29191 + dist_y: 15.8757 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.159311 + dist_y_std: 0.0878960 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 0.981685 + orientation: 2.63000e-07 + rcs: 8.74450 + score: 100.000 + life_cycles: 38 + box_valid: 1 + object_status: 3 + - object_id: 9 + dist_x: 27.4392 + dist_y: -5.39262 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 15.9257 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 10 + dist_x: 4.79751 + dist_y: 16.0405 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -3.59062 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 7 + dist_x: -2.04168 + dist_y: 6.11591 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.102545 + dist_y_std: 0.0842336 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.388278 + box_width: 0.446886 + orientation: 2.63000e-07 + rcs: -3.37079 + score: 100.000 + life_cycles: 25 + box_valid: 1 + object_status: 2 + - object_id: 2 + dist_x: 4.43129 + dist_y: 16.3427 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 6.64387 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 6 + dist_x: 13.5502 + dist_y: 10.6754 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.355246 + dist_y_std: 0.379051 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.161172 + box_width: 0.205128 + orientation: 2.63000e-07 + rcs: 1.58769 + score: 100.000 + life_cycles: 5 + box_valid: 1 + object_status: 2 + - object_id: 14 + dist_x: 3.85449 + dist_y: 21.4057 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -13.2145 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 3 + dist_x: 12.7354 + dist_y: 4.01929 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -10.4543 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 23 + dist_x: 7.93787 + dist_y: 6.15254 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.106208 + dist_y_std: 0.119026 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.278388 + box_width: 0.271062 + orientation: 2.63000e-07 + rcs: -2.12506 + score: 100.000 + life_cycles: 46 + box_valid: 1 + object_status: 2 + - object_id: 12 + dist_x: -0.292973 + dist_y: 6.02436 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.106208 + dist_y_std: 0.0805713 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.234432 + box_width: 0.344322 + orientation: 2.63000e-07 + rcs: -4.29897 + score: 100.000 + life_cycles: 82 + box_valid: 1 + object_status: 2 + - object_id: 17 + dist_x: 29.3527 + dist_y: -5.52995 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.0293040 + box_width: 0.0586081 + orientation: 2.63000e-07 + rcs: 24.4504 + score: 73.3333 + life_cycles: 2 + box_valid: 1 + object_status: 2 + - object_id: 15 + dist_x: 3.66223 + dist_y: 19.8401 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -4.59209 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 16 + dist_x: 44.7249 + dist_y: 1.68463 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 13.5076 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 32 + dist_x: 9.12809 + dist_y: 10.6388 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.108039 + dist_y_std: 0.0842336 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 0.996337 + orientation: 2.63000e-07 + rcs: 3.59062 + score: 100.000 + life_cycles: 39 + box_valid: 1 + object_status: 2 + - object_id: 20 + dist_x: 3.69885 + dist_y: 15.4180 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.172130 + dist_y_std: 0.113532 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.732601 + box_width: 0.783883 + orientation: 2.63000e-07 + rcs: -0.952614 + score: 86.6667 + life_cycles: 70 + box_valid: 1 + object_status: 3 + - object_id: 35 + dist_x: 11.0874 + dist_y: 4.29396 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0677531 + dist_y_std: 0.119026 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 1.00366 + orientation: 2.63000e-07 + rcs: -2.54030 + score: 100.000 + life_cycles: 56 + box_valid: 1 + object_status: 2 + - object_id: 18 + dist_x: 4.02845 + dist_y: 17.7709 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -15.1685 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 19 + dist_x: 20.6641 + dist_y: 12.0670 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.282000 + dist_y_std: 0.459623 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.212454 + box_width: 0.190476 + orientation: 2.63000e-07 + rcs: 4.71422 + score: 66.6667 + life_cycles: 11 + box_valid: 1 + object_status: 3 + - object_id: 28 + dist_x: 9.36614 + dist_y: 5.99689 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.104376 + dist_y_std: 0.135506 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.637363 + box_width: 0.256410 + orientation: 2.63000e-07 + rcs: 1.53884 + score: 100.000 + life_cycles: 36 + box_valid: 1 + object_status: 2 + - object_id: 34 + dist_x: 6.46383 + dist_y: 11.6733 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.373558 + dist_y_std: 0.146493 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.219780 + box_width: 0.336996 + orientation: 2.63000e-07 + rcs: -10.0147 + score: 100.000 + life_cycles: 5 + box_valid: 1 + object_status: 2 + - object_id: 22 + dist_x: 4.90738 + dist_y: 15.5187 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -4.20127 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 26 + dist_x: 4.35805 + dist_y: 16.3610 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 6.93698 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 31 + dist_x: 1.15360 + dist_y: 6.40889 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0970518 + dist_y_std: 0.0677531 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 0.893773 + orientation: 2.63000e-07 + rcs: -6.88813 + score: 100.000 + life_cycles: 63 + box_valid: 1 + object_status: 2 + - object_id: 21 + dist_x: 6.18916 + dist_y: 13.0558 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.267350 + dist_y_std: 0.130013 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.527472 + box_width: 0.271062 + orientation: 2.63000e-07 + rcs: -7.18124 + score: 100.000 + life_cycles: 13 + box_valid: 1 + object_status: 3 + - object_id: 33 + dist_x: 20.2338 + dist_y: 5.07218 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.126350 + dist_y_std: 0.415675 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.322344 + box_width: 0.630037 + orientation: 2.63000e-07 + rcs: -4.42110 + score: 100.000 + life_cycles: 9 + box_valid: 1 + object_status: 2 + - object_id: 27 + dist_x: 31.5226 + dist_y: -2.06915 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 7.79189 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 25 + dist_x: 18.7964 + dist_y: 3.58898 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.104376 + dist_y_std: 0.357077 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.534799 + box_width: 0.688645 + orientation: 2.63000e-07 + rcs: -2.95555 + score: 93.3333 + life_cycles: 17 + box_valid: 1 + object_status: 3 + - object_id: 24 + dist_x: 3.40587 + dist_y: 15.6743 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -10.5276 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 29 + dist_x: 24.5460 + dist_y: 3.90943 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0769090 + dist_y_std: 0.258194 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.307692 + box_width: 0.622711 + orientation: 2.63000e-07 + rcs: 4.29897 + score: 100.000 + life_cycles: 23 + box_valid: 1 + object_status: 2 + - object_id: 30 + dist_x: 31.0099 + dist_y: -6.34479 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 24.3283 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 36 + dist_x: 3.23192 + dist_y: 21.4698 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -10.9184 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 41 + dist_x: -2.84737 + dist_y: 6.15254 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0952206 + dist_y_std: 0.0897271 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.315018 + box_width: 0.234432 + orientation: 2.63000e-07 + rcs: 1.34343 + score: 100.000 + life_cycles: 91 + box_valid: 1 + object_status: 2 + - object_id: 39 + dist_x: 4.69680 + dist_y: 16.0039 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 1.85637 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 13 + dist_x: 29.0964 + dist_y: -3.08542 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.188610 + dist_y_std: 0.783739 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.117216 + box_width: 0.271062 + orientation: 2.63000e-07 + rcs: 13.2633 + score: 40.0000 + life_cycles: 7 + box_valid: 1 + object_status: 3 + - object_id: 43 + dist_x: 16.6631 + dist_y: 4.09254 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0787401 + dist_y_std: 0.186779 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.358974 + box_width: 0.681319 + orientation: 2.63000e-07 + rcs: 6.39961 + score: 100.000 + life_cycles: 48 + box_valid: 1 + object_status: 2 + - object_id: 40 + dist_x: 16.9836 + dist_y: 11.8748 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.250870 + dist_y_std: 0.351584 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.373626 + box_width: 0.593407 + orientation: 2.63000e-07 + rcs: -4.49438 + score: 80.0000 + life_cycles: 11 + box_valid: 1 + object_status: 3 diff --git a/nebula_tests/data/continental/srr520/1708578209/1708578209.db3 b/nebula_tests/data/continental/srr520/1708578209/1708578209.db3 new file mode 100644 index 000000000..1300ded38 Binary files /dev/null and b/nebula_tests/data/continental/srr520/1708578209/1708578209.db3 differ diff --git a/nebula_tests/data/continental/srr520/1708578209/metadata.yaml b/nebula_tests/data/continental/srr520/1708578209/metadata.yaml new file mode 100644 index 000000000..42b615187 --- /dev/null +++ b/nebula_tests/data/continental/srr520/1708578209/metadata.yaml @@ -0,0 +1,26 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 158949270 + starting_time: + nanoseconds_since_epoch: 1709891621083222124 + message_count: 4 + topics_with_message_count: + - topic_metadata: + name: /sensing/radar/front_left/nebula_packets + type: nebula_msgs/msg/NebulaPackets + serialization_format: cdr + offered_qos_profiles: "- history: 1\n depth: 5\n reliability: 2\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 1\n depth: 10\n reliability: 2\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 4 + compression_format: "" + compression_mode: "" + relative_file_paths: + - 1708578209.db3 + files: + - path: 1708578209.db3 + starting_time: + nanoseconds_since_epoch: 1709891621083222124 + duration: + nanoseconds: 158949270 + message_count: 4 diff --git a/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml b/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml new file mode 100644 index 000000000..556a9532c --- /dev/null +++ b/nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml @@ -0,0 +1,2147 @@ +header: + stamp: + sec: 1708578209 + nanosec: 45566935 + frame_id: some_sensor_frame +internal_time_stamp_usec: 468555644 +global_time_stamp_sync_status: 1 +signal_status: 1 +sequence_counter: 195 +cycle_counter: 909 +v_ambiguous: 21.4454 +max_range: 0.00000 +detections: + - range: 4.81074 + azimuth_angle: 0.344959 + range_rate: -0.0293251 + rcs: -25.7143 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 4.83516 + azimuth_angle: 0.394239 + range_rate: 0.249267 + rcs: -25.2381 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 5.44567 + azimuth_angle: 0.733038 + range_rate: 2.52199 + rcs: -2.85714 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 33.1000 + - range: 6.12943 + azimuth_angle: -0.683758 + range_rate: -5.83578 + rcs: -3.33333 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 28.0000 + - range: 6.56899 + azimuth_angle: -0.320319 + range_rate: -4.23754 + rcs: -4.76190 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 6.39805 + azimuth_angle: 1.01640 + range_rate: 4.14956 + rcs: -0.952381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 28.0000 + - range: 6.17827 + azimuth_angle: 0.850078 + range_rate: 3.19648 + rcs: -13.3333 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 6.42247 + azimuth_angle: -0.609838 + range_rate: -5.58651 + rcs: -10.4762 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 6.71551 + azimuth_angle: -0.535919 + range_rate: -5.27859 + rcs: -6.66667 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 22.9000 + - range: 7.35043 + azimuth_angle: -0.591359 + range_rate: -5.49853 + rcs: -9.04762 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 7.37485 + azimuth_angle: -0.431199 + range_rate: -4.80938 + rcs: -7.14286 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 22.9000 + - range: 7.57021 + azimuth_angle: 1.16424 + range_rate: 5.04399 + rcs: -1.42857 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 7.93651 + azimuth_angle: -0.498959 + range_rate: -5.11730 + rcs: 6.19048 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 34.8000 + - range: 8.05861 + azimuth_angle: -0.800798 + range_rate: -6.65689 + rcs: -2.85714 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 8.35165 + azimuth_angle: 1.21968 + range_rate: 5.27859 + rcs: 1.42857 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 8.84005 + azimuth_angle: 0.363439 + range_rate: 0.0293259 + rcs: -11.9048 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 9.20635 + azimuth_angle: -0.837758 + range_rate: -6.30499 + rcs: -11.4286 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 9.52381 + azimuth_angle: -0.917838 + range_rate: -1.59824 + rcs: -21.4286 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 9.27961 + azimuth_angle: 1.03000e-07 + range_rate: -2.25806 + rcs: -27.1429 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 9.15751 + azimuth_angle: -0.788478 + range_rate: -6.14369 + rcs: -16.1905 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 10.5739 + azimuth_angle: -0.135520 + range_rate: -3.19648 + rcs: -10.4762 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 10.4029 + azimuth_angle: 0.425039 + range_rate: 0.513197 + rcs: -13.8095 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 10.7448 + azimuth_angle: 0.0554400 + range_rate: -2.00880 + rcs: -7.14286 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 11.0134 + azimuth_angle: -0.215599 + range_rate: -3.69501 + rcs: 3.80952 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 31.4000 + - range: 10.6960 + azimuth_angle: 0.548239 + range_rate: 1.36364 + rcs: -13.3333 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 10.7692 + azimuth_angle: 0.542079 + range_rate: 1.14370 + rcs: -12.8571 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 10.5739 + azimuth_angle: 0.486639 + range_rate: 0.879766 + rcs: -9.04762 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 10.9890 + azimuth_angle: -0.00615988 + range_rate: -2.44868 + rcs: -12.3810 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 10.5250 + azimuth_angle: -0.160160 + range_rate: -3.31378 + rcs: -11.4286 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 12.0147 + azimuth_angle: 0.154000 + range_rate: -1.36364 + rcs: -16.1905 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 12.1368 + azimuth_angle: 0.117040 + range_rate: -1.55425 + rcs: -11.9048 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 11.8193 + azimuth_angle: -0.0492798 + range_rate: -2.65396 + rcs: -9.52381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 12.5763 + azimuth_angle: 0.708399 + range_rate: 2.31672 + rcs: 4.28571 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 12.6496 + azimuth_angle: -0.375759 + range_rate: -4.58944 + rcs: -9.52381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 13.3578 + azimuth_angle: 0.652959 + range_rate: 1.97947 + rcs: 20.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 36.5000 + - range: 13.3822 + azimuth_angle: -0.942478 + range_rate: -6.49560 + rcs: 4.28571 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 13.5287 + azimuth_angle: 0.505119 + range_rate: 0.909091 + rcs: 3.33333 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 13.6264 + azimuth_angle: 0.696079 + range_rate: 2.28739 + rcs: 11.4286 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 22.9000 + - range: 13.6264 + azimuth_angle: 0.973278 + range_rate: 3.90029 + rcs: 16.6667 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 13.9438 + azimuth_angle: -0.406559 + range_rate: -4.69208 + rcs: -3.80952 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 14.2369 + azimuth_angle: -0.437359 + range_rate: -4.82405 + rcs: -3.33333 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 14.7741 + azimuth_angle: 0.394239 + range_rate: 0.249267 + rcs: 6.66667 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 14.2857 + azimuth_angle: 1.10880 + range_rate: 4.36950 + rcs: -1.90476 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 15.2869 + azimuth_angle: -0.973278 + range_rate: -6.55425 + rcs: -1.90476 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 15.2869 + azimuth_angle: 0.917838 + range_rate: 3.66569 + rcs: -3.80952 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 15.4335 + azimuth_angle: 0.307999 + range_rate: -0.351906 + rcs: 12.3810 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 36.5000 + - range: 15.5311 + azimuth_angle: 1.13960 + range_rate: 4.89736 + rcs: 2.85714 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 16.3858 + azimuth_angle: -0.967118 + range_rate: -6.55425 + rcs: -1.42857 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 16.1416 + azimuth_angle: -0.0985597 + range_rate: -1.81818 + rcs: -7.14286 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 16.1416 + azimuth_angle: 0.252560 + range_rate: -1.80352 + rcs: -8.57143 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 16.1172 + azimuth_angle: -0.209439 + range_rate: -3.54839 + rcs: -11.9048 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 16.3126 + azimuth_angle: -0.252559 + range_rate: -3.75367 + rcs: -15.2381 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 16.9231 + azimuth_angle: 0.548239 + range_rate: -2.37537 + rcs: 0.476190 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 28.0000 + - range: 16.9963 + azimuth_angle: 0.856238 + range_rate: 3.46041 + rcs: -12.3810 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 17.6313 + azimuth_angle: -0.652958 + range_rate: -2.36070 + rcs: -4.76190 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 22.9000 + - range: 17.7534 + azimuth_angle: 0.689919 + range_rate: 1.99414 + rcs: -5.23810 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 17.8510 + azimuth_angle: 0.184800 + range_rate: -1.12903 + rcs: -13.8095 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 17.8510 + azimuth_angle: 0.757678 + range_rate: 2.78592 + rcs: -4.28571 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 17.8022 + azimuth_angle: 0.733038 + range_rate: 2.49267 + rcs: -6.66667 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 17.9243 + azimuth_angle: 0.794638 + range_rate: 2.05279 + rcs: -9.52381 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 18.2906 + azimuth_angle: -0.295679 + range_rate: -3.97361 + rcs: -20.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 18.0708 + azimuth_angle: 0.542079 + range_rate: 2.09677 + rcs: -12.8571 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 19.0476 + azimuth_angle: -0.560559 + range_rate: -5.38123 + rcs: -10.9524 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 22.9000 + - range: 19.0965 + azimuth_angle: 0.905518 + range_rate: 3.54839 + rcs: -11.9048 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 19.2186 + azimuth_angle: 0.326479 + range_rate: -0.175953 + rcs: -5.23810 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 19.5116 + azimuth_angle: -0.240239 + range_rate: -3.78299 + rcs: -8.57143 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 19.6093 + azimuth_angle: 0.714558 + range_rate: 2.36070 + rcs: -9.04762 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 19.4628 + azimuth_angle: 0.240240 + range_rate: -0.718475 + rcs: -22.3810 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 19.8291 + azimuth_angle: 0.862398 + range_rate: 3.25513 + rcs: -14.2857 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 19.8779 + azimuth_angle: -0.517439 + range_rate: -5.13196 + rcs: -16.6667 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 20.0000 + azimuth_angle: 0.769998 + range_rate: 2.77126 + rcs: -6.19048 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 20.2198 + azimuth_angle: -0.646798 + range_rate: -5.64516 + rcs: -14.2857 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 20.5617 + azimuth_angle: 0.351119 + range_rate: -0.0586506 + rcs: -11.4286 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 20.8303 + azimuth_angle: -1.07184 + range_rate: -6.68622 + rcs: 3.33333 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 28.0000 + - range: 20.5617 + azimuth_angle: -0.141680 + range_rate: -1.67155 + rcs: -18.5714 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 20.9035 + azimuth_angle: -0.911678 + range_rate: -6.42229 + rcs: -13.8095 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 20.5861 + azimuth_angle: 0.357279 + range_rate: 0.0733142 + rcs: -13.3333 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 21.1722 + azimuth_angle: 0.702239 + range_rate: 2.34604 + rcs: -5.23810 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 22.9000 + - range: 21.1966 + azimuth_angle: 0.677599 + range_rate: 2.14076 + rcs: 7.14286 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 33.1000 + - range: 21.4652 + azimuth_angle: -0.591359 + range_rate: -5.43988 + rcs: -14.7619 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 21.6850 + azimuth_angle: 1.29360 + range_rate: 5.77713 + rcs: 0.476190 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 22.6862 + azimuth_angle: 0.720718 + range_rate: 2.06745 + rcs: -15.7143 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 12.7000 + - range: 22.7106 + azimuth_angle: 1.13344 + range_rate: 5.51320 + rcs: -8.57143 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 22.6374 + azimuth_angle: 0.948638 + range_rate: 4.23754 + rcs: -13.8095 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 23.3455 + azimuth_angle: -1.09648 + range_rate: -6.70088 + rcs: -2.38095 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 23.0769 + azimuth_angle: 0.689919 + range_rate: -0.777126 + rcs: -6.19048 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 23.4676 + azimuth_angle: -0.431199 + range_rate: -1.09971 + rcs: -21.9048 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 23.2967 + azimuth_angle: 0.271039 + range_rate: -3.16716 + rcs: -20.4762 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 23.6630 + azimuth_angle: 0.831598 + range_rate: -1.18768 + rcs: -15.2381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 24.1270 + azimuth_angle: -0.659118 + range_rate: -2.18475 + rcs: -17.1429 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 24.0781 + azimuth_angle: 0.585199 + range_rate: -2.37537 + rcs: -15.7143 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 23.8339 + azimuth_angle: 0.123200 + range_rate: 0.909091 + rcs: -20.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 24.5665 + azimuth_angle: -0.683758 + range_rate: -2.41935 + rcs: -19.0476 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 14.4000 + - range: 24.8352 + azimuth_angle: 0.843918 + range_rate: -1.36364 + rcs: -12.8571 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 24.6154 + azimuth_angle: 0.696079 + range_rate: -2.19941 + rcs: -15.7143 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 24.8840 + azimuth_angle: -0.843918 + range_rate: -6.26100 + rcs: -16.1905 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 24.7619 + azimuth_angle: 0.776158 + range_rate: 2.60997 + rcs: -14.2857 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 24.7619 + azimuth_angle: 0.154000 + range_rate: 0.366569 + rcs: -18.0952 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 25.2259 + azimuth_angle: 0.800798 + range_rate: 3.91496 + rcs: -1.90476 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 25.0549 + azimuth_angle: 1.25048 + range_rate: 5.89443 + rcs: -2.38095 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 25.8120 + azimuth_angle: -0.221759 + range_rate: -3.68035 + rcs: -20.9524 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 25.7143 + azimuth_angle: -1.47224 + range_rate: -6.67155 + rcs: 19.0476 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 25.8608 + azimuth_angle: 0.455839 + range_rate: 0.557185 + rcs: -16.1905 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 26.5934 + azimuth_angle: 0.665279 + range_rate: 4.07625 + rcs: -1.90476 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 26.6911 + azimuth_angle: 0.394239 + range_rate: 0.205279 + rcs: 6.66667 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 31.4000 + - range: 26.7155 + azimuth_angle: 0.659119 + range_rate: 4.28153 + rcs: -9.04762 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 26.9597 + azimuth_angle: 0.418879 + range_rate: 0.439883 + rcs: 1.42857 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 29.7000 + - range: 27.3748 + azimuth_angle: 1.18272 + range_rate: 4.98534 + rcs: -0.952381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 28.4005 + azimuth_angle: 0.948638 + range_rate: 3.81232 + rcs: 3.80952 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 28.2540 + azimuth_angle: 0.154000 + range_rate: 1.45161 + rcs: -9.04762 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 28.5958 + azimuth_angle: 0.154000 + range_rate: 0.835778 + rcs: -14.2857 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 28.9377 + azimuth_angle: -1.47840 + range_rate: -6.68622 + rcs: 20.0000 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 28.6935 + azimuth_angle: 0.868558 + range_rate: 3.29912 + rcs: -1.42857 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 22.9000 + - range: 29.5238 + azimuth_angle: 1.19504 + range_rate: 5.41056 + rcs: -5.23810 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 29.8168 + azimuth_angle: 1.15808 + range_rate: 5.01466 + rcs: -1.60000e-08 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 30.5494 + azimuth_angle: 1.47840 + range_rate: 5.86510 + rcs: 20.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 30.4518 + azimuth_angle: 0.837758 + range_rate: 3.44575 + rcs: -12.8571 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 30.2564 + azimuth_angle: 0.659119 + range_rate: 2.00880 + rcs: -10.9524 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 30.7692 + azimuth_angle: 0.314159 + range_rate: -0.835777 + rcs: -17.1429 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 30.5983 + azimuth_angle: -1.31824 + range_rate: -6.68622 + rcs: 0.476190 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 31.4042 + azimuth_angle: 1.15808 + range_rate: 4.85337 + rcs: 10.4762 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 34.8000 + - range: 31.4774 + azimuth_angle: 1.13344 + range_rate: 5.11730 + rcs: -9.04762 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 32.4786 + azimuth_angle: 0.437359 + range_rate: 0.366569 + rcs: -16.1905 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 32.8449 + azimuth_angle: 0.874718 + range_rate: 1.36364 + rcs: -15.2381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 33.5775 + azimuth_angle: 1.10880 + range_rate: 4.57478 + rcs: 1.90476 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 34.3834 + azimuth_angle: 0.283359 + range_rate: -0.410557 + rcs: -16.6667 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 34.5299 + azimuth_angle: -1.47840 + range_rate: -6.70088 + rcs: 20.0000 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 34.7985 + azimuth_angle: -0.190959 + range_rate: -3.57771 + rcs: -16.6667 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 35.1893 + azimuth_angle: -0.147840 + range_rate: -3.29912 + rcs: -6.66667 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 35.1160 + azimuth_angle: -0.917838 + range_rate: -6.46628 + rcs: 6.19048 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 33.1000 + - range: 35.3114 + azimuth_angle: 0.954798 + range_rate: 3.85631 + rcs: 4.76190 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 29.7000 + - range: 36.5324 + azimuth_angle: -0.141680 + range_rate: -3.31378 + rcs: -15.7143 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 17.8000 + - range: 36.8498 + azimuth_angle: 0.868558 + range_rate: 3.62170 + rcs: -13.3333 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 36.6545 + azimuth_angle: -0.197119 + range_rate: -3.60704 + rcs: -18.0952 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 37.1673 + azimuth_angle: -0.615999 + range_rate: -5.57185 + rcs: -13.8095 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 37.1917 + azimuth_angle: -1.28128 + range_rate: -6.71554 + rcs: -1.90476 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 37.1184 + azimuth_angle: -0.665278 + range_rate: -5.77713 + rcs: -18.5714 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 37.5336 + azimuth_angle: 0.659119 + range_rate: 1.99414 + rcs: -14.2857 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 38.2662 + azimuth_angle: -0.942478 + range_rate: -6.49560 + rcs: -14.2857 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 39.2918 + azimuth_angle: 0.677599 + range_rate: 2.14076 + rcs: -10.9524 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 39.5116 + azimuth_angle: -1.21352 + range_rate: -6.75953 + rcs: 15.2381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 33.1000 + - range: 40.7570 + azimuth_angle: -1.25664 + range_rate: -6.75953 + rcs: 15.2381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 29.7000 + - range: 40.8303 + azimuth_angle: -0.141680 + range_rate: -1.65689 + rcs: -12.8571 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 41.0501 + azimuth_angle: -0.240239 + range_rate: -3.63636 + rcs: -17.1429 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 40.8547 + azimuth_angle: 0.671439 + range_rate: 2.08211 + rcs: -10.4762 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 41.5385 + azimuth_angle: 0.375759 + range_rate: -1.53959 + rcs: -9.04762 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 42.1978 + azimuth_angle: -1.11496 + range_rate: -6.68622 + rcs: 2.38095 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 26.3000 + - range: 43.6386 + azimuth_angle: -1.05336 + range_rate: -6.51026 + rcs: -11.4286 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 44.6886 + azimuth_angle: -0.221759 + range_rate: -3.66569 + rcs: -9.04762 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 22.9000 + - range: 44.4200 + azimuth_angle: -1.09032 + range_rate: -6.70088 + rcs: -8.57143 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 45.3236 + azimuth_angle: 0.659119 + range_rate: 2.00880 + rcs: -7.61905 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 45.6899 + azimuth_angle: 0.671439 + range_rate: 2.19941 + rcs: -9.52381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 46.7399 + azimuth_angle: -0.942478 + range_rate: -6.55425 + rcs: -11.4286 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 46.7399 + azimuth_angle: -1.36136 + range_rate: -6.73021 + rcs: 8.09524 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 46.3736 + azimuth_angle: 0.899358 + range_rate: 3.28446 + rcs: -12.3810 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 46.5690 + azimuth_angle: 0.425039 + range_rate: 0.469209 + rcs: -10.0000 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 47.3260 + azimuth_angle: 0.381919 + range_rate: 0.263930 + rcs: -7.61905 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 47.3504 + azimuth_angle: 0.665279 + range_rate: 2.05279 + rcs: -10.9524 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 48.0830 + azimuth_angle: -0.948638 + range_rate: -6.53959 + rcs: -9.52381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 48.5714 + azimuth_angle: -1.34904 + range_rate: -6.73021 + rcs: 17.1429 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 51.6483 + azimuth_angle: -1.12728 + range_rate: -6.71554 + rcs: -1.60000e-08 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 22.9000 + - range: 52.4298 + azimuth_angle: -0.930158 + range_rate: -6.51026 + rcs: -10.9524 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 52.9182 + azimuth_angle: -1.29360 + range_rate: -6.74487 + rcs: 13.3333 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 24.6000 + - range: 53.6508 + azimuth_angle: -1.39832 + range_rate: -6.73021 + rcs: 20.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 53.3578 + azimuth_angle: -0.677598 + range_rate: -5.70381 + rcs: -13.3333 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 53.4310 + azimuth_angle: 0.406559 + range_rate: 0.586511 + rcs: -13.3333 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 57.9243 + azimuth_angle: -0.209439 + range_rate: -3.62170 + rcs: -9.04762 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 58.6569 + azimuth_angle: -1.12112 + range_rate: -6.73021 + rcs: 20.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 36.5000 + - range: 61.1966 + azimuth_angle: -1.15192 + range_rate: -6.71554 + rcs: 6.19048 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 61.8315 + azimuth_angle: 0.437359 + range_rate: 0.513197 + rcs: -12.3810 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 63.8828 + azimuth_angle: -0.973278 + range_rate: -6.53959 + rcs: -1.60000e-08 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 63.9316 + azimuth_angle: -1.15192 + range_rate: -6.71554 + rcs: -0.476190 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 65.8852 + azimuth_angle: -1.13344 + range_rate: -6.73021 + rcs: 3.80952 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 70.3541 + azimuth_angle: -0.794638 + range_rate: -6.30499 + rcs: -3.80952 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 71.6239 + azimuth_angle: -1.03488 + range_rate: -6.55425 + rcs: -4.28571 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 77.6801 + azimuth_angle: -1.12112 + range_rate: -6.71554 + rcs: 9.52381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 77.4359 + azimuth_angle: -0.831598 + range_rate: -4.26686 + rcs: -9.04762 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 79.0232 + azimuth_angle: -0.227919 + range_rate: -3.66569 + rcs: -10.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 12.7000 + - range: 79.9512 + azimuth_angle: -0.172480 + range_rate: -3.41642 + rcs: -1.90476 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 80.0488 + azimuth_angle: -1.15192 + range_rate: -6.74487 + rcs: 11.9048 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 22.9000 + - range: 81.2454 + azimuth_angle: 0.997918 + range_rate: 4.03226 + rcs: 20.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 34.8000 + - range: 82.5397 + azimuth_angle: 1.01024 + range_rate: 4.03226 + rcs: 8.09524 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 24.6000 + - range: 83.0281 + azimuth_angle: -0.154000 + range_rate: -3.44575 + rcs: -5.23810 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 83.0037 + azimuth_angle: -1.15808 + range_rate: -6.74487 + rcs: 3.80952 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 85.7387 + azimuth_angle: 0.609839 + range_rate: 1.67155 + rcs: 10.4762 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 28.0000 + - range: 85.9341 + azimuth_angle: -1.17656 + range_rate: -6.73021 + rcs: 5.71429 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 87.6190 + azimuth_angle: 0.474319 + range_rate: 0.821115 + rcs: -2.38095 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 14.4000 + - range: 88.5470 + azimuth_angle: -1.13344 + range_rate: -6.74487 + rcs: 7.14286 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 91.4286 + azimuth_angle: -1.16424 + range_rate: -6.74487 + rcs: 15.7143 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 28.0000 + - range: 95.2625 + azimuth_angle: -1.07184 + range_rate: -6.67155 + rcs: 3.33333 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 95.9951 + azimuth_angle: 0.474319 + range_rate: 0.791789 + rcs: -1.60000e-08 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 96.6300 + azimuth_angle: -1.04720 + range_rate: -6.56891 + rcs: 4.28571 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 97.4847 + azimuth_angle: -1.15192 + range_rate: -6.74487 + rcs: 15.7143 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 99.7802 + azimuth_angle: -1.17656 + range_rate: -6.74487 + rcs: 13.3333 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 diff --git a/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml b/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml new file mode 100644 index 000000000..cb8e3279c --- /dev/null +++ b/nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml @@ -0,0 +1,365 @@ +header: + stamp: + sec: 1708578209 + nanosec: 76111685 + frame_id: some_sensor_frame +internal_time_stamp_usec: 468586189 +global_time_stamp_sync_status: 1 +signal_status: 1 +sequence_counter: 196 +cycle_counter: 910 +v_ambiguous: 23.3772 +max_range: 41.3000 +detections: + - range: 7.76557 + azimuth_angle: 0.566719 + range_rate: 2.28739 + rcs: -40.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 10.7692 + azimuth_angle: -0.671438 + range_rate: -5.49853 + rcs: -40.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 22.2222 + azimuth_angle: -0.572879 + range_rate: -5.30792 + rcs: -40.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 29.8168 + azimuth_angle: -1.00408 + range_rate: -6.56891 + rcs: -40.0000 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 37.8999 + azimuth_angle: 0.314159 + range_rate: -0.278592 + rcs: -23.8095 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 38.1929 + azimuth_angle: 0.461999 + range_rate: 0.733138 + rcs: -20.4762 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 43.4921 + azimuth_angle: 0.794638 + range_rate: 2.74194 + rcs: -9.52381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 33.1000 + - range: 45.8852 + azimuth_angle: -0.252559 + range_rate: -3.92962 + rcs: -25.7143 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 16.1000 + - range: 45.9585 + azimuth_angle: -0.671438 + range_rate: -5.77713 + rcs: -5.71429 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 36.5000 + - range: 45.8608 + azimuth_angle: 0.967118 + range_rate: 3.81232 + rcs: -20.9524 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 49.9389 + azimuth_angle: -0.517439 + range_rate: -5.26393 + rcs: -2.85714 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 50.0366 + azimuth_angle: -0.283359 + range_rate: -4.10557 + rcs: -0.952381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 31.4000 + - range: 53.1136 + azimuth_angle: -0.486639 + range_rate: -5.01466 + rcs: -14.2857 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 19.5000 + - range: 53.8950 + azimuth_angle: 0.905518 + range_rate: 3.29912 + rcs: -22.3810 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 55.8486 + azimuth_angle: -0.480479 + range_rate: -4.83871 + rcs: -12.8571 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 55.8730 + azimuth_angle: -0.800798 + range_rate: -6.62757 + rcs: -5.23810 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 26.3000 + - range: 59.7314 + azimuth_angle: -0.480479 + range_rate: -5.08798 + rcs: -1.42857 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 60.5128 + azimuth_angle: -0.794638 + range_rate: -6.59824 + rcs: -0.952381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 60.7082 + azimuth_angle: 1.21968 + range_rate: 5.10264 + rcs: 0.952381 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 24.6000 + - range: 62.7106 + azimuth_angle: -0.646798 + range_rate: -5.65982 + rcs: -9.52381 + pdh00: 0 + pdh01: 0 + pdh02: 100 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 + - range: 66.5934 + azimuth_angle: 1.24432 + range_rate: 5.35191 + rcs: -2.38095 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 31.4000 + - range: 69.1331 + azimuth_angle: 0.375759 + range_rate: 0.0586514 + rcs: -12.3810 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 29.7000 + - range: 68.1319 + azimuth_angle: -0.843918 + range_rate: -6.31965 + rcs: -16.1905 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 21.2000 + - range: 71.8681 + azimuth_angle: -0.911678 + range_rate: -6.40762 + rcs: -17.6190 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 17.8000 + - range: 72.7473 + azimuth_angle: 1.13344 + range_rate: -1.55425 + rcs: -12.8571 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 73.5043 + azimuth_angle: 0.0369600 + range_rate: -2.18475 + rcs: -26.1905 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 74.9939 + azimuth_angle: -0.357279 + range_rate: -1.43695 + rcs: -26.1905 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 100 + pdh05: 0 + snr: 17.8000 + - range: 76.3614 + azimuth_angle: -0.0923997 + range_rate: -2.87390 + rcs: -26.6667 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 79.9512 + azimuth_angle: -0.135520 + range_rate: -3.22581 + rcs: -14.7619 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 19.5000 + - range: 82.9792 + azimuth_angle: 0.0923999 + range_rate: -1.86217 + rcs: -5.23810 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 22.9000 + - range: 84.5177 + azimuth_angle: -0.190959 + range_rate: -3.60704 + rcs: 5.71429 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 29.7000 + - range: 98.4371 + azimuth_angle: -0.979438 + range_rate: -6.70088 + rcs: -3.33333 + pdh00: 0 + pdh01: 0 + pdh02: 0 + pdh03: 0 + pdh04: 0 + pdh05: 0 + snr: 17.8000 diff --git a/nebula_tests/data/hesai/40p/1673400149711850138.pcd b/nebula_tests/data/hesai/40p/1673400149711850138.pcd index d49246b11..2ef6c1d27 100644 Binary files a/nebula_tests/data/hesai/40p/1673400149711850138.pcd and b/nebula_tests/data/hesai/40p/1673400149711850138.pcd differ diff --git a/nebula_tests/data/hesai/64/1673403880898440686.pcd b/nebula_tests/data/hesai/64/1673403880898440686.pcd index 7e51c804e..7bd4732fc 100644 Binary files a/nebula_tests/data/hesai/64/1673403880898440686.pcd and b/nebula_tests/data/hesai/64/1673403880898440686.pcd differ diff --git a/nebula_tests/data/hesai/64/all_points/all_points_0.db3 b/nebula_tests/data/hesai/64/all_points/all_points_0.db3 new file mode 100644 index 000000000..d45938c45 Binary files /dev/null and b/nebula_tests/data/hesai/64/all_points/all_points_0.db3 differ diff --git a/nebula_tests/data/hesai/64/all_points/metadata.yaml b/nebula_tests/data/hesai/64/all_points/metadata.yaml new file mode 100644 index 000000000..4a83a0964 --- /dev/null +++ b/nebula_tests/data/hesai/64/all_points/metadata.yaml @@ -0,0 +1,26 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 299063850 + starting_time: + nanoseconds_since_epoch: 1673403880599376836 + message_count: 4 + topics_with_message_count: + - topic_metadata: + name: /pandar_packets + type: pandar_msgs/msg/PandarScan + serialization_format: cdr + offered_qos_profiles: "" + message_count: 4 + compression_format: "" + compression_mode: "" + relative_file_paths: + - all_points_0.db3 + files: + - path: all_points_0.db3 + starting_time: + nanoseconds_since_epoch: 1673403880599376836 + duration: + nanoseconds: 299063850 + message_count: 4 diff --git a/nebula_tests/data/hesai/at128/1679653308704927652.pcd b/nebula_tests/data/hesai/at128/1679653308704927652.pcd index 1a11a3ca8..dd215e9c6 100644 Binary files a/nebula_tests/data/hesai/at128/1679653308704927652.pcd and b/nebula_tests/data/hesai/at128/1679653308704927652.pcd differ diff --git a/nebula_tests/data/hesai/at128/all_points/all_points_0.db3 b/nebula_tests/data/hesai/at128/all_points/all_points_0.db3 new file mode 100644 index 000000000..1e0098d9a Binary files /dev/null and b/nebula_tests/data/hesai/at128/all_points/all_points_0.db3 differ diff --git a/nebula_tests/data/hesai/at128/all_points/metadata.yaml b/nebula_tests/data/hesai/at128/all_points/metadata.yaml new file mode 100644 index 000000000..c2494def5 --- /dev/null +++ b/nebula_tests/data/hesai/at128/all_points/metadata.yaml @@ -0,0 +1,26 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 298889276 + starting_time: + nanoseconds_since_epoch: 1679653308406038376 + message_count: 4 + topics_with_message_count: + - topic_metadata: + name: /pandar_packets + type: pandar_msgs/msg/PandarScan + serialization_format: cdr + offered_qos_profiles: "" + message_count: 4 + compression_format: "" + compression_mode: "" + relative_file_paths: + - all_points_0.db3 + files: + - path: all_points_0.db3 + starting_time: + nanoseconds_since_epoch: 1679653308406038376 + duration: + nanoseconds: 298889276 + message_count: 4 diff --git a/nebula_tests/data/hesai/qt64/1673401196087570870.pcd b/nebula_tests/data/hesai/qt64/1673401196087570870.pcd index 5edff98fb..feef7aef2 100644 Binary files a/nebula_tests/data/hesai/qt64/1673401196087570870.pcd and b/nebula_tests/data/hesai/qt64/1673401196087570870.pcd differ diff --git a/nebula_tests/data/hesai/xt32/1673400678101968885.pcd b/nebula_tests/data/hesai/xt32/1673400678101968885.pcd index d6dca526b..896b4665b 100644 Binary files a/nebula_tests/data/hesai/xt32/1673400678101968885.pcd and b/nebula_tests/data/hesai/xt32/1673400678101968885.pcd differ diff --git a/nebula_tests/data/hesai/xt32m/1660893203292679064.pcd b/nebula_tests/data/hesai/xt32m/1660893203292679064.pcd index 65f85d599..620f19cb8 100644 Binary files a/nebula_tests/data/hesai/xt32m/1660893203292679064.pcd and b/nebula_tests/data/hesai/xt32m/1660893203292679064.pcd differ diff --git a/nebula_tests/data/velodyne/vlp32/1713492677464078412/metadata.yaml b/nebula_tests/data/velodyne/vlp32/1713492677464078412/metadata.yaml index 536456380..54e2fec22 100644 --- a/nebula_tests/data/velodyne/vlp32/1713492677464078412/metadata.yaml +++ b/nebula_tests/data/velodyne/vlp32/1713492677464078412/metadata.yaml @@ -23,4 +23,4 @@ rosbag2_bagfile_information: nanoseconds_since_epoch: 1713492677464078412 duration: nanoseconds: 376987098 - message_count: 5 \ No newline at end of file + message_count: 5 diff --git a/nebula_tests/data/velodyne/vls128/1614315746471294674/metadata.yaml b/nebula_tests/data/velodyne/vls128/1614315746471294674/metadata.yaml index 81489216f..1ee009d7e 100644 --- a/nebula_tests/data/velodyne/vls128/1614315746471294674/metadata.yaml +++ b/nebula_tests/data/velodyne/vls128/1614315746471294674/metadata.yaml @@ -23,4 +23,4 @@ rosbag2_bagfile_information: nanoseconds_since_epoch: 1585897255313147068 duration: nanoseconds: 263164997 - message_count: 4 \ No newline at end of file + message_count: 4 diff --git a/nebula_tests/hesai/CMakeLists.txt b/nebula_tests/hesai/CMakeLists.txt index 8a06fd07f..35c5d919c 100644 --- a/nebula_tests/hesai/CMakeLists.txt +++ b/nebula_tests/hesai/CMakeLists.txt @@ -1,22 +1,38 @@ -ament_auto_add_library(hesai_ros_decoder_test SHARED - hesai_ros_decoder_test.cpp - ) -target_link_libraries(hesai_ros_decoder_test ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) -ament_add_gtest(hesai_ros_decoder_test_main - hesai_ros_decoder_test_main.cpp - ) +add_library(hesai_ros_decoder_test SHARED + hesai_ros_decoder_test.cpp +) + +target_include_directories(hesai_ros_decoder_test PUBLIC + ${NEBULA_TEST_INCLUDE_DIRS} +) +target_link_libraries(hesai_ros_decoder_test + ${HESAI_TEST_LIBRARIES}) -ament_target_dependencies(hesai_ros_decoder_test_main - ${NEBULA_TEST_DEPENDENCIES} - ) +ament_add_gtest(hesai_ros_decoder_test_main + hesai_ros_decoder_test_main.cpp +) target_include_directories(hesai_ros_decoder_test_main PUBLIC - ${PROJECT_SOURCE_DIR}/src/hesai - include - ) + ${PROJECT_SOURCE_DIR}/src/hesai + include + ${NEBULA_TEST_INCLUDE_DIRS} +) target_link_libraries(hesai_ros_decoder_test_main - ${PCL_LIBRARIES} - hesai_ros_decoder_test - ) + hesai_ros_decoder_test +) + +ament_add_gtest(hesai_ros_scan_cutting_test_main + hesai_ros_scan_cutting_test_main.cpp +) + +target_include_directories(hesai_ros_scan_cutting_test_main PUBLIC + ${PROJECT_SOURCE_DIR}/src/hesai + include + ${NEBULA_TEST_INCLUDE_DIRS} +) + +target_link_libraries(hesai_ros_scan_cutting_test_main + hesai_ros_decoder_test +) diff --git a/nebula_tests/hesai/hesai_common.hpp b/nebula_tests/hesai/hesai_common.hpp index 784d8cd55..69554e67e 100644 --- a/nebula_tests/hesai/hesai_common.hpp +++ b/nebula_tests/hesai/hesai_common.hpp @@ -1,21 +1,28 @@ -#pragma once +// 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_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" +#pragma once #include +#include +#include +#include +#include +#include #include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" #include -#include #include @@ -24,8 +31,10 @@ namespace nebula namespace test { -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pc, pcl::PointCloud::Ptr pc_ref) +inline void checkPCDs( + nebula::drivers::NebulaPointCloudPtr pc, pcl::PointCloud::Ptr pc_ref) { + ASSERT_GT(pc->points.size(), 0); EXPECT_EQ(pc->points.size(), pc_ref->points.size()); auto bound = std::min(pc->points.size(), pc_ref->points.size()); for (uint32_t i = 0; i < bound; i++) { @@ -43,7 +52,8 @@ void checkPCDs(nebula::drivers::NebulaPointCloudPtr pc, pcl::PointCloudpoints.size(), pp2->points.size()); for (uint32_t i = 0; i < pp1->points.size(); i++) { @@ -60,7 +70,7 @@ void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::Nebula } } -void printPCD(nebula::drivers::NebulaPointCloudPtr pp) +inline void printPCD(nebula::drivers::NebulaPointCloudPtr pp) { for (auto p : pp->points) { std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " @@ -69,5 +79,5 @@ void printPCD(nebula::drivers::NebulaPointCloudPtr pp) } } -} // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace test +} // namespace nebula diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index a07a4dabd..dc423bf43 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -1,17 +1,22 @@ +// Copyright 2024 TIER IV, Inc. + #include "hesai_ros_decoder_test.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" -#include "rosbag2_storage/storage_options.hpp" +#include "nebula_common/hesai/hesai_common.hpp" +#include "nebula_common/nebula_common.hpp" + +#include +#include +#include +#include +#include +#include + +#include #include +#include #include #include @@ -31,7 +36,7 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration, correction_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -46,12 +51,11 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -59,26 +63,11 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( Status HesaiRosDecoderTest::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) + std::shared_ptr calibration_configuration) { - // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } @@ -96,138 +85,25 @@ Status HesaiRosDecoderTest::GetParameters( calibration_dir /= "hesai"; std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; bag_root_dir /= "hesai"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", params_.sensor_model, descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", params_.return_mode, descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", params_.frame_id, descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", params_.scan_phase, descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "calibration_file", (calibration_dir / params_.calibration_file).string(), descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("min_range", params_.min_range, descriptor); - sensor_configuration.min_range = this->get_parameter("min_range").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("max_range", params_.max_range, descriptor); - sensor_configuration.max_range = this->get_parameter("max_range").as_double(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "correction_file", (calibration_dir / params_.correction_file).string(), descriptor); - params_.correction_file = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / params_.bag_path).string(), descriptor); - params_.bag_path = this->get_parameter("bag_path").as_string(); - RCLCPP_DEBUG_STREAM(get_logger(), "Bag path relative to test root: " << params_.bag_path); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", params_.storage_id, descriptor); - params_.storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", params_.format, descriptor); - params_.format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", params_.target_topic, descriptor); - params_.target_topic = this->get_parameter("target_topic").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.00).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter( - "dual_return_distance_threshold", params_.dual_return_distance_threshold, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } + + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(params_.sensor_model); + sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( + params_.return_mode, sensor_configuration.sensor_model); + sensor_configuration.frame_id = params_.frame_id; + sensor_configuration.sync_angle = params_.sync_angle; + sensor_configuration.cut_angle = params_.cut_angle; + sensor_configuration.cloud_min_angle = params_.fov_start; + sensor_configuration.cloud_max_angle = params_.fov_end; + sensor_configuration.min_range = params_.min_range; + sensor_configuration.max_range = params_.max_range; + + sensor_configuration.calibration_path = (calibration_dir / params_.calibration_file).string(); + params_.bag_path = (bag_root_dir / params_.bag_path).string(); + RCLCPP_DEBUG_STREAM(get_logger(), "Bag path relative to test root: " << params_.bag_path); + params_.storage_id = params_.storage_id; + params_.format = params_.format; + params_.target_topic = params_.target_topic; + sensor_configuration.dual_return_distance_threshold = params_.dual_return_distance_threshold; if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -235,36 +111,33 @@ Status HesaiRosDecoderTest::GetParameters( if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { return Status::INVALID_ECHO_MODE; } - if ( - sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + if (sensor_configuration.frame_id.empty()) { return Status::SENSOR_CONFIG_ERROR; } - if (calibration_configuration.calibration_file.empty()) { + + if (sensor_configuration.calibration_path.empty()) { return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + } + + if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + auto cal_status = correction_configuration.LoadFromFile(sensor_configuration.calibration_path); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); + "Invalid correction file: '" << sensor_configuration.calibration_path << "'"); return cal_status; } - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - if (params_.correction_file.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(params_.correction_file); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << params_.correction_file << "'"); - return cal_status; - } + } else { + auto cal_status = calibration_configuration.LoadFromFile(sensor_configuration.calibration_path); + if (cal_status != Status::OK) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Invalid calibration file: '" << sensor_configuration.calibration_path << "'"); + return cal_status; } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "Sensor configuration: " << sensor_configuration); return Status::OK; } @@ -289,9 +162,8 @@ void HesaiRosDecoderTest::ReadBag( storage_options.uri = params_.bag_path; storage_options.storage_id = params_.storage_id; - converter_options.output_serialization_format = params_.format; //"cdr"; + converter_options.output_serialization_format = params_.format; rclcpp::Serialization serialization; - nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); rosbag2_cpp::Reader bag_reader(std::make_unique()); bag_reader.open(storage_options, converter_options); @@ -310,12 +182,19 @@ void HesaiRosDecoderTest::ReadBag( "Found data in topic " << bag_message->topic_name << ": " << bag_message->time_stamp); auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - auto scan_timestamp = std::get<1>(pointcloud_ts); - pointcloud = std::get<0>(pointcloud_ts); - scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); - pointcloud.reset(new nebula::drivers::NebulaPointCloud); + for (auto & pkt : extracted_msg_ptr->packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud = std::get<0>(pointcloud_ts); + auto scan_timestamp = std::get<1>(pointcloud_ts); + + if (!pointcloud) { + continue; + } + + scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); + } } } } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp index a0477a366..54e34d60e 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -1,22 +1,31 @@ -#pragma once +// 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 "hesai_common.hpp" -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" +#pragma once #include +#include +#include +#include +#include #include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" #include #include +#include +#include #ifndef _SRC_CALIBRATION_DIR_PATH #define _SRC_CALIBRATION_DIR_PATH "" @@ -35,11 +44,13 @@ struct HesaiRosDecoderTestParams { std::string sensor_model; std::string return_mode; - std::string calibration_file = ""; + std::string calibration_file; std::string bag_path; - std::string correction_file = ""; std::string frame_id = "hesai"; - double scan_phase = 0.; + uint16_t sync_angle = 0; + double cut_angle = 0.; + double fov_start = 0.; + double fov_end = 360.; double min_range = 0.3; double max_range = 300.; std::string storage_id = "sqlite3"; @@ -48,9 +59,29 @@ struct HesaiRosDecoderTestParams double dual_return_distance_threshold = 0.1; }; +inline std::ostream & operator<<( + std::ostream & os, nebula::ros::HesaiRosDecoderTestParams const & arg) +{ + return os << "sensor_model=" << arg.sensor_model << ", " + << "return_mode=" << arg.return_mode << ", " + << "calibration_file=" << arg.calibration_file << ", " + << "bag_path=" << arg.bag_path << ", " + << "frame_id=" << arg.frame_id << ", " + << "sync_angle=" << arg.sync_angle << ", " + << "cut_angle=" << arg.cut_angle << ", " + << "fov_start=" << arg.fov_start << ", " + << "fov_end=" << arg.fov_end << ", " + << "min_range=" << arg.min_range << ", " + << "max_range=" << arg.max_range << ", " + << "storage_id=" << arg.storage_id << ", " + << "format=" << arg.format << ", " + << "target_topic=" << arg.target_topic << ", " + << "dual_return_distance_threshold=" << arg.dual_return_distance_threshold << ", "; +} + /// @brief Testing decoder of pandar 40p (Keeps HesaiDriverRosWrapper structure as much as /// possible) -class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test +class HesaiRosDecoderTest final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -65,17 +96,7 @@ class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBas /// @return Resulting status Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); /// @brief Get configurations (Magic numbers for Pandar40P is described, each function can be /// integrated if the ros parameter can be passed to Google Test) diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.yaml b/nebula_tests/hesai/hesai_ros_decoder_test.yaml index ea4b66a98..23ff28ea1 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.yaml +++ b/nebula_tests/hesai/hesai_ros_decoder_test.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - sensor_model: "PandarAT128" # See readme for supported models - sensor_ip: "192.168.1.201" # Lidar Sensor IP - host_ip: "255.255.255.255" # Broadcast IP from Sensor - frame_id: "hesai" + sensor_model: PandarAT128 # See readme for supported models + sensor_ip: 192.168.1.201 # Lidar Sensor IP + host_ip: 255.255.255.255 # Broadcast IP from Sensor + frame_id: hesai data_port: 2368 # LiDAR Data Port gnss_port: 10110 # LiDAR GNSS Port - return_mode: "LastStrongest" # See readme for supported return modes + return_mode: LastStrongest # See readme for supported return modes scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] packet_mtu_size: 1500 # Packet MTU size rotation_speed: 200 # Motor RPM, the sensor's internal spin rate. @@ -15,9 +15,9 @@ min_range: 0.3 # Minimum range. max_range: 300. # Maximum range. diag_span: 1000 # milliseconds - calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarAT128.csv" - correction_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarAT128.dat" + calibration_file: ./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarAT128.csv + correction_file: ./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarAT128.dat - storage_id: "sqlite3" - format: "cdr" - target_topic: "/pandar_packets" + storage_id: sqlite3 + format: cdr + target_topic: /pandar_packets diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_launch.py b/nebula_tests/hesai/hesai_ros_decoder_test_launch.py index f4286b6c2..96b01cd3a 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_launch.py +++ b/nebula_tests/hesai/hesai_ros_decoder_test_launch.py @@ -10,16 +10,22 @@ @pytest.mark.rostest def generate_test_description(): test_node = launch_ros.actions.Node( - package='nebula_tests', - executable='hesai_ros_decoder_test_node_standalone', - parameters=['test/hesai_ros_decoder_test.yaml'], + package="nebula_tests", + executable="hesai_ros_decoder_test_node_standalone", + parameters=["test/hesai_ros_decoder_test.yaml"], ) - return launch.LaunchDescription([ - # other fixture actions - test_node, - launch_testing.util.KeepAliveProc(), - launch_testing.actions.ReadyToTest() - ]), locals() + return ( + launch.LaunchDescription( + [ + # other fixture actions + test_node, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + locals(), + ) + @launch_testing.post_shutdown_test() class TestOutcome(unittest.TestCase): diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index 96ff00c3c..1c044ff0a 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -1,17 +1,25 @@ +// Copyright 2024 TIER IV, Inc. + #include "hesai_ros_decoder_test_main.hpp" #include "hesai_common.hpp" #include "hesai_ros_decoder_test.hpp" +#include +#include #include #include -#include -#include +#include +#include +#include -#include +#include +#include #include #include +#include +#include namespace nebula { @@ -19,72 +27,18 @@ namespace test { const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = { - { - "Pandar40P", - "Dual", - "Pandar40P.csv", - "40p/1673400149412331409", - "", - "hesai", - 0, - 0.3f, - 200.f - }, - { - "Pandar64", - "Dual", - "Pandar64.csv", - "64/1673403880599376836", - "", - "hesai", - 0, - 0.3f, - 200.f - }, - { - "PandarAT128", - "LastStrongest", - "PandarAT128.csv", - "at128/1679653308406038376", - "PandarAT128.dat", - "hesai", - 0, - 1.f, - 180.f - }, - { - "PandarQT64", - "Dual", - "PandarQT64.csv", - "qt64/1673401195788312575", - "", - "hesai", - 0, - 0.1f, - 60.f - }, - { - "PandarXT32", - "Dual", - "PandarXT32.csv", - "xt32/1673400677802009732", - "", - "hesai", - 0, - 0.05f, - 120.f - }, - { - "PandarXT32M", - "LastStrongest", - "PandarXT32M.csv", - "xt32m/1660893203042895158", - "", - "hesai", - 0, - 0.5f, - 300.f - }}; + {"Pandar40P", "Dual", "Pandar40P.csv", "40p/1673400149412331409", "hesai", 0, 0.0, 0., 360., 0.3f, + 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/1673403880599376836", "hesai", 0, 0.0, 0., 360., 0.3f, + 200.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/1679653308406038376", "hesai", 0, + 150.0, 30., 150., 1.f, 180.f}, + {"PandarQT64", "Dual", "PandarQT64.csv", "qt64/1673401195788312575", "hesai", 0, 0.0, 0., 360., + 0.1f, 60.f}, + {"PandarXT32", "Dual", "PandarXT32.csv", "xt32/1673400677802009732", "hesai", 0, 0.0, 0., 360., + 0.05f, 120.f}, + {"PandarXT32M", "LastStrongest", "PandarXT32M.csv", "xt32m/1660893203042895158", "hesai", 0, 0.0, + 0., 360., 0.5f, 300.f}}; // Compares geometrical output of decoder against pre-recorded reference pointcloud. TEST_P(DecoderTest, TestPcd) @@ -93,7 +47,7 @@ TEST_P(DecoderTest, TestPcd) rcpputils::fs::path bag_dir(hesai_driver_->params_.bag_path); rcpputils::fs::path pcd_dir = bag_dir.parent_path(); - pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); + auto ref_pointcloud = std::make_shared>(); int check_cnt = 0; auto scan_callback = [&]( @@ -112,7 +66,7 @@ TEST_P(DecoderTest, TestPcd) checkPCDs(pointcloud, ref_pointcloud); check_cnt++; // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); + ref_pointcloud = std::make_shared>(); } }; @@ -124,6 +78,9 @@ TEST_P(DecoderTest, TestPcd) // are not affected by timezones and are always output in UST. TEST_P(DecoderTest, TestTimezone) { + TearDown(); + SetUp(); + // For each pointcloud decoded, this will contain the pointcloud timestamp returned by the driver std::vector decoded_timestamps; @@ -142,6 +99,8 @@ TEST_P(DecoderTest, TestTimezone) auto gmt = timezone; hesai_driver_->ReadBag(scan_callback); + ASSERT_GT(decoded_timestamps.size(), 0U); + // Then, reset driver and timestamps vector for the next decode run TearDown(); SetUp(); @@ -175,16 +134,12 @@ void DecoderTest::SetUp() logger_->set_level(rclcpp::Logger::Level::Info); RCLCPP_DEBUG_STREAM(*logger_, "Testing " << decoder_params.sensor_model); - setvbuf(stdout, NULL, _IONBF, BUFSIZ); std::string node_name = "nebula_hesai_decoder_test"; - - rclcpp::executors::SingleThreadedExecutor exec; rclcpp::NodeOptions options; hesai_driver_ = std::make_shared(options, node_name, decoder_params); - exec.add_node(hesai_driver_->get_node_base_interface()); nebula::Status driver_status = hesai_driver_->GetStatus(); if (driver_status != nebula::Status::OK) { diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp index dca0f05d7..e4c610954 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp @@ -1,10 +1,27 @@ +// 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. + #pragma once #include "hesai_ros_decoder_test.hpp" -#include #include +#include + +#include + namespace nebula { namespace test diff --git a/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.cpp b/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.cpp new file mode 100644 index 000000000..3d26d8103 --- /dev/null +++ b/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.cpp @@ -0,0 +1,180 @@ +// Copyright 2024 TIER IV, Inc. + +#include "hesai_ros_scan_cutting_test_main.hpp" + +#include "hesai_ros_decoder_test.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula::test +{ +const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[] = { + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 0.0, 0., 360., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 180.0, 0., 360., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 90.0, 90., 270., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 180.0, 90., 270., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 270.0, 90., 270., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 0.0, 270., 90., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 90.0, 270., 90., 0.3f, 200.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 31.0, 30., + 150., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 90.0, 30., + 150., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 149.0, 30., + 150., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 150.0, 30., + 150., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 46.0, 45., + 135., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 90.0, 45., + 135., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 134.0, 45., + 135., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 135.0, 45., + 135., 1.f, 180.f}, +}; + +// Checks that there are never any points outside the defined FoV, and that there are points close +// to the FoV boundaries +TEST_P(ScanCuttingTest, FovAlignment) +{ + int check_cnt = 0; + + auto fov_min_rad = drivers::deg2rad(hesai_driver_->params_.fov_start); + auto fov_max_rad = drivers::deg2rad(hesai_driver_->params_.fov_end); + // The threshold near the FoV borders within which each channel has to have at least one point + auto near_threshold_rad = drivers::deg2rad(.3); + + // Skip the first n clouds (because the recorded rosbags do not always contain full scans and the + // tests would thus fail) + int skip_first = 2; + + auto scan_callback = [&](uint64_t, uint64_t, nebula::drivers::NebulaPointCloudPtr pointcloud) { + if (!pointcloud) return; + + if (skip_first > 0) { + skip_first--; + return; + } + + bool none_outside_fov = true; + std::map has_points_near_start; + std::map has_points_near_end; + + for (const drivers::NebulaPoint & p : pointcloud->points) { + none_outside_fov &= drivers::angle_is_between(fov_min_rad, fov_max_rad, p.azimuth); + + if (has_points_near_start.find(p.channel) == has_points_near_start.end()) { + has_points_near_start.emplace(p.channel, false); + } + auto threshold_angle = drivers::normalize_angle(fov_min_rad + near_threshold_rad, 2 * M_PIf); + has_points_near_start[p.channel] |= drivers::angle_is_between( + drivers::normalize_angle(fov_min_rad, 2 * M_PIf), threshold_angle, p.azimuth); + + if (has_points_near_end.find(p.channel) == has_points_near_end.end()) { + has_points_near_end.emplace(p.channel, false); + } + threshold_angle = drivers::normalize_angle(fov_max_rad - near_threshold_rad, 2 * M_PIf); + has_points_near_end[p.channel] |= drivers::angle_is_between( + threshold_angle, drivers::normalize_angle(fov_max_rad, 2 * M_PIf), p.azimuth); + } + + bool is_at128 = drivers::SensorModelFromString(hesai_driver_->params_.sensor_model) == + drivers::SensorModel::HESAI_PANDARAT128; + // There is a bug in AT128's firm- or hardware that skips a few channels at the beginning of the + // defined FoV. for that reason, relax test conditions in the case where that bug happens. + // Bug observed on SW ver. 3.50.15, FW ver. 3.10b830, RPU ver. 3.50.008 + bool is_at128_bug = is_at128 && hesai_driver_->params_.fov_start <= 30 + 3; + + if (is_at128_bug) { + // The bug affects 32 channels, so 96 valid ones remain + bool most_channels_have_points_near_start = + std::count_if( + has_points_near_start.begin(), has_points_near_start.end(), + [](const auto & entry) { return entry.second; }) >= 96; + + EXPECT_TRUE(most_channels_have_points_near_start); + } else { + bool all_channels_have_points_near_start = std::all_of( + has_points_near_start.begin(), has_points_near_start.end(), + [](const auto & entry) { return entry.second; }); + + EXPECT_TRUE(all_channels_have_points_near_start); + } + + bool all_channels_have_points_near_end = std::all_of( + has_points_near_end.begin(), has_points_near_end.end(), + [](const auto & entry) { return entry.second; }); + EXPECT_TRUE(all_channels_have_points_near_end); + EXPECT_TRUE(none_outside_fov); + check_cnt++; + }; + + hesai_driver_->ReadBag(scan_callback); + EXPECT_GT(check_cnt, 0); +} + +void ScanCuttingTest::SetUp() +{ + auto decoder_params = GetParam(); + logger_ = std::make_shared(rclcpp::get_logger("UnitTest")); + logger_->set_level(rclcpp::Logger::Level::Info); + + RCLCPP_DEBUG_STREAM(*logger_, "Testing " << decoder_params.sensor_model); + + std::string node_name = "nebula_hesai_scan_cutting_test"; + rclcpp::NodeOptions options; + + hesai_driver_ = + std::make_shared(options, node_name, decoder_params); + + nebula::Status driver_status = hesai_driver_->GetStatus(); + if (driver_status != nebula::Status::OK) { + throw std::runtime_error("Could not initialize driver"); + } +} + +void ScanCuttingTest::TearDown() +{ + RCLCPP_INFO_STREAM(*logger_, "Tearing down"); + hesai_driver_.reset(); + logger_.reset(); +} + +INSTANTIATE_TEST_SUITE_P( + TestMain, ScanCuttingTest, testing::ValuesIn(TEST_CONFIGS), + [](const testing::TestParamInfo & p) { + return p.param.sensor_model + "__cut" + std::to_string(static_cast(p.param.cut_angle)) + + "__fov_start" + std::to_string(static_cast(p.param.fov_start)) + "__fov_end" + + std::to_string(static_cast(p.param.fov_end)); + }); + +} // namespace nebula::test + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.hpp b/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.hpp new file mode 100644 index 000000000..ebdc6a887 --- /dev/null +++ b/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.hpp @@ -0,0 +1,40 @@ +// 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. + +#pragma once + +#include "hesai_ros_decoder_test.hpp" + +#include + +#include + +#include + +namespace nebula::test +{ +class ScanCuttingTest : public ::testing::TestWithParam +{ +protected: + /// @brief Instantiates the Hesai driver node with the given test parameters + void SetUp() override; + + /// @brief Destroys the Hesai driver node + void TearDown() override; + + std::shared_ptr hesai_driver_; + std::shared_ptr logger_; +}; + +} // namespace nebula::test diff --git a/nebula_tests/package.xml b/nebula_tests/package.xml index 57593c96f..f593b2e01 100644 --- a/nebula_tests/package.xml +++ b/nebula_tests/package.xml @@ -2,7 +2,7 @@ nebula_tests - 0.1.0 + 0.2.0 Nebula Lidar Drivers MAP IV @@ -12,18 +12,9 @@ ament_cmake_auto ros_environment - continental_msgs - diagnostic_msgs diagnostic_updater - libpcl-all-dev nebula_common nebula_decoders - nebula_hw_interfaces - nebula_msgs - nebula_ros - pcl_conversions - rclcpp - rclcpp_components rosbag2_cpp ament_cmake_gtest diff --git a/nebula_tests/velodyne/CMakeLists.txt b/nebula_tests/velodyne/CMakeLists.txt index f166997bd..7991c53df 100644 --- a/nebula_tests/velodyne/CMakeLists.txt +++ b/nebula_tests/velodyne/CMakeLists.txt @@ -1,62 +1,72 @@ # Velodyne VLP16 -ament_auto_add_library(velodyne_ros_decoder_test_vlp16 SHARED - velodyne_ros_decoder_test_vlp16.cpp - ) -target_link_libraries(velodyne_ros_decoder_test_vlp16 ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) + +add_library(velodyne_ros_decoder_test_vlp16 SHARED + velodyne_ros_decoder_test_vlp16.cpp +) +target_include_directories(velodyne_ros_decoder_test_vlp16 PUBLIC + ${NEBULA_TEST_INCLUDE_DIRS} +) +target_link_libraries(velodyne_ros_decoder_test_vlp16 + ${VELODYNE_TEST_LIBRARIES}) ament_add_gtest(velodyne_ros_decoder_test_main_vlp16 - velodyne_ros_decoder_test_main_vlp16.cpp - ) + velodyne_ros_decoder_test_main_vlp16.cpp +) ament_target_dependencies(velodyne_ros_decoder_test_main_vlp16 - ${NEBULA_TEST_DEPENDENCIES} - ) + ${NEBULA_TEST_DEPENDENCIES} +) target_include_directories(velodyne_ros_decoder_test_main_vlp16 PUBLIC - ${PROJECT_SOURCE_DIR}/src/velodyne - include - ) + ${PROJECT_SOURCE_DIR}/src/velodyne + include + ${NEBULA_TEST_INCLUDE_DIRS} +) target_link_libraries(velodyne_ros_decoder_test_main_vlp16 - ${PCL_LIBRARIES} - velodyne_ros_decoder_test_vlp16 - ) + velodyne_ros_decoder_test_vlp16 +) # Velodyne VLS128 -ament_auto_add_library(velodyne_ros_decoder_test_vls128 SHARED - velodyne_ros_decoder_test_vls128.cpp - ) +add_library(velodyne_ros_decoder_test_vls128 SHARED + velodyne_ros_decoder_test_vls128.cpp +) +target_include_directories(velodyne_ros_decoder_test_vls128 PUBLIC + ${NEBULA_TEST_INCLUDE_DIRS} +) ament_add_gtest(velodyne_ros_decoder_test_main_vls128 - velodyne_ros_decoder_test_main_vls128.cpp - ) -target_link_libraries(velodyne_ros_decoder_test_vls128 ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) + velodyne_ros_decoder_test_main_vls128.cpp +) +target_link_libraries(velodyne_ros_decoder_test_vls128 + ${VELODYNE_TEST_LIBRARIES}) -ament_target_dependencies(velodyne_ros_decoder_test_main_vls128 - ${NEBULA_TEST_DEPENDENCIES} - ) target_include_directories(velodyne_ros_decoder_test_main_vls128 PUBLIC - ${PROJECT_SOURCE_DIR}/src/velodyne - include - ) + ${PROJECT_SOURCE_DIR}/src/velodyne + include + ${NEBULA_TEST_INCLUDE_DIRS} +) target_link_libraries(velodyne_ros_decoder_test_main_vls128 - ${PCL_LIBRARIES} - velodyne_ros_decoder_test_vls128 - ) + velodyne_ros_decoder_test_vls128 +) # Velodyne VLP32 -ament_auto_add_library(velodyne_ros_decoder_test_vlp32 SHARED -velodyne_ros_decoder_test_vlp32.cpp +add_library(velodyne_ros_decoder_test_vlp32 SHARED + velodyne_ros_decoder_test_vlp32.cpp +) +target_include_directories(velodyne_ros_decoder_test_vlp32 PUBLIC + ${NEBULA_TEST_INCLUDE_DIRS} +) +target_link_libraries(velodyne_ros_decoder_test_vlp32 + ${VELODYNE_TEST_LIBRARIES} ) -target_link_libraries(velodyne_ros_decoder_test_vlp32 ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) ament_add_gtest(velodyne_ros_decoder_test_main_vlp32 -velodyne_ros_decoder_test_main_vlp32.cpp -) -ament_target_dependencies(velodyne_ros_decoder_test_main_vlp32 -${NEBULA_TEST_DEPENDENCIES} + velodyne_ros_decoder_test_main_vlp32.cpp ) + target_include_directories(velodyne_ros_decoder_test_main_vlp32 PUBLIC -${PROJECT_SOURCE_DIR}/src/velodyne -include + ${PROJECT_SOURCE_DIR}/src/velodyne + include + ${NEBULA_TEST_INCLUDE_DIRS} ) target_link_libraries(velodyne_ros_decoder_test_main_vlp32 -${PCL_LIBRARIES} -velodyne_ros_decoder_test_vlp32 -) \ No newline at end of file + ${PCL_LIBRARIES} + velodyne_ros_decoder_test_vlp32 +) diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp16.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp16.cpp index 77496bcd2..f17abef87 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp16.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp16.cpp @@ -1,3 +1,5 @@ +// Copyright 2024 TIER IV, Inc. + #include "velodyne_ros_decoder_test_vlp16.hpp" #include diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp32.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp32.cpp index eda4969c6..e07f1212b 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp32.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp32.cpp @@ -1,3 +1,5 @@ +// Copyright 2024 TIER IV, Inc. + #include "velodyne_ros_decoder_test_vlp32.hpp" #include diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vls128.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vls128.cpp index 94502b363..0fd0b77e2 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vls128.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vls128.cpp @@ -1,3 +1,5 @@ +// Copyright 2024 TIER IV, Inc. + #include "velodyne_ros_decoder_test_vls128.hpp" #include diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp index fa3604485..818726bf1 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp @@ -1,19 +1,21 @@ +// Copyright 2024 TIER IV, Inc. + #include "velodyne_ros_decoder_test_vlp16.hpp" #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" #include "rosbag2_storage/storage_options.hpp" #include #include +#include #include +#include +#include namespace nebula { @@ -28,36 +30,37 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); + std::make_shared(calibration_configuration); - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + sensor_cfg_ptr_ = + std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } Status VelodyneRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + driver_ptr_ = + std::make_shared(sensor_configuration, calibration_configuration); return driver_ptr_->GetStatus(); } -Status VelodyneRosDecoderTest::GetStatus() { return wrapper_status_; } +Status VelodyneRosDecoderTest::GetStatus() +{ + return wrapper_status_; +} Status VelodyneRosDecoderTest::GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, @@ -248,7 +251,7 @@ Status VelodyneRosDecoderTest::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "Sensor Configuration: " << sensor_configuration); return Status::OK; } @@ -314,10 +317,9 @@ void VelodyneRosDecoderTest::ReadBag() storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; + converter_options.output_serialization_format = format; rclcpp::Serialization serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); { rosbag2_cpp::Reader bag_reader(std::make_unique()); @@ -336,27 +338,29 @@ void VelodyneRosDecoderTest::ReadBag() << bag_message->time_stamp << std::endl; auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - pointcloud = std::get<0>(pointcloud_ts); - - // There are very rare cases where has_scanned_ does not become true, but it is not known - // whether it is because of decoder or deserialize_message. - if (!pointcloud) continue; - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); + for (auto & pkt : extracted_msg.packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), pkt.data.end()), pkt.stamp.sec); + auto pointcloud = std::get<0>(pointcloud_ts); + + if (!pointcloud) { + continue; + } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + auto target_pcd_path = (pcd_dir / fn); + std::cout << target_pcd_path << std::endl; + if (target_pcd_path.exists()) { + std::cout << "exists: " << target_pcd_path << std::endl; + auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); + std::cout << rt << " loaded: " << target_pcd_path << std::endl; + checkPCDs(pointcloud, ref_pointcloud); + check_cnt++; + ref_pointcloud.reset(new pcl::PointCloud); + } + pointcloud.reset(new nebula::drivers::NebulaPointCloud); } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); } } EXPECT_GT(check_cnt, 0); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp index 2b889358e..47885ab2d 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp @@ -1,42 +1,55 @@ +// 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_VelodyneRosDecoderTestVlp16_H #define NEBULA_VelodyneRosDecoderTestVlp16_H -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include +#include +#include +#include +#include #include -#include #include #include #include +#include +#include + namespace nebula { namespace ros { /// @brief Testing decoder of VLP16 (Keeps VelodyneDriverRosWrapper structure as much as possible) -class VelodyneRosDecoderTest final : public rclcpp::Node, - NebulaDriverRosWrapperBase //, testing::Test +class VelodyneRosDecoderTest final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; /// @brief Initializing ros wrapper /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver /// @return Resulting status Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration); /// @brief Get configurations (Magic numbers for VLP16 is described, each function can be /// integrated if the ros parameter can be passed to Google Test) diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp index c7623ca50..871f6dab7 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp @@ -1,19 +1,19 @@ +// Copyright 2024 TIER IV, Inc. + #include "velodyne_ros_decoder_test_vlp32.hpp" #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" #include "rosbag2_storage/storage_options.hpp" #include #include #include +#include namespace nebula { @@ -28,32 +28,30 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); + std::make_shared(calibration_configuration); - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + sensor_cfg_ptr_ = + std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } Status VelodyneRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + driver_ptr_ = + std::make_shared(sensor_configuration, calibration_configuration); return driver_ptr_->GetStatus(); } @@ -252,7 +250,7 @@ Status VelodyneRosDecoderTest::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "Sensor Configuration: " << sensor_configuration); return Status::OK; } @@ -318,10 +316,9 @@ void VelodyneRosDecoderTest::ReadBag() storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; + converter_options.output_serialization_format = format; rclcpp::Serialization serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); { rosbag2_cpp::Reader bag_reader(std::make_unique()); @@ -340,28 +337,30 @@ void VelodyneRosDecoderTest::ReadBag() << bag_message->time_stamp << std::endl; auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - pointcloud = std::get<0>(pointcloud_ts); - - // There are very rare cases where has_scanned_ does not become true, but it is not known - // whether it is because of decoder or deserialize_message. - if (!pointcloud) continue; - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); + for (auto & pkt : extracted_msg.packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), pkt.data.end()), pkt.stamp.sec); + auto pointcloud = std::get<0>(pointcloud_ts); + + if (!pointcloud) { + continue; + } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + auto target_pcd_path = (pcd_dir / fn); + std::cout << target_pcd_path << std::endl; + + if (target_pcd_path.exists()) { + std::cout << "exists: " << target_pcd_path << std::endl; + auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); + std::cout << rt << " loaded: " << target_pcd_path << std::endl; + checkPCDs(pointcloud, ref_pointcloud); + check_cnt++; + ref_pointcloud.reset(new pcl::PointCloud); + } + pointcloud.reset(new nebula::drivers::NebulaPointCloud); } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); } } EXPECT_GT(check_cnt, 0); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp index 2b889358e..47885ab2d 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp @@ -1,42 +1,55 @@ +// 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_VelodyneRosDecoderTestVlp16_H #define NEBULA_VelodyneRosDecoderTestVlp16_H -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include +#include +#include +#include +#include #include -#include #include #include #include +#include +#include + namespace nebula { namespace ros { /// @brief Testing decoder of VLP16 (Keeps VelodyneDriverRosWrapper structure as much as possible) -class VelodyneRosDecoderTest final : public rclcpp::Node, - NebulaDriverRosWrapperBase //, testing::Test +class VelodyneRosDecoderTest final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; /// @brief Initializing ros wrapper /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver /// @return Resulting status Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration); /// @brief Get configurations (Magic numbers for VLP16 is described, each function can be /// integrated if the ros parameter can be passed to Google Test) diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp index 5fe5e1dbc..6075f8d79 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp @@ -1,19 +1,24 @@ +// Copyright 2024 TIER IV, Inc. + #include "velodyne_ros_decoder_test_vls128.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" -#include "rosbag2_storage/storage_options.hpp" +#include +#include +#include +#include +#include +#include +#include +#include #include +#include #include +#include #include +#include +#include namespace nebula { @@ -28,36 +33,37 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); + std::make_shared(calibration_configuration); - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + sensor_cfg_ptr_ = + std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } Status VelodyneRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + driver_ptr_ = + std::make_shared(sensor_configuration, calibration_configuration); return driver_ptr_->GetStatus(); } -Status VelodyneRosDecoderTest::GetStatus() { return wrapper_status_; } +Status VelodyneRosDecoderTest::GetStatus() +{ + return wrapper_status_; +} Status VelodyneRosDecoderTest::GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, @@ -222,8 +228,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter( - "target_topic", "/velodyne_packets", descriptor); + this->declare_parameter("target_topic", "/velodyne_packets", descriptor); target_topic = this->get_parameter("target_topic").as_string(); } @@ -249,7 +254,7 @@ Status VelodyneRosDecoderTest::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "Sensor Configuration: " << sensor_configuration); return Status::OK; } @@ -317,7 +322,6 @@ void VelodyneRosDecoderTest::ReadBag() storage_options.storage_id = storage_id; rclcpp::Serialization serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); { rosbag2_cpp::Reader bag_reader(std::make_unique()); @@ -336,27 +340,29 @@ void VelodyneRosDecoderTest::ReadBag() << bag_message->time_stamp << std::endl; auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - pointcloud = std::get<0>(pointcloud_ts); - - // There are very rare cases where has_scanned_ does not become true, but it is not known - // whether it is because of decoder or deserialize_message. - if (!pointcloud) continue; - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); + for (auto & pkt : extracted_msg.packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), pkt.data.end()), pkt.stamp.sec); + auto pointcloud = std::get<0>(pointcloud_ts); + + if (!pointcloud) { + continue; + } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + auto target_pcd_path = (pcd_dir / fn); + std::cout << target_pcd_path << std::endl; + if (target_pcd_path.exists()) { + std::cout << "exists: " << target_pcd_path << std::endl; + auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); + std::cout << rt << " loaded: " << target_pcd_path << std::endl; + checkPCDs(pointcloud, ref_pointcloud); + check_cnt++; + ref_pointcloud.reset(new pcl::PointCloud); + } + pointcloud.reset(new nebula::drivers::NebulaPointCloud); } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); } } EXPECT_GT(check_cnt, 0); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp index a2432838f..8787e8b0d 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp @@ -1,37 +1,50 @@ +// 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_VelodyneRosDecoderTestVls128_H #define NEBULA_VelodyneRosDecoderTestVls128_H -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include +#include +#include +#include +#include #include -#include #include #include #include +#include +#include + namespace nebula { namespace ros { -class VelodyneRosDecoderTest final : public rclcpp::Node, - NebulaDriverRosWrapperBase //, testing::Test +class VelodyneRosDecoderTest final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration); Status GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, @@ -50,16 +63,7 @@ class VelodyneRosDecoderTest final : public rclcpp::Node, void ReceiveScanMsgCallback(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg); Status GetStatus(); void ReadBag(); - /* - void SetUp() override { - // Setup things that should occur before every test instance should go here - RCLCPP_ERROR_STREAM(this->get_logger(), "DONE WITH SETUP!!"); - } - void TearDown() override { - std::cout << "DONE WITH TEARDOWN" << std::endl; - } -*/ private: std::string bag_path; std::string storage_id; diff --git a/scripts/hesai_config.py b/scripts/hesai_config.py index fc45b7a65..b7f586ee9 100644 --- a/scripts/hesai_config.py +++ b/scripts/hesai_config.py @@ -1,14 +1,16 @@ from __future__ import annotations +from argparse import ArgumentParser +from dataclasses import dataclass +from ipaddress import IPv4Address +from ipaddress import IPv4Network +from ipaddress import ip_interface import socket import struct import sys -from argparse import ArgumentParser -from dataclasses import dataclass -from ipaddress import IPv4Address, IPv4Network, ip_interface from typing import Any -from rich import print +from rich import print # noqa: A004 from rich.syntax import Syntax from rich.table import Table @@ -19,15 +21,15 @@ SET_CONTROL_PORT_COMMAND = 0x21 -def unpack(format: str, buffer: bytes) -> Any: - return struct.unpack(format, buffer)[0] +def unpack(struct_fields: str, buffer: bytes) -> Any: + return struct.unpack(struct_fields, buffer)[0] class PtcCommandBase: def parse(self) -> dict[str, str]: raise NotImplementedError() - def print(self, title: str) -> None: + def print(self, title: str) -> None: # noqa: A003 table = Table("Parameter", "Value", title=title, highlight=True, min_width=45) for key, value in self.parse().items(): table.add_row(key, str(value)) @@ -93,13 +95,9 @@ def parse(self) -> dict[str, str]: "stop_angle": unpack("!H", self.stop_angle), "clock_source": "PTP" if unpack("!B", self.clock_source) else "Unknown", "udp_seq": "ON" if unpack("!B", self.udp_seq) else "OFF", - "trigger_method": "Time-based" - if unpack("!B", self.trigger_method) - else "Angle-based", + "trigger_method": "Time-based" if unpack("!B", self.trigger_method) else "Angle-based", "return_mode": RETURN_MODE_MAP[unpack("!B", self.return_mode)], - "standby_mode": "Standby" - if unpack("!B", self.standby_mode) - else "In operation", + "standby_mode": "Standby" if unpack("!B", self.standby_mode) else "In operation", "motor_status": unpack("!B", self.motor_status), "vlan_flag": unpack("!B", self.vlan_flag), "vlan_id": unpack("!H", self.vlan_id), @@ -168,21 +166,17 @@ class PtcCommandGetPtpConfig(PtcCommandBase): def parse(self) -> dict[str, str]: return { "status": "Enabled" if unpack("!B", self.status) else "Disabled", - "profile": "802.1AS (AutoSAR)" - if unpack("!B", self.profile) == 3 - else "Unknown", + "profile": "802.1AS (AutoSAR)" if unpack("!B", self.profile) == 3 else "Unknown", "domain": unpack("!B", self.domain), "network": "L2" if unpack("!B", self.network) == 1 else "Unknown", "tsn_switch": "TSN" if unpack("!B", self.tsn_switch) else "Non-TSN", } -def compose_packet( - command: str, payload_length: int = 0, payload: bytes = b"" -) -> bytes: - format = ">B B B B I {}s".format(payload_length) +def compose_packet(command: str, payload_length: int = 0, payload: bytes = b"") -> bytes: + struct_fields = ">B B B B I {}s".format(payload_length) - return struct.pack(format, 0x47, 0x74, command, 0x00, payload_length, payload) + return struct.pack(struct_fields, 0x47, 0x74, command, 0x00, payload_length, payload) def get_config_info(sock: socket.socket): @@ -192,9 +186,7 @@ def get_config_info(sock: socket.socket): payload_length = sum(response[4:]) payload = sock.recv(payload_length) - fields = struct.unpack( - "4s 4s 4s 4s 2s 2s 2s s 2s 2s 2s s s s s s s s 2s s s s", payload[:41] - ) + fields = struct.unpack("4s 4s 4s 4s 2s 2s 2s s 2s 2s 2s s s s s s s s 2s s s s", payload[:41]) return PtcCommandGetConfigInfo(*fields) @@ -339,8 +331,7 @@ class NameSpace: if args.data_port is not None: if args.data_port < 0 or 65535 < args.data_port: raise ValueError( - f"Invalid data port {args.data_port}. " - "It must be in the range of 0 to 65535." + f"Invalid data port {args.data_port}. " "It must be in the range of 0 to 65535." ) sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) @@ -369,14 +360,10 @@ class NameSpace: current_mask = parsed_config_info["mask"] destination_ip = ( - current_destination_ip - if args.destination_ip is None - else args.destination_ip + current_destination_ip if args.destination_ip is None else args.destination_ip ) data_port = current_data_port if args.data_port is None else args.data_port - new_sensor_ip = ( - args.sensor_ip if args.new_sensor_ip is None else args.new_sensor_ip - ) + new_sensor_ip = args.sensor_ip if args.new_sensor_ip is None else args.new_sensor_ip mask = current_mask if args.mask is None else args.mask # Set Destination IP (0x20) @@ -397,9 +384,7 @@ class NameSpace: payload += struct.pack("!H", data_port) payload += struct.pack("!H", 2369) - sock.sendall( - compose_packet(SET_DESTINATION_IP_COMMAND, len(payload), payload) - ) + sock.sendall(compose_packet(SET_DESTINATION_IP_COMMAND, len(payload), payload)) print("Done") @@ -418,9 +403,7 @@ class NameSpace: payload += struct.pack("!B", 0) payload += struct.pack("!H", 0) - sock.sendall( - compose_packet(SET_CONTROL_PORT_COMMAND, len(payload), payload) - ) + sock.sendall(compose_packet(SET_CONTROL_PORT_COMMAND, len(payload), payload)) print("Done") @@ -428,9 +411,7 @@ class NameSpace: print( f"Make sure the new sensor IP is successfully set to {new_sensor_ip}/{mask} by the following command" ) - print( - Syntax(f"python3 {sys.argv[0]} --sensor-ip {new_sensor_ip}", "console") - ) + print(Syntax(f"python3 {sys.argv[0]} --sensor-ip {new_sensor_ip}", "console")) if ( args.destination_ip is not None @@ -445,9 +426,7 @@ class NameSpace: if new_sensor_ip != args.sensor_ip: table.add_row("sensor_ip", str(args.sensor_ip), str(new_sensor_ip)) if destination_ip != current_destination_ip: - table.add_row( - "destination_ip", str(current_destination_ip), str(destination_ip) - ) + table.add_row("destination_ip", str(current_destination_ip), str(destination_ip)) if data_port != current_data_port: table.add_row("data_port", str(current_data_port), str(data_port)) if mask != current_mask: diff --git a/scripts/plot_instrumentation.py b/scripts/plot_instrumentation.py new file mode 100755 index 000000000..7540aa35f --- /dev/null +++ b/scripts/plot_instrumentation.py @@ -0,0 +1,79 @@ +#!/usr/bin/python3 + +import argparse +from collections import defaultdict +import json +import os +import re + +from matplotlib import pyplot as plt +import pandas as pd + + +def condition_data(log_file: str): + with open(log_file, "r") as f: + lines = f.readlines() + lines = [re.search(r'(\{"type":.*?"tag":.*?\})', line) for line in lines] + lines = [re.sub(r'([0-9])"', r"\1", line[1]) for line in lines if line] + lines = [json.loads(line) for line in lines if line] + + cols = defaultdict(list) + + for record in lines: + for key in record.keys(): + if key in ["tag", "type"]: + continue + + colname = f"{record['type']}.{key}.{record['tag']}" + cols[colname] += [num for num in record[key] if num is not None] + + def quantile_filter(series, quantile): + q = series.quantile(quantile) + return series[series <= q] + + cols = {k: pd.Series(v, name=k) / 1e3 for k, v in cols.items()} + cols = {k: quantile_filter(v, 0.999) for k, v in cols.items()} + + for v in cols.values(): + v: pd.Series + v.attrs["file"] = os.path.basename(os.path.splitext(log_file)[0]) + + return cols + + +def plot(conditioned_logs): + + conditioned_logs = {k: [dic[k] for dic in conditioned_logs] for k in conditioned_logs[0]} + + fig, axs = plt.subplots(1, len(conditioned_logs), figsize=(15, 8), dpi=120) + + handles, labels = [], [] + + for i, k in enumerate(sorted(conditioned_logs.keys())): + k: str + v = conditioned_logs[k] + ax: plt.Axes = axs[i] + art = ax.boxplot(v) + handles.append(art) + labels.append(k) + ax.set_ylabel("dt [µs]") + ax.set_title(k.replace(".", "\n")) + ax.set_xticks([1 + i for i in range(len(v))], [ser.attrs["file"] for ser in v]) + ax.tick_params(axis="x", labelrotation=90) + + fig.tight_layout() + plt.savefig("instrumentation.png") + plt.show() + + +def main(args): + conditioned_logs = [condition_data(f) for f in args.log_files] + plot(conditioned_logs) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("log_files", nargs="+") + args = parser.parse_args() + + main(args) diff --git a/scripts/plot_times.py b/scripts/plot_times.py index 521dfd347..07dbee302 100644 --- a/scripts/plot_times.py +++ b/scripts/plot_times.py @@ -1,11 +1,13 @@ -#%% +# %% +import argparse +import glob import os -import pandas as pd import re -import glob -import argparse + from matplotlib import pyplot as plt +import pandas as pd + def parse_logs(run_name): dfs = [] @@ -15,22 +17,20 @@ def parse_logs(run_name): with open(path) as f: lines = f.readlines() lines = filter(lambda line: "PROFILING" in line, lines) - lines = [re.sub(r'.*PROFILING (\{.*?\}).*', r'\1', line) for line in lines] + lines = [re.sub(r".*PROFILING (\{.*?\}).*", r"\1", line) for line in lines] records = [eval(line) for line in lines] dfs.append(pd.DataFrame(records)) df = pd.concat(dfs) - + for col in [c for c in df.columns if c.startswith("d_")]: df[col] /= 1e6 # ns to ms - df['d_total'] = sum([df[c] for c in df.columns if c.startswith("d_")]) # type: ignore return df + def plot_timing_comparison(run_names): - scenario_dfs = { - run_name: parse_logs(run_name) for run_name in run_names - } + scenario_dfs = {run_name: parse_logs(run_name) for run_name in run_names} n_cols = sum(1 for c in next(iter(scenario_dfs.values())).columns if c.startswith("d_")) @@ -41,16 +41,20 @@ def plot_timing_comparison(run_names): boxes = axs[1:] for i, (label, df) in enumerate(scenario_dfs.items()): - durations = df['d_total'] + # durations = df['d_total'] - ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') + # ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') for col in filter(lambda col: col.startswith("n_"), df.columns): - ax_n.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.', color='black') - + ax_n.plot( + df.index, df[col], label=f"{label}::{col}", linestyle="", marker=".", color="black" + ) + for col in filter(lambda col: col.startswith("d_"), df.columns): + ax_d.plot(df.index, df[col], label=f"{label}::{col}", linestyle="", marker=".") + d_columns = [col for col in df.columns if col.startswith("d_")] n_cols = len(d_columns) for j, col in enumerate(d_columns): - if (i == 0): + if i == 0: boxes[j].set_ylabel(f"{col} (ms)") boxes[j].boxplot(df[col], positions=[i], labels=[label]) @@ -58,13 +62,11 @@ def plot_timing_comparison(run_names): ax_d.set_ylabel("time (ms)") ax_d.set_xlabel("iteration") - df_means = pd.DataFrame({ - label: df.describe().loc["mean"] for label, df in scenario_dfs.items() - }) + df_means = pd.DataFrame( + {label: df.describe().loc["mean"] for label, df in scenario_dfs.items()} + ) - df_stds = pd.DataFrame({ - label: df.describe().loc["std"] for label, df in scenario_dfs.items() - }) + df_stds = pd.DataFrame({label: df.describe().loc["std"] for label, df in scenario_dfs.items()}) df_means = df_means.rename(index=lambda label: f"{label} AVG") df_stds = df_stds.rename(index=lambda label: f"{label} STD") @@ -74,15 +76,23 @@ def plot_timing_comparison(run_names): baseline_name = run_names[0] df_base = df_stat[baseline_name] for label in df_base.index: - df_stat.loc[f"{label} % rel to {baseline_name}", :] = df_stat.loc[label, :] / df_base.loc[label] * 100 + df_stat.loc[f"{label} % rel to {baseline_name}", :] = ( + df_stat.loc[label, :] / df_base.loc[label] * 100 + ) print(df_stat.to_markdown()) plt.show() + + # %% if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument("run_names", nargs='+', help="The names of the runs to plot, as entered in test_runner.bash (e.g. 'baseline', 'optimized')\nAll files of the form -[0-9].log will be plotted.") + parser.add_argument( + "run_names", + nargs="+", + help="The names of the runs to plot, as entered in test_runner.bash (e.g. 'baseline', 'optimized')\nAll files of the form -[0-9].log will be plotted.", + ) args = parser.parse_args() - plot_timing_comparison(args.run_names) \ No newline at end of file + plot_timing_comparison(args.run_names) diff --git a/scripts/profiling_runner.bash b/scripts/profiling_runner.bash index 029faf647..107c15ff6 100755 --- a/scripts/profiling_runner.bash +++ b/scripts/profiling_runner.bash @@ -2,10 +2,9 @@ # Configuration -print_usage () { +print_usage() { # Print optionally passed error message - if [ -n "$1" ] - then + if [ -n "$1" ]; then echo "$1" fi echo "Usage: $0 [-t|--timeout ] [-m|--sensor-model ] [-n|--n-runs ] [-f|--maxfreq ] [-c|--taskset-cores ] [-b|--rosbag-path ]" @@ -18,15 +17,14 @@ print_usage () { } # Default parameters -runtime=20 # seconds (rosbag runtime is timeout-3s for startup) -n_runs=3 # number of runs per test -maxfreq=2300000 # 2.3 GHz -n_cores=$(nproc --all) # number of cores of the CPU -taskset_cores=0-$(( $n_cores - 1 )) # cores to run Nebula on -nebula_dir=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )/.." &> /dev/null && pwd ) - -if [ $# -lt 1 ] -then +runtime=20 # seconds (rosbag runtime is timeout-3s for startup) +n_runs=3 # number of runs per test +maxfreq=2300000 # 2.3 GHz +n_cores=$(nproc --all) # number of cores of the CPU +taskset_cores=0-$((n_cores - 1)) # cores to run Nebula on +nebula_dir=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." &>/dev/null && pwd) + +if [ $# -lt 1 ]; then print_usage "No run name specified" exit 1 fi @@ -35,62 +33,59 @@ run_name=$1 shift #do the parsing -while [[ $# -gt 0 ]] -do +while [[ $# -gt 0 ]]; do key="$1" case $key in - -t|--runtime) - runtime="$2" - shift - shift - ;; - -m|--sensor-model) - sensor_model="$2" - shift - shift - ;; - -n|--n-runs) - n_runs="$2" - shift - shift - ;; - -f|--maxfreq) - maxfreq="$2" - shift - shift - ;; - -c|--taskset-cores) - taskset_cores="$2" - shift - shift - ;; - -d|--nebula-dir) - nebula_dir="$2" - shift - shift - ;; - -b|--rosbag-path) - rosbag_path="$2" - shift - shift - ;; - *) - print_usage "Unknown option $1" - exit 1 - ;; + -t | --runtime) + runtime="$2" + shift + shift + ;; + -m | --sensor-model) + sensor_model="$2" + shift + shift + ;; + -n | --n-runs) + n_runs="$2" + shift + shift + ;; + -f | --maxfreq) + maxfreq="$2" + shift + shift + ;; + -c | --taskset-cores) + taskset_cores="$2" + shift + shift + ;; + -d | --nebula-dir) + nebula_dir="$2" + shift + shift + ;; + -b | --rosbag-path) + rosbag_path="$2" + shift + shift + ;; + *) + print_usage "Unknown option $1" + exit 1 + ;; esac done # Enf of configuration -if [ -z "$rosbag_path" ] -then +if [ -z "$rosbag_path" ]; then print_usage "No --rosbag-path specified" exit 1 fi -if [ -z "$sensor_model" ] -then +if [ -z "$sensor_model" ]; then print_usage "No --sensor-model specified" exit 1 fi @@ -100,13 +95,12 @@ output_dir="$nebula_dir/profiling_output" mkdir -p "$output_dir" echo "Outputting to '$output_dir/$run_name-[1-$n_runs].log'" -cd "$nebula_dir" +cd "$nebula_dir" || exit -lockfreq () { - for (( i=0; i<$n_cores; i++ )) - do - echo userspace | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_governor > /dev/null - echo $maxfreq | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_setspeed > /dev/null +lockfreq() { + for ((i = 0; i < n_cores; i++)); do + echo userspace | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_governor >/dev/null + echo "$maxfreq" | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_setspeed >/dev/null echo -n "CPU $i's frequency is: " sudo cat /sys/devices/system/cpu/cpu$i/cpufreq/cpuinfo_cur_freq done @@ -114,10 +108,9 @@ lockfreq () { sudo sh -c "echo 0 > /sys/devices/system/cpu/cpufreq/boost" } -unlockfreq () { - for (( i=0; i<$n_cores; i++ )) - do - echo schedutil | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_governor > /dev/null +unlockfreq() { + for ((i = 0; i < n_cores; i++)); do + echo schedutil | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_governor >/dev/null echo -n "CPU $i's frequency is: " sudo cat /sys/devices/system/cpu/cpu$i/cpufreq/cpuinfo_cur_freq done @@ -129,6 +122,7 @@ echo "Setting up for compiling" unlockfreq colcon build --symlink-install --packages-up-to nebula_ros --cmake-args -DCMAKE_BUILD_TYPE=Release || exit 1 +# shellcheck disable=SC1091 source "$nebula_dir/install/setup.bash" ros2 daemon stop @@ -137,15 +131,14 @@ ros2 daemon start echo "Setting up for test run" lockfreq -for (( i=1; i<=$n_runs; i++ )) -do +for ((i = 1; i <= n_runs; i++)); do echo "Running iteration $i of $n_runs" # set the log level to debug for all ros 2 nodes - timeout -s INT $runtime taskset -c "$taskset_cores" ros2 launch nebula_ros nebula_launch.py "sensor_model:=$sensor_model" launch_hw:=false debug_logging:=true > "$output_dir/$run_name-$i.log" 2>&1 & - (sleep 3 && timeout -s KILL $(( $runtime - 3 )) nohup ros2 bag play -l "$rosbag_path") > /dev/null & + timeout -s INT "$runtime" taskset -c "$taskset_cores" ros2 launch nebula_ros nebula_launch.py "sensor_model:=$sensor_model" launch_hw:=false debug_logging:=true >"$output_dir/$run_name-$i.log" 2>&1 & + (sleep 3 && timeout -s KILL $((runtime - 3)) nohup ros2 bag play -l "$rosbag_path") >/dev/null & wait done echo "Unlocking frequency" -unlockfreq \ No newline at end of file +unlockfreq