diff --git a/data/radar/radar.parm.yaml b/data/radar/radar.parm.yaml new file mode 100644 index 0000000..ac99859 --- /dev/null +++ b/data/radar/radar.parm.yaml @@ -0,0 +1,27 @@ +/smart_radar: + ros__parameters: + master_inst_serial_type: "port_based" + master_data_serial_type: "port_based" + adapters: + adapter_0: + hw_type: "eth" + hw_dev_id: 2 + hw_iface_name: "enp3s0" + port: 55555 + sensors: + sensor_0: + link_type: "eth" + pub_type: "target" + model: "umrr9f_v2_4_1" + dev_id: 2 + id: 100 + ip: "192.168.11.11" + port: 55555 + frame_id: "umrr" + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + uifname: "umrr9f_t169_automotive" + uifmajorv: 2 + uifminorv: 4 + uifpatchv: 1 \ No newline at end of file diff --git a/launch.hailbopp_full.py b/launch.hailbopp_full.py index ca961f3..58f031b 100644 --- a/launch.hailbopp_full.py +++ b/launch.hailbopp_full.py @@ -30,6 +30,7 @@ def generate_launch_description(): joystick_microsoft, # #camera, gnss, + radar, # Old lidar (left and right): # lidar_driver_left, # lidar_pointcloud_left, diff --git a/launch_node_definitions.py b/launch_node_definitions.py index cb985fa..27cfdeb 100644 --- a/launch_node_definitions.py +++ b/launch_node_definitions.py @@ -29,6 +29,13 @@ executable='clock_node' ) +radar = Node( + package="umrr_ros2_driver", + executable="smartmicro_radar_node_exe", + name="smart_radar", + parameters=['/vehicle_interface/data/radar/radar.param.yaml'], +) + # I don't think we use this... maybe the old gps? # but we should keep it for broader vehicle interface... gps_node = Node( diff --git a/src/drivers/smartmicro_ros2_radars/.github/workflows/dockerbuild.yml b/src/drivers/smartmicro_ros2_radars/.github/workflows/dockerbuild.yml new file mode 100644 index 0000000..1fb3e19 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/.github/workflows/dockerbuild.yml @@ -0,0 +1,35 @@ +name: Build and test + +on: + pull_request: + push: + branches: + - master + +jobs: + build-and-test: + runs-on: ubuntu-latest + strategy: + matrix: + ros_distro: [foxy, humble, jazzy] + steps: + - name: Checkout + uses: actions/checkout@v2 + + - name: Get the Smart Access binaries + run: echo "yes" | ./smart_extract.sh + + - name: Build docker container + run: docker build --build-arg ROS_DISTRO=${{ matrix.ros_distro }} -t umrr-ros:${{ matrix.ros_distro }} . + + - name: Build the driver + run: docker run --rm -v $(pwd):/code umrr-ros:${{ matrix.ros_distro }} colcon build --packages-skip smart_rviz_plugin + + - name: Run tests + run: ROS_DISTRO=${{ matrix.ros_distro }} docker compose up + + - name: Collect test results + run: docker run --rm -v`pwd`:/code umrr-ros:${{ matrix.ros_distro }} colcon test-result --all --verbose + + - name: Shut down containers + run: docker compose down diff --git a/src/drivers/smartmicro_ros2_radars/.gitignore b/src/drivers/smartmicro_ros2_radars/.gitignore new file mode 100644 index 0000000..69310c6 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/.gitignore @@ -0,0 +1,16 @@ +build +install +log +*.pyc +umrr_ros2_driver/__pycache__ +simulator/simulation/build_dir +simulator/simulation/out +umrr_ros2_driver/include/umrr_ros2_driver/sensor_params.hpp +umrr_ros2_driver/include/umrr_ros2_driver/sensor_commands.hpp +umrr_ros2_driver/smartmicro/* +!umrr_ros2_driver/smartmicro/include +umrr_ros2_driver/smartmicro/include/* +!umrr_ros2_driver/smartmicro/include/DataServicesIface.h +!umrr_ros2_driver/smartmicro/include/InstructionBuffer.h +!umrr_ros2_driver/smartmicro/include/Types.h +!umrr_ros2_driver/smartmicro/Readme.md diff --git a/src/drivers/smartmicro_ros2_radars/CHANGELOG.md b/src/drivers/smartmicro_ros2_radars/CHANGELOG.md new file mode 100644 index 0000000..dc97002 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/CHANGELOG.md @@ -0,0 +1,135 @@ +# Change Log +All notable changes to this project will be documented in this file. This project adheres to [Semantic Versioning](http://semver.org/). + +## v1.0.0 - 2021-11-01 + +### Initial Release +- **smartmicro_ros2_radars**: First official versioned release. +- **ROS 2 Node**: Wrapped around `Smart Access C++ API v3.3.15`. +- **Functionality**: Interfaces with smartmicro automotive radars and publishes incoming data as point cloud. + +## v2.0.0 - 2022-05-05 + +### Major Update +- **Multi-User Interface**: Supports and publishes data from UMRR96 and UMRR11. +- **Testing Enhancements**: Implemented a new test approach simulating sensors and interfacing with the node. +- **New Sensor Firmware Requirement For**: + - UMRR11-T132: V5.1.4 + - UMRR96-T153: V5.2.4 + +# v2.1.0 - 2022-06-02 + +### Minor Update +- **ROS 2 Services**: Added services to communicate with the sensor. +- **Mode Changes**: Supports mode changes for UMRR96 and UMRR11. +- **IP Configuration**: Allows configuring sensor IP addresses via ROS 2 services. +- **Extended Testing**: Includes ROS 2 services check. +- **New Sensor Firmware Requirement For**: + - UMRR11-T132: V5.1.4 + - UMRR96-T153: V5.2.4 + +## v3.0.0 - 2022-09-23 + +### Major Update +- **New Sensor Support**: Added support for smartmicro sensor DRVEGRD 169. +- **Functionality**: Mode changes and configuration for DRVEGRD 169, publishes radar targets as point cloud data. +- **Callback Changes**: Data stream callbacks now require a clientID. + +## v3.1.0 - 2022-10-19 + +### Minor Update +- **New Sensor Support**: Added support for smartmicro sensor DRVEGRD 152. +- **Functionality**: Mode changes and configuration for DRVEGRD 152, publishes radar targets as point cloud data. +- **Callback Changes**: Data stream callbacks now require a clientID. + +## v3.2.0 - 2022-11-11 + +### Minor Update +- **Point Cloud Enhancement**: Introduced signal-to-noise field. +- **Bug Fixes**: Fixed the max number of sensors that could be connected simultaneously. + +## v3.2.1 - 2022-12-16 + +### Patch Update +- **Bug Fixes**: + - Fixed offset causing anomalies in point clouds from DRVEGRD 152 sensor. + - Fixed timestamp calculation bug causing RViz to crash. +- **Updates**: Updated simulator source files. + +## v4.0.0 - 2023-02-06 + +### Major Update +- **New UI**: Added user-interface for DRVEGRD 169. +- **New Modes**: Introduced new modes for DRVEGRD 169. +- **New Service**: Added ROS 2 service to save configurations. +- **Model Update**: 'UMRR9F' radar parameter model split into 'UMRR9F_V1_1_1' and 'UMRR9F_V2_0_0'. + +## v4.1.0 - 2023-08-21 + +### Minor Update +- **New UI**: Added user-interfaces for DRVEGRD 171 and DRVEGRD 152. +- **Parameter List**: Complete list of all parameters and commands for all sensors accessible via ROS 2 services. + +## v5.0.0 - 2023-09-22 + +### Major Update +- **CAN Communication**: Enabled for all provided sensor types and interfaces. +- **Parameter Expansion**: Extended parameters to include settings for connected adapters along with sensor parameters. + +## v6.0.0 - 2023-12-20 + +### Major Update +- **Firmware Download**: Added features for downloading sensor firmware onto the sensors. +- **RViz Plugins**: + - Log target list data, record, and save it. + - Send instructions to the sensors. +- **Python GUI**: Added a GUI to send custom CAN messages. +- **Parameter Templates**: Merged radar parameter templates into one file. +- **Point Cloud Enhancement**: Added polar coordinates. +- **New UI**: Added user-interface for sensor A4 T171. + +## v6.1.0 - 2024-01-26 + +### Minor Update +- **New UIs**: Added user interfaces for DRVEGRD 169 and DRVEGRD 152. +- **Bug Fixes**: + - Fixed issues with the smart record plugin. + - Added azimuth and elevation angles in degrees. + +## v7.0.0 - 2024-06-07 + +### New Features +- **User Interface for DRVEGRD 169**: Introduced a new UI with integrated object tracking capabilities. +- **RViz Plugins**: Added plugins to RViz for decoding targets, objects, and header information. +- **Enhanced Command Configurator**: Improved the command configurator for better usability and functionality. +- **New ROS 2 Parameter - `pub_type`**: Added the `pub_type` parameter to manage the desired publication type. +- **New message definitions**: + - CanObjectHeader.msg includes object status for sensors connected over CAN. + - CanTargetHeader.msg includes target status for sensors connected over CAN. + - PortObjectHeader.msg includes object status for sensors connected over ethernet. + - PortTargetHeader.msg includes target status for sensors connected over ethernet. + +## v7.1.0 - 2025-02-12 + +### New Features +- **User Interface for DRVEGRD 169 MSE v1.1.0**: Introduced a new UI with integrated object tracking capabilities. +- **User Interface for DRVEGRD 171 MSE v1.0.0**: Introduced a new UI with integrated object tracking capabilities. +- **New User Interfaces**: Added user interfaces for DRVEGRD 152 v1.5.0 and DRVEGRD 171 v1.4.0. +- **RViz Plugins**: Extended service configurator with DRVEGRD 171 MSE instruction set. +- **Adopted Testing**: Includes check for new DRVEGRD 171 v1.5.0. +- **Updated External libraries**: Updated external libraries compatible with the new smart access release. + +## v8.0.0 - 2026-03-03 + +### New Features +- **User Interface for DRVEGRD 166 v1.0.0**: Introduced a new UI. +- **User Interface for DRVEGRD 166 v2.0.0**: Introduced a new UI. +- **New User Interfaces**: Added user interfaces for DRVEGRD 169 v3.0.0, DRVEGRD 169 MSE v1.3.0 and DRVEGRD 171 MSE v2.1.0. +- **Multi-Distro Support**: Added full compatibility for ROS 2 Foxy, Humble, and Jazzy via updated Docker configurations and CI/CD pipelines. +- **Batch Instruction Interface**: Introduced a new extensible service structure allowing multiple parameters of varying types (float, uint8/16/32) to be sent in a single request. +- **Asynchronous Firmware Updates**: Refactored the download protocol to use a thread-safe, non-blocking architecture, preventing RViz UI freezes during sensor updates. +- **Expanded Service Set**: Integrated new GetMode and GetStatus services into the smartmicro node logic. +- **Enhanced RViz Plugins**: Updated the Service Configurator and Download plugins with improved layouts, section-based parameters, and read/write capabilities. + +### Breaking Changes +- **Message Structure Refactor**: Replaced single-field param and value fields with array-based params, values, and value_types to support batch processing. Existing integrations must be updated to the new array format. \ No newline at end of file diff --git a/src/drivers/smartmicro_ros2_radars/CONTRIBUTING.md b/src/drivers/smartmicro_ros2_radars/CONTRIBUTING.md new file mode 100644 index 0000000..c7960a4 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/CONTRIBUTING.md @@ -0,0 +1,47 @@ +# Contributions + +Contributions to the `smartmicro_ros2_radars` are welcomed and highly +appreciated! :tada::+1: + + +## Contribution Guidelines + +First and foremost to get an overview of the project, please read the +[Readme](Readme.md). + +### Gitflow Workflow + +This document provides certain guidelines and rules that should be +followed while contributing to `smartmicro_ros2_radars` and its +packages. The project follows the Gitflow guidelines and so it is +expected from the contributor to firstly read the Gitflow guide and +adhere to it. + +If not familiar with Gitflow Workflow, please refer to the following +link: +- [Gitflow Workflow](https://www.atlassian.com/git/tutorials/comparing-workflows/gitflow-workflow) + +### Bugs and Features + +We use GitHub issues to report bugs and make feature requests. For +each bug/feature a new issue should be created first. It is advised to +give each issue a label. Once creating a pull request which stems from +an issue, the pull request must be linked with the respective issue. +The issue should be closed once the pull request has been successfully +merged. Also, it is advisable to name the pull request as such that it +includes the issue number. + +In case of suggestions or enhancements, simply create an issue. +:smile_cat: + +### Commits + +We would like to have clear and understandable commits, please refer to +the following link for certain good commit practices: +- [Commit practices](https://chris.beams.io/posts/git-commit/) + +### Coding Style + +It is advised to maintain a consistency in the coding style. Refer to +the following link for ROS2 coding guidelines: +- [ROS2 code style](https://docs.ros.org/en/foxy/Contributing/Code-Style-Language-Versions.html) diff --git a/src/drivers/smartmicro_ros2_radars/Dockerfile b/src/drivers/smartmicro_ros2_radars/Dockerfile new file mode 100644 index 0000000..21f6c49 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/Dockerfile @@ -0,0 +1,35 @@ +# ROS 2 distro (default = foxy) +ARG ROS_DISTRO=foxy +FROM ros:${ROS_DISTRO} + +# Fix repo keys ONLY for Foxy (snapshots issue). +RUN if [ "$ROS_DISTRO" = "foxy" ]; then \ + rm -f /etc/apt/sources.list.d/ros2-snapshots.list || true && \ + apt-get update && apt-get install -y curl gnupg2 lsb-release && \ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \ + http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" \ + | tee /etc/apt/sources.list.d/ros2.list > /dev/null ; \ + fi + +# Common dependencies +RUN apt-get update && apt-get install -y \ + iputils-ping \ + python3 \ + python3-pip \ + wget \ + git \ + ca-certificates \ + && update-ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +# ROS-specific dependencies +RUN apt-get update && apt-get install -y \ + ros-${ROS_DISTRO}-point-cloud-msg-wrapper \ + ros-${ROS_DISTRO}-rviz2 \ + ros-${ROS_DISTRO}-rviz-common \ + ros-${ROS_DISTRO}-rviz-default-plugins \ + ros-${ROS_DISTRO}-rviz-rendering + +WORKDIR /code diff --git a/src/drivers/smartmicro_ros2_radars/LICENSE b/src/drivers/smartmicro_ros2_radars/LICENSE new file mode 100644 index 0000000..2bb9ad2 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/LICENSE @@ -0,0 +1,176 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS \ No newline at end of file diff --git a/src/drivers/smartmicro_ros2_radars/Readme.md b/src/drivers/smartmicro_ros2_radars/Readme.md new file mode 100644 index 0000000..7dbeab7 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/Readme.md @@ -0,0 +1,328 @@ +# ROS2 smartmicro radar driver + +[![Build and test](https://github.com/smartmicro/smartmicro_ros2_radars/actions/workflows/dockerbuild.yml/badge.svg)](https://github.com/smartmicro/smartmicro_ros2_radars/actions/workflows/dockerbuild.yml) + +## Purpose / Use cases +There is a need for a node that will interface with a smartmicro radar driver and publish the data +acquired by the sensor through the ROS2 pipeline. This package implements such a node. + +## Get the Smart Access release +```bash +./smart_extract.sh +``` + +## How to launch this node +``` +ros2 launch umrr_ros2_driver radar.launch.py +``` + +## How to launch the rviz with recorder plugin +From a separate terminal and after sourcing workspace +``` +rviz2 -d smartmicro_ros2_radars/umrr_ros2_driver/config/rviz/smart_plugin.rviz +``` + +![Recorder](smart_rviz_plugin/config/images/smart_rviz_plugin.png "Rviz Outlook") + +## How to start the custom can message sender +From smart_rviz_plugin folder +``` +python custom_can_sender.py +``` + +![Sender](smart_rviz_plugin/config/images/can_sender.png "Custom CAN Sender") + +## Prerequisites + +### Supported ROS distributions: +- ROS2 foxy + +### UMRR radars and Smart Access API version +A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVEGRD 171, DRVEGRD 152, DRVEGRD 169, DRVEGRD 169 MSE or DRVEGRD 171 MSE radar is +required to run this node. This code is bundled with a version of Smart Access API. Please make +sure the version used to publish the data is compatible with this version: + +- Date of release: `March 03, 2026` +- Smart Access Automotive version: `v3.11.0` + +For each sensor user interface there is a corressponding sensor firmware. The following list all the possible combinations. + +| **User Interface Version** | **Sensor Firmware Version** | +|--------------------------------------------------|-------------------------------------| +| UMRR96 Type 153 AUTOMOTIVE v1.2.1 | UMRR96 Type 153: V5.2.4 | +| UMRR96 Type 153 AUTOMOTIVE v1.2.2 | UMRR96 Type 153: V5.2.4 | +| UMRR11 Type 132 AUTOMOTIVE v1.1.1 | UMRR11 Type 132: V5.1.4 | +| UMRR11 Type 132 AUTOMOTIVE v1.1.2 | UMRR11 Type 132: V5.1.4 | +| UMRR9F Type 169 AUTOMOTIVE v1.1.1 | UMRR9F Type 169: V1.3.0 | +| UMRR9F Type 169 AUTOMOTIVE v2.0.0 | UMRR9F Type 169: V2.0.1 | +| UMRR9F Type 169 AUTOMOTIVE v2.1.1 | UMRR9F Type 169: V2.0.1 | +| UMRR9F Type 169 AUTOMOTIVE v2.2.0 | UMRR9F Type 169: V2.2.0 | +| UMRR9F Type 169 AUTOMOTIVE v2.2.1 | UMRR9F Type 169: V2.2.0 | +| UMRR9F Type 169 AUTOMOTIVE v2.4.1 | UMRR9F Type 169: V2.4.0 | +| UMRR9D Type 152 AUTOMOTIVE v1.0.2 | UMRR9D Type 152: V2.1.0 | +| UMRR9D Type 152 AUTOMOTIVE v1.0.3 | UMRR9D Type 152: V2.5.0 | +| UMRR9D Type 152 AUTOMOTIVE v1.2.2 | UMRR9D Type 152: V2.5.0 | +| UMRR9D Type 152 AUTOMOTIVE v1.4.1 | UMRR9D Type 152: V2.7.0 | +| UMRR9D Type 152 AUTOMOTIVE v1.5.0 | UMRR9D Type 152: V3.3.0 | +| UMRR9D Type 152 AUTOMOTIVE v1.5.0 | UMRR9D Type 152: V3.6.0 | +| UMRRA4 Type 171 AUTOMOTIVE v1.0.0 | UMRRA4 Type 171: V1.0.0 | +| UMRRA4 Type 171 AUTOMOTIVE v1.0.1 | UMRRA4 Type 171: V1.0.0 | +| UMRRA4 Type 171 AUTOMOTIVE v1.2.1 | UMRRA4 Type 171: V1.2.1 | +| UMRRA4 Type 171 AUTOMOTIVE v1.4.0 | UMRRA4 Type 171: V2.0.0 | +| UMRRA4 Type 171 AUTOMOTIVE v1.4.0 | UMRRA4 Type 171: V2.3.0 | +| UMRR11 Type 132 MSE v1.1.1 | UMRR11 Type 132-MSE: V6.1.2 | +| UMRR9F Type 169 MSE v1.0.0 | UMRR9F Type 169-MSE: V1.1.0 | +| UMRR9F Type 169 MSE v1.1.0 | UMRR9F Type 169-MSE: V1.3.0 | +| UMRR9F Type 169 MSE v1.3.0 | UMRR9F Type 169-MSE: V1.5.0 | +| UMRRA4 Type 171 MSE v1.0.0 | UMRR9F Type 171-MSE: V1.0.0 | +| UMRRA4 Type 171 MSE v1.3.0 | UMRR9F Type 171-MSE: V2.1.0 | +| UMRRA1 Type 166 AUTOMOTIVE v1.0.0 | UMRRA1 Type 166: V1.0.0 | +| UMRRA1 Type 166 AUTOMOTIVE v2.0.0 | UMRRA1 Type 166: V1.0.0 | + +### Point cloud message wrapper library +To add targets to the point cloud in a safe and quick fashion a +[`point_cloud_msg_wrapper`](https://gitlab.com/ApexAI/point_cloud_msg_wrapper) library is used within +this project's node. This project can be installed either through `rosdep` or manually by executing: +``` +sudo apt install ros-foxy-point-cloud-msg-wrapper +``` + +To use the GUI provided, it is required to install the following package: +``` +pip install python-can +``` + +## Inputs / Outputs / Configuration + +### The inputs: +The inputs are coming as network packages generated in either of the following two ways: +- Through directly interfacing with the sensor +- Through a provided pcap file +- Through using the sensor simulators + +These inputs are processed through the Smart Access C++ API and trigger a callback. Every time this +callback is triggered a new point cloud message is created and published. + +### The outputs: +The driver publishes `sensor_msgs::msg::PointCloud2` messages with the radar targets on the topic +`umrr/targets` which can be remapped through the parameters. + +### Interface Configuration: +For setting up a sensor with ethernet or can, the interfaces of the should be set properly prior to configuring the node. +The sensor is equipped with three physical layers (RS485, CAN and ethernet) however the driver uses only ethernet and can: +- ethernet: to set-up an ethernet interface the following command could be used `ifconfig my_interface_name 192.168.11.17 netmask 255.255.255.0`. +The command above uses the default source ip address used by the sensors. +- can: if using LAWICEL to set-up a can interface the following commands could be used `slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx`and than `ip link set up my_interface_name`. +This uses the default baudrate of _500000_. When using Peak CAN the interfaces are recognized by linux and is only needed to set the baudrate. + +### Node Configuration: +The node is configured through the parameters. Here is a short recap of the most important parts. +For more details, see the [`radar.sensor.example.yaml`](umrr_ros2_driver/param/radar.sensor.example.yaml) and +[`radar.adapter.example.yaml`](umrr_ros2_driver/param/radar.adapter.example.yaml) files. + +To set up the ***sensors***, configure the following parameters: + +- **`link_type`**: Specifies the type of hardware connection. + +- **`model`**: Defines the model of the sensor being used. + - **CAN Models**: + 'umrra4_can_mse_v1_0_0', 'umrra4_can_mse_v2_1_0', 'umrr9f_can_mse_v1_1_0', 'umrr9f_can_mse_v1_0_0', 'umrr96_can_v1_2_2', + 'umrr11_can_v1_1_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1', 'umrr9f_can_v2_4_1', 'umrr9f_can_v3_0_0', 'umrr9d_can_v1_0_3', + 'umrr9d_can_v1_2_2', 'umrr9d_can_v1_4_1', 'umrr9d_can_v1_5_0', 'umrra4_can_v1_0_1', 'umrra4_can_v1_2_1', 'umrra4_can_v1_4_0' + - **Port Models**: + 'umrra1_v2_0_0', 'umrra1_v1_0_0', 'umrra4_mse_v1_0_0', 'umrra4_mse_v2_1_0', 'umrr9f_mse_v1_3_0', 'umrr9f_mse_v1_1_0', + 'umrr9f_mse_v1_0_0', 'umrr96_v1_2_2', 'umrr11_v1_1_2', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1', 'umrr9f_v2_4_1','umrr9f_v3_0_0', + 'umrr9d_v1_0_3', 'umrr9d_v1_2_2', 'umrr9d_v1_4_1', 'umrr9d_v1_5_0', 'umrra4_v1_0_1', 'umrra4_v1_2_1', 'umrra4_v1_4_0' + +- **`dev_id`**: Adapter ID to which the sensor is connected. + ***Note:*** The adapter and sensor must have the same `dev_id`. + +- **`id`**: The client ID of the sensor/source. + ***Must be a unique, non-zero integer.*** + +- **`ip`**: The ***unique*** IP address of the sensor or source acting as a sensor. + ***Required only for sensors using Ethernet.*** + +- **`port`**: Port used to receive packets. + ***Required only for sensors using Ethernet and default is:*** `55555`. + +- **`frame_id`**: Name of the frame in which messages will be published. + +- **`history_size`**: Size of the history buffer for the message publisher. + +- **`inst_type`**: Instruction serialization type. + ***Relevant for Ethernet sensors.***. + ***Should be set to:*** `port_based`. + +- **`data_type`**: Data serialization type. + ***Relevant for Ethernet sensors.***. + ***Should be set to:*** `port_based`. + +- **`uifname`**: User interface name of the sensor (refer to the [`user_interfaces`](umrr_ros2_driver/smartmicro/user_interfaces/)). + - **`uifmajorv`**: Major version of the sensor user interface. + - **`uifminorv`**: Minor version of the sensor user interface. + - **`uifpatchv`**: Patch version of the sensor user interface. + +To set up the ***adapters***, configure the following parameters: + +- **`master_inst_serial_type`**: Instruction serilization type of the master. + ***Note:*** When using a hybrid of `can` and `port` use `can_based`. + +- **`master_data_serial_type`**: Data serilization type of the master. + ***Note:*** When using a hybrid of `can` and `port` use `can_based`. + +- **`hw_type`**: Specifies the type of the hardware connection. + +- **`hw_dev_id`**: Adapter id of the hardware. + ***Note:*** The adapter and sensor must have the same `dev_id`. + +- **`hw_iface_name`**: Name of the used network interface. + +- **`baudrate`**: Baudrate of the sensor connected with CAN. + ***Required only for sensors using CAN and default is:*** _500000_. + +- **`port`**: Port used to receive packets. + ***Required only for sensors using Ethernet and default is:*** _55555_. + +## Mode of operations of the sensors +The smartmicro radars come equipped with numerous features and modes of operation. Using the ros2 services provided one +may access these modes and send commands to the sensor. A list of available sensor operations is given in the [`user_interfaces`](umrr_ros2_driver/smartmicro/user_interfaces/). + +A ros2 `SetMode` service should be called to implement these mode changes. These are the inputs to a ros2 `SetMode` service call: +- `params`: name/names of the mode instructions (specific to the sensor). +- `values`: the mode of operation (specific to sensor where the modes are same). +- `sensor_id`: the id of the sensor to which the service call should be sent. +- `value_types`: the data types for the params `0: float32, 1: uint32, 2: uint16, 3: uint8`. +- `section_name`: the name of the section in the instruction file. + +For instance, changing the `Index of center frequency (center_frequency_idx)` of a UMRR-A4 sensor to `(1)` mode would require the following call: +`ros2 service call /smart_radar/set_radar_mode umrr_ros2_msgs/srv/SetMode "{section_name: auto_interface_0dim, sensor_id: 100, params: ['center_frequency_idx'], values: ['1'], value_types: [3]}"` + +A ros2 'GetMode' service can be called to get the actual sensor modes. The inputs for this call are: +- `params`: name/names of the mode instructions (specific to the sensor). +- `sensor_id`: the id of the sensor to which the service call should be sent. +- `param_types`: the data types for the params `0: float32, 1: uint32, 2: uint16, 3: uint8`. +- `section_name`: the name of the section in the instruction file. + +For instance, getting the `Index of center frequency (center_frequency_idx)` of a UMRR-A4 sensor to `(1)` mode would require the following call: +`ros2 service call /smart_radar/get_radar_mode umrr_ros2_msgs/srv/GetMode "{section_name: auto_interface_0dim, sensor_id: 100, params: ['center_frequency_idx'], param_types: [3]}"` + +Similarly, a ros2 `SendCommand` service could be used to send commands to the sensors. There are three inputs for sending a command: +- `command`: name of the command (specific to the sensor interface) +- `value`: the value of the command +- `sensor_id`: the id of the sensor to which the service call should be sent. +- `section_name`: the name of the section in the instruction file. + +The call for such a service would be as follows: +`ros2 service call /smart_radar/send_command umrr_ros2_msgs/srv/SendCommand "{section_name: auto_interface_command, command: "comp_eeprom_ctrl_default_param_sec", value: 2, sensor_id: 100}"` + +Apart from commands and modes we can also access the status of the sensor using a ros2 `GetStatus` service. It has three inputs: +- `statuses`: name/names of the status (specific to the sensor). +- `sensor_id`: the id of the sensor to which the service call should be sent. +- `status_types`: the data types for the status `0: uint32, 1: uint16`. +- `section_name`: the name of the section in the instruction file. + +The call would be like follows: +`ros2 service call /smart_radar/get_radar_status umrr_ros2_msgs/srv/GetStatus "{section_name: auto_interface, sensor_id: 100, statuses: ["sw_version_major", "sw_version_minor"], status_types: [1, 1]}"` + +## Configuration of the sensors +In order to use multiple sensors (maximum of up to eight sensors) with the node the sensors should be configured separately. +The IP addresses of the sensors could be assigned using: +- The smartmicro tool `DriveRecorder`. +- Using the `Smart Access C++ API` +- Using `Sensor Services` provided by the node + +Each sensor has to be assigned a unique IP address! + +To use the ros2 `SetIp`service we require two inputs: +- `value_ip`: the value of the ip address in decimal. For instance to set the IP to `192.168.11.64` its corresponding +value in decimal `3232238400` should be used. +- `sensor_id`: the sensor whose ip address is to be changed. + +The call for such a service would be as follows: +`ros2 service call /smart_radar/set_ip_address umrr_ros2_msgs/srv/SetIp "{value_ip: 3232238400, sensor_id: 100}"` + +Note: For successful execution of this call it is important that the sensor is restarted, the ip address in the +[`radar.template.yaml`](umrr_ros2_driver/param/radar.template.yaml) is updated and the driver is build again. + +## Firmware download +All the smartmicro radar sensors have independent firmware which are updated every now and than. To keep the sensor updated a firmware download +needs to be performed. + +A ros2 `FirmwareDownload` service should be called to implement these mode changes. There are two inputs to a ros2 service call: +- `file_path`: the path where the firmware is located +- `sensor_id`: the id of the sensor to which the service call should be sent. + +The call for such a service would be as follows: +`ros2 service call /smart_radar/firmware_download umrr_ros2_msgs/srv/FirmwareDownload "{sensor_id: 100, file_path: '/path/to/firmware/file'}"` + +Note: The download could be performed only for one sensor at a time! +Important: The download requires that the transfer length of the interface is set to minimum 4k! + +## Sensor Service Responses +The sensor services respond with certain value codes. The following is a lookup table for the possible responses: + +**Value** | **Description** +--- | --- +0 | No instruction Response +1 | Instruction Response was processed successfully +2 | General error +6 | Invalid protection +7 | Value out of minimal bounds +8 | Value out of maximal bounds + +## RVIZ plugins and custom CAN sender +Custom plugins for rviz has been provided. This plugin provides logging of the target list, object list and their respective headers. +It provides a command configurator plugin through which commands, status and mode reqeust could be send. It also provides a plugin for initiating a firmware download. +A config file is available which adds this plugin to the rviz. Along with logging the data the plugin also gives the possibility to record +the target/object list data, convert it into a csv format and save it. + +Separately, a python GUI is also provided with which it is possible to send custom CAN messages. + +## Development +The dockerfile can be used to build and test the ros driver. + +### Prerequisites + +- Docker version >= 20.10.14 +- Docker compose version >= 1.29.2 + +## Building and Testing +Accept the agreement and get the smartaccess release +```bash +./smart_extract.sh +``` + +Building docker container +```bash +docker build . -t umrr-ros:latest +``` + +Building the driver with the docker container +```bash +docker run --rm -v`pwd`:/code umrr-ros colcon build --packages-skip smart_rviz_plugin +``` + +Running the unit and integration tests via the docker compose +```bash +docker-compose up +``` + +Getting the test coverage via the docker container +```bash +docker run --rm -v`pwd`:/code umrr-ros colcon test-result --all --verbose +``` + +Stop and remove docker containers and networks +```bash +docker-compose down +``` +## ARMv8 Support +The Smart Access release which will be downloaded using the script also offers platform support for armv8. In order to build the driver on an armv8 machine, the [`CMakeLists.txt`](umrr_ros2_driver/CMakeLists.txt) should be adopted. +Instead of using the default `lib-linux-x86_64_gcc_9` the user should plugin the `lib-linux-armv8-gcc_9` for armv8. + +## Contribution +This project is a joint effort between [smartmicro](https://www.smartmicro.com/) and [Apex.AI](https://www.apex.ai/). The initial version of the code was developed by Igor Bogoslavskyi of Apex.AI (@niosus) and was thereafter adapted and extended by smartmicro. + +## License +Licensed under the [Apache 2.0 License](LICENSE). diff --git a/src/drivers/smartmicro_ros2_radars/docker-compose.yml b/src/drivers/smartmicro_ros2_radars/docker-compose.yml new file mode 100644 index 0000000..e139d33 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/docker-compose.yml @@ -0,0 +1,101 @@ +# Copyright (c) 2021, s.m.s, smart microwave sensors GmbH, Brunswick, Germany +# +# 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. + +version: "3" +services: + build_simulator: + image: umrr-ros:${ROS_DISTRO:-foxy} + volumes: + - .:/code + entrypoint: ["/code/simulator/simulation/build_simulator.sh"] + sensor_0: + depends_on: + build_simulator: + condition: service_completed_successfully + image: umrr-ros:${ROS_DISTRO:-foxy} + volumes: + - .:/code + environment: + - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr11/com_lib_config.json + - LD_LIBRARY_PATH=/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 + entrypoint: ["/code/simulator/simulation/out/bin/simulator"] + command: ["4", "1", "1", "A"] + networks: + device_network: + ipv4_address: 172.22.10.101 + sensor_1: + depends_on: + build_simulator: + condition: service_completed_successfully + image: umrr-ros:${ROS_DISTRO:-foxy} + volumes: + - .:/code + environment: + - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr96/com_lib_config.json + - LD_LIBRARY_PATH=/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 + entrypoint: ["/code/simulator/simulation/out/bin/simulator"] + command: ["5", "1", "2", "A"] + networks: + device_network: + ipv4_address: 172.22.10.102 + sensor_2: + depends_on: + build_simulator: + condition: service_completed_successfully + image: umrr-ros:${ROS_DISTRO:-foxy} + volumes: + - .:/code + environment: + - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr9f/com_lib_config.json + - LD_LIBRARY_PATH=/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 + entrypoint: ["/code/simulator/simulation/out/bin/simulator"] + command: ["9", "2", "1", "C"] + networks: + device_network: + ipv4_address: 172.22.10.103 + sensor_3: + depends_on: + build_simulator: + condition: service_completed_successfully + image: umrr-ros:${ROS_DISTRO:-foxy} + volumes: + - .:/code + environment: + - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr9d/com_lib_config.json + - LD_LIBRARY_PATH=/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 + entrypoint: ["/code/simulator/simulation/out/bin/simulator"] + command: ["11", "1", "5", "D"] + networks: + device_network: + ipv4_address: 172.22.10.104 + ros_node: + image: umrr-ros:${ROS_DISTRO:-foxy} + depends_on: + - "sensor_0" + - "sensor_1" + - "sensor_2" + - "sensor_3" + volumes: + - .:/code + command: ["colcon", "test", "--packages-above", "umrr_ros2_driver"] + networks: + device_network: + ipv4_address: 172.22.10.100 + +networks: + device_network: + ipam: + driver: default + config: + - subnet: 172.22.10.0/24 diff --git a/src/drivers/smartmicro_ros2_radars/simulator/COLCON_IGNORE b/src/drivers/smartmicro_ros2_radars/simulator/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr11/com_lib_config.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr11/com_lib_config.json new file mode 100644 index 0000000..b86f707 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr11/com_lib_config.json @@ -0,0 +1,16 @@ +{ + "name": "Com Lib Config", + "version": "1.0.0", + "client_id": 100, + "role": "slave", + "shared_lib_path": "/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9", + "config_path": "/code/simulator/config_umrr11", + "user_interface_name": "umrr11_t132_automotive", + "user_interface_major_v": 1, + "user_interface_minor_v": 1, + "user_interface_patch_v": 1, + "download_path": "", + "instruction_serialization_type": "port_based", + "data_serialization_type": "port_based", + "alive": false +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr11/hw_inventory.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr11/hw_inventory.json new file mode 100644 index 0000000..94f370a --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr11/hw_inventory.json @@ -0,0 +1,12 @@ +{ + "hwItems": [ + { + "type": "eth", + "dev_id": 1, + "iface_name": "eth0", + "port": 55555 + } + ], + "name": "HW inventory List", + "version": "1.1.0" +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr11/routing_table.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr11/routing_table.json new file mode 100644 index 0000000..1eaffd3 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr11/routing_table.json @@ -0,0 +1,14 @@ +{ + "clients": [ + { + "client_id": 1, + "data_serialization_type": "port_based", + "instruction_serialization_type": "port_based", + "ip": "172.22.10.100", + "link_type": "eth", + "port": 55555 + } + ], + "name": "Client Routing Table", + "version": "1.0.0" +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr96/com_lib_config.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr96/com_lib_config.json new file mode 100644 index 0000000..447e4b0 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr96/com_lib_config.json @@ -0,0 +1,16 @@ +{ + "name": "Com Lib Config", + "version": "1.0.0", + "client_id": 200, + "role": "slave", + "shared_lib_path": "/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9", + "config_path": "/code/simulator/config_umrr96", + "user_interface_name": "umrr96_t153_automotive", + "user_interface_major_v": 1, + "user_interface_minor_v": 2, + "user_interface_patch_v": 1, + "download_path": "", + "instruction_serialization_type": "port_based", + "data_serialization_type": "port_based", + "alive": false +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr96/hw_inventory.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr96/hw_inventory.json new file mode 100644 index 0000000..94f370a --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr96/hw_inventory.json @@ -0,0 +1,12 @@ +{ + "hwItems": [ + { + "type": "eth", + "dev_id": 1, + "iface_name": "eth0", + "port": 55555 + } + ], + "name": "HW inventory List", + "version": "1.1.0" +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr96/routing_table.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr96/routing_table.json new file mode 100644 index 0000000..1eaffd3 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr96/routing_table.json @@ -0,0 +1,14 @@ +{ + "clients": [ + { + "client_id": 1, + "data_serialization_type": "port_based", + "instruction_serialization_type": "port_based", + "ip": "172.22.10.100", + "link_type": "eth", + "port": 55555 + } + ], + "name": "Client Routing Table", + "version": "1.0.0" +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9d/com_lib_config.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9d/com_lib_config.json new file mode 100644 index 0000000..5541739 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9d/com_lib_config.json @@ -0,0 +1,16 @@ +{ + "name": "Com Lib Config", + "version": "1.0.0", + "client_id": 400, + "role": "slave", + "shared_lib_path": "/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9", + "config_path": "/code/simulator/config_umrr9d", + "user_interface_name": "umrr9d_t152_automotive", + "user_interface_major_v": 1, + "user_interface_minor_v": 5, + "user_interface_patch_v": 0, + "download_path": "", + "instruction_serialization_type": "port_based", + "data_serialization_type": "port_based", + "alive": false +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9d/hw_inventory.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9d/hw_inventory.json new file mode 100644 index 0000000..94f370a --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9d/hw_inventory.json @@ -0,0 +1,12 @@ +{ + "hwItems": [ + { + "type": "eth", + "dev_id": 1, + "iface_name": "eth0", + "port": 55555 + } + ], + "name": "HW inventory List", + "version": "1.1.0" +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9d/routing_table.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9d/routing_table.json new file mode 100644 index 0000000..1eaffd3 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9d/routing_table.json @@ -0,0 +1,14 @@ +{ + "clients": [ + { + "client_id": 1, + "data_serialization_type": "port_based", + "instruction_serialization_type": "port_based", + "ip": "172.22.10.100", + "link_type": "eth", + "port": 55555 + } + ], + "name": "Client Routing Table", + "version": "1.0.0" +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9f/com_lib_config.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9f/com_lib_config.json new file mode 100644 index 0000000..5a444bb --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9f/com_lib_config.json @@ -0,0 +1,16 @@ +{ + "name": "Com Lib Config", + "version": "1.0.0", + "client_id": 300, + "role": "slave", + "shared_lib_path": "/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9", + "config_path": "/code/simulator/config_umrr9f", + "user_interface_name": "umrr9f_t169_automotive", + "user_interface_major_v": 2, + "user_interface_minor_v": 1, + "user_interface_patch_v": 1, + "download_path": "", + "instruction_serialization_type": "port_based", + "data_serialization_type": "port_based", + "alive": false +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9f/hw_inventory.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9f/hw_inventory.json new file mode 100644 index 0000000..94f370a --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9f/hw_inventory.json @@ -0,0 +1,12 @@ +{ + "hwItems": [ + { + "type": "eth", + "dev_id": 1, + "iface_name": "eth0", + "port": 55555 + } + ], + "name": "HW inventory List", + "version": "1.1.0" +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9f/routing_table.json b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9f/routing_table.json new file mode 100644 index 0000000..1eaffd3 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/config_umrr9f/routing_table.json @@ -0,0 +1,14 @@ +{ + "clients": [ + { + "client_id": 1, + "data_serialization_type": "port_based", + "instruction_serialization_type": "port_based", + "ip": "172.22.10.100", + "link_type": "eth", + "port": 55555 + } + ], + "name": "Client Routing Table", + "version": "1.0.0" +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/simulation/CMakeLists.txt b/src/drivers/smartmicro_ros2_radars/simulator/simulation/CMakeLists.txt new file mode 100644 index 0000000..a9f111b --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/simulation/CMakeLists.txt @@ -0,0 +1,33 @@ +project(SensorSimulator) + +cmake_minimum_required(VERSION 3.13) + +set(CMAKE_SYSTEM_NAME Linux) +set(CMAKE_C_COMPILER gcc) +set(CMAKE_CXX_COMPILER g++) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -m64 -fPIC") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread -m64 -fPIC") +set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -m64 -fPIC") + +link_directories(${CMAKE_CURRENT_SOURCE_DIR}/${SMART_ACCESS_LIB_FOLDER}) + +add_executable(simulator + src/simulator.cpp) + +target_include_directories(simulator PUBLIC + ../../umrr_ros2_driver/smartmicro/include + umrr_ros2_driver/smartmicro/include/umrr11_t132_automotive_v1_1_2 + umrr_ros2_driver/smartmicro/include/umrr96_t153_automotive_v1_2_2 + umrr_ros2_driver/smartmicro/include/umrr9f_t169_automotive_v2_1_1 + umrr_ros2_driver/smartmicro/include/umrr9d_t152_automotive_v1_5_0) + +target_link_libraries(simulator + com_lib + osal + umrr11_t132_automotivev1.1.2_user_interface + umrr96_t153_automotivev1.2.2_user_interface + umrr9f_t169_automotivev2.1.1_user_interface + umrr9d_t152_automotivev1.5.0_user_interface + dl) + +install(TARGETS simulator DESTINATION bin) diff --git a/src/drivers/smartmicro_ros2_radars/simulator/simulation/build_simulator.sh b/src/drivers/smartmicro_ros2_radars/simulator/simulation/build_simulator.sh new file mode 100755 index 0000000..b6b8729 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/simulation/build_simulator.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +work_dir=$(pwd) +set -e + +cd /code/simulator/simulation + +mkdir -p build_dir +cmake -G "Unix Makefiles" -S. -Bbuild_dir \ +-DCMAKE_INSTALL_PREFIX=out \ +-DSMART_ACCESS_LIB_FOLDER=../../umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 +make -C build_dir +make -C build_dir install diff --git a/src/drivers/smartmicro_ros2_radars/simulator/simulation/src/simulator.cpp b/src/drivers/smartmicro_ros2_radars/simulator/simulation/src/simulator.cpp new file mode 100644 index 0000000..1e7d654 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/simulator/simulation/src/simulator.cpp @@ -0,0 +1,149 @@ +// Copyright (c) 2021, s.m.s, smart microwave sensors GmbH, Brunswick, Germany. +// +// 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 +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace com::common; +using namespace com::master; +using namespace com::types; + +namespace +{ +uint64_t identifier; +uint64_t majorVersion; +uint64_t minorVersion; +std::string port; +std::string portFile; +} // namespace + +std::shared_ptr dataServices = + com::common::DataServicesIface::Get(); + +void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) +{ + InstructionBuffer * receive = reinterpret_cast(buffer.GetBufferPtr()); + int sizeIncomingBuf = buffer.GetSize(); + uint32_t instnumber = receive->GetNumOfInstructions(); + auto instructions = receive->GetInstructions(); + + for (auto instruction : instructions) { + if (instruction->GetSectionId() == 2010 && instruction->GetId() == 2) { + std::cout << "UMRR96 mode frequency_sweep set!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 5) { + std::cout << "UMRR9F mode range_toggle_mode get!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else if (instruction->GetSectionId() == 2010 && instruction->GetId() == 4) { + std::cout << "UMRR11 mode angular_separation set!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else if (instruction->GetSectionId() == 2012 && instruction->GetId() == 3) { + std::cout << "Software major version read!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else if (instruction->GetSectionId() == 2012 && instruction->GetId() == 4) { + std::cout << "Software minorr version read!" << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else { + std::cout << "Unknown instruction received from ROS driver!" << std::endl; + } + } + + dataServices->SetInstructionBuffer(clientId, *receive, nullptr); +} + +void stream_port(std::string portFile) +{ + dataServices->RegisterInstRecvCallback(slave_callback); + + ClientId masterId = 1; + PortId portTargetListId = 66; + std::ifstream ifs(portFile, std::ifstream::binary | std::ios::binary); + std::filebuf * pbuf = ifs.rdbuf(); + int size = pbuf->pubseekoff(0, ifs.end, ifs.in); + pbuf->pubseekpos(0, ifs.in); + char * filebuffer = new (std::nothrow) char[size]; + + if (filebuffer == nullptr) { + std::cout << "error assigning memory!" << std::endl; + } + + pbuf->sgetn(filebuffer, size); + BufferDescriptor bufferdesc((uint8_t *)filebuffer, size); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + if (ERROR_CODE_OK != dataServices->StreamDataPort(masterId, portTargetListId, bufferdesc)) { + return; + } + + std::cout << "sensor is transmitting data! " << std::endl; + ifs.close(); + delete[] filebuffer; +} + +int main(int argc, char * argv[]) +{ + if (argc != 5) { + std::cout << "Specifiy User Interface for the sensor" << std::endl; + return 1; + } + + identifier = strtoll(argv[1], nullptr, 10); + majorVersion = strtoll(argv[2], nullptr, 10); + minorVersion = strtoll(argv[3], nullptr, 10); + port = argv[4]; + + if (!dataServices->Init()) { + throw std::runtime_error("Data services have not been initialized!"); + } + + auto Start = std::chrono::steady_clock::now(); + + while (1) { + if (port == "A") { + std::string portFile = "/code/simulator/targetlist_port_v2_1_0.bin"; + stream_port(portFile); + } else if (port == "B") { + std::string portFile = "/code/simulator/targetlist_port_v3_0_0.bin"; + stream_port(portFile); + } else if (port == "C") { + std::string portFile = "/code/simulator/targetlist_port_v4_0_0.bin"; + stream_port(portFile); + } else if (port == "D") { + std::string portFile = "/code/simulator/targetlist_port_v4_1_0.bin"; + stream_port(portFile); + } else { + std::cout << "Invalid input!" << std::endl; + } + if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(15)) { + break; + } + } + return 0; +} diff --git a/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v2_1_0.bin b/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v2_1_0.bin new file mode 100644 index 0000000..a4fc284 Binary files /dev/null and b/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v2_1_0.bin differ diff --git a/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v3_0_0.bin b/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v3_0_0.bin new file mode 100644 index 0000000..413fe5c Binary files /dev/null and b/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v3_0_0.bin differ diff --git a/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v4_0_0.bin b/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v4_0_0.bin new file mode 100644 index 0000000..c23d2f2 Binary files /dev/null and b/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v4_0_0.bin differ diff --git a/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v4_1_0.bin b/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v4_1_0.bin new file mode 100644 index 0000000..b6fdcaa Binary files /dev/null and b/src/drivers/smartmicro_ros2_radars/simulator/targetlist_port_v4_1_0.bin differ diff --git a/src/drivers/smartmicro_ros2_radars/smart_extract.sh b/src/drivers/smartmicro_ros2_radars/smart_extract.sh new file mode 100755 index 0000000..3f19c6a --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_extract.sh @@ -0,0 +1,53 @@ +#!/bin/bash +set -e + +smart_pack=SmartAccessAutomotive_3_11_0.tgz +URL_smartbinaries=https://www.smartmicro.com/fileadmin/media/Downloads/Automotive_Radar/Software/${smart_pack} + +cat << EOF + +The following clause is explicit for the Smart Access release. + +********************************************************************************* + +This software is licensed under the Apache 2.0 License + +Copyright (c) 2021, s.m.s, smart microwave sensors GmbH, Brunswick, Germany + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software +without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit +persons to whom the Software is furnished to do so, subject to the following conditions: + +The Software is provided "as is", without warranty of any kind, express or implied, including but not limited to the warranties of merchantability, fitness for a particular +purpose and noninfringement. In no event shall the authors or copyright holders be liable for any claim, damages or other liability, whether in an action of contract, tort or +otherwise, arising from, out of or in connection with the software or the use or other dealings in the Software. + +********************************************************************************* +EOF +echo +echo -n "Do you accept the agreement you just read? (yes/no)" +echo "" +read REPLY +echo "" +case "$REPLY" in + yes) + echo "You have accepted the agreement." + ;; + *) + echo "Agreement not accepted." + exit +esac +echo + +function getSmartaccessBinaries { + wget -c $URL_smartbinaries + echo "extracting smart access" + tar xfz $smart_pack --strip-components=1 -C umrr_ros2_driver/smartmicro/ +} + +function cleanup { + rm -rf $smart_pack +} + +getSmartaccessBinaries +cleanup diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/CMakeLists.txt b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000..50e5cf8 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.11) + +project(smart_rviz_plugin) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) +endif() + +add_definitions(-D_BUILD_DIR_PATH="${CMAKE_CURRENT_BINARY_DIR}") +add_definitions(-D_SRC_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}") + +find_package(ament_cmake_auto REQUIRED) +find_package(umrr_ros2_msgs REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_default_plugins REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(Qt5 REQUIRED COMPONENTS Widgets Test) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(OpenCV REQUIRED) +find_package(cv_bridge REQUIRED) + +set(CMAKE_AUTOMOC ON) + +set(smart_rviz_plugin_SRCS + src/smart_recorder.cpp + src/smart_services.cpp + src/smart_download.cpp + src/smart_status.cpp +) + +set(smart_rviz_plugin_HDRS + include/smart_rviz_plugin/smart_recorder.hpp + include/smart_rviz_plugin/smart_services.hpp + include/smart_rviz_plugin/smart_download.hpp + include/smart_rviz_plugin/smart_status.hpp +) + +add_library(smart_rviz_plugin SHARED + ${smart_rviz_plugin_SRCS} + ${smart_rviz_plugin_HDRS} +) + +set(dependencies + umrr_ros2_msgs + pluginlib + Qt5 + rclcpp + rviz_common + rviz_default_plugins + rviz_ogre_vendor + rviz_rendering + std_msgs + OpenCV + cv_bridge +) + +ament_target_dependencies(smart_rviz_plugin + ${dependencies} +) + +target_include_directories(smart_rviz_plugin PUBLIC + $ + $ + ${Qt5Widgets_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + + +target_link_libraries(smart_rviz_plugin + rviz_common::rviz_common +) + + +target_compile_definitions(smart_rviz_plugin PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") + +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +install( + TARGETS smart_rviz_plugin + EXPORT smart_rviz_plugin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include/ +) + +ament_export_targets(smart_rviz_plugin HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + rviz_common + rviz_ogre_vendor + sensor_msgs + umrr_ros2_msgs +) + +ament_package() diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/config/images/can_sender.png b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/config/images/can_sender.png new file mode 100644 index 0000000..c202ae8 Binary files /dev/null and b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/config/images/can_sender.png differ diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/config/images/smart_rviz_plugin.png b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/config/images/smart_rviz_plugin.png new file mode 100644 index 0000000..fb19657 Binary files /dev/null and b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/config/images/smart_rviz_plugin.png differ diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/config/rviz/smart_plugin.rviz b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/config/rviz/smart_plugin.rviz new file mode 100644 index 0000000..d5e90ac --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/config/rviz/smart_plugin.rviz @@ -0,0 +1,212 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 295 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: smart_rviz_plugin/Smart Recorder + Name: Smart Recorder + - Class: smart_rviz_plugin/Smart Status + Name: Smart Status + - Class: smart_rviz_plugin/Smart Command Configurator + Name: Smart Command Configurator + - Class: smart_rviz_plugin/Smart Status + Name: Smart Status + - Class: smart_rviz_plugin/Smart Firmware Download + Name: Smart Firmware Download +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: power + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 88.64950561523438 + Min Color: 0; 0; 0 + Min Intensity: 56.63246154785156 + Name: Port_Tgts_S0 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /smart_radar/port_targets_0 + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 138; 226; 52 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Can_Tgts_S1 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /smart_radar/can_targets_1 + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: umrr + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 38.8543701171875 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 10.84825325012207 + Y: 0.366975337266922 + Z: 0.01084582507610321 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.1353957653045654 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1757 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 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 + Selection: + collapsed: false + Smart Command Configurator: + collapsed: false + Smart Firmware Download: + collapsed: false + Smart Recorder: + collapsed: false + Smart Status: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3151 + X: 374 + Y: 214 diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/custom_can_sender.py b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/custom_can_sender.py new file mode 100644 index 0000000..f179c82 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/custom_can_sender.py @@ -0,0 +1,95 @@ +import can +import tkinter as tk +from tkinter import Entry, Label, Button, Text, Checkbutton + +class CANMessageSender: + def __init__(self, master): + self.master = master + master.title("Custom CAN Message Sender") + + self.label_interface = Label(master, text="Interface:") + self.label_id = Label(master, text="CAN ID:") + self.label_payload = Label(master, text="Payload:") + self.entry_interface = Entry(master) + self.entry_id = Entry(master, validate='key', validatecommand=(master.register(self.validate_hex), '%P')) + self.entry_payload = [Entry(master, width=3, validate='key', validatecommand=(master.register(self.validate_hex), '%P')) for _ in range(8)] + + self.label_interface.grid(row=0, column=0, padx=10, pady=10) + self.label_id.grid(row=0, column=2, padx=2, pady=2) + self.label_payload.grid(row=0, column=4, padx=10, pady=10) + self.entry_interface.grid(row=0, column=1, padx=10, pady=10) + self.entry_id.grid(row=0, column=3, padx=2, pady=2) + for i, entry in enumerate(self.entry_payload): + entry.grid(row=0, column=5+i, padx=2, pady=10) + + self.text_output = Text(master, height=10, width=60) + self.text_output.grid(row=1, columnspan=12, padx=10, pady=10) + + self.send_button = Button(master, text="Send Message", command=self.send_message) + self.send_button.grid(row=2, columnspan=12, pady=10) + + self.loop_button = Checkbutton(master, text="Loop", command=self.toggle_loop) + self.loop_button.grid(row=2, column=1, pady=10) + + self.loop_task_id = None + self.loop_enabled = False + + + def send_message(self): + try: + can_interface = self.entry_interface.get() + can_id_str = self.entry_id.get() + can_id = int(can_id_str, 16) + payload = [int(entry.get(), 16) for entry in self.entry_payload] + message_str = f"{can_interface} {can_id:X} {' '.join(map(lambda x: f'{x:02X}', payload))}" + send_custom_message_from_string(message_str) + + self.text_output.insert(tk.END, message_str + "\n") + self.text_output.yview(tk.END) + except ValueError as e: + print(f"Error: {e}") + + def toggle_loop(self): + self.loop_enabled = not self.loop_enabled + + if self.loop_enabled: + self.start_loop() + elif self.loop_task_id: + self.master.after_cancel(self.loop_task_id) + self.loop_task_id = None + + def start_loop(self): + if self.loop_task_id: + self.master.after_cancel(self.loop_task_id) + + if self.loop_enabled: + self.loop_task_id = self.master.after(1000, self.send_loop_message) + + def send_loop_message(self): + self.send_message() + if self.loop_enabled: + self.loop_task_id = self.master.after(1000, self.send_loop_message) + + def validate_hex(self, value): + return all(c in '0123456789ABCDEFabcdef' for c in value) + +def send_custom_message_from_string(message_str): + parts = message_str.split() + if len(parts) < 3: + raise ValueError("Invalid message format") + + can_interface = parts[0] + can_id = int(parts[1], 16) + data = [int(byte, 16) for byte in parts[2:]] + + bus = can.interface.Bus(channel=can_interface, bustype='socketcan') + try: + message = can.Message(arbitration_id=can_id, is_extended_id=False, data=data) + bus.send(message) + finally: + bus.shutdown() + +if __name__ == "__main__": + root = tk.Tk() + app = CANMessageSender(root) + root.mainloop() diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp new file mode 100644 index 0000000..b9dcfdf --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp @@ -0,0 +1,89 @@ +#ifndef SMART_RVIZ_PLUGIN__SMART_DOWNLOAD_HPP_ +#define SMART_RVIZ_PLUGIN__SMART_DOWNLOAD_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "umrr_ros2_msgs/srv/firmware_download.hpp" + +namespace smart_rviz_plugin +{ +/// +/// @brief The class for the firmware download onto the sensor. +/// +/// This class provides a graphical user interface (GUI) panel for downloading +/// firmware to a sensor within the RViz environment. It extends the rviz_common::Panel +/// class and includes functionalities for browsing firmware files, initiating the +/// download process, and displaying responses from the download service. +/// +class SmartDownloadService : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit SmartDownloadService(QWidget * parent = nullptr); + ~SmartDownloadService() override; + +private slots: + /// + /// @brief Slot function to handle firmware download action. + /// + /// This function is triggered when the download button is pressed. It sends + /// a request to the firmware download service with the specified file path + /// and sensor ID. + /// + void download_firmware(); + + /// + /// @brief Slot function to handle file browsing action. + /// + /// This function is triggered when the browse button is pressed. It opens a + /// file dialog to allow the user to select a firmware file, and sets the file + /// path input field with the selected file. + /// + void browse_file(); + +private: + /// + /// @brief Initializes the ROS2 clients. + /// + /// This function initializes the ROS2 node and + /// client for the firmware download service + /// + void initialize_ros(); + + /// + /// @brief Initializes the panel's components. + /// + /// This function sets up the GUI elements and connects the signals and + /// slots. + /// + void setup_ui(); + + rclcpp::Node::SharedPtr download_node_; + rclcpp::Client::SharedPtr download_client_; + std::shared_ptr executor_; + std::thread ros_thread_; + + QLineEdit * file_path_input_; + QLineEdit * sensor_id_input_; + QPushButton * start_download_button_; + QPushButton * browse_button_; + QTextEdit * response_text_edit_; +}; + +} // namespace smart_rviz_plugin + +#endif // SMART_RVIZ_PLUGIN__SMART_DOWNLOAD_HPP_ diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_recorder.hpp b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_recorder.hpp new file mode 100644 index 0000000..3b019fd --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_recorder.hpp @@ -0,0 +1,196 @@ +#ifndef SMART_RVIZ_PLUGIN__SMART_RECORDER_HPP_ +#define SMART_RVIZ_PLUGIN__SMART_RECORDER_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "std_msgs/msg/string.hpp" + +namespace smart_rviz_plugin +{ +struct TargetData +{ + float range; + float power; + float azimuth_deg; + float elevation_deg; + float rcs; + float noise; + float snr; + float radial_speed; + float azimuth_angle; + float elevation_angle; + uint32_t timestamp_sec; + uint32_t timestamp_nanosec; +}; + +struct ObjectData +{ + float x_pos; + float y_pos; + float z_pos; + float speed_abs; + float heading; + float length; + float quality; + float acceleration; + uint16_t object_id; + uint32_t timestamp_sec; + uint32_t timestamp_nanosec; +}; + +/// +/// @brief The class for the target and object recorder. +/// +/// This class provides a graphical user interface (GUI) panel for viewing +/// the sensor data within the RViz environment. It extends the rviz_common::Panel +/// class and includes functionalities for selecting, recording +/// and saving the sensor data as csv format. +/// +class SmartRadarRecorder : public rviz_common::Panel +{ + Q_OBJECT + +public: + /// + /// @brief Constructor for the SmartRadarRecorder class. + /// + /// @param parent The parent widget. Defaults to nullptr. + /// + SmartRadarRecorder(QWidget * parent = nullptr); + +private slots: + /// + /// @brief Slot function to start recording the sensor data being viewed. + /// + void start_recording(); + + /// + /// @brief Slot function to stop recording the sensor data. + /// + void stop_recording(); + + /// + /// @brief Slot function to save the recorded sensor data as csv fomrat. + /// + void save_data(); + + /// + /// @brief Slot function to check the data is being published. + /// + void check_data(); + + /// + /// @brief Slot function to update the table based on the topic selected. + /// + void update_table(); + +private: + /// + /// @brief Initializes the panel's components and ROS2 client. + /// + /// This function sets up the GUI elements, initializes the ROS2 node and + /// client for the recorder, and connects the signals and + /// slots. + /// + void initialize(); + + /// + /// @brief Subscribe to the port targets topics publisheb by the smartmicro_radar_node. + /// + void port_target_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name); + + /// + /// @brief Subscribe to the can targets topics publisheb by the smartmicro_radar_node. + /// + void can_target_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name); + + /// + /// @brief Subscribe to the port objects topics publisheb by the smartmicro_radar_node. + /// + void port_object_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name); + + /// + /// @brief Subscribe to the can objects topics publisheb by the smartmicro_radar_node. + /// + void can_object_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name); + + /// + /// @brief Subscribe to the raw image topic. + /// + void image_callback(const sensor_msgs::msg::CompressedImage::SharedPtr msg); + + /// + /// @brief Function to handle the data recording for target topics. + /// + void update_target_recorded_data( + float range, float power, float azimuth_deg, float elevation_deg, float rcs, float noise, + float snr, float radial_speed, float azimuth_angle, float elevation_angle, + uint32_t timestamp_sec, uint32_t timestamp_nanosec); + + /// + /// @brief Function to handle the data recording for objects topics. + /// + void update_object_recorded_data( + float x_pos, float y_pos, float z_pos, float speed_abs, float heading, float length, + float quality, float acceleration, uint16_t object_id, uint32_t timestamp_sec, + uint32_t timestamp_nanosec); + + QTableWidget * table_data_; + QTableWidget * table_data_2_; + QTableWidget * table_timestamps_; + QWidget * widget_1; + QWidget * widget_2; + QSplitter * splitter_; + QSplitter * horiz_splitter_; + QComboBox * topic_dropdown_; + QComboBox * topic_dropdown_2_; + QVBoxLayout * gui_layout_; + QVBoxLayout * vert_layout_1; + QVBoxLayout * vert_layout_2; + QPushButton * start_button_; + QPushButton * stop_button_; + QPushButton * save_button_; + QString selectedTopic; + QTimer * timer_; + rclcpp::Node::SharedPtr node_; + std::unordered_map::SharedPtr> + subscribers_{}; + std::unordered_map::SharedPtr> + object_subscribers_{}; + + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Publisher::SharedPtr publisher_; + + std::vector target_recorded_data; + std::vector object_recorded_data; + std::string selected_topic_; + std::string selected_topic_2_; + bool recording_active_{false}; +}; + +} // namespace smart_rviz_plugin + +#endif // SMART_RVIZ_PLUGIN__SMART_RECORDER_HPP_ diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp new file mode 100644 index 0000000..0c484ee --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp @@ -0,0 +1,160 @@ +#ifndef SMART_RVIZ_PLUGIN__SMART_SERVICES_HPP_ +#define SMART_RVIZ_PLUGIN__SMART_SERVICES_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "umrr_ros2_msgs/srv/send_command.hpp" +#include "umrr_ros2_msgs/srv/set_mode.hpp" +#include "umrr_ros2_msgs/srv/get_status.hpp" +#include "umrr_ros2_msgs/srv/get_mode.hpp" + +namespace smart_rviz_plugin +{ +/// +/// @brief The class for the sending instructions to the sensors. +/// +/// This class provides a graphical user interface (GUI) panel for sending +/// instructions to the sensor within the RViz environment. It extends the +/// rviz_common::Panel class and includes functionalities for viewing, recording +/// sensor data and sending instructions to the sensor. +/// +class SmartRadarService : public rviz_common::Panel +{ + Q_OBJECT + +public: + /// @brief Constructor for the SmartRadarService class. + /// @param parent The parent widget. Defaults to nullptr. + explicit SmartRadarService(QWidget * parent = nullptr); + +private slots: + /// @brief Slot function to send a parameter instruction to the sensor. + void on_send_param(); + + /// @brief Slot function to send a command instruction to the sensor. + void on_send_command(); + + /// @brief Slot function to send a status instruction to the sensor. + void on_get_status(); + + /// @brief Slot function to display instruction set of selected sensor model. + /// @param index The index of the selected file in the combo box. + void on_file_selected(int index); + + /// @brief Slot function to handle param selection from the table. + void on_param_selection(); + + /// @brief Slot function to handle command selection from the table. + void on_command_selection(); + + /// @brief Slot function to handle status selection from the table. + void on_status_selection(); + +private: + /// @brief Initializes the panel's components. + void initialize(); + + /// @brief Initializes the ROS2 client. + void setup_ros_clients(); + + /// @brief Create widgets for the plugin. + void create_widgets(); + + /// @brief Create layout. + void setup_layout(); + + /// @brief Set up the connections based on activity. + void setup_connections(); + + /// @brief Read parameters from user interface json file. + void read_param_json_data(); + + /// @brief Read commands from user interface json file. + void read_command_json_data(); + + /// @brief Read statuses from user interface json file. + void read_status_json_data(); + + /// @brief Populates the table with the available user interface files. + void populate_file_menu(); + + // File paths + const QString current_directory = QDir::currentPath(); + QString param_json_file_path; + QString command_json_file_path; + QString status_json_file_path; + + // Layout components + QVBoxLayout * main_layout{nullptr}; + QTabWidget * tab_widget{nullptr}; + QWidget * param_tab{nullptr}; + QWidget * command_tab{nullptr}; + QWidget * status_tab{nullptr}; + + // Parameter input fields + QLineEdit * param_name_line_edit{nullptr}; + QLineEdit * param_value_line_edit{nullptr}; + QLineEdit * param_sensor_id{nullptr}; + QLineEdit * param_section_name{nullptr}; + + // Command input fields + QLineEdit * command_name_line_edit{nullptr}; + //QLineEdit * command_comment_line_edit{nullptr}; + QLineEdit * command_sensor_id{nullptr}; + QLineEdit * command_value_line_edit{nullptr}; + QLineEdit * command_section_name{nullptr}; + + // Status input fields + QLineEdit * status_name_line_edit{nullptr}; + QLineEdit * status_sensor_id{nullptr}; + QLineEdit * status_section_name{nullptr}; + + // Buttons + QPushButton * send_param_button{nullptr}; + QPushButton * send_command_button{nullptr}; + QPushButton * send_status_button{nullptr}; + + // Tables and combo boxes + QTableWidget * param_table_widget{nullptr}; + QTableWidget * command_table_widget{nullptr}; + QTableWidget * status_table_widget{nullptr}; + QTextEdit * response_text_edit{nullptr}; + QComboBox * file_selector_combo_box{nullptr}; + QComboBox * param_value_type{nullptr}; + QComboBox * status_value_type{nullptr}; + QComboBox * param_action_combo{nullptr}; + + // Forms + QFormLayout * param_form{nullptr}; + QFormLayout * command_form{nullptr}; + QFormLayout * status_form{nullptr}; + + // ROS2 components + rclcpp::Node::SharedPtr client_node; + rclcpp::Client::SharedPtr mode_client; + rclcpp::Client::SharedPtr command_client; + rclcpp::Client::SharedPtr status_client; + rclcpp::Client::SharedPtr get_param_client; + + // Constants + static constexpr auto SERVICE_AVAILABILITY_TIMEOUT = std::chrono::seconds(2); +}; +} // namespace smart_rviz_plugin + +#endif // SMART_RVIZ_PLUGIN__SMART_SERVICES_HPP_ diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_status.hpp b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_status.hpp new file mode 100644 index 0000000..4ab2e1b --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/include/smart_rviz_plugin/smart_status.hpp @@ -0,0 +1,127 @@ +#ifndef SMART_RVIZ_PLUGIN__SMART_STATUS_HPP_ +#define SMART_RVIZ_PLUGIN__SMART_STATUS_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "std_msgs/msg/string.hpp" +#include "umrr_ros2_msgs/msg/can_object_header.hpp" +#include "umrr_ros2_msgs/msg/can_target_header.hpp" +#include "umrr_ros2_msgs/msg/port_object_header.hpp" +#include "umrr_ros2_msgs/msg/port_target_header.hpp" + +namespace smart_rviz_plugin +{ +/// +/// @brief The class for the target and object headers. +/// +/// This class provides a graphical user interface (GUI) panel for viewing +/// the sensor header data within the RViz environment. It extends the rviz_common::Panel +/// class and includes functionalities for selecting, viewing +/// the sensor header data. +/// +class SmartRadarStatus : public rviz_common::Panel +{ + Q_OBJECT + +public: + /// + /// @brief Constructor for the SmartRadarStatus class. + /// + /// @param parent The parent widget. Defaults to nullptr. + /// + SmartRadarStatus(QWidget * parent = nullptr); + +private slots: + /// + /// @brief Slot function to update the table based on the topic selected. + /// + void update_table(); + + /// + /// @brief Slot function to check the data is being published. + /// + void check_data(); + +private: + /// + /// @brief Initializes the panel's components and ROS2 client. + /// + /// This function sets up the GUI elements, initializes the ROS2 node and + /// client for the status, and connects the signals and + /// slots. + /// + void initialize(); + + /// + /// @brief Subscribe to the port target headers publisheb by the smartmicro_radar_node. + /// + void port_targetheader_callback( + const umrr_ros2_msgs::msg::PortTargetHeader::SharedPtr msg, const std::string topic_name); + + /// + /// @brief Subscribe to the can target headers publisheb by the smartmicro_radar_node. + /// + void can_targetheader_callback( + const umrr_ros2_msgs::msg::CanTargetHeader::SharedPtr msg, const std::string topic_name); + + /// + /// @brief Subscribe to the port object headers publisheb by the smartmicro_radar_node. + /// + void port_objectheader_callback( + const umrr_ros2_msgs::msg::PortObjectHeader::SharedPtr msg, const std::string topic_name); + + /// + /// @brief Subscribe to the can object headers publisheb by the smartmicro_radar_node. + /// + void can_objectheader_callback( + const umrr_ros2_msgs::msg::CanObjectHeader::SharedPtr msg, const std::string topic_name); + + QTableWidget * table_data_; + QTableWidget * table_data_2_; + QTableWidget * table_timestamps_; + QSplitter * splitter_; + QComboBox * topic_dropdown_; + QVBoxLayout * gui_layout_; + QPushButton * start_button_; + QPushButton * stop_button_; + QPushButton * save_button_; + QTimer * timer_; + rclcpp::Node::SharedPtr node_; + std::string selected_topic_; + std::unordered_map< + std::string, rclcpp::Subscription::SharedPtr> + port_header_target_subscribers_{}; + std::unordered_map< + std::string, rclcpp::Subscription::SharedPtr> + can_header_target_subscribers_{}; + std::unordered_map< + std::string, rclcpp::Subscription::SharedPtr> + port_header_object_subscribers_{}; + std::unordered_map< + std::string, rclcpp::Subscription::SharedPtr> + can_header_object_subscribers_{}; +}; + +} // namespace smart_rviz_plugin + +#endif // SMART_RVIZ_PLUGIN__SMART_RECORDER_HPP_ diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/package.xml b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/package.xml new file mode 100644 index 0000000..c4266b8 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/package.xml @@ -0,0 +1,38 @@ + + + + smart_rviz_plugin + 3.0.0 + TargetList Recorder + shahrukh + Apache 2.0 License + + ament_cmake + + qtbase5-dev + + pluginlib + rclcpp + umrr_ros2_msgs + resource_retriever + rviz_common + rviz_default_plugins + rviz_ogre_vendor + rviz_rendering + std_msgs + visualization_msgs + + libqt5-core + libqt5-gui + xacro + rviz + libqt5-opengl + libqt5-widgets + + ament_lint_common + ament_lint_auto + + + ament_cmake + + diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/plugins_description.xml b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/plugins_description.xml new file mode 100644 index 0000000..d7a4f89 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/plugins_description.xml @@ -0,0 +1,26 @@ + + + The Smart TargetList Recorder. + + + The Smart Command Controller. + + + The Smart Download. + + + The Smart Status. + + diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_download.cpp b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_download.cpp new file mode 100644 index 0000000..9104faa --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_download.cpp @@ -0,0 +1,141 @@ +#include "smart_rviz_plugin/smart_download.hpp" +#include + +namespace smart_rviz_plugin +{ + +SmartDownloadService::SmartDownloadService(QWidget * parent) +: rviz_common::Panel(parent) +{ + initialize_ros(); + setup_ui(); +} + +SmartDownloadService::~SmartDownloadService() +{ + if (executor_) { + executor_->cancel(); + } + if (ros_thread_.joinable()) { + ros_thread_.join(); + } + rclcpp::shutdown(); +} + +void SmartDownloadService::initialize_ros() +{ + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + download_node_ = rclcpp::Node::make_shared("smart_download_gui"); + download_client_ = download_node_->create_client( + "smart_radar/firmware_download"); + + executor_ = std::make_shared(); + executor_->add_node(download_node_); + + ros_thread_ = std::thread([this]() { executor_->spin(); }); + + RCLCPP_INFO(download_node_->get_logger(), "SmartDownloadService node initialized."); +} + +void SmartDownloadService::setup_ui() +{ + file_path_input_ = new QLineEdit(this); + sensor_id_input_ = new QLineEdit(this); + start_download_button_ = new QPushButton("Start Download", this); + browse_button_ = new QPushButton("Browse", this); + response_text_edit_ = new QTextEdit(this); + response_text_edit_->setReadOnly(true); + response_text_edit_->setFixedHeight(120); + + // Compact file path layout + auto * file_layout = new QHBoxLayout; + file_layout->addWidget(file_path_input_); + file_layout->addWidget(browse_button_); + + // Main layout + auto * layout = new QVBoxLayout(this); + layout->addWidget(new QLabel("Firmware File Path:")); + layout->addLayout(file_layout); + layout->addWidget(new QLabel("Sensor ID:")); + layout->addWidget(sensor_id_input_); + layout->addWidget(start_download_button_); + layout->addWidget(response_text_edit_); + setLayout(layout); + + // Connect signals and slots + connect(start_download_button_, &QPushButton::clicked, this, &SmartDownloadService::download_firmware); + connect(browse_button_, &QPushButton::clicked, this, &SmartDownloadService::browse_file); +} + +void SmartDownloadService::download_firmware() +{ + if (!download_client_) { + RCLCPP_ERROR(download_node_->get_logger(), "Download client not created."); + return; + } + + QString file_path = file_path_input_->text().trimmed(); + QString sensor_id_str = sensor_id_input_->text().trimmed(); + + if (file_path.isEmpty()) { + QMessageBox::warning(this, "Warning", "Please select a firmware file.", QMessageBox::Ok); + return; + } + if (sensor_id_str.isEmpty() || !sensor_id_str.toInt()) { + QMessageBox::warning(this, "Warning", "Please enter a valid numeric sensor ID.", QMessageBox::Ok); + return; + } + + int sensor_id = sensor_id_str.toInt(); + start_download_button_->setEnabled(false); + start_download_button_->setText("Downloading..."); + + auto request = std::make_shared(); + request->file_path = file_path.toStdString(); + request->sensor_id = sensor_id; + + if (!download_client_->wait_for_service(std::chrono::seconds(2))) { + QMessageBox::critical(this, "Error", "Firmware download service not available."); + start_download_button_->setEnabled(true); + start_download_button_->setText("Start Download"); + return; + } + + auto future = download_client_->async_send_request(request); + + // Async callback, do non block rviz + std::thread([this, future]() mutable { + try { + auto result = future.get(); + QString response_msg = QString::fromStdString(result->res); + QMetaObject::invokeMethod(this, [this, response_msg]() { + response_text_edit_->append("Response: " + response_msg); + start_download_button_->setEnabled(true); + start_download_button_->setText("Start Download"); + }); + RCLCPP_INFO(download_node_->get_logger(), "Firmware download succeeded: %s", result->res.c_str()); + } catch (const std::exception & e) { + QMetaObject::invokeMethod(this, [this, e]() { + response_text_edit_->append(QString("Error: %1").arg(e.what())); + start_download_button_->setEnabled(true); + start_download_button_->setText("Start Download"); + }); + RCLCPP_ERROR(download_node_->get_logger(), "Firmware download failed: %s", e.what()); + } + }).detach(); +} + +void SmartDownloadService::browse_file() +{ + QString file_path = QFileDialog::getOpenFileName(this, tr("Select Firmware File"), "", tr("All Files (*)")); + if (!file_path.isEmpty()) { + file_path_input_->setText(file_path); + } +} + +} // namespace smart_rviz_plugin + +PLUGINLIB_EXPORT_CLASS(smart_rviz_plugin::SmartDownloadService, rviz_common::Panel) diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_recorder.cpp b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_recorder.cpp new file mode 100644 index 0000000..2b8acbf --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_recorder.cpp @@ -0,0 +1,571 @@ +#include "smart_rviz_plugin/smart_recorder.hpp" + +#include +#include +#include + +const double radToDeg = 180.0 / M_PI; +namespace smart_rviz_plugin +{ +SmartRadarRecorder::SmartRadarRecorder(QWidget * parent) : rviz_common::Panel(parent) +{ + initialize(); +} + +void SmartRadarRecorder::initialize() +{ + node_ = rclcpp::Node::make_shared("smart_radar_gui_node"); + + subscription_ = node_->create_subscription( + "/ip_camera_front_right/image_raw/compressed", 10, + [this](const sensor_msgs::msg::CompressedImage::SharedPtr msg) { image_callback(msg); }); + + publisher_ = + node_->create_publisher("/ip_camera_front_right/image_raw", 10); + + // Reorder setup + gui_layout_ = new QVBoxLayout(); + topic_dropdown_ = new QComboBox(); + topic_dropdown_->addItem("Select a Topic"); + + // Retrieve available topics + auto topic_names_and_types = node_->get_topic_names_and_types(); + + // Create subscribers for selected topics + for (const auto & topic : topic_names_and_types) { + if (topic.first.find("port_targets") != std::string::npos) { + subscribers_[topic.first] = node_->create_subscription( + topic.first, 10, [this, topic](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + port_target_callback(msg, topic.first); + }); + + topic_dropdown_->addItem(QString::fromStdString(topic.first)); + } else if (topic.first.find("can_targets") != std::string::npos) { + subscribers_[topic.first] = node_->create_subscription( + topic.first, 10, [this, topic](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + can_target_callback(msg, topic.first); + }); + + topic_dropdown_->addItem(QString::fromStdString(topic.first)); + } else if (topic.first.find("port_objects") != std::string::npos) { + object_subscribers_[topic.first] = node_->create_subscription( + topic.first, 10, [this, topic](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + port_object_callback(msg, topic.first); + }); + + topic_dropdown_->addItem(QString::fromStdString(topic.first)); + } else if (topic.first.find("can_objects") != std::string::npos) { + object_subscribers_[topic.first] = node_->create_subscription( + topic.first, 10, [this, topic](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + can_object_callback(msg, topic.first); + }); + + topic_dropdown_->addItem(QString::fromStdString(topic.first)); + } + } + + connect(topic_dropdown_, SIGNAL(currentIndexChanged(int)), this, SLOT(update_table())); + + // Table one layout + table_data_ = new QTableWidget(); + table_data_->setColumnCount(13); + table_data_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + table_data_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + + table_timestamps_ = new QTableWidget(); + table_timestamps_->setColumnCount(2); + table_timestamps_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + table_timestamps_->setHorizontalHeaderLabels({"TsSec", "TsNanoSec"}); + + splitter_ = new QSplitter(Qt::Vertical); + splitter_->addWidget(topic_dropdown_); + splitter_->addWidget(table_data_); + + horiz_splitter_ = new QSplitter(Qt::Horizontal); + horiz_splitter_->addWidget(splitter_); + horiz_splitter_->addWidget(table_timestamps_); + horiz_splitter_->setSizes(QList({400, 100})); + + gui_layout_->addWidget(horiz_splitter_); + + timer_ = new QTimer(); + connect(timer_, SIGNAL(timeout()), this, SLOT(check_data())); + timer_->start(20); + + start_button_ = new QPushButton("Record"); + connect(start_button_, SIGNAL(clicked()), this, SLOT(start_recording())); + + stop_button_ = new QPushButton("Stop Recording"); + connect(stop_button_, SIGNAL(clicked()), this, SLOT(stop_recording())); + + save_button_ = new QPushButton("Save Data as CSV"); + connect(save_button_, SIGNAL(clicked()), this, SLOT(save_data())); + + gui_layout_->addWidget(start_button_); + gui_layout_->addWidget(stop_button_); + gui_layout_->addWidget(save_button_); + + setLayout(gui_layout_); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Recorder Plugin Created!"); +} + +void SmartRadarRecorder::image_callback(const sensor_msgs::msg::CompressedImage::SharedPtr msg) +{ + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "cv_bridge exception: %s", e.what()); + return; + } + + sensor_msgs::msg::Image ros_image; + ros_image.header = msg->header; + ros_image.height = cv_ptr->image.rows; + ros_image.width = cv_ptr->image.cols; + ros_image.encoding = sensor_msgs::image_encodings::BGR8; + ros_image.is_bigendian = false; + ros_image.step = sizeof(unsigned char) * cv_ptr->image.cols * cv_ptr->image.channels(); + ros_image.data.assign( + cv_ptr->image.data, cv_ptr->image.data + cv_ptr->image.total() * cv_ptr->image.elemSize()); + + publisher_->publish(ros_image); +} + +void SmartRadarRecorder::update_target_recorded_data( + float range, float power, float azimuth_deg, float elevation_deg, float rcs, float noise, + float snr, float radial_speed, float azimuth_angle, float elevation_angle, uint32_t timestamp_sec, + uint32_t timestamp_nanosec) +{ + TargetData data; + data.range = range; + data.power = power; + data.azimuth_deg = azimuth_deg; + data.elevation_deg = elevation_deg; + data.rcs = rcs; + data.noise = noise; + data.snr = snr; + data.radial_speed = radial_speed; + data.azimuth_angle = azimuth_angle; + data.elevation_angle = elevation_angle; + data.timestamp_sec = timestamp_sec; + data.timestamp_nanosec = timestamp_nanosec; + + target_recorded_data.push_back(data); +} + +void SmartRadarRecorder::update_object_recorded_data( + float x_pos, float y_pos, float z_pos, float speed_abs, float heading, float length, + float quality, float acceleration, uint16_t object_id, uint32_t timestamp_sec, + uint32_t timestamp_nanosec) +{ + ObjectData data; + data.x_pos = x_pos; + data.y_pos = y_pos; + data.z_pos = z_pos; + data.speed_abs = speed_abs; + data.heading = heading; + data.length = length; + data.quality = quality; + data.object_id = object_id; + data.acceleration = acceleration; + data.timestamp_sec = timestamp_sec; + data.timestamp_nanosec = timestamp_nanosec; + + object_recorded_data.push_back(data); +} + +void SmartRadarRecorder::port_target_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name) +{ + if (!selected_topic_.empty() && topic_name == selected_topic_) { + auto timestamp_sec = msg->header.stamp.sec; + auto timestamp_nanosec = msg->header.stamp.nanosec; + + // Create iterators for the pc2 fields + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*msg, "z"); + sensor_msgs::PointCloud2ConstIterator iter_radial_speed(*msg, "radial_speed"); + sensor_msgs::PointCloud2ConstIterator iter_power(*msg, "power"); + sensor_msgs::PointCloud2ConstIterator iter_rcs(*msg, "rcs"); + sensor_msgs::PointCloud2ConstIterator iter_noise(*msg, "noise"); + sensor_msgs::PointCloud2ConstIterator iter_snr(*msg, "snr"); + sensor_msgs::PointCloud2ConstIterator iter_azimuth_angle(*msg, "azimuth_angle"); + sensor_msgs::PointCloud2ConstIterator iter_elevation_angle(*msg, "elevation_angle"); + sensor_msgs::PointCloud2ConstIterator iter_range(*msg, "range"); + + table_data_->setRowCount(0); + + for (size_t i = 0; i < msg->height * msg->width; ++i, ++iter_x, ++iter_y, ++iter_z, + ++iter_radial_speed, ++iter_power, ++iter_rcs, ++iter_noise, ++iter_snr, + ++iter_azimuth_angle, ++iter_elevation_angle, ++iter_range) { + double azimuth_deg = *iter_azimuth_angle * radToDeg; + double elevation_deg = *iter_elevation_angle * radToDeg; + + // Update the recorded data + if (recording_active_) { + update_target_recorded_data( + *iter_range, *iter_power, azimuth_deg, elevation_deg, *iter_rcs, *iter_noise, *iter_snr, + *iter_radial_speed, *iter_azimuth_angle, *iter_elevation_angle, timestamp_sec, + timestamp_nanosec); + } + + // Add items to the table + int row_index = table_data_->rowCount(); + table_data_->insertRow(row_index); + table_data_->setItem(row_index, 0, new QTableWidgetItem(QString::number(*iter_x, 'f', 2))); + table_data_->setItem(row_index, 1, new QTableWidgetItem(QString::number(*iter_y, 'f', 2))); + table_data_->setItem(row_index, 2, new QTableWidgetItem(QString::number(*iter_z, 'f', 2))); + table_data_->setItem( + row_index, 3, new QTableWidgetItem(QString::number(*iter_radial_speed, 'f', 2))); + table_data_->setItem( + row_index, 4, new QTableWidgetItem(QString::number(*iter_power, 'f', 2))); + table_data_->setItem(row_index, 5, new QTableWidgetItem(QString::number(*iter_rcs, 'f', 2))); + table_data_->setItem( + row_index, 6, new QTableWidgetItem(QString::number(*iter_noise, 'f', 2))); + table_data_->setItem(row_index, 7, new QTableWidgetItem(QString::number(*iter_snr, 'f', 2))); + table_data_->setItem( + row_index, 8, new QTableWidgetItem(QString::number(azimuth_deg, 'f', 2))); + table_data_->setItem( + row_index, 9, new QTableWidgetItem(QString::number(elevation_deg, 'f', 2))); + table_data_->setItem( + row_index, 10, new QTableWidgetItem(QString::number(*iter_range, 'f', 2))); + table_data_->setItem( + row_index, 11, new QTableWidgetItem(QString::number(*iter_azimuth_angle, 'f', 2))); + table_data_->setItem( + row_index, 12, new QTableWidgetItem(QString::number(*iter_elevation_angle, 'f', 2))); + } + + // Update the timestamp table + table_timestamps_->setRowCount(0); + table_timestamps_->insertRow(0); + table_timestamps_->setItem(0, 0, new QTableWidgetItem(QString::number(timestamp_sec))); + table_timestamps_->setItem(0, 1, new QTableWidgetItem(QString::number(timestamp_nanosec))); + } +} + +void SmartRadarRecorder::can_target_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name) +{ + if (!selected_topic_.empty() && topic_name == selected_topic_) { + auto timestamp_sec = msg->header.stamp.sec; + auto timestamp_nanosec = msg->header.stamp.nanosec; + + // Create iterators for the pc2 fields + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*msg, "z"); + sensor_msgs::PointCloud2ConstIterator iter_radial_speed(*msg, "radial_speed"); + sensor_msgs::PointCloud2ConstIterator iter_power(*msg, "power"); + sensor_msgs::PointCloud2ConstIterator iter_rcs(*msg, "rcs"); + sensor_msgs::PointCloud2ConstIterator iter_noise(*msg, "noise"); + sensor_msgs::PointCloud2ConstIterator iter_snr(*msg, "snr"); + sensor_msgs::PointCloud2ConstIterator iter_azimuth_angle(*msg, "azimuth_angle"); + sensor_msgs::PointCloud2ConstIterator iter_elevation_angle(*msg, "elevation_angle"); + sensor_msgs::PointCloud2ConstIterator iter_range(*msg, "range"); + + table_data_->setRowCount(0); + + for (size_t i = 0; i < msg->height * msg->width; ++i, ++iter_x, ++iter_y, ++iter_z, + ++iter_radial_speed, ++iter_power, ++iter_rcs, ++iter_noise, ++iter_snr, + ++iter_azimuth_angle, ++iter_elevation_angle, ++iter_range) { + double azimuth_deg = *iter_azimuth_angle * radToDeg; + double elevation_deg = *iter_elevation_angle * radToDeg; + + // Update the recorded data + if (recording_active_) { + update_target_recorded_data( + *iter_range, *iter_power, azimuth_deg, elevation_deg, *iter_rcs, *iter_noise, *iter_snr, + *iter_radial_speed, *iter_azimuth_angle, *iter_elevation_angle, timestamp_sec, + timestamp_nanosec); + } + + // Add items to the table + int row_index = table_data_->rowCount(); + table_data_->insertRow(row_index); + table_data_->setItem(row_index, 0, new QTableWidgetItem(QString::number(*iter_x, 'f', 2))); + table_data_->setItem(row_index, 1, new QTableWidgetItem(QString::number(*iter_y, 'f', 2))); + table_data_->setItem(row_index, 2, new QTableWidgetItem(QString::number(*iter_z, 'f', 2))); + table_data_->setItem( + row_index, 3, new QTableWidgetItem(QString::number(*iter_radial_speed, 'f', 2))); + table_data_->setItem( + row_index, 4, new QTableWidgetItem(QString::number(*iter_power, 'f', 2))); + table_data_->setItem(row_index, 5, new QTableWidgetItem(QString::number(*iter_rcs, 'f', 2))); + table_data_->setItem( + row_index, 6, new QTableWidgetItem(QString::number(*iter_noise, 'f', 2))); + table_data_->setItem(row_index, 7, new QTableWidgetItem(QString::number(*iter_snr, 'f', 2))); + table_data_->setItem( + row_index, 8, new QTableWidgetItem(QString::number(azimuth_deg, 'f', 2))); + table_data_->setItem( + row_index, 9, new QTableWidgetItem(QString::number(elevation_deg, 'f', 2))); + table_data_->setItem( + row_index, 10, new QTableWidgetItem(QString::number(*iter_range, 'f', 2))); + table_data_->setItem( + row_index, 11, new QTableWidgetItem(QString::number(*iter_azimuth_angle, 'f', 2))); + table_data_->setItem( + row_index, 12, new QTableWidgetItem(QString::number(*iter_elevation_angle, 'f', 2))); + } + + // Update the timestamp table + table_timestamps_->setRowCount(0); + table_timestamps_->insertRow(0); + table_timestamps_->setItem(0, 0, new QTableWidgetItem(QString::number(timestamp_sec))); + table_timestamps_->setItem(0, 1, new QTableWidgetItem(QString::number(timestamp_nanosec))); + // } + } +} + +void SmartRadarRecorder::port_object_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name) +{ + if (!selected_topic_.empty() && topic_name == selected_topic_) { + auto timestamp_sec = msg->header.stamp.sec; + auto timestamp_nanosec = msg->header.stamp.nanosec; + + // Create iterators for the pc2 fields + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*msg, "z"); + sensor_msgs::PointCloud2ConstIterator iter_speed_absolute(*msg, "speed_absolute"); + sensor_msgs::PointCloud2ConstIterator iter_heading(*msg, "heading"); + sensor_msgs::PointCloud2ConstIterator iter_length(*msg, "length"); + sensor_msgs::PointCloud2ConstIterator iter_mileage(*msg, "mileage"); + sensor_msgs::PointCloud2ConstIterator iter_quality(*msg, "quality"); + sensor_msgs::PointCloud2ConstIterator iter_acceleration(*msg, "acceleration"); + sensor_msgs::PointCloud2ConstIterator iter_object_id(*msg, "object_id"); + sensor_msgs::PointCloud2ConstIterator iter_idle_cycles(*msg, "idle_cycles"); + sensor_msgs::PointCloud2ConstIterator iter_spline_idx(*msg, "spline_idx"); + sensor_msgs::PointCloud2ConstIterator iter_object_class(*msg, "object_class"); + sensor_msgs::PointCloud2ConstIterator iter_status(*msg, "status"); + + table_data_->setRowCount(0); + size_t num_points = msg->height * msg->width; + + for (size_t i = num_points; i > 0; --i, ++iter_x, ++iter_y, ++iter_z, ++iter_speed_absolute, + ++iter_heading, ++iter_length, ++iter_mileage, ++iter_quality, ++iter_acceleration, + ++iter_object_id, ++iter_idle_cycles, ++iter_spline_idx, ++iter_object_class, + ++iter_status) { + double heading_deg = *iter_heading * radToDeg; + + // Update the recorded data + if (recording_active_) { + update_object_recorded_data( + *iter_x, *iter_y, *iter_z, *iter_speed_absolute, *iter_heading, *iter_length, + *iter_quality, *iter_acceleration, *iter_object_id, timestamp_sec, timestamp_nanosec); + } + + // Add items to the table + int row_index = table_data_->rowCount(); + table_data_->insertRow(row_index); + table_data_->setItem(row_index, 0, new QTableWidgetItem(QString::number(*iter_x, 'f', 2))); + table_data_->setItem(row_index, 1, new QTableWidgetItem(QString::number(*iter_y, 'f', 2))); + table_data_->setItem(row_index, 2, new QTableWidgetItem(QString::number(*iter_z, 'f', 2))); + table_data_->setItem( + row_index, 3, new QTableWidgetItem(QString::number(*iter_speed_absolute, 'f', 2))); + table_data_->setItem( + row_index, 4, new QTableWidgetItem(QString::number(heading_deg, 'f', 2))); + table_data_->setItem( + row_index, 5, new QTableWidgetItem(QString::number(*iter_length, 'f', 2))); + table_data_->setItem( + row_index, 6, new QTableWidgetItem(QString::number(*iter_quality, 'f', 2))); + table_data_->setItem( + row_index, 7, new QTableWidgetItem(QString::number(*iter_acceleration, 'f', 2))); + table_data_->setItem(row_index, 8, new QTableWidgetItem(QString::number(*iter_object_id))); + } + + // Update the timestamp table + table_timestamps_->setRowCount(0); + table_timestamps_->insertRow(0); + table_timestamps_->setItem(0, 0, new QTableWidgetItem(QString::number(timestamp_sec))); + table_timestamps_->setItem(0, 1, new QTableWidgetItem(QString::number(timestamp_nanosec))); + } +} + +void SmartRadarRecorder::can_object_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name) +{ + if (!selected_topic_.empty() && topic_name == selected_topic_) { + auto timestamp_sec = msg->header.stamp.sec; + auto timestamp_nanosec = msg->header.stamp.nanosec; + + // Create iterators for the pc2 fields + sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*msg, "z"); + sensor_msgs::PointCloud2ConstIterator iter_speed_abs(*msg, "speed_absolute"); + sensor_msgs::PointCloud2ConstIterator iter_heading(*msg, "heading"); + sensor_msgs::PointCloud2ConstIterator iter_length(*msg, "length"); + sensor_msgs::PointCloud2ConstIterator iter_quality(*msg, "quality"); + sensor_msgs::PointCloud2ConstIterator iter_acceleration(*msg, "acceleration"); + sensor_msgs::PointCloud2ConstIterator iter_object_id(*msg, "object_id"); + sensor_msgs::PointCloud2ConstIterator iter_status(*msg, "status"); + + table_data_->setRowCount(0); + + size_t num_points = msg->height * msg->width; + + for (size_t i = num_points; i > 0; --i, ++iter_x, ++iter_y, ++iter_z, ++iter_speed_abs, + ++iter_heading, ++iter_length, ++iter_quality, ++iter_acceleration, + ++iter_object_id) { + // Update the recorded data + if (recording_active_) { + update_object_recorded_data( + *iter_x, *iter_y, *iter_z, *iter_speed_abs, *iter_heading, *iter_length, *iter_quality, + *iter_acceleration, *iter_object_id, timestamp_sec, timestamp_nanosec); + } + + int row_index = table_data_->rowCount(); + table_data_->insertRow(row_index); + table_data_->setItem(row_index, 0, new QTableWidgetItem(QString::number(*iter_x, 'f', 2))); + table_data_->setItem(row_index, 1, new QTableWidgetItem(QString::number(*iter_y, 'f', 2))); + table_data_->setItem(row_index, 2, new QTableWidgetItem(QString::number(*iter_z, 'f', 2))); + table_data_->setItem( + row_index, 3, new QTableWidgetItem(QString::number(*iter_speed_abs, 'f', 2))); + table_data_->setItem( + row_index, 4, new QTableWidgetItem(QString::number(*iter_heading, 'f', 2))); + table_data_->setItem( + row_index, 5, new QTableWidgetItem(QString::number(*iter_length, 'f', 2))); + table_data_->setItem( + row_index, 6, new QTableWidgetItem(QString::number(*iter_quality, 'f', 2))); + table_data_->setItem( + row_index, 7, new QTableWidgetItem(QString::number(*iter_acceleration, 'f', 2))); + table_data_->setItem( + row_index, 8, new QTableWidgetItem(QString::number(*iter_object_id, 'f', 2))); + } + + // Update the timestamp table + table_timestamps_->setRowCount(0); + table_timestamps_->insertRow(0); + table_timestamps_->setItem(0, 0, new QTableWidgetItem(QString::number(timestamp_sec))); + table_timestamps_->setItem(0, 1, new QTableWidgetItem(QString::number(timestamp_nanosec))); + } +} + +void SmartRadarRecorder::update_table() +{ + table_data_->setRowCount(0); + selected_topic_ = topic_dropdown_->currentText().toStdString(); + if (selected_topic_.find("port_targets") != std::string::npos) { + table_data_->setHorizontalHeaderLabels( + {"X_pos [m]", "Y_pos [m]", "Z_pos [m]", "RadialSpeed [m/s]", "Power [dB]", "RCS [m^2]", + "Noise [dB]", "SNR [dB]", "AzimuthAngle [Deg]", "ElevationAngle [Deg]", "Range [m]", + "AzimuthAngle [rad]", "ElevationAngle [rad]"}); + } else if (selected_topic_.find("can_targets") != std::string::npos) { + table_data_->setRowCount(0); + table_data_->setHorizontalHeaderLabels( + {"X_pos [m]", "Y_pos [m]", "Z_pos [m]", "RadialSpeed [m/s]", "Power [dB]", "RCS [dB]", + "Noise [dB]", "SNR [dB]", "AzimuthAngle [Deg]", "ElevationAngle [Deg]", "Range [m]", + "AzimuthAngle [rad]", "ElevationAngle [rad]"}); + } else if (selected_topic_.find("can_objects") != std::string::npos) { + table_data_->setRowCount(0); + table_data_->setHorizontalHeaderLabels( + {"X_pos [m]", "Y_pos [m]", "Z_pos [m]", "AbsoluteSpeed [m/s]", "Heading [Deg]", + "ObjectLength [m]", "Quality", "Acceleration [m/s^2]", "Object_ID", "", "", "", ""}); + } else if (selected_topic_.find("port_objects") != std::string::npos) { + table_data_->setRowCount(0); + table_data_->setHorizontalHeaderLabels( + {"PosX [m]", "PosY [m]", "PosZ [m]", "AbsoluteSpeed [m/s]", "Heading [Deg]", + "ObjectLength [m]", "Quality", "Acceleration [m/s^2]", "ObjectId", "", "", "", ""}); + } +} + +void SmartRadarRecorder::start_recording() +{ + qDebug() << "Recording started!"; + recording_active_ = true; + start_button_->setText("Recording..."); +} + +void SmartRadarRecorder::stop_recording() +{ + qDebug() << "Recording stopped!"; + recording_active_ = false; + start_button_->setText("Record"); +} + +void SmartRadarRecorder::save_data() +{ + qDebug() << "Saving data to CSV!"; + if (!target_recorded_data.empty() || !object_recorded_data.empty()) { + QFileDialog file_dialog; + QString file_path = + file_dialog.getSaveFileName(this, "Save Data", "", "CSV Files (*.csv);;All Files (*)"); + + if (!file_path.isEmpty()) { + QFile csvfile(file_path); + if (csvfile.open(QIODevice::WriteOnly | QIODevice::Text)) { + QTextStream csv_writer(&csvfile); + csv_writer + << "Type, Range [m], Power [dB], AzimuthAngle [Deg], ElevationAngle [Deg], RCS [dB], " + "Noise [dB], SNR [dB], " + "RadialSpeed [m/s], ElevationAngle [rad], AzimuthAngle [rad], " + "TimestampSec, TimestampNanoSec\n"; + + for (const auto & data_row : target_recorded_data) { + QStringList data_str_list; + data_str_list << "Target"; + data_str_list << QString::number(data_row.range, 'f', 2); + data_str_list << QString::number(data_row.power, 'f', 2); + data_str_list << QString::number(data_row.azimuth_angle * 180.0 / M_PI, 'f', 2); + data_str_list << QString::number(data_row.elevation_angle * 180.0 / M_PI, 'f', 2); + data_str_list << QString::number(data_row.rcs, 'f', 2); + data_str_list << QString::number(data_row.noise, 'f', 2); + data_str_list << QString::number(data_row.snr, 'f', 2); + data_str_list << QString::number(data_row.radial_speed, 'f', 2); + data_str_list << QString::number(data_row.azimuth_angle, 'f', 2); + data_str_list << QString::number(data_row.elevation_angle, 'f', 2); + data_str_list << QString::number(data_row.timestamp_sec); + data_str_list << QString::number(data_row.timestamp_nanosec); + + csv_writer << data_str_list.join(", ") << "\n"; + } + + csv_writer << "Type, PosX [m], PosY [m], PosZ [m], AbsoluteSpeed [m/s], Heading [Deg], " + "ObjectLength [m], Quality, Acceleration [m/s^2], ObjectId, " + "TimestampSec, TimestampNanoSec\n"; + // Write object data + for (const auto & object : object_recorded_data) { + QStringList data_str_list; + data_str_list << "Object"; + data_str_list << QString::number(object.x_pos, 'f', 2); + data_str_list << QString::number(object.y_pos, 'f', 2); + data_str_list << QString::number(object.z_pos, 'f', 2); + data_str_list << QString::number(object.speed_abs, 'f', 2); + data_str_list << QString::number(object.heading * 180.0 / M_PI, 'f', 2); + data_str_list << QString::number(object.length, 'f', 2); + data_str_list << QString::number(object.quality, 'f', 2); + data_str_list << QString::number(object.acceleration, 'f', 2); + data_str_list << QString::number(object.object_id); + data_str_list << QString::number(object.timestamp_sec); + data_str_list << QString::number(object.timestamp_nanosec); + + csv_writer << data_str_list.join(", ") << "\n"; + } + + csvfile.close(); + } else { + qDebug() << "Error: Could not open the file for writing."; + } + } + } else { + qDebug() << "No recorded data to save."; + } + // Clear recorded_data after saving + target_recorded_data.clear(); + object_recorded_data.clear(); +} + +void SmartRadarRecorder::check_data() +{ + if (rclcpp::ok()) // Check if ROS2 is still running + { + rclcpp::spin_some(node_); + } +} + +} // namespace smart_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(smart_rviz_plugin::SmartRadarRecorder, rviz_common::Panel) \ No newline at end of file diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_services.cpp b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_services.cpp new file mode 100644 index 0000000..05522f4 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_services.cpp @@ -0,0 +1,664 @@ +#include "smart_rviz_plugin/smart_services.hpp" + +#include +#include +#include + +namespace smart_rviz_plugin +{ +SmartRadarService::SmartRadarService(QWidget * parent) : rviz_common::Panel(parent) +{ + initialize(); +} + +void SmartRadarService::initialize() +{ + // Initialize ROS 2 + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + + setup_ros_clients(); + create_widgets(); + setup_layout(); + setup_connections(); +} + +void SmartRadarService::setup_ros_clients() +{ + client_node = rclcpp::Node::make_shared("smart_service_gui"); + mode_client = client_node->create_client("smart_radar/set_radar_mode"); + command_client = client_node->create_client("smart_radar/send_command"); + status_client = client_node->create_client("/smart_radar/get_radar_status"); + get_param_client = client_node->create_client("smart_radar/get_radar_mode"); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Client node created!"); +} + +void SmartRadarService::create_widgets() +{ + // Parameter tab widget + param_name_line_edit = new QLineEdit(this); + param_name_line_edit->setPlaceholderText("Enter parameter name"); + + param_value_line_edit = new QLineEdit(this); + param_value_line_edit->setPlaceholderText("Enter value"); + + param_sensor_id = new QLineEdit(this); + param_sensor_id->setPlaceholderText("Enter sensor ID"); + + param_section_name = new QLineEdit(this); + param_section_name->setPlaceholderText("Enter param section name"); + + param_action_combo = new QComboBox(this); + param_action_combo->addItem("Write Parameter"); + param_action_combo->addItem("Read Parameter"); + + param_value_type = new QComboBox(this); + param_value_type->addItem("float32, (0)"); + param_value_type->addItem("uint32, (1)"); + param_value_type->addItem("uint16, (2)"); + param_value_type->addItem("uint8, (3)"); + + send_param_button = new QPushButton("Send Parameter", this); + param_table_widget = new QTableWidget(this); + param_table_widget->setSelectionBehavior(QAbstractItemView::SelectRows); + param_table_widget->setSelectionMode(QAbstractItemView::SingleSelection); + + // Command tab widgets + command_name_line_edit = new QLineEdit(this); + command_name_line_edit->setPlaceholderText("Enter command name"); + + command_value_line_edit = new QLineEdit(this); + command_value_line_edit->setPlaceholderText("Enter command value"); + + command_sensor_id = new QLineEdit(this); + command_sensor_id->setPlaceholderText("Enter sensor ID"); + + command_section_name = new QLineEdit(this); + command_section_name->setPlaceholderText("Enter command sectionn name"); + + send_command_button = new QPushButton("Send Command", this); + command_table_widget = new QTableWidget(this); + command_table_widget->setSelectionBehavior(QAbstractItemView::SelectRows); + command_table_widget->setSelectionMode(QAbstractItemView::SingleSelection); + + // Status tab widget + status_name_line_edit = new QLineEdit(this); + status_name_line_edit->setPlaceholderText("Enter status name"); + + status_sensor_id = new QLineEdit(this); + status_sensor_id->setPlaceholderText("Enter sensor ID"); + + status_section_name = new QLineEdit(this); + status_section_name->setPlaceholderText("Enter status section name"); + + status_value_type = new QComboBox(this); + status_value_type->addItem("uint32, (0)"); + status_value_type->addItem("uint16, (1)"); + + send_status_button = new QPushButton("Get Status", this); + status_table_widget = new QTableWidget(this); + status_table_widget->setSelectionBehavior(QAbstractItemView::SelectRows); + status_table_widget->setSelectionMode(QAbstractItemView::SingleSelection); + + // Create tabs + tab_widget = new QTabWidget(this); + param_tab = new QWidget(tab_widget); + command_tab = new QWidget(tab_widget); + status_tab = new QWidget(tab_widget); + + // Add dropdown menu for file selection + file_selector_combo_box = new QComboBox(this); + populate_file_menu(); + + response_text_edit = new QTextEdit(this); + response_text_edit->setReadOnly(true); + response_text_edit->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed); + response_text_edit->setFixedHeight(100); + +} + +void SmartRadarService::setup_layout() +{ + // Parameter tab layout + QVBoxLayout * param_layout = new QVBoxLayout(param_tab); + param_layout->setSpacing(10); + param_layout->setContentsMargins(10, 10, 10, 10); + + QFormLayout * param_form = new QFormLayout(); + param_form->addRow("Action:", param_action_combo); + param_form->addRow("Parameter Name:", param_name_line_edit); + param_form->addRow("Section Name:", param_section_name); + param_form->addRow("Value Type:", param_value_type); + param_form->addRow("Value:", param_value_line_edit); + param_form->addRow("Sensor ID:", param_sensor_id); + + param_layout->addWidget(param_table_widget); + param_layout->addLayout(param_form); + param_layout->addWidget(send_param_button); + + // Command tab widget + QVBoxLayout * command_layout = new QVBoxLayout(command_tab); + command_layout->setSpacing(10); + command_layout->setContentsMargins(10, 10, 10, 10); + + QFormLayout * command_form = new QFormLayout(); + command_form->addRow("Command Name:", command_name_line_edit); + command_form->addRow("Section Name:", command_section_name); + command_form->addRow("Value:", command_value_line_edit); + command_form->addRow("Sensor ID:", command_sensor_id); + + command_layout->addWidget(command_table_widget); + command_layout->addLayout(command_form); + command_layout->addWidget(send_command_button); + + // Status tab layout + QVBoxLayout * status_layout = new QVBoxLayout(status_tab); + status_layout->setSpacing(10); + status_layout->setContentsMargins(10, 10, 10, 10); + + QFormLayout * status_form = new QFormLayout(); + status_form->addRow("Status Name:", status_name_line_edit); + status_form->addRow("Section Name:", status_section_name); + status_form->addRow("Value Type:", status_value_type); + status_form->addRow("Sensor ID:", status_sensor_id); + + status_layout->addWidget(status_table_widget); + status_layout->addLayout(status_form); + status_layout->addWidget(send_status_button); + + // Add tabs to tab widget + tab_widget->addTab(param_tab, "Parameter"); + tab_widget->addTab(command_tab, "Command"); + tab_widget->addTab(status_tab, "Status"); + + // Main layout + QVBoxLayout * main_layout = new QVBoxLayout(this); + main_layout->setSpacing(10); + main_layout->setContentsMargins(10, 10, 10, 10); + + main_layout->addWidget(file_selector_combo_box); + main_layout->addWidget(tab_widget); + main_layout->addWidget(response_text_edit); +} + +void SmartRadarService::setup_connections() +{ + // Connect dropdown menu signal to slot + connect(file_selector_combo_box, QOverload::of(&QComboBox::activated), this, &SmartRadarService::on_file_selected); + connect(send_param_button, &QPushButton::clicked, this, &SmartRadarService::on_send_param); + connect(send_command_button, &QPushButton::clicked, this, &SmartRadarService::on_send_command); + connect(send_status_button, &QPushButton::clicked, this, &SmartRadarService::on_get_status); + connect(param_table_widget, &QTableWidget::itemSelectionChanged, this, &SmartRadarService::on_param_selection); + connect(command_table_widget, &QTableWidget::itemSelectionChanged, this, &SmartRadarService::on_command_selection); + connect(status_table_widget, &QTableWidget::itemSelectionChanged, this, &SmartRadarService::on_status_selection); + connect(param_action_combo, QOverload::of(&QComboBox::currentIndexChanged), + [this](int index) { + // Disable value input when "Read Parameter" + param_value_line_edit->setEnabled(index == 0); + }); +} + +void SmartRadarService::populate_file_menu() +{ + // Add file names to the dropdown menu + file_selector_combo_box->clear(); + file_selector_combo_box->addItem("Choose Sensor Type"); + file_selector_combo_box->addItem("UMRR9F MSE"); + file_selector_combo_box->addItem("UMRR9F"); + file_selector_combo_box->addItem("UMRR9D"); + file_selector_combo_box->addItem("UMRRA4"); + file_selector_combo_box->addItem("UMRRA4 MSE"); + file_selector_combo_box->addItem("UMRRA1"); +} + +void SmartRadarService::on_param_selection() +{ + QList selected = param_table_widget->selectedItems(); + if (!selected.isEmpty()) { + int row = selected[0]->row(); + param_name_line_edit->setText(param_table_widget->item(row, 1)->text()); + param_section_name->setText(param_table_widget->item(row, 0)->text()); + } +} + +void SmartRadarService::on_command_selection() +{ + QList selected = command_table_widget->selectedItems(); + if (!selected.isEmpty()) { + int row = selected[0]->row(); + command_name_line_edit->setText(command_table_widget->item(row, 1)->text()); + command_section_name->setText(command_table_widget->item(row, 0)->text()); + } +} + +void SmartRadarService::on_status_selection() +{ + QList selected = status_table_widget->selectedItems(); + if (!selected.isEmpty()) { + int row = selected[0]->row(); + status_name_line_edit->setText(status_table_widget->item(row, 1)->text()); + status_section_name->setText(status_table_widget->item(row, 0)->text()); + } +} + +void SmartRadarService::on_file_selected(int index) +{ + // Update file paths based on selected file + if (index == 0) { + // Clear param tab + param_table_widget->setRowCount(0); + param_name_line_edit->clear(); + param_value_line_edit->clear(); + param_sensor_id->clear(); + param_section_name->clear(); + + // Clear command tab + command_table_widget->setRowCount(0); + command_name_line_edit->clear(); + command_value_line_edit->clear(); + command_sensor_id->clear(); + command_section_name->clear(); + + // Clear status tab + status_table_widget->setRowCount(0); + status_name_line_edit->clear(); + status_sensor_id->clear(); + status_section_name->clear(); + + return; + } + + QString base_path = current_directory + + "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/"; + + switch (index) { + case 1: + param_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_mseV1.0.0/instructions/params/auto_interface_0dim.param"; + command_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_mseV1.0.0/instructions/command/auto_interface.command"; + status_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_mseV1.0.0/instructions/status/auto_interface.status"; + break; + case 2: + param_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_automotiveV2.4.1/instructions/params/auto_interface_0dim.param"; + command_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_automotiveV2.4.1/instructions/command/auto_interface.command"; + status_json_file_path = base_path + + "UserInterfaceUmrr9f_t169_automotiveV2.4.1/instructions/status/auto_interface.status"; + break; + case 3: + param_json_file_path = base_path + + "UserInterfaceUmrr9d_t152_automotiveV1.4.1/instructions/params/auto_interface_0dim.param"; + command_json_file_path = base_path + + "UserInterfaceUmrr9d_t152_automotiveV1.4.1/instructions/command/auto_interface.command"; + status_json_file_path = base_path + + "UserInterfaceUmrr9d_t152_automotiveV1.4.1/instructions/status/auto_interface.status"; + break; + case 4: + param_json_file_path = base_path + + "UserInterfaceUmrra4_automotiveV1.2.1/instructions/params/auto_interface_0dim.param"; + command_json_file_path = base_path + + "UserInterfaceUmrra4_automotiveV1.2.1/instructions/command/auto_interface.command"; + status_json_file_path = base_path + + "UserInterfaceUmrra4_automotiveV1.2.1/instructions/status/auto_interface.status"; + break; + case 5: + param_json_file_path = base_path + + "UserInterfaceUmrra4_mseV1.0.0/instructions/params/auto_interface_0dim.param"; + command_json_file_path = base_path + + "UserInterfaceUmrra4_mseV1.0.0/instructions/command/auto_interface.command"; + status_json_file_path = base_path + + "UserInterfaceUmrra4_mseV1.0.0/instructions/status/auto_interface.status"; + break; + case 6: + param_json_file_path = base_path + + "user_interface_umrra1_t166_b_automotive_v2_0_0/instructions/params/auto_interface_rrm.param"; + command_json_file_path = base_path + + "user_interface_umrra1_t166_b_automotive_v2_0_0/instructions/command/auto_interface_rrm.command"; + status_json_file_path = base_path + + "user_interface_umrra1_t166_b_automotive_v2_0_0/instructions/status/auto_interface_rrm.status"; + break; + } + + read_param_json_data(); + read_command_json_data(); + read_status_json_data(); +} + +void SmartRadarService::read_command_json_data() +{ + QFile command_json_file(command_json_file_path); + if (!command_json_file.open(QIODevice::ReadOnly)) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Parse Failed!"); + return; + } + + QByteArray json_data = command_json_file.readAll(); + QJsonDocument doc(QJsonDocument::fromJson(json_data)); + QJsonObject json_object = doc.object(); + + // Extract gloabl section name + QString global_section_name = json_object["name"].toString(); + + // Extract the "commands" array + QJsonArray commands_array = json_object["commands"].toArray(); + + // Set up table headers + QStringList command_header_labels = {"Section", "Name", "Argument", "Comment"}; + command_table_widget->setColumnCount(3); + command_table_widget->setHorizontalHeaderLabels(command_header_labels); + + // Populate the table with command data + command_table_widget->setRowCount(commands_array.size()); + for (int i = 0; i < commands_array.size(); ++i) { + QJsonObject command_object = commands_array[i].toObject(); + QString name = command_object["name"].toString(); + QString argument = command_object["argument"].toString(); + QString comment = command_object["comment"].toString(); + + QTableWidgetItem * section_item = new QTableWidgetItem(global_section_name); + QTableWidgetItem * name_item = new QTableWidgetItem(name); + QTableWidgetItem * argument_item = new QTableWidgetItem(argument); + QTableWidgetItem * comment_item = new QTableWidgetItem(comment); + + command_table_widget->setItem(i, 0, section_item); + command_table_widget->setItem(i, 1, name_item); + command_table_widget->setItem(i, 2, argument_item); + command_table_widget->setItem(i, 3, comment_item); + } +} + +void SmartRadarService::read_param_json_data() +{ + QFile param_json_file(param_json_file_path); + if (!param_json_file.open(QIODevice::ReadOnly)) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Parse Failed!"); + return; + } + + QByteArray json_data = param_json_file.readAll(); + QJsonDocument doc(QJsonDocument::fromJson(json_data)); + QJsonObject json_object = doc.object(); + + // Extract gloabl section name + QString global_section_name = json_object["name"].toString(); + + // Extract the "parameters" array + QJsonArray params_array = json_object["parameters"].toArray(); + + // Set up table headers + QStringList param_header_labels = {"Section", "Name", "Comment", "Type"}; + param_table_widget->setColumnCount(3); + param_table_widget->setHorizontalHeaderLabels(param_header_labels); + + // Populate the table with parameter data + param_table_widget->setRowCount(params_array.size()); + for (int i = 0; i < params_array.size(); ++i) { + QJsonObject param_object = params_array[i].toObject(); + QString name = param_object["name"].toString(); + QString comment = param_object["comment"].toString(); + QString type = param_object["type"].toString(); + + QTableWidgetItem * section_item = new QTableWidgetItem(global_section_name); + QTableWidgetItem * name_item = new QTableWidgetItem(name); + QTableWidgetItem * comment_item = new QTableWidgetItem(comment); + QTableWidgetItem * type_item = new QTableWidgetItem(type); + + param_table_widget->setItem(i, 0, section_item); + param_table_widget->setItem(i, 1, name_item); + param_table_widget->setItem(i, 2, comment_item); + param_table_widget->setItem(i, 3, type_item); + } +} + +void SmartRadarService::read_status_json_data() +{ + QFile status_json_file(status_json_file_path); + if (!status_json_file.open(QIODevice::ReadOnly)) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Parse Failed!"); + return; + } + + QByteArray json_data = status_json_file.readAll(); + QJsonDocument doc(QJsonDocument::fromJson(json_data)); + QJsonObject json_object = doc.object(); + + // Extract gloabl section name + QString global_section_name = json_object["name"].toString(); + + // Extract the "status" array + QJsonArray status_array = json_object["status"].toArray(); + + // Set up table headers + QStringList status_header_labels = {"Section", "Name", "Comment", "Type"}; + status_table_widget->setColumnCount(3); + status_table_widget->setHorizontalHeaderLabels(status_header_labels); + + // Populate the table with status data + status_table_widget->setRowCount(status_array.size()); + for (int i = 0; i < status_array.size(); ++i) { + QJsonObject status_object = status_array[i].toObject(); + QString name = status_object["name"].toString(); + QString comment = status_object["comment"].toString(); + QString type = status_object["type"].toString(); + + QTableWidgetItem * section_item = new QTableWidgetItem(global_section_name); + QTableWidgetItem * name_item = new QTableWidgetItem(name); + QTableWidgetItem * comment_item = new QTableWidgetItem(comment); + QTableWidgetItem * type_item = new QTableWidgetItem(type); + + status_table_widget->setItem(i, 0, section_item); + status_table_widget->setItem(i, 1, name_item); + status_table_widget->setItem(i, 2, comment_item); + status_table_widget->setItem(i, 3, type_item); + } +} + +void SmartRadarService::on_send_param() +{ + // Validate common inputs + if (param_name_line_edit->text().isEmpty() || param_sensor_id->text().isEmpty()) { + response_text_edit->append("Error: Parameter name and sensor ID fields must be filled!"); + return; + } + + if (param_action_combo->currentIndex() == 0) { + // Writing param + if (param_value_line_edit->text().isEmpty()) { + response_text_edit->append("Error: Value must be provided for write!"); + return; + } + if (!mode_client->wait_for_service(SERVICE_AVAILABILITY_TIMEOUT)) { + response_text_edit->append("SetMode service not available!"); + return; + } + auto request = std::make_shared(); + request->section_name = param_section_name->text().toStdString(); + request->params.push_back(param_name_line_edit->text().toStdString()); + request->sensor_id = std::stoi(param_sensor_id->text().toStdString()); + request->value_types.push_back(param_value_type->currentIndex()); + request->values.push_back(param_value_line_edit->text().toStdString()); + auto result = mode_client->async_send_request(request); + auto status = rclcpp::spin_until_future_complete(client_node, result); + switch (status) { + case rclcpp::FutureReturnCode::SUCCESS: + { + auto response = result.get(); + QString response_msg = QString::fromStdString(response->res); + response_text_edit->append(response_msg); + } + break; + + case rclcpp::FutureReturnCode::TIMEOUT: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service timed out!"); + response_text_edit->append("Service timed out!"); + break; + + case rclcpp::FutureReturnCode::INTERRUPTED: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call was interrupted!"); + response_text_edit->append("Service call was interrupted!"); + break; + + default: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call failed due to unknown reason!"); + response_text_edit->append("Service call failed due to unknown reason!"); + break; + } + } + else { + // Reading param + if (!get_param_client->wait_for_service(SERVICE_AVAILABILITY_TIMEOUT)) { + response_text_edit->append("GetMode service not available!"); + return; + } + auto request = std::make_shared(); + request->section_name = param_section_name->text().toStdString(); + request->params.push_back(param_name_line_edit->text().toStdString()); + request->sensor_id = std::stoi(param_sensor_id->text().toStdString()); + request->param_types.push_back(param_value_type->currentIndex()); + auto result = get_param_client->async_send_request(request); + auto status = rclcpp::spin_until_future_complete(client_node, result); + switch (status) { + case rclcpp::FutureReturnCode::SUCCESS: + { + auto response = result.get(); + QString response_msg = QString::fromStdString(response->res); + response_text_edit->append(response_msg); + } + break; + + case rclcpp::FutureReturnCode::TIMEOUT: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service timed out!"); + response_text_edit->append("Service timed out!"); + break; + + case rclcpp::FutureReturnCode::INTERRUPTED: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call was interrupted!"); + response_text_edit->append("Service call was interrupted!"); + break; + + default: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call failed due to unknown reason!"); + response_text_edit->append("Service call failed due to unknown reason!"); + break; + } + } +} + +void SmartRadarService::on_send_command() +{ + // Validate inputs + if (command_name_line_edit->text().isEmpty() || command_value_line_edit->text().isEmpty() || command_sensor_id->text().isEmpty()) { + response_text_edit->append("Error: All command fields must be filled. Add dummy value of command if not specified!"); + return; + } + + if (!command_client) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to create command client"); + return; + } + + if(!command_client->wait_for_service(SERVICE_AVAILABILITY_TIMEOUT)) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service not available!"); + response_text_edit->append("Service not available! Is the radar node running?"); + return; + } + + auto request = std::make_shared(); + request->section_name = command_section_name->text().toStdString(); + request->command = command_name_line_edit->text().toStdString(); + request->value = std::stoi(command_value_line_edit->text().toUtf8().constData()); + request->sensor_id = std::stoi(command_sensor_id->text().toUtf8().constData()); + + auto result = command_client->async_send_request(request); + auto status = rclcpp::spin_until_future_complete(client_node, result); + + switch (status) { + case rclcpp::FutureReturnCode::SUCCESS: + { + auto response = result.get(); + QString response_msg = QString::fromStdString(response->res); + response_text_edit->append(response_msg); + } + break; + + case rclcpp::FutureReturnCode::TIMEOUT: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service timed out!"); + response_text_edit->append("Service timed out!"); + break; + + case rclcpp::FutureReturnCode::INTERRUPTED: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call was interrupted!"); + response_text_edit->append("Service call was interrupted!"); + break; + + default: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call failed due to unknown reason!"); + response_text_edit->append("Service call failed due to unknown reason!"); + break; + } +} + +void SmartRadarService::on_get_status() +{ + // Validate inputs + if (status_name_line_edit->text().isEmpty() || status_sensor_id->text().isEmpty()) { + response_text_edit->append("Error: Status name and sensor ID must be filled!"); + return; + } + + if (!status_client) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to create status_client"); + return; + } + + if(!status_client->wait_for_service(SERVICE_AVAILABILITY_TIMEOUT)) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service not available!"); + response_text_edit->append("Service not available! Is the radar node running?"); + return; + } + + auto request = std::make_shared(); + + request->section_name = status_section_name->text().toStdString(); + request->statuses.push_back(status_name_line_edit->text().toStdString()); + request->sensor_id = std::stoi(status_sensor_id->text().toUtf8().constData()); + request->status_types.push_back(status_value_type->currentIndex()); + + auto result = status_client->async_send_request(request); + auto status = rclcpp::spin_until_future_complete(client_node, result); + + switch (status) { + case rclcpp::FutureReturnCode::SUCCESS: + { + auto response = result.get(); + QString response_msg = QString::fromStdString(response->res); + response_text_edit->append(response_msg); + } + break; + + case rclcpp::FutureReturnCode::TIMEOUT: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service timed out!"); + response_text_edit->append("Service timed out!"); + break; + + case rclcpp::FutureReturnCode::INTERRUPTED: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call was interrupted!"); + response_text_edit->append("Service call was interrupted!"); + break; + + default: + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Service call failed due to unknown reason!"); + response_text_edit->append("Service call failed due to unknown reason!"); + break; + } +} + +} // namespace smart_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(smart_rviz_plugin::SmartRadarService, rviz_common::Panel) diff --git a/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_status.cpp b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_status.cpp new file mode 100644 index 0000000..73237c0 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/smart_rviz_plugin/src/smart_status.cpp @@ -0,0 +1,236 @@ +#include "smart_rviz_plugin/smart_status.hpp" + +#include +#include +#include +#include + +namespace smart_rviz_plugin +{ +SmartRadarStatus::SmartRadarStatus(QWidget * parent) : rviz_common::Panel(parent) { initialize(); } + +void SmartRadarStatus::initialize() +{ + node_ = rclcpp::Node::make_shared("smart_radar_gui_node"); + + // Status setup + gui_layout_ = new QVBoxLayout(); + topic_dropdown_ = new QComboBox(); + topic_dropdown_->addItem("Select a Topic"); + + // Retrieve available topics + auto topic_names_and_types = node_->get_topic_names_and_types(); + + // Create subscribers for selected topics + for (const auto & topic : topic_names_and_types) { + if (topic.first.find("port_targetheader") != std::string::npos) { + port_header_target_subscribers_[topic.first] = + node_->create_subscription( + topic.first, 10, + [this, topic](const umrr_ros2_msgs::msg::PortTargetHeader::SharedPtr msg) { + port_targetheader_callback(msg, topic.first); + }); + + topic_dropdown_->addItem(QString::fromStdString(topic.first)); + } else if (topic.first.find("can_targetheader") != std::string::npos) { + can_header_target_subscribers_[topic.first] = + node_->create_subscription( + topic.first, 10, + [this, topic](const umrr_ros2_msgs::msg::CanTargetHeader::SharedPtr msg) { + can_targetheader_callback(msg, topic.first); + }); + + topic_dropdown_->addItem(QString::fromStdString(topic.first)); + } else if (topic.first.find("port_objectheader") != std::string::npos) { + port_header_object_subscribers_[topic.first] = + node_->create_subscription( + topic.first, 10, + [this, topic](const umrr_ros2_msgs::msg::PortObjectHeader::SharedPtr msg) { + port_objectheader_callback(msg, topic.first); + }); + topic_dropdown_->addItem(QString::fromStdString(topic.first)); + } else if (topic.first.find("can_objectheader") != std::string::npos) { + can_header_object_subscribers_[topic.first] = + node_->create_subscription( + topic.first, 10, + [this, topic](const umrr_ros2_msgs::msg::CanObjectHeader::SharedPtr msg) { + can_objectheader_callback(msg, topic.first); + }); + topic_dropdown_->addItem(QString::fromStdString(topic.first)); + } + } + + connect(topic_dropdown_, SIGNAL(currentIndexChanged(int)), this, SLOT(update_table())); + + table_data_ = new QTableWidget(); + table_data_->setRowCount(16); + table_data_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + table_data_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + + splitter_ = new QSplitter(Qt::Vertical); + splitter_->addWidget(topic_dropdown_); + splitter_->addWidget(table_data_); + + gui_layout_->addWidget(splitter_); + + timer_ = new QTimer(); + connect(timer_, SIGNAL(timeout()), this, SLOT(check_data())); + timer_->start(20); + + setLayout(gui_layout_); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Status Plugin Created!"); +} + +void SmartRadarStatus::port_targetheader_callback( + const umrr_ros2_msgs::msg::PortTargetHeader::SharedPtr msg, const std::string topic_name) +{ + if (!selected_topic_.empty() && topic_name == selected_topic_) { + table_data_->setColumnCount(0); + int col_index = 0; + std::uint64_t ntp_timestamp = msg->acquisition_start; + // Extract the first 4 bytes (seconds) - most significant 32 bits + std::uint32_t seconds = static_cast(ntp_timestamp >> 32); + // Extract the last 4 bytes (fraction of a second) - least significant 32 bits + std::uint32_t fraction_sec = static_cast(ntp_timestamp & 0xFFFFFFFF); + table_data_->insertColumn(col_index); + table_data_->setItem( + 0, col_index, new QTableWidgetItem(QString::number(msg->cycle_time, 'f', 3))); + table_data_->setItem( + 1, col_index, new QTableWidgetItem(QString::number(msg->number_of_targets))); + table_data_->setItem( + 2, col_index, new QTableWidgetItem(QString::number(msg->acquisition_tx_ant_idx))); + table_data_->setItem( + 3, col_index, new QTableWidgetItem(QString::number(msg->acquisition_sweep_idx))); + table_data_->setItem( + 4, col_index, new QTableWidgetItem(QString::number(msg->acquisition_cf_idx))); + table_data_->setItem(5, col_index, new QTableWidgetItem(QString::number(seconds))); + table_data_->setItem(6, col_index, new QTableWidgetItem(QString::number(fraction_sec))); + table_data_->setItem(7, col_index, new QTableWidgetItem(QString::number(msg->prf))); + table_data_->setItem( + 8, col_index, new QTableWidgetItem(QString::number(msg->umambiguous_speed, 'f', 2))); + table_data_->setItem(9, col_index, new QTableWidgetItem(QString::number(msg->port_identifier))); + table_data_->setItem(10, col_index, new QTableWidgetItem(QString::number(msg->port_ver_major))); + table_data_->setItem(11, col_index, new QTableWidgetItem(QString::number(msg->port_ver_minor))); + table_data_->setItem(12, col_index, new QTableWidgetItem(QString::number(msg->port_size))); + table_data_->setItem( + 13, col_index, new QTableWidgetItem(QString::number(msg->body_endianness))); + table_data_->setItem(14, col_index, new QTableWidgetItem(QString::number(msg->port_index))); + table_data_->setItem( + 15, col_index, new QTableWidgetItem(QString::number(msg->header_ver_major))); + table_data_->setItem( + 16, col_index, new QTableWidgetItem(QString::number(msg->header_ver_minor))); + } +} + +void SmartRadarStatus::can_targetheader_callback( + const umrr_ros2_msgs::msg::CanTargetHeader::SharedPtr msg, const std::string topic_name) +{ + if (!selected_topic_.empty() && topic_name == selected_topic_) { + table_data_->setColumnCount(0); + int col_index = 0; + table_data_->insertColumn(col_index); + table_data_->setItem( + 0, col_index, new QTableWidgetItem(QString::number(msg->cycle_time, 'f', 3))); + table_data_->setItem( + 1, col_index, new QTableWidgetItem(QString::number(msg->number_of_targets))); + table_data_->setItem(2, col_index, new QTableWidgetItem(QString::number(msg->cycle_count))); + table_data_->setItem( + 3, col_index, new QTableWidgetItem(QString::number(msg->acquisition_setup))); + table_data_->setItem(4, col_index, new QTableWidgetItem(QString::number(msg->time_stamp))); + table_data_->setItem(5, col_index, new QTableWidgetItem(QString::number(msg->acq_ts_fraction))); + } +} + +void SmartRadarStatus::port_objectheader_callback( + const umrr_ros2_msgs::msg::PortObjectHeader::SharedPtr msg, const std::string topic_name) +{ + if (!selected_topic_.empty() && topic_name == selected_topic_) { + float_t cycle_time = msg->cycle_time; + std::uint16_t no_objs = msg->number_of_objects; + std::uint64_t ntp_timestamp = msg->ts_measurement; + std::uint32_t seconds = static_cast(ntp_timestamp >> 32); + std::uint32_t fraction_sec = static_cast(ntp_timestamp & 0xFFFFFFFF); + + table_data_->setColumnCount(0); + int col_index = 0; + table_data_->insertColumn(col_index); + table_data_->setItem(0, col_index, new QTableWidgetItem(QString::number(cycle_time, 'f', 3))); + table_data_->setItem(1, col_index, new QTableWidgetItem(QString::number(no_objs))); + table_data_->setItem(2, col_index, new QTableWidgetItem(QString::number(seconds))); + table_data_->setItem(3, col_index, new QTableWidgetItem(QString::number(fraction_sec))); + table_data_->setItem(4, col_index, new QTableWidgetItem(QString::number(msg->port_identifier))); + table_data_->setItem(5, col_index, new QTableWidgetItem(QString::number(msg->port_ver_major))); + table_data_->setItem(6, col_index, new QTableWidgetItem(QString::number(msg->port_ver_minor))); + table_data_->setItem(7, col_index, new QTableWidgetItem(QString::number(msg->port_size))); + table_data_->setItem(8, col_index, new QTableWidgetItem(QString::number(msg->body_endianness))); + table_data_->setItem(9, col_index, new QTableWidgetItem(QString::number(msg->port_index))); + table_data_->setItem( + 10, col_index, new QTableWidgetItem(QString::number(msg->header_ver_major))); + table_data_->setItem( + 11, col_index, new QTableWidgetItem(QString::number(msg->header_ver_minor))); + } +} + +void SmartRadarStatus::can_objectheader_callback( + const umrr_ros2_msgs::msg::CanObjectHeader::SharedPtr msg, const std::string topic_name) +{ + if (!selected_topic_.empty() && topic_name == selected_topic_) { + table_data_->setColumnCount(0); + int col_index = 0; + table_data_->insertColumn(col_index); + table_data_->setItem( + 0, col_index, new QTableWidgetItem(QString::number(msg->cycle_time, 'f', 3))); + table_data_->setItem( + 1, col_index, new QTableWidgetItem(QString::number(msg->number_of_objects))); + table_data_->setItem(2, col_index, new QTableWidgetItem(QString::number(msg->cycle_count))); + table_data_->setItem(3, col_index, new QTableWidgetItem(QString::number(msg->ego_speed))); + table_data_->setItem( + 4, col_index, new QTableWidgetItem(QString::number(msg->ego_speed_quality))); + table_data_->setItem(5, col_index, new QTableWidgetItem(QString::number(msg->ego_yaw_rate))); + table_data_->setItem( + 6, col_index, new QTableWidgetItem(QString::number(msg->ego_yaw_rate_quality))); + table_data_->setItem(7, col_index, new QTableWidgetItem(QString::number(msg->dyn_source))); + } +} + +void SmartRadarStatus::update_table() +{ + table_data_->setColumnCount(0); + selected_topic_ = topic_dropdown_->currentText().toStdString(); + if (selected_topic_.find("port_targetheader") != std::string::npos) { + table_data_->setVerticalHeaderLabels( + {"CycleDuration [s]", "NumOfTargets", "AcquisitionTxAntIdx", "AcquisitionSweepIdx", + "AcquisitionCfIdx", "AcqTimeStamp [s]", "AcqTimeStampfrac [NTP]", "PRF", "UmambiguousSpeed", + "PortIdentifier", "PortVersionMajor", "PortVersionMinor", "PortSize", "BodyEndianness", + "PortIndex", "HeaderVersionMajor", "HeaderVersionMinor"}); + } else if (selected_topic_.find("can_targetheader") != std::string::npos) { + table_data_->setVerticalHeaderLabels( + {"CycleDuration [s]", "NumOfTargets", "CycleCount", "AcquisitionSetup", "AcqTimeStamp [s]", + "AcqTimeStampfrac [NTP]", "", "", "", "", "", "", "", "", "", "", ""}); + } else if (selected_topic_.find("can_objectheader") != std::string::npos) { + table_data_->setColumnCount(0); + table_data_->setVerticalHeaderLabels( + {"CycleDuration [s]", "NumOfObjects", "CycleCount", "Speed [km/h]", "SpeedQuality", + "YawRate [rad/s]", "YawRateQuality", "DynamicSource", "", "", "", "", "", "", "", "", ""}); + } else if (selected_topic_.find("port_objectheader") != std::string::npos) { + table_data_->setColumnCount(0); + table_data_->setVerticalHeaderLabels( + {"CycleDuration [s]", "NumOfObjects", "AcqTimeStamp [s]", "AcqTimeStampfrac [NTP]", + "PortIdentifier", "PortVersionMajor", "PortVersionMinor", "PortSize", "BodyEndianness", + "PortIndex", "HeaderVersionMajor", "HeaderVersionMinor", "", "", "", "", ""}); + } +} + +void SmartRadarStatus::check_data() +{ + if (rclcpp::ok()) // Check if ROS2 is still running + { + rclcpp::spin_some(node_); + } +} + +} // namespace smart_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(smart_rviz_plugin::SmartRadarStatus, rviz_common::Panel) diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/CMakeLists.txt b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/CMakeLists.txt new file mode 100644 index 0000000..c650596 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/CMakeLists.txt @@ -0,0 +1,182 @@ +# Copyright 2021 Apex.AI, 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. +# +# The initial version of the code was developed by Apex.AI and +# was thereafter adapted and extended by smartmicro. + +cmake_minimum_required(VERSION 3.11) + +project(umrr_ros2_driver) +include(FetchContent) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wnarrowing -Wpedantic -Wno-unused-parameter -g) +endif() + +find_package(ament_cmake_auto REQUIRED) +find_package(Threads REQUIRED) +find_package(visualization_msgs REQUIRED) + +fetchcontent_declare(json + URL https://github.com/nlohmann/json/releases/download/v3.12.0/include.zip + URL_HASH SHA256=b8cb0ef2dd7f57f18933997c9934bb1fa962594f701cd5a8d3c2c80541559372 +) + +fetchcontent_getproperties(json) +if(NOT json_POPULATED) + fetchcontent_populate(json) + add_library(nlohmann_json INTERFACE) + target_include_directories(nlohmann_json INTERFACE ${json_SOURCE_DIR}/include) + add_library(nlohmann_json::nlohmann_json ALIAS nlohmann_json) +endif() + +ament_auto_find_build_dependencies() + +set(SMARTMICRO_LIB_DIR "lib-linux-x86_64-gcc_9") + +# specify the path to search for the libraries +link_directories(${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}) + +install(FILES + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libsmart_access.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libosal.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libcom_lib.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libbasev1.0.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr11_t132_automotivev1.1.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr96_t153_automotivev1.2.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev1.1.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.0.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.1.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.2.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.4.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev3.0.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.0.3_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.2.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.4.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.5.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_automotivev1.0.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_automotivev1.2.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_automotivev1.4.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_msev1.0.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_msev1.1.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_msev1.3.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_msev1.0.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_msev2.1.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra1_t166_b_automotivev1.0.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra1_t166_b_automotivev2.0.0_user_interface.so" +DESTINATION lib) + +set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") +set(CONFIG_FOLDER_PATH "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config") +set(CONFIG_FILE_PATH "${CONFIG_FOLDER_PATH}/smart_access_config.json") +set(HW_INVENTORY_FILE_PATH "${CONFIG_FOLDER_PATH}/hw_inventory.json") +set(ROUTING_TABLE_FILE_PATH "${CONFIG_FOLDER_PATH}/routing_table.json") +configure_file( + "${PROJECT_SOURCE_DIR}/cmake/smart_access_config.json.in" "${CONFIG_FILE_PATH}" @ONLY) +configure_file( + "${PROJECT_SOURCE_DIR}/cmake/config_path.hpp.in" + "${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/config_path.hpp" @ONLY) + +ament_auto_add_library(smartmicro_radar_node SHARED + "src/smartmicro_radar_node.cpp" + "src/update_service.cpp" + ) + +target_include_directories(smartmicro_radar_node PUBLIC ${CMAKE_INSTALL_PREFIX}/include/ +smartmicro/include/ +smartmicro/include/umrr11_t132_automotive_v1_1_2 +smartmicro/include/umrr96_t153_automotive_v1_2_2 +smartmicro/include/umrr9f_t169_automotive_v1_1_1 +smartmicro/include/umrr9f_t169_automotive_v2_0_0 +smartmicro/include/umrr9f_t169_automotive_v2_1_1 +smartmicro/include/umrr9f_t169_automotive_v2_2_1 +smartmicro/include/umrr9f_t169_automotive_v2_4_1 +smartmicro/include/umrr9f_t169_automotive_v3_0_0 +smartmicro/include/umrr9d_t152_automotive_v1_0_3 +smartmicro/include/umrr9d_t152_automotive_v1_2_2 +smartmicro/include/umrr9d_t152_automotive_v1_4_1 +smartmicro/include/umrr9d_t152_automotive_v1_5_0 +smartmicro/include/umrra4_automotive_v1_0_1 +smartmicro/include/umrra4_automotive_v1_2_1 +smartmicro/include/umrra4_automotive_v1_4_0 +smartmicro/include/umrr9f_t169_mse_v1_0_0 +smartmicro/include/umrr9f_t169_mse_v1_1_0 +smartmicro/include/umrr9f_t169_mse_v1_3_0 +smartmicro/include/umrra4_mse_v1_0_0 +smartmicro/include/umrra4_mse_v2_1_0 +smartmicro/include/umrra1_t166_b_automotive_v1_0_0 +smartmicro/include/umrra1_t166_b_automotive_v2_0_0) + +# link smart_access_lib-linux-x86_64-gcc_9 to the node +target_link_libraries(smartmicro_radar_node + smart_access + nlohmann_json::nlohmann_json + com_lib + osal + umrr11_t132_automotivev1.1.2_user_interface + umrr96_t153_automotivev1.2.2_user_interface + umrr9f_t169_automotivev1.1.1_user_interface + umrr9f_t169_automotivev2.0.0_user_interface + umrr9f_t169_automotivev2.1.1_user_interface + umrr9f_t169_automotivev2.2.1_user_interface + umrr9f_t169_automotivev2.4.1_user_interface + umrr9f_t169_automotivev3.0.0_user_interface + umrr9d_t152_automotivev1.0.3_user_interface + umrr9d_t152_automotivev1.2.2_user_interface + umrr9d_t152_automotivev1.4.1_user_interface + umrr9d_t152_automotivev1.5.0_user_interface + umrra4_automotivev1.0.1_user_interface + umrra4_automotivev1.2.1_user_interface + umrra4_automotivev1.4.0_user_interface + umrr9f_t169_msev1.0.0_user_interface + umrr9f_t169_msev1.1.0_user_interface + umrr9f_t169_msev1.3.0_user_interface + umrra4_msev1.0.0_user_interface + umrra4_msev2.1.0_user_interface + umrra1_t166_b_automotivev1.0.0_user_interface + umrra1_t166_b_automotivev2.0.0_user_interface) + +rclcpp_components_register_node(smartmicro_radar_node + PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" + EXECUTABLE smartmicro_radar_node_exe +) + +if(BUILD_TESTING) + find_package(launch_testing_ament_cmake) + add_launch_test(test/radar_node_test.launch.py TIMEOUT "100") + + # Not applying ros2 linters on external sources + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_copyright + ament_cmake_lint_cmake + ament_cmake_uncrustify + ament_cmake_cpplint + ament_cmake_flake8 + ament_cmake_cppcheck + ament_cmake_pep257 + ) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_copyright("src/smartmicro_radar_node.cpp") + ament_cppcheck("src/smartmicro_radar_node.cpp") + +endif() + +ament_auto_package(INSTALL_TO_SHARE config launch param) diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/cmake/config_path.hpp.in b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/cmake/config_path.hpp.in new file mode 100644 index 0000000..3c94fdc --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/cmake/config_path.hpp.in @@ -0,0 +1,33 @@ +// Copyright 2021 Apex.AI, 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 UMRR_ROS2_DRIVER__CMAKE_CONFIG_PATH_H_ +#define UMRR_ROS2_DRIVER__CMAKE_CONFIG_PATH_H_ + +namespace smartmicro +{ +namespace drivers +{ +namespace radar +{ + +constexpr auto kConfigFilePath = "@CONFIG_FILE_PATH@"; +constexpr auto kHwInventoryFilePath = "@HW_INVENTORY_FILE_PATH@"; +constexpr auto kRoutingTableFilePath = "@ROUTING_TABLE_FILE_PATH@"; + +} // namespace radar +} // namespace drivers +} // namespace smartmicro + +#endif // UMRR_ROS2_DRIVER__CMAKE_CONFIG_PATH_H_ diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/cmake/smart_access_config.json.in b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/cmake/smart_access_config.json.in new file mode 100644 index 0000000..644827d --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/cmake/smart_access_config.json.in @@ -0,0 +1,16 @@ +{ + "name": "Smart Access Lib Config", + "version" : "1.0.0", + "client_id": 1, + "role": "master", + "shared_lib_path": "@LIB_PATH@", + "config_path": "@CONFIG_FOLDER_PATH@", + "user_interface_name": "base", + "user_interface_major_v": 1, + "user_interface_minor_v": 0, + "user_interface_patch_v": 2, + "download_path": "", + "instruction_serialization_type": "can_based", + "data_serialization_type": "can_based", + "alive": false +} diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/config/hw_inventory.json b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/config/hw_inventory.json new file mode 100644 index 0000000..48d7e7a --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/config/hw_inventory.json @@ -0,0 +1,9 @@ +{ + "hwItems": [ + { + + } + ], + "name": "HW Inventory List", + "version": "1.1.0" +} diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/config/routing_table.json b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/config/routing_table.json new file mode 100644 index 0000000..bc6cee3 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/config/routing_table.json @@ -0,0 +1,9 @@ +{ + "clients": [ + { + "can_network_id": 0 + } + ], + "name": "Client Routing Table", + "version": "1.0.0" +} diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/config/rviz/smart_plugin.rviz b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/config/rviz/smart_plugin.rviz new file mode 100644 index 0000000..d5e90ac --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/config/rviz/smart_plugin.rviz @@ -0,0 +1,212 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 295 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: smart_rviz_plugin/Smart Recorder + Name: Smart Recorder + - Class: smart_rviz_plugin/Smart Status + Name: Smart Status + - Class: smart_rviz_plugin/Smart Command Configurator + Name: Smart Command Configurator + - Class: smart_rviz_plugin/Smart Status + Name: Smart Status + - Class: smart_rviz_plugin/Smart Firmware Download + Name: Smart Firmware Download +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: power + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 88.64950561523438 + Min Color: 0; 0; 0 + Min Intensity: 56.63246154785156 + Name: Port_Tgts_S0 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /smart_radar/port_targets_0 + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 138; 226; 52 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Can_Tgts_S1 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /smart_radar/can_targets_1 + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: umrr + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 38.8543701171875 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 10.84825325012207 + Y: 0.366975337266922 + Z: 0.01084582507610321 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.1353957653045654 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1757 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 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 + Selection: + collapsed: false + Smart Command Configurator: + collapsed: false + Smart Firmware Download: + collapsed: false + Smart Recorder: + collapsed: false + Smart Status: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3151 + X: 374 + Y: 214 diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp new file mode 100644 index 0000000..99eac67 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp @@ -0,0 +1,1120 @@ +// Copyright 2021 Apex.AI, 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. +// The initial version of the code was developed by Apex.AI and +// was thereafter adapted and extended by smartmicro. + +#ifndef UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ +#define UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ + +#include +#include +#include +#include +#include "umrr_ros2_driver/update_service.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "umrr_ros2_msgs/msg/can_object_header.hpp" +#include "umrr_ros2_msgs/msg/can_target_header.hpp" +#include "umrr_ros2_msgs/msg/port_object_header.hpp" +#include "umrr_ros2_msgs/msg/port_target_header.hpp" +#include "umrr_ros2_msgs/srv/firmware_download.hpp" +#include "umrr_ros2_msgs/srv/send_command.hpp" +#include "umrr_ros2_msgs/srv/set_ip.hpp" +#include "umrr_ros2_msgs/srv/set_mode.hpp" +#include "umrr_ros2_msgs/srv/get_status.hpp" +#include "umrr_ros2_msgs/srv/get_mode.hpp" + +namespace smartmicro +{ +namespace drivers +{ +namespace radar +{ +namespace detail +{ +constexpr auto kMaxSensorCount = 10UL; +constexpr auto kMaxHwCount = 6UL; + +struct SensorConfig +{ + std::uint32_t id{}; + std::uint32_t dev_id{}; + std::string frame_id{}; + std::uint32_t history_size{}; + std::string ip{}; + std::string link_type{}; + std::string model{}; + std::uint32_t port{}; + std::string inst_type{}; + std::string data_type{}; + std::string uifname{}; + std::uint32_t uifmajorv{}; + std::uint32_t uifminorv{}; + std::uint32_t uifpatchv{}; + std::string pub_type{}; +}; + +struct HWConfig +{ + std::uint32_t hw_dev_id{}; + std::string hw_iface_name{}; + std::string hw_type{}; + std::uint32_t baudrate{}; + std::uint32_t port{}; +}; +} // namespace detail + +/// +/// @brief The class for the Smartmicro radar node. +/// +class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node +{ +public: + /// + /// ROS 2 parameter constructor. + /// + /// @param[in] node_options Node options for this node. + /// + explicit SmartmicroRadarNode(const rclcpp::NodeOptions & node_options); + +protected: + /// + /// @brief A timer to handle the services. + /// + void my_timer_callback() { timer->cancel(); } + + /// + /// @brief A callback to initate shutdown. + /// + void on_shutdown_callback(); + +private: + /// + /// @brief A callback that is called when a new object list port for + /// umrra4_v1_0_0 T171 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_port_umrra4_mse_v1_0_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + void objectlist_callback_umrra4_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrra4_mse_v1_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrra4_v1_0_0 T171 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrra4_mse_v1_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra4_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_mse_v1_0_0, + const com::types::ClientId client_id); + + + /// + /// @brief A callback that is called when a new object list port for + /// umrra4_v2_1_0 T171 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_port_umrra4_mse_v2_1_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + void objectlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrra4_mse_v2_1_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrra4_v2_1_0 T171 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrra4_mse_v2_1_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_mse_v2_1_0, + const com::types::ClientId client_id); + + + + /// + /// @brief A callback that is called when a new object list port for + /// umrr9f_v1_0_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_port_umrr9f_mse_v1_0_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + void objectlist_callback_umrr9f_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrr9f_mse_v1_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v1_0_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_mse_v1_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_mse_v1_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new object list port for + /// umrr9f_v1_1_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_port_umrr9f_mse_v1_1_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + void objectlist_callback_umrr9f_mse_v1_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrr9f_mse_v1_1_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v1_1_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_mse_v1_1_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_mse_v1_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_mse_v1_1_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new object list port for + /// umrr9f_v1_3_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_port_umrr9f_mse_v1_3_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + void objectlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v1_3_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_mse_v1_3_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr11 T132 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr11 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr11( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr11_t132_automotive_v1_1_2::comtargetlist::ComTargetList> & + targetlist_port_umrr11, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr96 T153 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr96 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr96( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr96_t153_automotive_v1_2_2::comtargetlist::ComTargetList> & + targetlist_port_umrr96, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v1_1_1 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_v1_1_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_v1_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9f_v1_1_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v2_0_0 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_v2_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_v2_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_0_0::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9f_v2_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v2_1_1 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_v2_1_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_v2_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_1_1::comtargetlist::ComTargetList> & + targetlist_port_umrr9f_v2_1_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v2_2_1 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_v2_2_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_v2_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_2_1::comtargetlist::ComTargetList> & + targetlist_port_umrr9f_v2_2_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v2_4_1 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_v2_4_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_v2_4_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_4_1::comtargetlist::ComTargetList> & + targetlist_port_umrr9f_v2_4_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f_v3_0_0 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9f_v3_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_v3_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v3_0_0::comtargetlist::ComTargetList> & + targetlist_port_umrr9f_v3_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9d_v1_0_3 T152 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9d_v1_0_3 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9d_v1_0_3( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_3::comtargetlist::ComTargetList> & + targetlist_port_umrr9d_v1_0_3, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9d_v1_2_2 T152 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_port_umrr9d_v1_2_2 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9d_v1_2_2( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_2_2::comtargetlist::ComTargetList> & + targetlist_port_umrr9d_v1_2_2, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9d_v1_4_1 T152 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrr9d_v1_4_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9d_v1_4_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_4_1::comtargetlist::ComTargetList> & + targetlist_port_umrr9d_v1_4_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9d_v1_5_0 T152 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrr9d_v1_5_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9d_v1_5_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_5_0::comtargetlist::ComTargetList> & + targetlist_port_umrr9d_v1_5_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrra4_v1_0_1 T171 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrra4_v1_0_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra4_v1_0_1( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_v1_0_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrra4_v1_2_1 T171 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrra4_v1_2_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra4_v1_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_v1_2_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrra4_v1_4_0 T171 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrra4_v1_4_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra4_v1_4_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_v1_4_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrra1_v1_0_0 T166 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrra1_v1_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra1_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra1_v1_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrra1_v2_0_0 T166 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrra1_v2_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra1_v2_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra1_v2_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN object list for + /// umrra4_v1_0_0 T171 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_can_umrra4_mse_v1_0_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + /// + void CAN_objectlist_callback_umrra4_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_can_umrra4_mse_v1_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrra4_mse_v1_0_0 T171 MSE arrives. + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrra4_mse_v1_0_0 The target list port + /// @param[in] client_id The client_id of the sensor. + /// + /// + void CAN_targetlist_callback_umrra4_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_can_umrra4_mse_v1_0_0, + const com::types::ClientId client_id); + + + /// + /// @brief A callback that is called when a new CAN object list for + /// umrra4_v2_1_0 T171 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_can_umrra4_mse_v2_1_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + /// + void CAN_objectlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_can_umrra4_mse_v2_1_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrra4_mse_v2_1_0 T171 MSE arrives. + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrra4_mse_v2_1_0 The target list port + /// @param[in] client_id The client_id of the sensor. + /// + /// + void CAN_targetlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_can_umrra4_mse_v2_1_0, + const com::types::ClientId client_id); + + + /// + /// @brief A callback that is called when a new CAN object list for + /// umrr9f_v1_0_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_can_umrr9f_mse_v1_0_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + /// + void CAN_objectlist_callback_umrr9f_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_mse_v1_0_0::comobjectbaselist::ComObjectBaseList> & + objectlist_can_umrr9f_mse_v1_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_mse_v1_0_0 T169 MSE arrives. + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9f_mse_v1_0_0 The target list port + /// @param[in] client_id The client_id of the sensor. + /// + /// + void CAN_targetlist_callback_umrr9f_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_mse_v1_0_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_mse_v1_0_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN object list for + /// umrr9f_v1_1_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_can_umrr9f_mse_v1_1_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + /// + void CAN_objectlist_callback_umrr9f_mse_v1_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_mse_v1_1_0::comobjectbaselist::ComObjectBaseList> & + objectlist_can_umrr9f_mse_v1_1_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_mse_v1_1_0 T169 MSE arrives. + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9f_mse_v1_1_0 The target list port + /// @param[in] client_id The client_id of the sensor. + /// + /// + void CAN_targetlist_callback_umrr9f_mse_v1_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_mse_v1_1_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_mse_v1_1_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN object list for + /// umrr9f_v1_3_0 T169 MSE arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] objectlist_can_umrr9f_mse_v1_3_0 The object list port + /// @param[in] client_id The client_id of the sensor + /// + /// + void CAN_objectlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_mse_v1_3_0::comobjectbaselist::ComObjectBaseList> & + objectlist_can_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_mse_v1_3_0 T169 MSE arrives. + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9f_mse_v1_3_0 The target list port + /// @param[in] client_id The client_id of the sensor. + /// + /// + void CAN_targetlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_mse_v1_3_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr96_v1_2_2 T153 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr96 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr96( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr96_t153_automotive_v1_2_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr96, + const com::types::ClientId client_id); + + /// @brief A callback that is called when a new CAN target list for + /// umrr11_v1_1_2 T132 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr11 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr11( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr11_t132_automotive_v1_1_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr11, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_v2_1_1 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9f_v2_1_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr9f_v2_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_1_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v2_1_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_v2_2_1 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9f_v2_2_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr9f_v2_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_2_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v2_2_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_v2_4_1 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9f_v2_4_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr9f_v2_4_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_4_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v2_4_1, + const com::types::ClientId client_id); + + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_v3_0_0 T169 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9f_v3_0_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr9f_v3_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v3_0_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v3_0_0, + const com::types::ClientId client_id); + + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9d_v1_0_3 T152 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9d_v1_0_3 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr9d_v1_0_3( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_3::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_0_3, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9d_v1_2_2 T152 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9d_v1_2_2 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr9d_v1_2_2( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_2_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_2_2, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9d_v1_4_1 T152 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9d_v1_4_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr9d_v1_4_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_4_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_4_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9d_v1_5_0 T152 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrr9d_v1_5_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrr9d_v1_5_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_5_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_5_0, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrra4_v1_0_1 T171 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrra4_v1_0_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrra4_v1_0_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra4_automotive_v1_0_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrra4_v1_0_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrra4_v1_2_1 T171 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrra4_v1_2_1 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrra4_v1_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra4_automotive_v1_2_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrra4_v1_2_1, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrra4_v1_4_0 T171 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] targetlist_can_umrra4_v1_4_0 The target list port + /// @param[in] client_id The client_id of the sensor + /// + void CAN_targetlist_callback_umrra4_v1_4_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra4_automotive_v1_4_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrra4_v1_4_0, + const com::types::ClientId client_id); + + /// + /// @brief Read parameters and update the json config files required by + /// Smart Access C++ API. + /// + void update_config_files_from_params(); + + /// + /// @brief Creates publishers for sensors using ports. + /// + /// @param[in] sensor The sensor configuration. + /// @param[in] sensor_idx The sensor index. + /// + void port_publishers(const detail::SensorConfig & sensor, size_t sensor_idx); + + /// + /// @brief Creates publishers for sensors using CAN. + /// + /// @param[in] sensor The sensor configuration. + /// @param[in] sensor_idx The sensor index. + /// + void can_publishers(const detail::SensorConfig & sensor, size_t sensor_idx); + + /// + /// @brief Callback for getting the parameter response. + /// + /// @param[in] client_id The client identifier. + /// @param[in] response The response batch. + /// @param[in] instruction_name The instruction name. + /// + void mode_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::vector & instruction_names, + const std::string & section_name); + + /// + /// @brief Callback for getting the command response. + /// + /// @param[in] client_id The client identifier. + /// @param[in] response The response batch. + /// @param[in] command_name The command name. + /// + void command_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, const std::string command_name, + const std::string & section_name); + + /// + /// @brief Callback for changing IP address. + /// + /// @param[in] client_id The client identifier. + /// @param[in] response The response batch. + /// + void sensor_response_ip( + const com::types::ClientId client_id, + const std::shared_ptr & response); + + /// + /// @brief Sends instructions to the sensor. + /// + /// @param[in] request The request. + /// @param[out] response The response. + /// + void set_radar_mode( + const std::shared_ptr request, + std::shared_ptr response); + + /// + /// @brief Configures the sensor IP address. + /// + /// @param[in] request The request. + /// @param[out] response The response. + /// + void ip_address( + const std::shared_ptr request, + std::shared_ptr response); + + /// + /// @brief Sends command to the sensor. + /// + /// @param[in] request The request. + /// @param[out] response The response. + /// + void radar_command( + const std::shared_ptr request, + std::shared_ptr response); + + /// + /// @brief Service for firmware download. + /// + /// @param[in] request The request. + /// @param[out] result The result. + /// + void firmware_download( + const std::shared_ptr request, + std::shared_ptr result); + + /// + /// @brief Converts a timestamp from microseconds to seconds and nanoseconds. + /// + /// @param timestamp The input timestamp in microseconds as a `std::chrono::microseconds`. + /// @return A `std::pair` where: + /// - `first` is the number of seconds (`int32_t`). + /// - `second` is the number of nanoseconds (`uint32_t`). + /// + inline std::pair convert_timestamp(std::chrono::microseconds timestamp) + { + const auto sec = std::chrono::duration_cast(timestamp); + const auto nanosec = std::chrono::duration_cast(timestamp - sec); + return {sec.count(), nanosec.count()}; + } + + /// + /// @brief Initializes all the smart access and ros2 services. + /// + void initialize_services(); + + /// + /// @brief Check and set up publsihers w.r.t defined radar parameters. + /// + void setup_publishers(); + + /// + /// @brief Service to get sensor status. + /// + /// @param[in] request The request. + /// @param[out] response The response. + /// + void get_radar_status( + const std::shared_ptr request, + std::shared_ptr response); + + /// + /// @brief Service to get sensor modes. + /// + /// @param[in] request The request. + /// @param[out] response The response. + /// + void get_radar_mode( + const std::shared_ptr request, + std::shared_ptr response); + + /// + /// @brief Callback for getting the status response. + /// + /// @param[in] client_id The client identifier. + /// @param[in] response The response batch. + /// @param[in] statuses The status name of the sensor. + /// @param[in] section_name The section name of the interface. + /// + void status_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::vector & statuses, + const std::string & section_name); + + /// + /// @brief Callback for getting the reading param response. + /// + /// @param[in] client_id The client identifier. + /// @param[in] response The response batch. + /// @param[in] params The param name of the sensor. + /// + void param_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::vector & params, + const std::string & section_name); + + rclcpp::Service::SharedPtr mode_srv_; + rclcpp::Service::SharedPtr ip_addr_srv_; + rclcpp::Service::SharedPtr command_srv_; + rclcpp::Service::SharedPtr download_srv_; + rclcpp::Service::SharedPtr status_srv_; + rclcpp::Service::SharedPtr read_mode_srv_; + + std::array m_sensors{}; + std::array m_adapters{}; + + std::array::SharedPtr, detail::kMaxSensorCount> + m_publishers_obj{}; + + std::array< + rclcpp::Publisher::SharedPtr, detail::kMaxSensorCount> + m_publishers_port_obj_header{}; + + std::array< + rclcpp::Publisher::SharedPtr, detail::kMaxSensorCount> + m_publishers_can_obj_header{}; + + std::array::SharedPtr, detail::kMaxSensorCount> + m_publishers{}; + + std::array< + rclcpp::Publisher::SharedPtr, detail::kMaxSensorCount> + m_publishers_port_target_header{}; + + std::array< + rclcpp::Publisher::SharedPtr, detail::kMaxSensorCount> + m_publishers_can_target_header{}; + + std::size_t m_number_of_sensors{}; + std::size_t m_number_of_adapters{}; + rclcpp::TimerBase::SharedPtr timer; + com::types::ClientId client_id; + std::uint64_t response_type{}; + std::shared_ptr update_service; +}; + +bool check_signal = false; +std::string update_image{}; +std::shared_ptr m_services{}; +std::shared_ptr data_umrra4_v1_0_1{}; +std::shared_ptr data_umrra4_v1_2_1{}; +std::shared_ptr data_umrra4_v1_4_0{}; +std::shared_ptr data_umrr11{}; +std::shared_ptr data_umrr96{}; +std::shared_ptr + data_umrr9f_v1_1_1{}; +std::shared_ptr + data_umrr9f_v2_0_0{}; +std::shared_ptr + data_umrr9f_v2_1_1{}; +std::shared_ptr + data_umrr9f_v2_2_1{}; +std::shared_ptr + data_umrr9f_v2_4_1{}; +std::shared_ptr + data_umrr9f_v3_0_0{}; +std::shared_ptr + data_umrr9d_v1_0_3{}; +std::shared_ptr + data_umrr9d_v1_2_2{}; +std::shared_ptr + data_umrr9d_v1_4_1{}; +std::shared_ptr + data_umrr9d_v1_5_0{}; +std::shared_ptr + data_umrr9f_mse_v1_0_0{}; +std::shared_ptr + data_umrr9f_mse_v1_1_0{}; +std::shared_ptr + data_umrr9f_mse_v1_3_0{}; +std::shared_ptr data_umrra4_mse_v1_0_0{}; +std::shared_ptr data_umrra4_mse_v2_1_0{}; +std::shared_ptr + data_umrra1_v1_0_0{}; +std::shared_ptr + data_umrra1_v2_0_0{}; + +} // namespace radar +} // namespace drivers +} // namespace smartmicro + +#endif // UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/include/umrr_ros2_driver/update_service.hpp b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/include/umrr_ros2_driver/update_service.hpp new file mode 100644 index 0000000..abd9ecc --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/include/umrr_ros2_driver/update_service.hpp @@ -0,0 +1,33 @@ +#ifndef UPDATE_SERVICE_HPP +#define UPDATE_SERVICE_HPP + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +class UpdateService { + public: + UpdateService(); + ~UpdateService() = default; + + void StartSoftwareUpdate(com::types::ClientId client_id, std::string &update_image); + + private: + void UpdateCallback(com::types::SWUpdateInfo &info); + void HandleResult(); + + com::types::SWUpdateInfo updateInfo_; + std::mutex mutex_; + std::condition_variable cv_; +}; + +#endif // UPDATE_SERVICE_HPP diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/include/umrr_ros2_driver/visibility_control.hpp b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/include/umrr_ros2_driver/visibility_control.hpp new file mode 100644 index 0000000..75a5e49 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/include/umrr_ros2_driver/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 Apex.AI, 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 UMRR_ROS2_DRIVER__VISIBILITY_CONTROL_HPP_ +#define UMRR_ROS2_DRIVER__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) + #if defined(UMRR_ROS2_DRIVER_BUILDING_DLL) || defined(UMRR_ROS2_DRIVER_EXPORTS) + #define UMRR_ROS2_DRIVER_PUBLIC __declspec(dllexport) + #define UMRR_ROS2_DRIVER_LOCAL + #else // defined(UMRR_ROS2_DRIVER_BUILDING_DLL) || defined(UMRR_ROS2_DRIVER_EXPORTS) + #define UMRR_ROS2_DRIVER_PUBLIC __declspec(dllimport) + #define UMRR_ROS2_DRIVER_LOCAL + #endif // defined(UMRR_ROS2_DRIVER_BUILDING_DLL) || defined(UMRR_ROS2_DRIVER_EXPORTS) +#elif defined(__linux__) + #define UMRR_ROS2_DRIVER_PUBLIC __attribute__((visibility("default"))) + #define UMRR_ROS2_DRIVER_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define UMRR_ROS2_DRIVER_PUBLIC __attribute__((visibility("default"))) + #define UMRR_ROS2_DRIVER_LOCAL __attribute__((visibility("hidden"))) +#else + #error "Unsupported Build Configuration" +#endif + +#endif // UMRR_ROS2_DRIVER__VISIBILITY_CONTROL_HPP_ diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/launch/radar.launch.py b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/launch/radar.launch.py new file mode 100644 index 0000000..fb0bd5e --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/launch/radar.launch.py @@ -0,0 +1,43 @@ +# Copyright (c) 2021, s.m.s, smart microwave sensors GmbH, Brunswick, Germany +# +# 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 launch_ros + +from ament_index_python import get_package_share_directory +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +PACKAGE_NAME = 'umrr_ros2_driver' + + +def generate_launch_description(): + """Generate the launch description.""" + + radar__params = os.path.join( + get_package_share_directory(PACKAGE_NAME), 'param/radar.params.template.yaml') + radar_node = Node( + package=PACKAGE_NAME, + executable='smartmicro_radar_node_exe', + name='smart_radar', + parameters=[radar__params] + ) + return LaunchDescription([ + radar_node + ]) + + diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/package.xml b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/package.xml new file mode 100644 index 0000000..a32d764 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/package.xml @@ -0,0 +1,28 @@ + + + + umrr_ros2_driver + 8.0.0 + A node to publish data from a smartmicro radar. + s.m.s, smart microwave sensors GmbH. + Apache 2.0 License + + ament_cmake_auto + + rclcpp + rclpy + rclcpp_components + sensor_msgs + point_cloud_msg_wrapper + visualization_msgs + umrr_ros2_msgs + + ament_lint_auto + ros_testing + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/param/example/radar.adapter.example.yaml b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/param/example/radar.adapter.example.yaml new file mode 100644 index 0000000..c517a94 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/param/example/radar.adapter.example.yaml @@ -0,0 +1,34 @@ +--- +/**: + ros__parameters: + # This sets the iface_name in the hw_inventory.json + # sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx + # sudo ip link set up slcan0 + # set buffer length using /sbin/ip link set slcan0 txqueuelen 4096 + # For peak CAN adapters, only required to set baudrate + # sudo ip link set can0 up type can bitrate 500000 + # One sensor per interface for can, for ethernet we can use one interface/dev for multiple sensors + master_inst_serial_type: "can_based" + master_data_serial_type: "can_based" + adapters: + adapter_0: + hw_type: "can" + hw_dev_id: 2 + hw_iface_name: "slcan0" + baudrate: 500000 + adapter_1: + hw_type: "eth" + hw_dev_id: 3 + hw_iface_name: "eth0" + port: 55555 + adapter_2: + hw_type: "can" + hw_dev_id: 4 + hw_iface_name: "slcan1" + baudrate: 500000 + adapter_3: + hw_type: "can" + hw_dev_id: 5 + hw_iface_name: "can0" + baudrate: 500000 + diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/param/example/radar.sensor.example.yaml b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/param/example/radar.sensor.example.yaml new file mode 100644 index 0000000..7a38608 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/param/example/radar.sensor.example.yaml @@ -0,0 +1,122 @@ +--- +/**: + ros__parameters: +# An array of sensors to subscribe to. + sensors: + # As many as 10 sensors all named as "sensor_" in increasing order of numbers, + # e.g., sensor_0, sensor_1, etc. The list must start with sensor_0. + sensor_0: + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr9d_can" + # The client_id of the sensor/source, must be a unique integer. + id: 500 + # Adapter id to which sensor is connected + dev_id: 2 + # The interface name of the sensor + uifname: "umrr9d_t152_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 0 + # The pathc version of the interface + uifpatchv: 3 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + sensor_1: + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr9f_can_v2_1_1" + # The client_id of the sensor/source, must be a unique integer. + id: 600 + # Adapter id to which sensor is connected + dev_id: 4 + # The interface name of the sensor + uifname: "umrr9f_t169_automotive" + # The major version of the interface + uifmajorv: 2 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 1 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + sensor_2: + link_type: "eth" + # The model of the connected sensor. + model: "umrr9f_v2_1_1" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 700 + # The ip address of the sensor or of the source acting as a sensor. + ip: "192.168.11.11" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr9f_t169_automotive" + # The major version of the interface + uifmajorv: 2 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 1 + sensor_3: + link_type: "eth" + # The model of the connected sensor. + model: "umrr11" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 800 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.101" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr11_t132_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 2 + sensor_4: + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr9d_can" + # The client_id of the sensor/source, must be a unique integer. + id: 900 + # Adapter id to which sensor is connected + dev_id: 5 + # The interface name of the sensor + uifname: "umrr9d_t152_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 0 + # The pathc version of the interface + uifpatchv: 3 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/param/radar.params.template.yaml b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/param/radar.params.template.yaml new file mode 100644 index 0000000..97bf6df --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/param/radar.params.template.yaml @@ -0,0 +1,136 @@ +/smart_radar: + ros__parameters: + adapters: + adapter_0: + hw_type: "eth" + hw_dev_id: 4 + hw_iface_name: "eth0" + port: 55555 + master_data_serial_type: port_based + master_inst_serial_type: port_based + sensors: + # As many as 10 sensors all named as "sensor_" in increasing order of numbers, + # e.g., sensor_0, sensor_1, etc. The list must start with sensor_0. + # + # Sensor `model` names if using link type: + # can: 'umrra4_can_mse_v1_0_0', 'umrra4_can_mse_v2_1_0', 'umrr9f_can_mse_v1_1_0', 'umrr9f_can_mse_v1_0_0', 'umrr96_can_v1_2_2', 'umrr11_can_v1_1_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1', + # 'umrr9f_can_v2_4_1', 'umrr9f_can_v3_0_0', 'umrr9d_can_v1_0_3', 'umrr9d_can_v1_2_2', 'umrr9d_can_v1_4_1', 'umrr9d_can_v1_5_0', 'umrra4_can_v1_0_1', 'umrra4_can_v1_2_1', 'umrra4_can_v1_4_0' + # + # port: 'umrra1_v2_0_0', 'umrra1_v1_0_0', 'umrra4_mse_v1_0_0', 'umrra4_mse_v2_1_0', 'umrr9f_mse_v1_3_0', 'umrr9f_mse_v1_1_0', 'umrr9f_mse_v1_0_0', 'umrr96_v1_2_2', 'umrr11_v1_1_2', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1', 'umrr9f_v2_4_1', + # 'umrr9f_v3_0_0', 'umrr9d_v1_0_3', 'umrr9d_v1_2_2', 'umrr9d_v1_4_1', 'umrr9d_v1_5_0', 'umrra4_v1_0_1', 'umrra4_v1_2_1', 'umrra4_v1_4_0' + sensor_0: + # The link of the sensor either 'can' or 'eth' + link_type: "eth" + # The publisher type of the sensor + pub_type: "target" + # The model of the connected sensor. + model: "umrr11_v1_1_2" + # Adapter id to which sensor is connected + dev_id: 4 + # The client_id of the sensor/source, must be a unique integer. + id: 100 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + # The ip address of the sensor or of the source acting as a sensor. + ip: "192.168.11.11" + inst_type: "port_based" + data_type: "port_based" + # The port to be used. + port: 55555 + # The interface name of the sensor + uifname: "umrr11_t132_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 2 + sensor_1: + # The link of the sensor either 'can' or 'eth' + link_type: "eth" + # The publisher type of the sensor + pub_type: "target" + # The model of the connected sensor. + model: "umrr96_v1_2_2" + # Adapter id to which sensor is connected + dev_id: 4 + # The client_id of the sensor/source, must be a unique integer. + id: 200 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.102" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr96_t153_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 2 + # The pathc version of the interface + uifpatchv: 2 + sensor_2: + # The link of the sensor either 'can' or 'eth' + link_type: "eth" + # The publisher type of the sensor + pub_type: "target" + # The model of the connected sensor. + model: "umrr9f_v2_1_1" + # Adapter id to which sensor is connected + dev_id: 4 + # The client_id of the sensor/source, must be a unique integer. + id: 300 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.103" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr9f_t169_automotive" + # The major version of the interface + uifmajorv: 2 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 1 + sensor_3: + # The link of the sensor either 'can' or 'eth' + link_type: "eth" + # The publisher type of the sensor + pub_type: "target" + # The model of the connected sensor. + model: "umrr9d_v1_5_0" + # Adapter id to which sensor is connected + dev_id: 4 + # The client_id of the sensor/source, must be a unique integer. + id: 400 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.104" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr9d_t152_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 5 + # The pathc version of the interface + uifpatchv: 0 diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/Readme.md b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/Readme.md new file mode 100644 index 0000000..991748b --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/Readme.md @@ -0,0 +1,5 @@ +## Purpose / Use cases +This directory will include the Smart Access release once it has been download using the `smart_extract.sh` script! + +## License +Licensed under the [Apache 2.0 License](LICENSE). diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/include/DataServicesIface.h b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/include/DataServicesIface.h new file mode 100644 index 0000000..36e22bc --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/include/DataServicesIface.h @@ -0,0 +1,337 @@ +/************************************************************************ + * + * File: DataServicesIface.h + * + * Copyright + * + * s.m.s. smart microwave sensors GmbH + * In den Waashainen 1 + * 38108 Braunschweig - Germany + * + * Register Court: Amtsgericht Braunschweig / Register Number: HRB 4525 + * VAT ID: DE 193324798 / Tax Number: 13/207/02357 + ************************************************************************/ + +#ifndef COM_COMMON_DATA_SERVICES_IF_H +#define COM_COMMON_DATA_SERVICES_IF_H + +#include +#include +#include +#include + +#include + +using namespace com::types; + +namespace com { +namespace common { + +typedef std::function + DataReceiverCallback; +typedef std::function + ResponseCallback; +typedef std::function + LogReceiverCallback; + +typedef std::function UpdateCallback; +typedef std::function GetLocalTimeCallback; +typedef std::function SetTimeOffsetCallback; +typedef std::function OnUpdateInfoCallback; +typedef std::function + ConnectedInfoCallback; + +class EXPORT_COM_LIB DataServicesIface : public com::VersionedIface { + +public: + DataServicesIface() {} + virtual ~DataServicesIface() {} + + /* + * Function: Get() + * Arguments: none + * Return: std::shared_ptr + * Description: Returns a singletone instance of a this class, since this is + * only an abstract class, the instantiation should be done by a deriving + * class. + */ + static std::shared_ptr Get(); + + /* + * Function: SetInstructionBuffer + * Arguments: IN ClientId clientId - destination client id. + * IN const InstructionBuffer& instBuffer - instruction port + * object. IN ResponseCallback callback - callback function which should be + * called upon a response. + * + * Return: ErrorCode + * Description: This function sends an instruction port to the input client. + * It is used to send requests and responses. In the case of a response, no + * callback is required. + */ + virtual ErrorCode SetInstructionBuffer(IN ClientId clientId, + IN InstructionBuffer &instBuffer, + IN ResponseCallback callback) = 0; + /* + * Function: RegisterDataRecvCallback + * this function will pass all deserialzied data ports to centrally registered + * callback function + * Arguments: IN DataReceiverCallback callback - callback function which + * should be called upon a reception. Return: ErrorCode Description: Registers + * a receiver callback function , which should be called , when a new buffer + * arrives. + */ + virtual ErrorCode + RegisterDataRecvCallback(IN DataReceiverCallback callback) = 0; + + /* + * Function: RegisterDataRecvCallback + * Arguments: IN ClientId clientId - client id of the sending client + * IN PortId portId - port id of the required port + * IN DataReceiverCallback callback - callback function which + * should be called upon a reception. Return: ErrorCode Description: Registers + * a receiver callback function , which should be called , when a new buffer + * arrives. + */ + virtual ErrorCode + RegisterDataRecvCallback(IN ClientId clientId, IN PortId portId, + IN DataReceiverCallback callback) = 0; + + /* + * Function: RegisterComHubDataRecvCallback + * this function will pass all data ports from comhub device to registered + * callback function + * Arguments: IN DataReceiverCallback callback - callback function which + * should be called upon a reception. Return: ErrorCode Description: Registers + * a receiver callback function , which should be called , when a new buffer + * arrives. + */ + virtual ErrorCode + RegisterComHubDataRecvCallback(IN DataReceiverCallback callback) = 0; + + /* + * Function: RegisterInstRecvCallback + * Arguments: IN DataReceiverCallback callback - callback function will be + called upon a reception of new instruction port. + * Return: ErrorCode + * Description: Registers a receiver callback function, which should be + called, when new instructions arrive. + */ + virtual ErrorCode + RegisterInstRecvCallback(IN DataReceiverCallback callback) = 0; + + /* + * Function: StreamDataPort + * Arguments: IN ClientId clientId - destination client id. + * IN PortId portId - id of the streamed port. + * IN BufferDescriptor& buffer - data buffer + * Return: ErrorCode + * Description: Streams a data port to input client id + */ + virtual ErrorCode StreamDataPort(IN ClientId clientId, IN PortId portId, + IN BufferDescriptor &buffer) = 0; + + /* + * Function: StreamInternalDataPort + * Arguments: ClientId clientId - destination client id. + * PortId portId - id of the streamed port. + * BufferDescriptor& buffer - data buffer + * Return: ErrorCode + * Description: Streams a internal data port to input client id. + * This will return with an error code if the port is + * content of client user interface + */ + virtual ErrorCode StreamInternalDataPort(ClientId clientId, PortId portId, + BufferDescriptor &buffer) = 0; + + /* + * Function: Init + * Arguments: none + * Return: bool + * Description: Initializes the data services layer. + */ + virtual bool Init() = 0; + + /* + * Function: RegisterUpdCallback + * Arguments: UpdateCallback + * Return: ErrorCode + * Description: Register an update callback, will be called upon + * receiving a new download package + */ + virtual ErrorCode RegisterUpdCallback(IN UpdateCallback callback) = 0; + + /* + * Function: RegisterConnectedClientCallback + * Arguments: IN ConnectedInfoCallback callback + * Callback-Function which shall be called to inform about + * change in 'connected client list' + * Return: ErrorCode + * Description: Registers the ConnectedClientCallback + */ + virtual ErrorCode + RegisterConnectedClientCallback(IN ConnectedInfoCallback callback) = 0; + + /* + * Function: RegisterGetLocalTimeCallback + * Arguments: GetLocalTimeCallback + * Return: ErrorCode + * Description: Register a callback to receive the local time for the time + * sync feature. Valid for time sync master and slave + */ + virtual ErrorCode + RegisterGetTimeCallback(IN GetLocalTimeCallback callback) = 0; + + /* + * Function: RegisterSetTimeOffsetCallback + * Arguments: SetTimeOffsetCallback + * Return: ErrorCode + * Description: Register a callback to set adjust the local time with the + * offset Valid for time sync slave. + */ + virtual ErrorCode + RegisterSetTimeOffsetCallback(IN SetTimeOffsetCallback callback) = 0; + + /* + * Function: RegisterLogMessageCallBack + * Arguments: LogReceiverCallback + * Return: ErrorCode + * Description: Register a callback to receive log messages from + * BareMetal sensor. + */ + virtual ErrorCode + RegisterLogMessageCallBack(IN LogReceiverCallback callback) = 0; + + /* + * Function: AddTimeSyncSlave + * Arguments: uint8_t slaveDevId slave device id + * Return: ErrorCode + * Description: adds a time sync slave to be synchronized + * Valid for time sync master + */ + virtual ErrorCode AddTimeSyncSlave(IN uint8_t slaveDevId) = 0; + + /* + * Function: RemoveTimeSyncSlave + * Arguments: uint8_t slaveDevId slave device id + * Return: ErrorCode + * Description: removes a time sync slave. + * Valid for time sync master + */ + virtual ErrorCode RemoveTimeSyncSlave(IN uint8_t slaveDevId) = 0; + /* + * Function: SetTimeSyncSlaveId + * Arguments: uint8_t slaveDevId slave device id + * Return: ErrorCode + * Description: set time sync slave id. + * Valid for time sync slave + */ + virtual ErrorCode SetTimeSyncSlaveId(IN uint8_t slaveDevId) = 0; + /* + * Function: ActivateTimeSync + * Arguments: bool activate - true activates , false diactivates + * Return: ErrorCode + * Description: activates/diactivates the time sync protocol + * Valid for time sync slave and master + */ + virtual ErrorCode ActivateTimeSync(IN bool activate) = 0; + + /* + * Function: Configure Time Sync Slave. an only be configured if the service + * is not running. + * Arguments: uint8_t Number of required Iteration to synchronize timestamp. + * Return: ErrorCode + * Description: Valid only for time sync slave and sets the required number of + * iteration. + */ + virtual ErrorCode ConfigureTimeSyncSlave(uint8_t requireIteration) = 0; + + /* + * Function: Configure Time Sync Master. Can only be configured if the service + * is not running. + * Arguments: uint8_t Cooldown time between two time sync + * intervals. uint8_t Maximum number of iteration which support time sync + * master service. It is not possible to synchronize + * sensors that require a higher iteration number than the + * maximum number of iterations. + * Return: ErrorCode Description: Valid only for + * time sync master and sets cooldown time between two time sync intervals. + */ + virtual ErrorCode ConfigureTimeSyncMaster(uint8_t cooldownTime, + uint8_t maxNumberOfIteration) = 0; + + /* + * Function: StartSWUpdate + * Arguments: IN std::map updateList + * IN ClientId clientID + * IN OnUpdateInfoCallback callback + * Return : ErrorCode + * Description : Function for trigger a new Software-Update + * in Data-Service-Layer + */ + virtual ErrorCode + StartSWUpdate(IN std::map &updateList, + IN ClientId clientID, IN OnUpdateInfoCallback callback) = 0; + /* + * Function: AbortSWUpdate + * Arguments: None + * Return: void + * Description: Function for aborting a currently running Update-Service + */ + virtual void AbortSWUpdate() = 0; + + /* + * Function: GetConnectedClients + * Arguments: OUT std::map>& + * clients Return: bool Description: Returns a map of connected client + * descriptions. + */ + virtual bool GetConnectedClients( + OUT std::map> &clients) = 0; + + /* + * Function: GetConnectedClients + * Arguments: OUT std::map>& + * OUT std::map& + * clients Return: bool Description: Returns a map of connected client + * descriptions. + */ + virtual bool GetConnectedClients( + OUT std::map> &clients, + OUT std::map &client_ips) = 0; + + /* + * Function: SetClientUserInterfaceInfo + * Arguments: IN ClientId clientId client id + IN const std::string& userIfName user interface name + * IN uint32_t majorVer user interface major version + * IN uint32_t minorVer user interface minor version + * IN uint32_t patchVer user interface patch version + * Return: bool + * Description: set user interface information of a certain client + */ + virtual bool SetClientUserInterfaceInfo(IN ClientId clientId, + IN const std::string &userIfName, + IN uint32_t majorVer, + IN uint32_t minorVer, + IN uint32_t patchVer) = 0; + + /* + * Function: GetConfig + * Arguments: OUT ComLibConfig + * Return: bool + * Description: Get data from ComLibConfig file + */ + virtual bool GetConfig(OUT ComLibConfig &config) = 0; + /* + * Function: GetTimeSyncMasterTimeOutInfo + * Return: const TimeOutMap as reference + * Description: Gets the time-out details from TimeSync master + */ + virtual TimeOutMap GetTimeSyncMasterTimeOutInfo() = 0; +}; + +} // namespace common +} // namespace com + +#endif // COM_COMMON_DATA_SERVICES_IF_H diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/include/InstructionBuffer.h b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/include/InstructionBuffer.h new file mode 100644 index 0000000..ccbfb0a --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/include/InstructionBuffer.h @@ -0,0 +1,191 @@ +/************************************************************************ + * Copyright + * + * s.m.s. smart microwave sensors GmbH + * In den Waashainen 1 + * 38108 Braunschweig - Germany + * + * Register Court: Amtsgericht Braunschweig / Register Number: HRB 4525 + * VAT ID: DE 193324798 / Tax Number: 13/207/02357 + ************************************************************************/ + +#ifndef COM_COMMON_INSTRUCTION_BUFFER_H +#define COM_COMMON_INSTRUCTION_BUFFER_H + +#include + +#include + +using namespace com::types; + +namespace com { +namespace common { + +/** @} */ + +#define MAX_DIM_ELEMENT \ + 2U /**< @brief Max. numbers of available dimension elements */ +#define INSTR_HANDLER_CMD_DATA_TYPE 6U + +enum RequestType { + COM_INSTR_PORT_INVALID = 0U, /**< @brief Instruction Requests is invalid */ + COM_INSTR_PORT_PARAM_SET_PARAMETER = + 1U, /**< @brief Instruction Requests for setting a parameter */ + COM_INSTR_PORT_PARAM_GET_PARAMETER = + 2U, /**< @brief Instruction Requests for reading a parameter */ + COM_INSTR_PORT_STATUS_GET_STATUS = + 3U, /**< @brief Instruction Requests for reading a status */ + COM_INSTR_PORT_CMD = 4U, /**< @brief Instruction Requests for command */ + COM_INSTR_PORT_EXPORT = + 5U, /**< @brief Instruction Requests for export param/status */ + COM_INSTR_PORT_RESERVED_0 = 248U, /**< @brief Reserved 0 */ + COM_INSTR_PORT_RESERVED_1 = 249U, /**< @brief Reserved 1 */ + COM_INSTR_PORT_RESERVED_2 = 250U, /**< @brief Reserved 2 */ + COM_INSTR_PORT_RESERVED_3 = 251U, /**< @brief Reserved 3 */ + COM_INSTR_PORT_RESERVED_4 = 252U, /**< @brief Reserved 4 */ + COM_INSTR_PORT_RESERVED_5 = 253U, /**< @brief Reserved 5 */ + COM_INSTR_PORT_RESERVED_6 = 254U, /**< @brief Reserved 6 */ + COM_INSTR_PORT_RESERVED_7 = 255U, /**< @brief Reserved 7 */ +}; + +enum ResposeType { + COM_INSTR_PORT_NO_RESPONSE = 0U, /**< @brief No instruction Response */ + COM_INSTR_PORT_SUCCESS = + 1U, /**< @brief Instruction Response was processed successfully */ + COM_INSTR_PORT_ERROR = 2U, /**< @brief General error */ + COM_INSTR_PORT_ERROR_REQUEST = 3U, /**< @brief Invalid request */ + COM_INSTR_PORT_ERROR_SECTION = 4U, /**< @brief Invalid section */ + COM_INSTR_PORT_ERROR_ID = 5U, /**< @brief Invalid id */ + COM_INSTR_PORT_ERROR_PROT = 6U, /**< @brief Invalid protection */ + COM_INSTR_PORT_ERROR_MIN = 7U, /**< @brief Value out of minimal bounds */ + COM_INSTR_PORT_ERROR_MAX = 8U, /**< @brief Value out of maximal bounds */ + COM_INSTR_PORT_ERROR_NAN = 9U, /**< @brief Value is not a number */ + COM_INSTR_PORT_ERROR_TYPE = + 10U, /**< @brief Type of Instruction is not valid */ + COM_INSTR_PORT_ERROR_DIM = 11U, /**< @brief Dim of Instruction is not valid */ + COM_INSTR_PORT_ERROR_ELEMENT = + 12U, /**< @brief Element of Instruction is not valid */ + COM_INSTR_PORT_ERROR_SIGNATURE = + 13U, /**< @brief Signature of Instruction is not valid */ + COM_INSTR_PORT_ERROR_ACCESS_LVL = + 14U, /**< @brief Access level is not valid */ + COM_INSTR_PORT_ERROR_INTERNAL = + 15U, /**< @brief Internal error shall not be sent */ +}; + +class Instruction { + public: + Instruction(uint32_t sectionId, uint32_t id) + : _request(0), + _response(0), + _sectionId(sectionId), + _id(id), + _dataType(0), + _dimCount(0), + _signature(0), + _value(0) {} + + ~Instruction() {} + + inline uint8_t GetRequest() const { return _request; } + + inline void SetRequest(IN uint8_t request) { _request = request; } + + inline uint8_t GetResponse() const { return _response; } + + inline void SetResponse(IN uint8_t response) { _response = response; } + + inline uint16_t GetSectionId() const { return _sectionId; } + + inline void SetSectionId(IN uint16_t sectionId) { _sectionId = sectionId; } + + inline uint16_t GetId() const { return _id; } + + inline void SetId(IN uint16_t id) { _id = id; } + + inline uint8_t GetDataType() const { return _dataType; } + + inline void SetDataType(IN uint8_t dataType) { _dataType = dataType; } + + inline uint8_t GetDimCount() const { return _dimCount; } + + inline bool SetDimCount(IN uint8_t dimCount) { + if (dimCount <= MAX_DIM_ELEMENT) { + _dimCount = dimCount; + return true; + } + return false; + } + + inline uint16_t GetDimElement(IN uint8_t index) const { + return (index < _dimCount ? _dimElement[index] : 0); + } + + inline bool SetDimElement(IN uint8_t index, IN uint16_t element) { + if (index < _dimCount) { + _dimElement[index] = element; + return true; + } + return false; + } + + inline uint32_t GetSignature() const { return _signature; } + + inline void SetSignature(IN uint32_t signature) { _signature = signature; } + + inline uint64_t GetValue() const { return _value; } + + inline void SetValue(IN uint64_t value) { _value = value; } + + private: + uint8_t _request; /**< @brief Instruction request command */ + uint8_t _response; /**< @brief Instruction response */ + uint16_t _sectionId; /**< @brief Section number. */ + uint16_t _id; /**< @brief Each instruction has a unique ID */ + uint8_t _dataType; /**< @brief The data type. */ + uint8_t _dimCount; /**< @brief Number of dimensions. + Value 0 for non-array structures. */ + uint16_t _dimElement[MAX_DIM_ELEMENT]; /**< @brief Status element indices + addressing. */ + uint32_t _signature; /**< @brief Instruction signature. */ + uint64_t _value; /**< @brief Instruction value. */ +}; + +class InstructionBuffer { + public: + InstructionBuffer() : _seqCount(0) {} + + InstructionBuffer(uint32_t seqCount) : _seqCount(seqCount) {} + + ~InstructionBuffer() {} + + inline void AddInstruction(IN std::shared_ptr inst) { + _instructions.push_back(inst); + } + + inline std::vector>& GetInstructions() { + return _instructions; + } + + inline size_t GetNumOfInstructions() const { return _instructions.size(); } + + inline void SetSeqCount(IN SequenceNumber seqCount) { _seqCount = seqCount; } + + inline SequenceNumber GetSeqCount() const { return _seqCount; } + + inline void Reset() { _instructions.clear(); } + + void operator=(const InstructionBuffer& rhs) { + _instructions = rhs._instructions; + _seqCount = rhs._seqCount; + } + + private: + std::vector> _instructions; + uint32_t _seqCount; +}; + +} // namespace common +} // namespace com + +#endif // COM_COMMON_INSTRUCTION_BUFFER_H diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/include/Types.h b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/include/Types.h new file mode 100644 index 0000000..e8f7c70 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/include/Types.h @@ -0,0 +1,574 @@ +/************************************************************************ + * Copyright + * + * s.m.s. smart microwave sensors GmbH + * In den Waashainen 1 + * 38108 Braunschweig - Germany + * + * Register Court: Amtsgericht Braunschweig / Register Number: HRB 4525 + * VAT ID: DE 193324798 / Tax Number: 13/207/02357 + ************************************************************************/ + +#ifndef COM_TYPES_H +#define COM_TYPES_H + +#include +#include +#include + +#include +#include +#include +#include + +namespace com { + +namespace types { + +const uint8_t MAX_NUM_OF_INST = 255; + +#ifdef _WIN32 +#define USER_IF_LIB_PREFIX "" +#define USER_IF_LIB_EXT "_user_interface.dll" +#else +#define USER_IF_LIB_PREFIX "lib" +#define USER_IF_LIB_EXT "_user_interface.so" +#endif +#define CAN_MAX_DATA_BYTES 8 + +enum SerializationType { // NOLINT(*enum-size) + SERIALIZATION_TYPE_CAN_SPEC = 0, + SERIALIZATION_TYPE_PORT_BASED, + SERIALIZATION_TYPE_UNKNOWN +}; + +struct CanFormat { + uint16_t u16_identifier; + uint8_t u8_dlc; + uint8_t au8_data[CAN_MAX_DATA_BYTES]; // NOLINT(*arrays) +}; + +enum LinkType { // NOLINT(*enum-size) + LINK_TYPE_UDP = 0, + LINK_TYPE_UDP_DISCOVERY, + LINK_TYPE_CAN, + LINK_TYPE_CAN_DISCOVERY, + LINK_TYPE_RS485, + LIMK_TYPE_UNKNOWN, + LINK_TYPE_UNKNOWN = LIMK_TYPE_UNKNOWN +}; + +enum ProtocolType { // NOLINT(*enum-size) + PROTOCOL_TYPE_UNKNOWN = 0, + PROTOCOL_TYPE_SMS_CAN_BASE_DATA_V1, + PROTOCOL_TYPE_ATXMEGA_SDLC, + PROTOCOL_TYPE_STG, + PROTOCOL_TYPE_SMS_CAN_BASE_DATA_V2, + PROTOCOL_TYPE_DEBUG, + PROTOCOL_TYPE_LOG_MSG, + PROTOCOL_TYPE_ALIVE, + PROTOCOL_TYPE_PORT, + PROTOCOL_TYPE_INTERVIEW, + PROTOCOL_TYPE_DOWNLOAD, + PROTOCOL_TYPE_TIME_SYNC, + PROTOCOL_TYPE_DATA_STREAM, + PROTOCOL_TYPE_INSTRUCTION +}; + +enum LibraryRole { // NOLINT(*enum-size) + LIBRARY_ROLE_MASTER, + LIBRARY_ROLE_SLAVE, + LIBRARY_ROLE_UNKNOWN +}; + +enum TimeSyncRole { // NOLINT(*enum-size) + TIME_SYNC_ROLE_MASTER, + TIME_SYNC_ROLE_SLAVE, + TIME_SYNC_ROLE_UNKNOWN +}; + +using LinkId = uint16_t; +using SequenceNumber = uint32_t; +using CanNetId = uint8_t; +using CanId = uint16_t; +using UdtId = uint16_t; +using PhyDeviceId = uint8_t; + +struct TimeOutDetails { + uint16_t timeOutCount; + uint64_t latestOffsetValue; +}; +using TimeOutDeatils = TimeOutDetails; // TODO(RKO): Remove in next release +using TimeOutMap = std::map; +const PortId INSTRUCTION_PORT_ID = 46; + +class SharedLibDescriptor { + public: + // NOLINTNEXTLINE(*explicit*) + SharedLibDescriptor(const std::string& libName); + SharedLibDescriptor(const SharedLibDescriptor&) = delete; + SharedLibDescriptor(SharedLibDescriptor&&) = default; + SharedLibDescriptor& operator=(const SharedLibDescriptor&) = delete; + SharedLibDescriptor& operator=(SharedLibDescriptor&&) = default; + virtual ~SharedLibDescriptor(); + + bool Link(); + + void* GetHandle() { return _handle; } + + protected: + // NOLINTBEGIN(*non-private-member-variables-in-classes) + std::string _libName; + void* _handle{nullptr}; + // NOLINTEND(*non-private-member-variables-in-classes) + + private: + static std::mutex mutex_; +}; + +class ReceiverKey { + public: + ReceiverKey(ClientId clientId, PortId portId); + ReceiverKey() = default; + ReceiverKey(const ReceiverKey&) = default; + ReceiverKey(ReceiverKey&&) = delete; + ReceiverKey& operator=(const ReceiverKey&) = default; + ReceiverKey& operator=(ReceiverKey&&) = delete; + ~ReceiverKey() = default; + + ClientId GetClientId() const; + void SetClientId(const ClientId& clientId); + + PortId GetPortId() const; + void SetPortId(const PortId& portId); + + private: + ClientId _clientId{0}; + PortId _portId{0}; +}; + +inline bool operator<(const ReceiverKey& lhs, const ReceiverKey& rhs) { + return (lhs.GetClientId() < rhs.GetClientId()) || + ((lhs.GetClientId() == rhs.GetClientId()) && + (lhs.GetPortId() < rhs.GetPortId())); +} + +class ResponseKey { + public: + ResponseKey(ClientId clientId, SequenceNumber seqNum); + ResponseKey() = default; + ResponseKey(const ResponseKey&) = default; + ResponseKey(ResponseKey&&) = delete; + ResponseKey& operator=(const ResponseKey&) = default; + ResponseKey& operator=(ResponseKey&&) = delete; + ~ResponseKey() = default; + + ClientId GetClientId() const; + void SetClientId(IN const ClientId& clientId); + + SequenceNumber GetSequenceNumber() const; + void SetSequenceNumber(IN const SequenceNumber& seqNum); + + private: + ClientId _clientId{0}; + SequenceNumber _seqNum{0}; +}; + +inline bool operator<(IN const ResponseKey& lhs, IN const ResponseKey& rhs) { + return (lhs.GetClientId() < rhs.GetClientId()) || + ((lhs.GetClientId() == rhs.GetClientId()) && + (lhs.GetSequenceNumber() < rhs.GetSequenceNumber())); +} + +class EthNetDescriptor { + public: + EthNetDescriptor(const std::string& ip, uint32_t port, EthTransportType type); + EthNetDescriptor() = default; + EthNetDescriptor(const EthNetDescriptor&) = default; + EthNetDescriptor(EthNetDescriptor&&) = delete; + EthNetDescriptor& operator=(const EthNetDescriptor&) = default; + EthNetDescriptor& operator=(EthNetDescriptor&&) = delete; + ~EthNetDescriptor() = default; + + void GetIp(OUT std::string& ip) const; + void SetIp(IN const std::string& ip); + + uint32_t GetPort() const; + void SetPort(IN uint32_t port); + + EthTransportType GetType() const; + void SetType(IN EthTransportType type); + + private: + std::string _ip; + uint32_t _port{0}; + EthTransportType _type{ETH_TRANSPORT_TYPE_TCP}; +}; + +enum ClientPhyType { // NOLINT(*enum-size) + CLIENT_PHY_CAN = 0, + CLIENT_PHY_ETH, + CLIENT_PHY_RS485, + CLIENT_PHY_UNKNOWN +}; + +class ClientDescriptor { + public: + ClientDescriptor(ClientId clientId, ClientPhyType phyType, + SerializationType instSerialType, + SerializationType dataSerialType, uint8_t phyDevId); + ClientDescriptor(const ClientDescriptor&) = default; + ClientDescriptor(ClientDescriptor&&) = delete; + ClientDescriptor& operator=(const ClientDescriptor&) = default; + ClientDescriptor& operator=(ClientDescriptor&&) = delete; + virtual ~ClientDescriptor() = default; + + ClientPhyType GetPhyType() const; + void SetPhyType(IN ClientPhyType phyType); + + ClientId GetId() const; + void SetId(IN ClientId clientId); + + PhyDeviceId GetPhyDevId() const; + void SetPhyDevId(IN PhyDeviceId phyDevId); + + SerializationType GetInstSerialType() const; + void SetInstSerialType(IN SerializationType type); + + SerializationType GetDataSerialType() const; + void SetDataSerialType(IN SerializationType type); + + std::string GetUserIfName() const; + void SetUserIfName(IN const std::string& userIfName); + + uint32_t GetUserIfMajorVer() const; + void SetUserIfMajorVer(IN uint32_t majorVer); + + uint32_t GetUserIfMinorVer() const; + void SetUserIfMinorVer(IN uint32_t minorVer); + + uint32_t GetUserIfPatchVer() const; + void SetUserIfPatchVer(IN uint32_t patchVer); + + private: + ClientId _clientId{0}; + ClientPhyType _phyType{CLIENT_PHY_UNKNOWN}; + SerializationType _instSerialType{SERIALIZATION_TYPE_UNKNOWN}; + SerializationType _dataSerialType{SERIALIZATION_TYPE_UNKNOWN}; + PhyDeviceId _phyDevId{0}; + std::string _userIfName; + uint32_t _userIfMajorVer{0}; + uint32_t _userIfMinorVer{0}; + uint32_t _userIfPatchVer{0}; +}; + +class CanClientDescriptor : public ClientDescriptor { + public: + CanClientDescriptor(ClientId clientId, CanNetId canNetId, + PhyDeviceId phyDevId); + CanClientDescriptor(const CanClientDescriptor&) = default; + CanClientDescriptor(CanClientDescriptor&&) = delete; + CanClientDescriptor& operator=(const CanClientDescriptor&) = default; + CanClientDescriptor& operator=(CanClientDescriptor&&) = delete; + ~CanClientDescriptor() override = default; + + CanNetId GetCanNetId() const; + void SetCanNetId(IN CanNetId canNetId); + + private: + CanNetId _canNetId{0}; +}; + +class Rs485ClientDescriptor : public ClientDescriptor { + public: + Rs485ClientDescriptor(ClientId clientId, SerializationType instSerialType, + SerializationType dataSerialType, PhyDeviceId phyDevId); + Rs485ClientDescriptor(const Rs485ClientDescriptor&) = default; + Rs485ClientDescriptor(Rs485ClientDescriptor&&) = delete; + Rs485ClientDescriptor& operator=(const Rs485ClientDescriptor&) = default; + Rs485ClientDescriptor& operator=(Rs485ClientDescriptor&&) = delete; + ~Rs485ClientDescriptor() override = default; +}; + +class EthClientDescriptor : public ClientDescriptor { + public: + EthClientDescriptor(ClientId clientId, SerializationType instSerialType, + SerializationType dataSerialType); + EthClientDescriptor(const EthClientDescriptor&) = default; + EthClientDescriptor(EthClientDescriptor&&) = delete; + EthClientDescriptor& operator=(const EthClientDescriptor&) = default; + EthClientDescriptor& operator=(EthClientDescriptor&&) = delete; + ~EthClientDescriptor() override = default; +}; + +class SerializerConfig { + public: + SerializerConfig(ClientId clientId, SerializationType serialType); + SerializerConfig(const SerializerConfig&) = default; + SerializerConfig(SerializerConfig&&) = delete; + SerializerConfig& operator=(const SerializerConfig&) = default; + SerializerConfig& operator=(SerializerConfig&&) = delete; + virtual ~SerializerConfig() = default; + + ClientId GetClientId() const; + SerializationType GetType() const; + + private: + ClientId _clientId{0}; + SerializationType _serialType{SERIALIZATION_TYPE_UNKNOWN}; +}; + +class CanSerializerConfig : public SerializerConfig { + public: + CanSerializerConfig(ClientId clientId, LibraryRole role, CanNetId canNetId); + CanSerializerConfig(const CanSerializerConfig&) = default; + CanSerializerConfig(CanSerializerConfig&&) = delete; + CanSerializerConfig& operator=(const CanSerializerConfig&) = default; + CanSerializerConfig& operator=(CanSerializerConfig&&) = delete; + ~CanSerializerConfig() override = default; + + CanNetId GetCanNetId() const; + LibraryRole GetLibraryRole() const; + + private: + CanNetId _canNetId{0}; + LibraryRole _role{LIBRARY_ROLE_UNKNOWN}; +}; + +class PortBasedSerializerConfig : public SerializerConfig { + public: + PortBasedSerializerConfig(ClientId clientId, LibraryRole role); + PortBasedSerializerConfig(const PortBasedSerializerConfig&) = default; + PortBasedSerializerConfig(PortBasedSerializerConfig&&) = delete; + PortBasedSerializerConfig& operator=(const PortBasedSerializerConfig&) = + default; + PortBasedSerializerConfig& operator=(PortBasedSerializerConfig&&) = delete; + ~PortBasedSerializerConfig() override = default; + + LibraryRole GetLibraryRole() const; + + private: + LibraryRole _role{LIBRARY_ROLE_UNKNOWN}; +}; + +class InterfaceConfig { + public: + InterfaceConfig(LinkType linkType); // NOLINT(*explicit*) + InterfaceConfig(const InterfaceConfig&) = default; + InterfaceConfig(InterfaceConfig&&) = delete; + InterfaceConfig& operator=(const InterfaceConfig&) = default; + InterfaceConfig& operator=(InterfaceConfig&&) = delete; + virtual ~InterfaceConfig() = default; + + LinkType GetLinkType() const; + + private: + LinkType _linkType{LINK_TYPE_UNKNOWN}; +}; + +class CanInterfaceConfig : public InterfaceConfig { + public: + CanInterfaceConfig(CanNetId canNetId, CanDevId canDevId); + CanInterfaceConfig(const CanInterfaceConfig&) = default; + CanInterfaceConfig(CanInterfaceConfig&&) = delete; + CanInterfaceConfig& operator=(const CanInterfaceConfig&) = default; + CanInterfaceConfig& operator=(CanInterfaceConfig&&) = delete; + ~CanInterfaceConfig() override = default; + + CanNetId GetNetId() const; + CanDevId GetDevId() const; + + std::string GetUserIfName() const; + void SetUserIfName(IN const std::string& userIfName); + + uint32_t GetUserIfMajorVer() const; + void SetUserIfMajorVer(IN uint32_t majorVer); + + uint32_t GetUserIfMinorVer() const; + void SetUserIfMinorVer(IN uint32_t minorVer); + + uint32_t GetUserIfPatchVer() const; + void SetUserIfPatchVer(IN uint32_t patchVer); + + private: + CanNetId _canNetId{0}; + CanDevId _canDevId{0}; + std::string _userIfName; + uint32_t _userIfMajorVer{0}; + uint32_t _userIfMinorVer{0}; + uint32_t _userIfPatchVer{0}; +}; + +class Rs485InterfaceConfig : public InterfaceConfig { + public: + Rs485InterfaceConfig(SerialDevId devId); // NOLINT(*explicit*) + Rs485InterfaceConfig(const Rs485InterfaceConfig&) = default; + Rs485InterfaceConfig(Rs485InterfaceConfig&&) = delete; + Rs485InterfaceConfig& operator=(const Rs485InterfaceConfig&) = default; + Rs485InterfaceConfig& operator=(Rs485InterfaceConfig&&) = delete; + ~Rs485InterfaceConfig() override = default; + + SerialDevId GetDevId() const; + + private: + SerialDevId _devId{0}; +}; + +class IpConfig : public InterfaceConfig { + public: + IpConfig(const std::string& ip, uint16_t port, LinkType type); + IpConfig(const IpConfig&) = default; + IpConfig(IpConfig&&) = delete; + IpConfig& operator=(const IpConfig&) = default; + IpConfig& operator=(IpConfig&&) = delete; + ~IpConfig() override = default; + + std::string GetIp() const; + uint16_t GetPort() const; + + private: + std::string _ip; + uint16_t _port{0}; +}; + +class UdpConfig : public IpConfig { + public: + UdpConfig(const std::string& ip, uint16_t port); + UdpConfig(const UdpConfig&) = default; + UdpConfig(UdpConfig&&) = delete; + UdpConfig& operator=(const UdpConfig&) = default; + UdpConfig& operator=(UdpConfig&&) = delete; + ~UdpConfig() override = default; +}; + +class MulticastConfig : public IpConfig { + public: + MulticastConfig(const std::string& ip, uint16_t port); + MulticastConfig(const MulticastConfig&) = default; + MulticastConfig(MulticastConfig&&) = delete; + MulticastConfig& operator=(const MulticastConfig&) = default; + MulticastConfig& operator=(MulticastConfig&&) = delete; + ~MulticastConfig() override = default; +}; + +class ComLibConfig { + public: + ComLibConfig() = default; + ComLibConfig(uint16_t majorVer, uint16_t minorVer, uint16_t patchVer); + ComLibConfig(const ComLibConfig&) = default; + ComLibConfig(ComLibConfig&&) = delete; + ComLibConfig& operator=(const ComLibConfig&) = default; + ComLibConfig& operator=(ComLibConfig&&) = delete; + ~ComLibConfig() = default; + + LibraryRole GetRole() const; + ClientId GetClientId() const; + const std::string& GetConfigPath() const; + const std::string& GetSerialLibPath() const; + const std::string& GetUserInterfaceName() const; + uint16_t GetUserInterfaceMajorVersion() const; + uint16_t GetUserInterfaceMinorVersion() const; + uint16_t GetUserInterfacePatchVersion() const; + const std::string& GetDownloadPath() const; + SerializationType GetInstSerializationType() const; + SerializationType GetDataSerializationType() const; + ClientPhyType GetTimeSyncHwIfaceType() const; + uint8_t GetTimeSyncHwDeviceId() const; + bool IsTimeSyncSupported() const; + bool IsAliveSupported() const; + TimeSyncRole GetTimeSyncRole() const; + uint16_t GetMajorVersion() const; + uint16_t GetMinorVersion() const; + uint16_t GetPatchVersion() const; + + void SetRole(IN LibraryRole role); + void SetClientId(IN ClientId clientId); + void SetConfigPath(IN const std::string& path); + void SetSerialLibPath(IN const std::string& path); + void SetUserInterfaceName(IN const std::string& name); + void SetDownloadPath(IN const std::string& path); + void SetDataSerializationType(IN SerializationType type); + void SetInstSerializationType(IN SerializationType type); + void SetTimeSyncHwIfaceType(IN ClientPhyType hwIfType); + void SetTimeSyncHwDeviceId(IN uint8_t deviceId); + void SetTimeSyncSupport(IN bool isSupported); + void SetTimeSyncRole(IN TimeSyncRole role); + void SetAliveSupport(IN bool support); + void SetUserInterfaceMajorVersion(uint16_t majorVer); + void SetUserInterfaceMinorVersion(uint16_t minorVer); + void SetUserInterfacePatchVersion(uint16_t patchVer); + void SetComLibConfigMajorVersion(uint16_t majorVer); + void SetComLibConfigMinorVersion(uint16_t minorVer); + void SetComLibConfigPatchVersion(uint16_t patchVer); + + private: + uint16_t _majorVer{0}; + uint16_t _minorVer{0}; + uint16_t _patchVer{0}; + std::string _serialLibPath; + std::string _configFilesPath; + ClientId _clientId{0}; + LibraryRole _role{LIBRARY_ROLE_UNKNOWN}; + std::string _userIfName; + std::string _downloadPath; + SerializationType _instSerialization{SERIALIZATION_TYPE_UNKNOWN}; + SerializationType _dataSerialization{SERIALIZATION_TYPE_UNKNOWN}; + ClientPhyType _timeSyncHwIfaceType{CLIENT_PHY_UNKNOWN}; + uint8_t _timeSyncHwDevId{}; + bool _isTimeSyncSupported{false}; + TimeSyncRole _timeSyncRole{TIME_SYNC_ROLE_UNKNOWN}; + bool _isAliveSupported{false}; + uint16_t _userIfMajorVer{0}; + uint16_t _userIfMinorVer{0}; + uint16_t _userIfPatchVer{0}; +}; + +class SWUpdateConfigData { + public: + void SetData(IN bool pathFlag, IN std::string pathStr, IN bool dataFlag, + IN int32_t dataSize, IN uint8_t dataEncrypt, + IN uint8_t* dataPtr); + + void cleanData(); + + bool GetPathAvailableFlag() const; + std::string GetSegmentFilePath() const; + bool GetDataPtrValidFlag() const; + uint8_t GetSegmentEncryption() const; + int32_t GetSegmentBufferSize() const; + uint8_t* GetSegmentDataPtr(); + + private: + bool _pathAvailableFlag{false}; + std::string _segmentPath; + bool _dataPtrValidFlag{false}; + int32_t _segmentBufferSize{0}; + uint8_t _segmentEncryption{0}; + uint8_t* _segmentDataPtr{nullptr}; +}; + +class InternalTransportHeader { + public: + com::types::ProtocolType GetProtocolType() const; + void SetProtocolType(IN com::types::ProtocolType protoType); + + bool HasSrcClientId() const; + ClientId GetSrcClientId() const; + void SetSrcClientId(IN ClientId clientId); + + bool HasDstClientId() const; + ClientId GetDstClientId() const; + void SetDstClientId(IN ClientId clientId); + + private: + com::types::ProtocolType _protoType{PROTOCOL_TYPE_UNKNOWN}; + ClientId _srcClientId{0}; + bool _srcClientIdValid{false}; + ClientId _dstClientId{0}; + bool _dstClientIdValid{false}; +}; + +} // namespace types +} // namespace com + +#endif // COM_TYPES_H diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/src/smartmicro_radar_node.cpp new file mode 100644 index 0000000..41b3e4a --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -0,0 +1,4298 @@ +// Copyright 2021 Apex.AI, 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. +// The initial version of the code was developed by Apex.AI and +// was thereafter adapted and extended by smartmicro. + +#include "umrr_ros2_driver/smartmicro_radar_node.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "umrr_ros2_driver/config_path.hpp" + +using com::common::Instruction; +using com::master::CmdRequest; +using com::master::CommunicationServicesIface; +using com::master::GetParamRequest; +using com::master::GetStatusRequest; +using com::master::InstructionBatch; +using com::master::InstructionServiceIface; +using com::master::Response; +using com::master::ResponseBatch; +using com::master::SetParamRequest; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using std::literals::string_view_literals::operator""sv; + +namespace +{ +constexpr auto kMseType = "mse"sv; +constexpr auto kTargetType = "target"sv; +constexpr auto kEthLink = "eth"sv; +constexpr auto kCanLink = "can"sv; + +constexpr auto kDefaultClientId = 0; +constexpr auto kDefaultInterfaceName = "lo"; +constexpr auto kDefaultIp = "127.0.0.1"; +constexpr auto kDefaultPort = 55555; +constexpr auto kDefaultHistorySize = 10; +constexpr auto kDefaultFrameId = "umrr"; +constexpr auto kDefaultSensorType = "umrr11"; +constexpr auto kDefaultInstType = "port_based"; +constexpr auto kDefaultDataType = "port_based"; + +constexpr auto kDefaultHwDevId = 1; +constexpr auto kDefaultHwDevIface = "slcan"; +constexpr auto kDefaultHwLinkType = "can"; + +constexpr auto kHwDevLinkTag = "type"; +constexpr auto kClientLinkTag = "link_type"; +constexpr auto kHwDevIdTag = "dev_id"; +constexpr auto kHwDevIfaceNameTag = "iface_name"; + +constexpr auto kHwDevPortTag = "hw_port"; + +constexpr auto kClientIdTag = "client_id"; +constexpr auto kPortTag = "port"; +constexpr auto kBaudRateTag = "baudrate"; +constexpr auto kIpTag = "ip"; + +constexpr auto kInstSerialTypeTag = "master_inst_serial_type"; +constexpr auto kDataSerialTypeTag = "master_data_serial_type"; + +constexpr auto kInstSerialTypeJsonTag = "instruction_serialization_type"; +constexpr auto kDataSerialTypeJsonTag = "data_serialization_type"; + +constexpr auto kClientsJsonTag = "clients"; +constexpr auto kHwItemsJsonTag = "hwItems"; +constexpr auto kUINameTag = "user_interface_name"; +constexpr auto kUIMajorVTag = "user_interface_major_v"; +constexpr auto kUIMinorVTag = "user_interface_minor_v"; +constexpr auto kUIPatchVTag = "user_interface_patch_v"; + +constexpr bool float_eq(const float a, const float b) noexcept +{ + const auto maximum = std::max(std::fabs(a), std::fabs(b)); + return std::fabs(a - b) <= maximum * std::numeric_limits::epsilon(); +} +struct RadarPoint +{ + float x{}; + float y{}; + float z{}; + float radial_speed{}; + float power{}; + float rcs{}; + float noise{}; + float snr{}; + float azimuth_angle{}; + float elevation_angle{}; + float range{}; + constexpr friend bool operator==(const RadarPoint & p1, const RadarPoint & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && float_eq(p1.z, p2.z) && + float_eq(p1.radial_speed, p2.radial_speed) && float_eq(p1.power, p2.power) && + float_eq(p1.rcs, p2.rcs) && float_eq(p1.noise, p2.noise) && float_eq(p1.snr, p2.snr) && + float_eq(p1.azimuth_angle, p2.azimuth_angle) && + float_eq(p1.elevation_angle, p2.elevation_angle) && float_eq(p1.range, p2.range); + } +}; + +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(radial_speed); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(power); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(rcs); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(noise); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(snr); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth_angle); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation_angle); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(range); +using Generators = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, field_radial_speed_generator, field_power_generator, + field_rcs_generator, field_noise_generator, field_snr_generator, field_azimuth_angle_generator, + field_elevation_angle_generator, field_range_generator>; +using RadarCloudModifier = PointCloud2Modifier; + +struct ObjectPoint +{ + float x{}; + float y{}; + float z{}; + float speed_absolute{}; + float heading{}; + float length{}; + float mileage{}; + float quality{}; + float acceleration{}; + int16_t object_id{}; + uint16_t idle_cycles{}; + uint16_t spline_idx{}; + uint8_t object_class{}; + uint16_t status{}; + + constexpr friend bool operator==(const ObjectPoint & p1, const ObjectPoint & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && float_eq(p1.z, p2.z) && + float_eq(p1.speed_absolute, p2.speed_absolute) && float_eq(p1.heading, p2.heading) && + float_eq(p1.length, p2.length) && float_eq(p1.mileage, p2.mileage) && + float_eq(p1.quality, p2.quality) && float_eq(p1.acceleration, p2.acceleration); + } +}; + +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(speed_absolute); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(heading); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(length); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(mileage); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(quality); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(acceleration); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(object_id); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(idle_cycles); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(spline_idx); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(object_class); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(status); +using GeneratorsObjectPoint = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, field_speed_absolute_generator, + field_heading_generator, field_length_generator, field_mileage_generator, field_quality_generator, + field_acceleration_generator, field_object_id_generator, field_idle_cycles_generator, + field_spline_idx_generator, field_object_class_generator, field_status_generator>; + +using ObjectPointCloudModifier = PointCloud2Modifier; +} // namespace + +namespace smartmicro +{ +namespace drivers +{ +namespace radar +{ +SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node{"smartmicro_radar_node", node_options} +{ + update_config_files_from_params(); + update_service = std::make_shared(); + + const auto override = false; + setenv("SMART_ACCESS_CFG_FILE_PATH", kConfigFilePath, override); + + initialize_services(); + setup_publishers(); + + rclcpp::on_shutdown(std::bind(&SmartmicroRadarNode::on_shutdown_callback, this)); +} + +void SmartmicroRadarNode::initialize_services() +{ + // Getting the communication services + m_services = CommunicationServicesIface::Get(); + if (!m_services->Init()) { + throw std::runtime_error("Communication Service initialization failed"); + } + + // Getting the data stream service + data_umrra4_v1_0_1 = com::master::umrra4_automotive_v1_0_1::DataStreamServiceIface::Get(); + data_umrra4_v1_2_1 = com::master::umrra4_automotive_v1_2_1::DataStreamServiceIface::Get(); + data_umrra4_v1_4_0 = com::master::umrra4_automotive_v1_4_0::DataStreamServiceIface::Get(); + data_umrr11 = com::master::umrr11_t132_automotive_v1_1_2::DataStreamServiceIface::Get(); + data_umrr96 = com::master::umrr96_t153_automotive_v1_2_2::DataStreamServiceIface::Get(); + data_umrr9f_v1_1_1 = com::master::umrr9f_t169_automotive_v1_1_1::DataStreamServiceIface::Get(); + data_umrr9f_v2_0_0 = com::master::umrr9f_t169_automotive_v2_0_0::DataStreamServiceIface::Get(); + data_umrr9f_v2_1_1 = com::master::umrr9f_t169_automotive_v2_1_1::DataStreamServiceIface::Get(); + data_umrr9f_v2_2_1 = com::master::umrr9f_t169_automotive_v2_2_1::DataStreamServiceIface::Get(); + data_umrr9f_v2_4_1 = com::master::umrr9f_t169_automotive_v2_4_1::DataStreamServiceIface::Get(); + data_umrr9f_v3_0_0 = com::master::umrr9f_t169_automotive_v3_0_0::DataStreamServiceIface::Get(); + data_umrr9d_v1_0_3 = com::master::umrr9d_t152_automotive_v1_0_3::DataStreamServiceIface::Get(); + data_umrr9d_v1_2_2 = com::master::umrr9d_t152_automotive_v1_2_2::DataStreamServiceIface::Get(); + data_umrr9d_v1_4_1 = com::master::umrr9d_t152_automotive_v1_4_1::DataStreamServiceIface::Get(); + data_umrr9d_v1_5_0 = com::master::umrr9d_t152_automotive_v1_5_0::DataStreamServiceIface::Get(); + data_umrr9f_mse_v1_0_0 = com::master::umrr9f_t169_mse_v1_0_0::DataStreamServiceIface::Get(); + data_umrr9f_mse_v1_1_0 = com::master::umrr9f_t169_mse_v1_1_0::DataStreamServiceIface::Get(); + data_umrr9f_mse_v1_3_0 = com::master::umrr9f_t169_mse_v1_3_0::DataStreamServiceIface::Get(); + data_umrra4_mse_v1_0_0 = com::master::umrra4_mse_v1_0_0::DataStreamServiceIface::Get(); + data_umrra4_mse_v2_1_0 = com::master::umrra4_mse_v2_1_0::DataStreamServiceIface::Get(); + data_umrra1_v1_0_0 = com::master::umrra1_t166_b_automotive_v1_0_0::DataStreamServiceIface::Get(); + data_umrra1_v2_0_0 = com::master::umrra1_t166_b_automotive_v2_0_0::DataStreamServiceIface::Get(); + + // Wait for initailization + std::this_thread::sleep_for(std::chrono::seconds(2)); + RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); + + // create a ros2 service to change the radar parameters + mode_srv_ = create_service( + "smart_radar/set_radar_mode", + std::bind( + &SmartmicroRadarNode::set_radar_mode, this, std::placeholders::_1, std::placeholders::_2)); + + // create a ros2 service to change the IP address + ip_addr_srv_ = create_service( + "smart_radar/set_ip_address", + std::bind( + &SmartmicroRadarNode::ip_address, this, std::placeholders::_1, std::placeholders::_2)); + + // create a ros2 service to send command to radar + command_srv_ = create_service( + "smart_radar/send_command", + std::bind( + &SmartmicroRadarNode::radar_command, this, std::placeholders::_1, std::placeholders::_2)); + + // create a ros2 service to perform firmware download + download_srv_ = create_service( + "smart_radar/firmware_download", + std::bind( + &SmartmicroRadarNode::firmware_download, this, std::placeholders::_1, std::placeholders::_2)); + + // create a ros2 service to read the radar status + status_srv_ = create_service( + "smart_radar/get_radar_status", + std::bind( + &SmartmicroRadarNode::get_radar_status, this, std::placeholders::_1, std::placeholders::_2)); + + // create a ros2 service to read the radar modes + read_mode_srv_ = create_service( + "smart_radar/get_radar_mode", + std::bind( + &SmartmicroRadarNode::get_radar_mode, this, std::placeholders::_1, std::placeholders::_2)); + + RCLCPP_INFO(this->get_logger(), "Radar services are ready."); +} + +void SmartmicroRadarNode::setup_publishers() +{ + const auto validate_sensor = [](const auto & sensor) { + std::string_view pub_type{sensor.pub_type}; + std::string_view model{sensor.model}; + + const bool is_mse = (pub_type == kMseType); + const bool has_mse = (model.find(kMseType) != std::string_view::npos); + + if (is_mse == !has_mse) { + throw std::runtime_error( + std::string("Model name ") + (is_mse ? "must" : "must not") + + " contain 'mse' when pub_type is '" + std::string(pub_type) + "'"); + } + }; + + for (size_t i = 0; i < m_number_of_sensors; ++i) { + const auto & sensor = m_sensors[i]; + + validate_sensor(sensor); + + std::string_view link_type{sensor.link_type}; + if (link_type == kEthLink) { + port_publishers(sensor, i); + } else if (link_type == kCanLink) { + can_publishers(sensor, i); + } else { + RCLCPP_WARN(get_logger(), "Unknown link type for sensor %zu", i); + } + } +} + +void SmartmicroRadarNode::port_publishers(const detail::SensorConfig & sensor, size_t sensor_idx) +{ + std::string_view pub_type{m_sensors[sensor_idx].pub_type}; + + try { + if (pub_type == kMseType) { + m_publishers_obj[sensor_idx] = create_publisher( + "smart_radar/port_objects_" + std::to_string(sensor_idx), sensor.history_size); + m_publishers_port_obj_header[sensor_idx] = + create_publisher( + "smart_radar/port_objectheader_" + std::to_string(sensor_idx), sensor.history_size); + m_publishers[sensor_idx] = create_publisher( + "smart_radar/port_targets_" + std::to_string(sensor_idx), sensor.history_size); + m_publishers_port_target_header[sensor_idx] = + create_publisher( + "smart_radar/port_targetheader_" + std::to_string(sensor_idx), sensor.history_size); + + } else if (pub_type == kTargetType) { + m_publishers[sensor_idx] = create_publisher( + "smart_radar/port_targets_" + std::to_string(sensor_idx), sensor.history_size); + m_publishers_port_target_header[sensor_idx] = + create_publisher( + "smart_radar/port_targetheader_" + std::to_string(sensor_idx), sensor.history_size); + } else { + RCLCPP_ERROR(get_logger(), "Unknown publish type: %s", sensor.pub_type.c_str()); + throw std::invalid_argument("Unknown publish type"); + } + + RCLCPP_INFO(get_logger(), "Successfully created PORT publishers for sensor %zu", sensor_idx); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_logger(), "Failed to create publishers for sensor %zu: %s", sensor_idx, e.what()); + throw; + } + + if (sensor.model == "umrra4_mse_v2_1_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v2_1_0->RegisterComObjectListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::objectlist_callback_umrra4_mse_v2_1_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register objectlist callback for sensor umrra4_mse_v2_1_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v2_1_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra4_mse_v2_1_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrra4_mse_v2_1_0"); + } + } + if (sensor.model == "umrra4_mse_v1_0_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v1_0_0->RegisterComObjectListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::objectlist_callback_umrra4_mse_v1_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register objectlist callback for sensor umrra4_mse_v1_0_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v1_0_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra4_mse_v1_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrra4_mse_v1_0_0"); + } + } + if (sensor.model == "umrr9f_mse_v1_3_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_3_0->RegisterComObjectListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::objectlist_callback_umrr9f_mse_v1_3_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register objectlist callback for sensor umrr9f_mse_v1_3_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_3_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_mse_v1_3_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_mse_v1_3_0"); + } + } + if (sensor.model == "umrr9f_mse_v1_1_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_1_0->RegisterComObjectListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::objectlist_callback_umrr9f_mse_v1_1_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register objectlist callback for sensor umrr9f_mse_v1_1_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_1_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_mse_v1_1_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_mse_v1_1_0"); + } + } + if (sensor.model == "umrr9f_mse_v1_0_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_0_0->RegisterComObjectListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::objectlist_callback_umrr9f_mse_v1_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register objectlist callback for sensor umrr9f_mse_v1_0_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_0_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_mse_v1_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_mse_v1_0_0"); + } + } + if ( + sensor.model == "umrr96_v1_2_2" && + com::types::ERROR_CODE_OK != + data_umrr96->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr96, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr96_v1_2_2"); + } + if ( + sensor.model == "umrr11_v1_1_2" && + com::types::ERROR_CODE_OK != + data_umrr11->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr11, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr11_v1_1_2"); + } + if ( + sensor.model == "umrr9f_v1_1_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v1_1_1->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_v1_1_1"); + } + if ( + sensor.model == "umrr9f_v2_0_0" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_0_0->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_v2_0_0"); + } + if ( + sensor.model == "umrr9f_v2_1_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_1_1->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_v2_1_1"); + } + if ( + sensor.model == "umrr9f_v2_2_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_2_1->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_v2_2_1"); + } + if ( + sensor.model == "umrr9f_v2_4_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_4_1->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_4_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_v2_4_1"); + } + if ( + sensor.model == "umrr9f_v3_0_0" && + com::types::ERROR_CODE_OK != + data_umrr9f_v3_0_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_v3_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9f_v3_0_0"); + } + if ( + sensor.model == "umrr9d_v1_0_3" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_0_3->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_3, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9d_v1_0_3"); + } + if ( + sensor.model == "umrr9d_v1_2_2" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_2_2->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_2, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9d_v1_2_2"); + } + if ( + sensor.model == "umrr9d_v1_4_1" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_4_1->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_4_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9d_v1_4_1"); + } + if ( + sensor.model == "umrr9d_v1_5_0" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_5_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_5_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrr9d_v1_5_0"); + } + if ( + sensor.model == "umrra4_v1_0_1" && + com::types::ERROR_CODE_OK != + data_umrra4_v1_0_1->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrra4_v1_0_1"); + } + if ( + sensor.model == "umrra4_v1_2_1" && + com::types::ERROR_CODE_OK != + data_umrra4_v1_2_1->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra4_v1_2_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrra4_v1_2_1"); + } + if ( + sensor.model == "umrra4_v1_4_0" && + com::types::ERROR_CODE_OK != + data_umrra4_v1_4_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra4_v1_4_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrra4_v1_4_0"); + } + if ( + sensor.model == "umrra1_v1_0_0" && + com::types::ERROR_CODE_OK != + data_umrra1_v1_0_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra1_v1_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrra1_v1_0_0"); + } + if ( + sensor.model == "umrra1_v2_0_0" && + com::types::ERROR_CODE_OK != + data_umrra1_v2_0_0->RegisterComTargetListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra1_v2_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register targetlist callback for sensor umrra1_v2_0_0"); + } +} + +void SmartmicroRadarNode::can_publishers(const detail::SensorConfig & sensor, size_t sensor_idx) +{ + std::string_view pub_type{m_sensors[sensor_idx].pub_type}; + + try { + if (pub_type == kMseType) { + m_publishers_obj[sensor_idx] = create_publisher( + "smart_radar/can_objects_" + std::to_string(sensor_idx), sensor.history_size); + m_publishers_can_obj_header[sensor_idx] = + create_publisher( + "smart_radar/can_objectheader_" + std::to_string(sensor_idx), sensor.history_size); + + m_publishers[sensor_idx] = create_publisher( + "smart_radar/can_targets_" + std::to_string(sensor_idx), sensor.history_size); + m_publishers_can_target_header[sensor_idx] = + create_publisher( + "smart_radar/can_targetheader_" + std::to_string(sensor_idx), sensor.history_size); + } else if (pub_type == kTargetType) { + m_publishers[sensor_idx] = create_publisher( + "smart_radar/can_targets_" + std::to_string(sensor_idx), sensor.history_size); + m_publishers_can_target_header[sensor_idx] = + create_publisher( + "smart_radar/can_targetheader_" + std::to_string(sensor_idx), sensor.history_size); + } else { + RCLCPP_INFO(this->get_logger(), "Unkwon publish type!"); + } + + RCLCPP_INFO(get_logger(), "Successfully created CAN publishers for sensor %zu", sensor_idx); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_logger(), "Failed to create publishers for sensor %zu: %s", sensor_idx, e.what()); + throw; + } + + if (sensor.model == "umrra4_can_mse_v2_1_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v2_1_0->RegisterComObjectBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_objectlist_callback_umrra4_mse_v2_1_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register objectlist callback for sensor umrra4_can_mse_v2_1_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v2_1_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrra4_mse_v2_1_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register targetlist callback for sensor umrra4_can_mse_v2_1_0"); + } + } + if (sensor.model == "umrra4_can_mse_v1_0_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v1_0_0->RegisterComObjectBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_objectlist_callback_umrra4_mse_v1_0_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register objectlist callback for sensor umrra4_can_mse_v1_0_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrra4_mse_v1_0_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrra4_mse_v1_0_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register targetlist callback for sensor umrra4_can_mse_v1_0_0"); + } + } + if (sensor.model == "umrr9f_can_mse_v1_3_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_3_0->RegisterComObjectBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_objectlist_callback_umrr9f_mse_v1_3_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register objectlist callback for sensor umrr9f_can_mse_v1_3_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_3_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_mse_v1_3_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register targetlist callback for sensor umrr9f_can_mse_v1_3_0"); + } + } + if (sensor.model == "umrr9f_can_mse_v1_1_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_1_0->RegisterComObjectBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_objectlist_callback_umrr9f_mse_v1_1_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register objectlist callback for sensor umrr9f_can_mse_v1_1_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_1_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_mse_v1_1_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register targetlist callback for sensor umrr9f_can_mse_v1_1_0"); + } + } + if (sensor.model == "umrr9f_can_mse_v1_0_0") { + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_0_0->RegisterComObjectBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_objectlist_callback_umrr9f_mse_v1_0_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register objectlist callback for sensor umrr9f_can_mse_v1_0_0"); + } + if ( + com::types::ERROR_CODE_OK != + data_umrr9f_mse_v1_0_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_mse_v1_0_0, this, + sensor_idx, std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), + "Failed to register targetlist callback for sensor umrr9f_can_mse_v1_0_0"); + } + } + if ( + sensor.model == "umrr96_can_v1_2_2" && + com::types::ERROR_CODE_OK != + data_umrr96->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr96, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr96_can_v1_2_2"); + } + if ( + sensor.model == "umrr11_can_v1_1_2" && + com::types::ERROR_CODE_OK != + data_umrr11->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr11, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr11_can_v1_1_2"); + } + if ( + sensor.model == "umrr9f_can_v2_1_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_1_1->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr9f_can_v2_1_1"); + } + if ( + sensor.model == "umrr9f_can_v2_2_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_2_1->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr9f_can_v2_2_1"); + } + if ( + sensor.model == "umrr9f_can_v2_4_1" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_4_1->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_4_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr9f_can_v2_4_1"); + } + if ( + sensor.model == "umrr9f_can_v3_0_0" && + com::types::ERROR_CODE_OK != + data_umrr9f_v3_0_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v3_0_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr9f_can_v3_0_0"); + } + if ( + sensor.model == "umrr9d_can_v1_0_3" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_0_3->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr9d_can_v1_0_3"); + } + if ( + sensor.model == "umrr9d_can_v1_2_2" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_2_2->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr9d_can_v1_2_2"); + } + if ( + sensor.model == "umrr9d_can_v1_4_1" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_4_1->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_4_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr9d_can_v1_4_1"); + } + if ( + sensor.model == "umrr9d_can_v1_5_0" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_5_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_5_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrr9d_can_v1_5_0"); + } + if ( + sensor.model == "umrra4_can_v1_0_1" && + com::types::ERROR_CODE_OK != + data_umrra4_v1_0_1->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrra4_can_v1_0_1"); + } + if ( + sensor.model == "umrra4_can_v1_2_1" && + com::types::ERROR_CODE_OK != + data_umrra4_v1_2_1->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_2_1, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrra4_can_v1_2_1"); + } + if ( + sensor.model == "umrra4_can_v1_4_0" && + com::types::ERROR_CODE_OK != + data_umrra4_v1_4_0->RegisterComTargetBaseListReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_4_0, this, sensor_idx, + std::placeholders::_1, std::placeholders::_2))) { + RCLCPP_INFO( + this->get_logger(), "Failed to register CAN targetlist for sensor umrra4_can_v1_4_0"); + } +} + +void SmartmicroRadarNode::on_shutdown_callback() +{ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Shutdown called!"); + check_signal = true; + rclcpp::Rate sleepRate(std::chrono::milliseconds(100)); + sleepRate.sleep(); + m_services.reset(); +} + +void SmartmicroRadarNode::firmware_download( + const std::shared_ptr request, + std::shared_ptr result) +{ + bool check_flag_id = false; + client_id = request->sensor_id; + update_image = request->file_path; + + for (auto & sensor : m_sensors) { + if (client_id == sensor.id) { + check_flag_id = true; + break; + } + } + if (!check_flag_id) { + result->res = "The sensor ID value entered is invalid! "; + return; + } + + update_service->StartSoftwareUpdate(client_id, update_image); + result->res = "Service ended, check the console output for status! "; +} + +void SmartmicroRadarNode::set_radar_mode( + const std::shared_ptr request, + std::shared_ptr result) +{ + // Validate sensor ID + bool check_flag_id = false; + client_id = request->sensor_id; + for (auto & sensor : m_sensors) { + if (client_id == sensor.id) { + check_flag_id = true; + break; + } + } + if (!check_flag_id) { + result->res = "Error: Sensor ID is invalid! "; + return; + } + + auto section_name = request->section_name; + if ( + section_name != "auto_interface_0dim" && section_name != "auto_interface_rrm" && + section_name != "Parameter") { + result->res = + "Error: Invalid section name specified! Must be 'auto_interface_0dim', " + "'auto_interface_rrm', or 'Parameter'."; + return; + } + + // Check arrays have same length + if ( + request->params.size() != request->values.size() || + request->params.size() != request->value_types.size()) { + result->res = "Error: param, values and value_types arrays must have same length"; + return; + } + + std::shared_ptr inst{m_services->GetInstructionService()}; + if (!inst) { + result->res = "Error: Failed to get instruction service"; + return; + } + + timer = this->create_wall_timer( + std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + + std::shared_ptr batch; + if (!inst->AllocateInstructionBatch(client_id, batch)) { + result->res = "Error: Failed to allocate instruction! "; + return; + } + + for (size_t i = 0; i < request->params.size(); i++) { + const auto & param = request->params[i]; + const auto & value = request->values[i]; + const auto & value_type = request->value_types[i]; + bool request_added = false; + try { + switch (value_type) { + case 0: { + float float_value = std::stof(value); + auto radar_mode_float = + std::make_shared>(section_name, param, float_value); + request_added = batch->AddRequest(radar_mode_float); + break; + } + case 1: { + if (value.find('.') != std::string::npos) { + result->res = "Error: uint32 value cannot contain decimal points"; + return; + } + uint32_t u32_value = static_cast(std::stoul(value)); + auto radar_mode_u32 = + std::make_shared>(section_name, param, u32_value); + request_added = batch->AddRequest(radar_mode_u32); + break; + } + case 2: { + if (value.find('.') != std::string::npos) { + result->res = "Error: uint16 value cannot contain decimal points"; + return; + } + uint64_t temp = std::stoul(value); + if (temp > 65535) { + result->res = "Error: uint16 value must be between 0 and 65535"; + return; + } + uint16_t u16_value = static_cast(temp); + auto radar_mode_u16 = + std::make_shared>(section_name, param, u16_value); + request_added = batch->AddRequest(radar_mode_u16); + break; + } + case 3: { + if (value.find('.') != std::string::npos) { + result->res = "Error: uint8 value cannot contain decimal points"; + return; + } + uint64_t temp = std::stoul(value); + if (temp > 255) { + result->res = "Error: uint8 value must be between 0 and 255"; + return; + } + uint8_t u8_value = static_cast(temp); + auto radar_mode_u8 = + std::make_shared>(section_name, param, u8_value); + request_added = batch->AddRequest(radar_mode_u8); + break; + } + default: + result->res = + "Error: Invalid value_type specified. Must be 0 (f32), 1 (u32), 2 (u16), 3 (u8)"; + return; + } + } catch (const std::invalid_argument & e) { + result->res = "Error: Failed to convert value string, invalid format"; + return; + } catch (const std::out_of_range & e) { + result->res = "Error: Value is out of range for the specified type"; + return; + } + + if (!request_added) { + result->res = "Error: Failed to add instruction '" + param + "'! "; + return; + } + } + + if ( + com::types::ERROR_CODE_OK != + inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::mode_response, this, client_id, std::placeholders::_2, + request->params, section_name))) { + result->res = "Error: Check params are valid for this sensor and values within range!"; + return; + } + result->res = "Success: Request sent successfully. Check main terminal for sensor response!"; + RCLCPP_INFO(this->get_logger(), "Service call result: %s", result->res.c_str()); +} + +void SmartmicroRadarNode::ip_address( + const std::shared_ptr request, + std::shared_ptr result) +{ + std::shared_ptr inst{m_services->GetInstructionService()}; + timer = this->create_wall_timer( + std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + bool check_flag = false; + client_id = request->sensor_id; + for (auto & sensor : m_sensors) { + if (client_id == sensor.id) { + check_flag = true; + break; + } + } + if (!check_flag) { + result->res_ip = "Sensor ID entered is not listed in the param file! "; + return; + } + + std::shared_ptr batch; + if (!inst->AllocateInstructionBatch(client_id, batch)) { + result->res_ip = "Failed to allocate instruction! "; + return; + } + + std::shared_ptr> ip_address = + std::make_shared>( + "auto_interface_0dim", "ip_source_address", request->value_ip); + + std::shared_ptr cmd = + std::make_shared("auto_interface_command", "comp_eeprom_ctrl_save_param_sec", 2010); + + if (!batch->AddRequest(ip_address)) { + result->res_ip = "Failed to add instruction! "; + return; + } + if (!batch->AddRequest(cmd)) { + result->res_ip = "Failed to add instruction! "; + return; + } + // send instruction batch to the device + if ( + com::types::ERROR_CODE_OK != + inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::sensor_response_ip, this, client_id, std::placeholders::_2))) { + result->res_ip = "Service not conducted"; + return; + } else { + RCLCPP_INFO( + this->get_logger(), + "Radar must be restarted and the parameters in the param file " + "must be updated !!."); + result->res_ip = + "Success: IP change executed successfully. Radar must be restarted " + "and the parameters in the param file must be updated"; + } +} + +void SmartmicroRadarNode::radar_command( + const std::shared_ptr request, + std::shared_ptr result) +{ + std::string command_name{}; + bool check_flag_id = false; + + command_name = request->command; + client_id = request->sensor_id; + + for (auto & sensor : m_sensors) { + if (client_id == sensor.id) { + check_flag_id = true; + break; + } + } + if (!check_flag_id) { + result->res = "The sensor ID value entered is invalid! "; + return; + } + + auto section_name = request->section_name; + if ( + section_name != "auto_interface_command" && section_name != "auto_interface_rrm_command" && + section_name != "Command") { + result->res = + "Error: Invalid section name specified! Must be 'auto_interface_command', " + "'auto_interface_rrm_command', or 'Command'."; + return; + } + + std::shared_ptr inst{m_services->GetInstructionService()}; + timer = this->create_wall_timer( + std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + + std::shared_ptr batch; + + if (!inst->AllocateInstructionBatch(client_id, batch)) { + result->res = "Failed to allocate instruction! "; + return; + } + + std::shared_ptr radar_command = + std::make_shared(section_name, request->command, request->value); + + if (!batch->AddRequest(radar_command)) { + result->res = "Failed to add instruction! "; + return; + } + + if ( + com::types::ERROR_CODE_OK != inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::command_response, this, client_id, + std::placeholders::_2, command_name, section_name))) { + result->res = "Error in sending command to the sensor!"; + return; + } + result->res = "Success: Request sent successfully."; +} + +void SmartmicroRadarNode::get_radar_status( + const std::shared_ptr request, + std::shared_ptr result) +{ + // Validate sensor ID + bool check_flag_id = false; + client_id = request->sensor_id; + for (auto & sensor : m_sensors) { + if (client_id == sensor.id) { + check_flag_id = true; + break; + } + } + if (!check_flag_id) { + result->res = "Error: Sensor ID is invalid! "; + return; + } + + auto section_name = request->section_name; + if ( + section_name != "auto_interface" && section_name != "auto_interface_rrm" && + section_name != "Status") { + result->res = + "Error: Invalid section name specified! Must be 'auto_interface', 'auto_interface_rrm', or " + "'Status'."; + return; + } + + // Check arrays have same length + if (request->statuses.size() != request->status_types.size()) { + result->res = "Error: status and status_types arrays must have same length"; + return; + } + + std::shared_ptr inst{m_services->GetInstructionService()}; + if (!inst) { + result->res = "Error: Failed to get instruction service"; + return; + } + + timer = this->create_wall_timer( + std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + + std::shared_ptr batch; + if (!inst->AllocateInstructionBatch(client_id, batch)) { + result->res = "Error: Failed to allocate instruction! "; + return; + } + + for (size_t i = 0; i < request->statuses.size(); i++) { + const auto & status = request->statuses[i]; + const auto & status_type = request->status_types[i]; + bool request_added = false; + + switch (status_type) { + case 0: { + auto radar_status_u32 = std::make_shared>(section_name, status); + request_added = batch->AddRequest(radar_status_u32); + break; + } + case 1: { + auto radar_status_u16 = std::make_shared>(section_name, status); + request_added = batch->AddRequest(radar_status_u16); + break; + } + default: + result->res = "Error: Invalid value_type specified. Must be 0 (u32) or 1 (u8)"; + return; + } + + if (!request_added) { + result->res = "Error: Failed to add instruction '" + status + "' ! "; + return; + } + } + + if ( + com::types::ERROR_CODE_OK != + inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::status_response, this, client_id, std::placeholders::_2, + request->statuses, section_name))) { + result->res = "Error: Check status are valid for this sensor!"; + return; + } + result->res = "Success: Request sent successfully. Check main terminal for sensor response!"; +} + +void SmartmicroRadarNode::get_radar_mode( + const std::shared_ptr request, + std::shared_ptr result) +{ + // Validate sensor ID + bool check_flag_id = false; + client_id = request->sensor_id; + for (auto & sensor : m_sensors) { + if (client_id == sensor.id) { + check_flag_id = true; + break; + } + } + if (!check_flag_id) { + result->res = "Error: Sensor ID is invalid! "; + return; + } + + auto section_name = request->section_name; + if ( + section_name != "auto_interface_0dim" && section_name != "auto_interface_rrm" && + section_name != "Parameter") { + result->res = + "Error: Invalid section name specified! Must be 'auto_interface_0dim', 'auto_interface_rrm', " + "or 'Parameter'."; + return; + } + + // Check arrays have same length + if (request->params.size() != request->param_types.size()) { + result->res = "Error: param and value_types arrays must have same length"; + return; + } + + std::shared_ptr inst{m_services->GetInstructionService()}; + if (!inst) { + result->res = "Error: Failed to get instruction service"; + return; + } + + timer = this->create_wall_timer( + std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + + std::shared_ptr batch; + if (!inst->AllocateInstructionBatch(client_id, batch)) { + result->res = "Error: Failed to allocate instruction! "; + return; + } + + for (size_t i = 0; i < request->params.size(); i++) { + const auto & param = request->params[i]; + const auto & param_type = request->param_types[i]; + bool request_added = false; + + switch (param_type) { + case 0: { + auto radar_param_float = std::make_shared>(section_name, param); + request_added = batch->AddRequest(radar_param_float); + break; + } + case 1: { + auto radar_param_u32 = std::make_shared>(section_name, param); + request_added = batch->AddRequest(radar_param_u32); + break; + } + case 2: { + auto radar_param_u16 = std::make_shared>(section_name, param); + request_added = batch->AddRequest(radar_param_u16); + break; + } + case 3: { + auto radar_param_u8 = std::make_shared>(section_name, param); + request_added = batch->AddRequest(radar_param_u8); + break; + } + default: + result->res = + "Error: Invalid value_type specified. Must be 0(u32), 1(u16), 2(u8) or 3(float)"; + return; + } + + if (!request_added) { + result->res = "Error: Failed to add instruction '" + param + "' Check param types match! "; + return; + } + } + + if ( + com::types::ERROR_CODE_OK != + inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::param_response, this, client_id, std::placeholders::_2, + request->params, section_name))) { + result->res = "Error: Check params are valid for this sensor!"; + return; + } + result->res = "Success: Request sent successfully. Check main terminal for sensor response!"; +} + +void SmartmicroRadarNode::mode_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::vector & instruction_names, const std::string & section_name) +{ + for (const auto & instruction_name : instruction_names) { + std::vector>> resp_u8; + std::vector>> resp_u32; + std::vector>> resp_f; + bool response_found = false; + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u8)) { + response_found = true; + for (auto & resp : resp_u8) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u32)) { + response_found = true; + for (auto & resp : resp_u32) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_f)) { + response_found = true; + for (auto & resp : resp_f) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %f\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (!response_found) { + RCLCPP_WARN(this->get_logger(), "No response received!"); + } + } +} + +void SmartmicroRadarNode::sensor_response_ip( + const com::types::ClientId client_id, + const std::shared_ptr & response) +{ + std::vector>> resp_ip; + if (response->GetResponse("auto_interface_0dim", "ip_source_address", resp_ip)) { + for (auto & resp : resp_ip) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } +} + +void SmartmicroRadarNode::command_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, const std::string command_name, + const std::string & section_name) +{ + std::vector>> command_resp; + if (response->GetResponse(section_name, command_name.c_str(), command_resp)) { + for (auto & resp : command_resp) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } +} + +void SmartmicroRadarNode::status_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::vector & statuses, const std::string & section_name) +{ + for (const auto & instruction_name : statuses) { + std::vector>> resp_u16; + std::vector>> resp_u32; + bool response_found = false; + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u16)) { + response_found = true; + for (auto & resp : resp_u16) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u32)) { + response_found = true; + for (auto & resp : resp_u32) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (!response_found) { + RCLCPP_WARN(this->get_logger(), "No response received!"); + } + } +} + +void SmartmicroRadarNode::param_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::vector & statuses, const std::string & section_name) +{ + for (const auto & instruction_name : statuses) { + std::vector>> resp_u16; + std::vector>> resp_u32; + std::vector>> resp_u8; + std::vector>> resp_f; + bool response_found = false; + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u16)) { + response_found = true; + for (auto & resp : resp_u16) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u32)) { + response_found = true; + for (auto & resp : resp_u32) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_u8)) { + response_found = true; + for (auto & resp : resp_u8) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %u\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (response->GetResponse(section_name, instruction_name.c_str(), resp_f)) { + response_found = true; + for (auto & resp : resp_f) { + RCLCPP_INFO( + this->get_logger(), + "Response details:\n" + " Instruction: %s\n" + " Response type: %u\n" + " Value: %f\n", + resp->GetInstructionName().c_str(), resp->GetResponseType(), resp->GetValue()); + } + } + + if (!response_found) { + RCLCPP_WARN(this->get_logger(), "No response received!"); + } + } +} + +void SmartmicroRadarNode::objectlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrra4_mse_v2_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrra4_mse_v2_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_port_umrra4_mse_v2_1_0->GetPortHeader(); + std::shared_ptr object_header; + object_header = objectlist_port_umrra4_mse_v2_1_0->GetObjectListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = object_header->GetCycleTime(); + header.number_of_objects = object_header->GetNumberOfObjects(); + header.ts_measurement = object_header->GetTimestampOfMeasurement(); + for (const auto & object : objectlist_port_umrra4_mse_v2_1_0->GetObjectList()) { + const auto x_pos = object->GetPosX(); + const auto y_pos = object->GetPosY(); + const auto z_pos = object->GetPosZ(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeading(); + const auto length = object->GetLength(); + const auto mileage = object->GetMileage(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = object->GetObjectId(); + const auto idle_cycles = object->GetIdleCycles(); + const auto status = object->GetStatus(); + + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, mileage, quality, acceleration, object_id, + idle_cycles, status}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_port_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_mse_v2_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Port Targetlist for umrra4_mse_v2_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrra4_mse_v2_1_0->GetPortHeader(); + std::shared_ptr target_header; + target_header = targetlist_port_umrra4_mse_v2_1_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrra4_mse_v2_1_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::objectlist_callback_umrra4_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrra4_mse_v1_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrra4_mse_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_port_umrra4_mse_v1_0_0->GetPortHeader(); + std::shared_ptr object_header; + object_header = objectlist_port_umrra4_mse_v1_0_0->GetObjectListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = object_header->GetCycleTime(); + header.number_of_objects = object_header->GetNumberOfObjects(); + header.ts_measurement = object_header->GetTimestampOfMeasurement(); + for (const auto & object : objectlist_port_umrra4_mse_v1_0_0->GetObjectList()) { + const auto x_pos = object->GetPosX(); + const auto y_pos = object->GetPosY(); + const auto z_pos = object->GetPosZ(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeading(); + const auto length = object->GetLength(); + const auto mileage = object->GetMileage(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = object->GetObjectId(); + const auto idle_cycles = object->GetIdleCycles(); + const auto status = object->GetStatus(); + + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, mileage, quality, acceleration, object_id, + idle_cycles, status}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_port_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrra4_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_mse_v1_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Port Targetlist for umrra4_mse_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrra4_mse_v1_0_0->GetPortHeader(); + std::shared_ptr target_header; + target_header = targetlist_port_umrra4_mse_v1_0_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrra4_mse_v1_0_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::objectlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrr9f_mse_v1_3_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_port_umrr9f_mse_v1_3_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_port_umrr9f_mse_v1_3_0->GetObjectListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = object_header->GetCycleTime(); + header.number_of_objects = object_header->GetNumberOfObjects(); + header.ts_measurement = object_header->GetTimestampOfMeasurement(); + for (const auto & object : objectlist_port_umrr9f_mse_v1_3_0->GetObjectList()) { + const auto x_pos = object->GetPosX(); + const auto y_pos = object->GetPosY(); + const auto z_pos = object->GetPosZ(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeading(); + const auto length = object->GetLength(); + const auto mileage = object->GetMileage(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = object->GetObjectId(); + const auto idle_cycles = object->GetIdleCycles(); + const auto status = object->GetStatus(); + + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, mileage, quality, acceleration, object_id, + idle_cycles, status}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_port_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id) +{ + std::cout << "Port Targetlist for umrr9f_mse_v1_3_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrr9f_mse_v1_3_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_mse_v1_3_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9f_mse_v1_3_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::objectlist_callback_umrr9f_mse_v1_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrr9f_mse_v1_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrr9f_mse_v1_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_port_umrr9f_mse_v1_1_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_port_umrr9f_mse_v1_1_0->GetObjectListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = object_header->GetCycleTime(); + header.number_of_objects = object_header->GetNumberOfObjects(); + header.ts_measurement = object_header->GetTimestampOfMeasurement(); + for (const auto & object : objectlist_port_umrr9f_mse_v1_1_0->GetObjectList()) { + const auto x_pos = object->GetPosX(); + const auto y_pos = object->GetPosY(); + const auto z_pos = object->GetPosZ(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeading(); + const auto length = object->GetLength(); + const auto mileage = object->GetMileage(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = object->GetObjectId(); + const auto idle_cycles = object->GetIdleCycles(); + const auto status = object->GetStatus(); + + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, mileage, quality, acceleration, object_id, + idle_cycles, status}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_port_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_mse_v1_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_mse_v1_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Port Targetlist for umrr9f_mse_v1_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrr9f_mse_v1_1_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_mse_v1_1_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9f_mse_v1_1_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::objectlist_callback_umrr9f_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_port_umrr9f_mse_v1_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrr9f_mse_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_port_umrr9f_mse_v1_0_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_port_umrr9f_mse_v1_0_0->GetObjectListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = object_header->GetCycleTime(); + header.number_of_objects = object_header->GetNumberOfObjects(); + header.ts_measurement = object_header->GetTimestampOfMeasurement(); + for (const auto & object : objectlist_port_umrr9f_mse_v1_0_0->GetObjectList()) { + const auto x_pos = object->GetPosX(); + const auto y_pos = object->GetPosY(); + const auto z_pos = object->GetPosZ(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeading(); + const auto length = object->GetLength(); + const auto mileage = object->GetMileage(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = object->GetObjectId(); + const auto idle_cycles = object->GetIdleCycles(); + const auto status = object->GetStatus(); + + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, mileage, quality, acceleration, object_id, + idle_cycles, status}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_port_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_mse_v1_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Port Targetlist for umrr9f_mse_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrr9f_mse_v1_0_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_mse_v1_0_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9f_mse_v1_0_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr96( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr96, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr96_v1_2_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr96->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr96->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + for (const auto & target : targetlist_port_umrr96->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr11( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr11, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr11_v1_1_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr11->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr11->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + for (const auto & target : targetlist_port_umrr11->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9f_v1_1_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_v1_1_1" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr9f_v1_1_1->GetGenericPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_v1_1_1->GetStaticPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortId(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + for (const auto & target : targetlist_port_umrr9f_v1_1_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetTgtNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRCS(), target->GetTgtNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_0_0::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9f_v2_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrr9f_v2_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_0_0::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr9f_v2_0_0->GetGenericPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_v2_0_0->GetStaticPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortId(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAnt(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweep(); + header.acquisition_cf_idx = target_header->GetAcquisitionTx(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9f_v2_0_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetTgtNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRCS(), target->GetTgtNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_v2_1_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_v2_1_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9f_v2_1_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_v2_1_1->GetTargetListHeader(); + umrr_ros2_msgs::msg::PortTargetHeader header; + sensor_msgs::msg::PointCloud2 msg; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9f_v2_1_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_v2_2_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f v2_2_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9f_v2_2_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_v2_2_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9f_v2_2_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_4_1( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_v2_4_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f v2_4_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9f_v2_4_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_v2_4_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9f_v2_4_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9f_v3_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_v3_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f v3_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9f_v3_0_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9f_v3_0_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9f_v3_0_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_3( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9d_v1_0_3, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_0_3" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9d_v1_0_3->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9d_v1_0_3->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9d_v1_0_3->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_2( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9d_v1_2_2, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_2_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9d_v1_2_2->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9d_v1_2_2->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9d_v1_2_2->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_4_1( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9d_v1_4_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_4_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9d_v1_4_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9d_v1_4_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9d_v1_4_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_5_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9d_v1_5_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_5_0" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9d_v1_5_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrr9d_v1_5_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrr9d_v1_5_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_v1_0_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrra4_v1_0_1" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrra4_v1_0_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrra4_v1_0_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrra4_v1_0_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrra4_v1_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_v1_2_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrra4_v1_2_1" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrra4_v1_2_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrra4_v1_2_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrra4_v1_2_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrra4_v1_4_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrra4_v1_4_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrra4_v1_4_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_port_umrra4_v1_4_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrra4_v1_4_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrra4_v1_4_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrra1_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra1_t166_b_automotive_v1_0_0::comtargetlist::ComTargetList> & + targetlist_port_umrra1_v1_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrra1_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrra1_v1_0_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrra1_v1_0_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrra1_v1_0_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrra1_v2_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra1_t166_b_automotive_v2_0_0::comtargetlist::ComTargetList> & + targetlist_port_umrra1_v2_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrra1_v2_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrra1_v2_0_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_port_umrra1_v2_0_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::PortTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.port_identifier = port_header->GetPortIdentifier(); + header.port_ver_major = port_header->GetPortVersionMajor(); + header.port_ver_minor = port_header->GetPortVersionMinor(); + header.port_size = port_header->GetPortSize(); + header.body_endianness = port_header->GetBodyEndianness(); + header.port_index = port_header->GetPortIndex(); + header.header_ver_major = port_header->GetHeaderVersionMajor(); + header.header_ver_minor = port_header->GetHeaderVersionMinor(); + + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleTime(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_tx_ant_idx = target_header->GetAcquisitionTxAntIdx(); + header.acquisition_sweep_idx = target_header->GetAcquisitionSweepIdx(); + header.acquisition_cf_idx = target_header->GetAcquisitionCfIdx(); + header.prf = target_header->GetPrf(); + header.umambiguous_speed = target_header->GetUmambiguousSpeed(); + header.acquisition_start = target_header->GetAcquisitionStart(); + for (const auto & target : targetlist_port_umrra1_v2_0_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_objectlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_can_umrra4_mse_v2_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrra4_mse_can_v2_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_can_umrra4_mse_v2_1_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_can_umrra4_mse_v2_1_0->GetComObjectBaseListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = object_header->GetCycleDuration(); + header.cycle_count = object_header->GetCycleCount(); + header.number_of_objects = object_header->GetNoOfObjects(); + header.ego_speed = object_header->GetSpeed(); + header.ego_speed_quality = object_header->GetSpeedQuality(); + header.ego_yaw_rate = object_header->GetYawRate(); + header.ego_yaw_rate_quality = object_header->GetYawRateQuality(); + header.dyn_source = object_header->GetDynamicSource(); + for (const auto & object : objectlist_can_umrra4_mse_v2_1_0->GetObjectList()) { + const auto x_pos = object->GetXPoint1(); + const auto y_pos = object->GetYPoint1(); + const auto z_pos = object->GetZPoint1(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeadingDeg(); + const auto length = object->GetObjectLen(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = static_cast(object->GetObjectId()); + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, quality, acceleration, object_id}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_can_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_mse_v2_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_can_umrra4_mse_v2_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrra4_mse_can_v2_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_can_umrra4_mse_v2_1_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrra4_mse_v2_1_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + header.time_stamp = target_header->GetTimeStamp(); + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + for (const auto & target : targetlist_can_umrra4_mse_v2_1_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_objectlist_callback_umrra4_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_can_umrra4_mse_v1_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrra4_mse_can_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_can_umrra4_mse_v1_0_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_can_umrra4_mse_v1_0_0->GetComObjectBaseListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = object_header->GetCycleDuration(); + header.cycle_count = object_header->GetCycleCount(); + header.number_of_objects = object_header->GetNoOfObjects(); + header.ego_speed = object_header->GetSpeed(); + header.ego_speed_quality = object_header->GetSpeedQuality(); + header.ego_yaw_rate = object_header->GetYawRate(); + header.ego_yaw_rate_quality = object_header->GetYawRateQuality(); + header.dyn_source = object_header->GetDynamicSource(); + for (const auto & object : objectlist_can_umrra4_mse_v1_0_0->GetObjectList()) { + const auto x_pos = object->GetXPoint1(); + const auto y_pos = object->GetYPoint1(); + const auto z_pos = object->GetZPoint1(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeadingDeg(); + const auto length = object->GetObjectLen(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = static_cast(object->GetObjectId()); + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, quality, acceleration, object_id}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_can_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_can_umrra4_mse_v1_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrra4_mse_can_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_can_umrra4_mse_v1_0_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrra4_mse_v1_0_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + header.time_stamp = target_header->GetTimeStamp(); + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + for (const auto & target : targetlist_can_umrra4_mse_v1_0_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_objectlist_callback_umrr9f_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_can_umrr9f_mse_v1_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrr9f_mse_can_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_can_umrr9f_mse_v1_0_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_can_umrr9f_mse_v1_0_0->GetComObjectBaseListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = object_header->GetCycleDuration(); + header.cycle_count = object_header->GetCycleCount(); + header.number_of_objects = object_header->GetNoOfObjects(); + header.ego_speed = object_header->GetSpeed(); + header.ego_speed_quality = object_header->GetSpeedQuality(); + header.ego_yaw_rate = object_header->GetYawRate(); + header.ego_yaw_rate_quality = object_header->GetYawRateQuality(); + header.dyn_source = object_header->GetDynamicSource(); + for (const auto & object : objectlist_can_umrr9f_mse_v1_0_0->GetObjectList()) { + const auto x_pos = object->GetXPoint1(); + const auto y_pos = object->GetYPoint1(); + const auto z_pos = object->GetZPoint1(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeadingDeg(); + const auto length = object->GetObjectLen(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = static_cast(object->GetObjectId()); + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, quality, acceleration, object_id}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_can_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_mse_v1_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_can_umrr9f_mse_v1_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_mse_can_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_can_umrr9f_mse_v1_0_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9f_mse_v1_0_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + header.time_stamp = target_header->GetTimeStamp(); + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + for (const auto & target : targetlist_can_umrr9f_mse_v1_0_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_objectlist_callback_umrr9f_mse_v1_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_can_umrr9f_mse_v1_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrr9f_mse_can_v1_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_can_umrr9f_mse_v1_1_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_can_umrr9f_mse_v1_1_0->GetComObjectBaseListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = object_header->GetCycleDuration(); + header.cycle_count = object_header->GetCycleCount(); + header.number_of_objects = object_header->GetNoOfObjects(); + header.ego_speed = object_header->GetSpeed(); + header.ego_speed_quality = object_header->GetSpeedQuality(); + header.ego_yaw_rate = object_header->GetYawRate(); + header.ego_yaw_rate_quality = object_header->GetYawRateQuality(); + header.dyn_source = object_header->GetDynamicSource(); + for (const auto & object : objectlist_can_umrr9f_mse_v1_1_0->GetObjectList()) { + const auto x_pos = object->GetXPoint1(); + const auto y_pos = object->GetYPoint1(); + const auto z_pos = object->GetZPoint1(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeadingDeg(); + const auto length = object->GetObjectLen(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = static_cast(object->GetObjectId()); + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, quality, acceleration, object_id}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_can_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_mse_v1_1_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_can_umrr9f_mse_v1_1_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_mse_can_v1_1_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_can_umrr9f_mse_v1_1_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9f_mse_v1_1_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + header.time_stamp = target_header->GetTimeStamp(); + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + for (const auto & target : targetlist_can_umrr9f_mse_v1_1_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_objectlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + objectlist_can_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id) +{ + std::cout << "Objectlist for umrr9f_mse_can_v1_3_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = objectlist_can_umrr9f_mse_v1_3_0->GetPortHeader(); + std::shared_ptr + object_header; + object_header = objectlist_can_umrr9f_mse_v1_3_0->GetComObjectBaseListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanObjectHeader header; + ObjectPointCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = object_header->GetCycleDuration(); + header.cycle_count = object_header->GetCycleCount(); + header.number_of_objects = object_header->GetNoOfObjects(); + header.ego_speed = object_header->GetSpeed(); + header.ego_speed_quality = object_header->GetSpeedQuality(); + header.ego_yaw_rate = object_header->GetYawRate(); + header.ego_yaw_rate_quality = object_header->GetYawRateQuality(); + header.dyn_source = object_header->GetDynamicSource(); + for (const auto & object : objectlist_can_umrr9f_mse_v1_3_0->GetObjectList()) { + const auto x_pos = object->GetXPoint1(); + const auto y_pos = object->GetYPoint1(); + const auto z_pos = object->GetZPoint1(); + const auto speed_abs = object->GetSpeedAbs(); + const auto heading = object->GetHeadingDeg(); + const auto length = object->GetObjectLen(); + const auto quality = object->GetQuality(); + const auto acceleration = object->GetAcceleration(); + const auto object_id = static_cast(object->GetObjectId()); + modifier.push_back( + {x_pos, y_pos, z_pos, speed_abs, heading, length, quality, acceleration, object_id}); + } + + m_publishers_obj[sensor_idx]->publish(msg); + m_publishers_can_obj_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_mse_v1_3_0( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_can_umrr9f_mse_v1_3_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_mse_can_v1_3_0" << std::endl; + if (!check_signal) { + std::shared_ptr port_header; + port_header = targetlist_can_umrr9f_mse_v1_3_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9f_mse_v1_3_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + header.time_stamp = target_header->GetTimeStamp(); + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + for (const auto & target : targetlist_can_umrr9f_mse_v1_3_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr96( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr96_t153_automotive_v1_2_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr96, + const com::types::ClientId client_id) +{ + std::cout << "CAN Targetlist for umrr96_v1_2_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr96->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr96->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr96->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr11( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr11_t132_automotive_v1_1_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr11, + const com::types::ClientId client_id) +{ + std::cout << "CAN Targetlist for umrr11_v1_1_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr11->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr11->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr11->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_3::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_0_3, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_0_3" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9d_v1_0_3->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9d_v1_0_3->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr9d_v1_0_3->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_2_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_2_2, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_2_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9d_v1_2_2->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9d_v1_2_2->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr9d_v1_2_2->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_4_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_4_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_4_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_4_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9d_v1_4_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9d_v1_4_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr9d_v1_4_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_5_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_5_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_5_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_5_0" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9d_v1_5_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9d_v1_5_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr9d_v1_5_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_1_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v2_1_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_v2_1_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9f_v2_1_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9f_v2_1_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr9f_v2_1_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_2_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v2_2_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_v2_2_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9f_v2_2_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9f_v2_2_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr9f_v2_2_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_4_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_4_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v2_4_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_v2_4_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9f_v2_4_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9f_v2_4_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr9f_v2_4_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v3_0_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v3_0_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v3_0_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_v3_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9f_v3_0_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrr9f_v3_0_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrr9f_v3_0_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra4_automotive_v1_0_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrra4_v1_0_1, + const com::types::ClientId client_id) +{ + std::cout << "CAN Targetlist for umrra4_v1_0_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrra4_v1_0_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrra4_v1_0_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + header.time_stamp = target_header->GetTimeStamp(); + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + for (const auto & target : targetlist_can_umrra4_v1_0_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra4_automotive_v1_2_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrra4_v1_2_1, + const com::types::ClientId client_id) +{ + std::cout << "CAN Targetlist for umrra4_v1_2_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrra4_v1_2_1->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrra4_v1_2_1->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + header.time_stamp = target_header->GetTimeStamp(); + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + for (const auto & target : targetlist_can_umrra4_v1_2_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_4_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra4_automotive_v1_4_0::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrra4_v1_4_0, + const com::types::ClientId client_id) +{ + std::cout << "CAN Targetlist for umrra4_v1_4_0" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrra4_v1_4_0->GetPortHeader(); + std::shared_ptr + target_header; + target_header = targetlist_can_umrra4_v1_4_0->GetTargetListHeader(); + sensor_msgs::msg::PointCloud2 msg; + umrr_ros2_msgs::msg::CanTargetHeader header; + RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; + const auto [sec, nanosec] = + convert_timestamp(std::chrono::microseconds{port_header->GetTimestamp()}); + msg.header.stamp.sec = sec; + msg.header.stamp.nanosec = nanosec; + header.frame_id = m_sensors[sensor_idx].frame_id; + header.cycle_time = target_header->GetCycleDuration(); + header.number_of_targets = target_header->GetNumberOfTargets(); + header.acquisition_setup = target_header->GetAcquisitionSetup(); + header.cycle_count = target_header->GetCycleCount(); + header.time_stamp = target_header->GetTimeStamp(); + header.acq_ts_fraction = target_header->GetAcqTimeStampFraction(); + for (const auto & target : targetlist_can_umrra4_v1_4_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr, azimuth_angle, elevation_angle, range}); + } + + m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); + } +} + +void SmartmicroRadarNode::update_config_files_from_params() +{ + const auto master_inst_serial_type = declare_parameter(kInstSerialTypeTag, std::string{}); + const auto master_data_serial_type = declare_parameter(kDataSerialTypeTag, std::string{}); + + auto read_adapter_params_if_possible = [&](const std::uint32_t index) { + auto & current_adapter = m_adapters[index]; + const auto prefix_2 = "adapters.adapter_" + std::to_string(index); + current_adapter.hw_dev_id = this->declare_parameter(prefix_2 + ".hw_dev_id", kDefaultHwDevId); + if (current_adapter.hw_dev_id == kDefaultHwDevId) { + // The id was not set, so the adapter with this index was not defined. + // Stop here. + return false; + } + current_adapter.hw_iface_name = + this->declare_parameter(prefix_2 + ".hw_iface_name", kDefaultHwDevIface); + current_adapter.hw_type = this->declare_parameter(prefix_2 + ".hw_type", kDefaultHwLinkType); + current_adapter.baudrate = this->declare_parameter(prefix_2 + ".baudrate", 500000); + current_adapter.port = this->declare_parameter(prefix_2 + ".port", kDefaultPort); + + return true; + }; + + auto read_sensor_params_if_possible = [&](const std::uint32_t index) { + auto & sensor = m_sensors[index]; + const auto prefix_3 = "sensors.sensor_" + std::to_string(index); + sensor.dev_id = this->declare_parameter(prefix_3 + ".dev_id", kDefaultHwDevId); + sensor.uifname = this->declare_parameter(prefix_3 + ".uifname", ""); + sensor.uifmajorv = this->declare_parameter(prefix_3 + ".uifmajorv", 0); + sensor.uifminorv = this->declare_parameter(prefix_3 + ".uifminorv", 0); + sensor.uifpatchv = this->declare_parameter(prefix_3 + ".uifpatchv", 0); + sensor.model = this->declare_parameter(prefix_3 + ".model", kDefaultSensorType); + sensor.id = this->declare_parameter(prefix_3 + ".id", kDefaultClientId); + if (sensor.id == kDefaultClientId) { + // The id was not set, so the sensor with this index was not defined. Stop + // here. + return false; + } + sensor.ip = this->declare_parameter(prefix_3 + ".ip", ""); + sensor.port = this->declare_parameter(prefix_3 + ".port", 0); + sensor.frame_id = this->declare_parameter(prefix_3 + ".frame_id", kDefaultFrameId); + sensor.history_size = this->declare_parameter(prefix_3 + ".history_size", kDefaultHistorySize); + sensor.inst_type = this->declare_parameter(prefix_3 + ".inst_type", ""); + sensor.data_type = this->declare_parameter(prefix_3 + ".data_type", ""); + sensor.link_type = this->declare_parameter(prefix_3 + ".link_type", kDefaultHwLinkType); + sensor.pub_type = this->declare_parameter(prefix_3 + ".pub_type", ""); + return true; + }; + + for (auto j = 0UL; j < m_adapters.size(); ++j) { + if (!read_adapter_params_if_possible(j)) { + m_number_of_adapters = j; + break; + } + } + + if (!m_number_of_adapters) { + throw std::runtime_error("At least one adapter must be configured."); + } + + for (auto i = 0UL; i < m_sensors.size(); ++i) { + if (!read_sensor_params_if_possible(i)) { + m_number_of_sensors = i; + break; + } + } + if (!m_number_of_sensors) { + throw std::runtime_error("At least one sensor must be configured."); + } + + auto config = nlohmann::json::parse(std::ifstream{kConfigFilePath}); + config[kDataSerialTypeJsonTag] = master_data_serial_type; + config[kInstSerialTypeJsonTag] = master_inst_serial_type; + std::ofstream{kConfigFilePath, std::ios::trunc} << config; + + auto hw_inventory = nlohmann::json::parse(std::ifstream{kHwInventoryFilePath}); + auto & hw_items = hw_inventory[kHwItemsJsonTag]; + if (hw_items.empty()) { + throw std::runtime_error("There are no 'hwItems' defined in the hw_inventory.json file."); + } + auto hw_item = hw_items.front(); + hw_items.clear(); + for (auto j = 0UL; j < m_number_of_adapters; ++j) { + const auto & adapter = m_adapters[j]; + hw_item[kPortTag] = adapter.port; + hw_item[kHwDevLinkTag] = adapter.hw_type; + hw_item[kHwDevIdTag] = adapter.hw_dev_id; + hw_item[kHwDevIfaceNameTag] = adapter.hw_iface_name; + hw_item[kBaudRateTag] = adapter.baudrate; + hw_items.push_back(hw_item); + } + std::ofstream{kHwInventoryFilePath, std::ios::trunc} << hw_inventory; + + auto routing_table = nlohmann::json::parse(std::ifstream{kRoutingTableFilePath}); + auto & clients = routing_table[kClientsJsonTag]; + if (clients.empty()) { + throw std::runtime_error("There are no 'clients' defined in the routing_table.json file."); + } + auto client = clients.front(); // Make a copy of the first client. + clients.clear(); + for (auto i = 0UL; i < m_number_of_sensors; ++i) { + const auto & sensor = m_sensors[i]; + client[kClientLinkTag] = sensor.link_type; + client[kClientIdTag] = sensor.id; + client[kHwDevIdTag] = sensor.dev_id; + client[kPortTag] = sensor.port; + client[kIpTag] = sensor.ip; + client[kInstSerialTypeJsonTag] = sensor.inst_type; + client[kDataSerialTypeJsonTag] = sensor.data_type; + client[kUINameTag] = sensor.uifname; + client[kUIMajorVTag] = sensor.uifmajorv; + client[kUIMinorVTag] = sensor.uifminorv; + client[kUIPatchVTag] = sensor.uifpatchv; + clients.push_back(client); + } + + std::ofstream{kRoutingTableFilePath, std::ios::trunc} << std::setw(4) << routing_table; +} + +} // namespace radar +} // namespace drivers +} // namespace smartmicro + +RCLCPP_COMPONENTS_REGISTER_NODE(smartmicro::drivers::radar::SmartmicroRadarNode) diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/src/update_service.cpp b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/src/update_service.cpp new file mode 100644 index 0000000..67aaca2 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/src/update_service.cpp @@ -0,0 +1,95 @@ +#include "umrr_ros2_driver/update_service.hpp" + +#include +#include + +using namespace com::types; +using namespace com::master; + +UpdateService::UpdateService() {} + +void UpdateService::StartSoftwareUpdate(ClientId client_id, std::string & update_image) +{ + std::ifstream fileStream(update_image, std::ios::binary | std::ios::ate); + if (!fileStream.is_open()) { + RCLCPP_ERROR( + rclcpp::get_logger("FirmwareUpdater"), "Couldn't open file: %s", update_image.c_str()); + return; + } + + const uint64_t totalSize = fileStream.tellg(); + + auto comServicesPtr = CommunicationServicesIface::Get(); + auto updateService = comServicesPtr->GetUpdateService(); + + { + std::lock_guard lock(mutex_); + updateInfo_.SetUpdateStatus(RUNNING); + updateInfo_.SetCurrentDownloadedBytes(0); + } // mutex(unlocked) + + RCLCPP_INFO( + rclcpp::get_logger("UpdateService"), "Starting firmware download of %lu bytes...", totalSize); + + if (updateService->SoftwareUpdate(update_image, client_id, [this](SWUpdateInfo & info) { + this->UpdateCallback(info); + }) != ERROR_CODE_OK) { + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Start of software download failed"); + return; + } + + { + std::unique_lock lock( + mutex_); + cv_.wait( + lock, + [this, + totalSize] { + return updateInfo_.GetUpdateStatus() != RUNNING || + updateInfo_.GetCurrentDownloadedBytes() >= totalSize; + }); + } + + HandleResult(); +} + +void UpdateService::UpdateCallback(SWUpdateInfo & info) +{ + { + std::lock_guard lock(mutex_); + updateInfo_ = info; + RCLCPP_INFO( + rclcpp::get_logger("FirmwareUpdater"), "Downloaded %lu bytes...", + info.GetCurrentDownloadedBytes()); + } + cv_.notify_one(); +} + +void UpdateService::HandleResult() +{ + std::lock_guard lock(mutex_); + switch (updateInfo_.GetUpdateStatus()) { + case READY_SUCCESS: + RCLCPP_INFO( + rclcpp::get_logger("FirmwareUpdater"), "Firmware download completed successfully."); + break; + case STOPPED_BY_MASTER: + RCLCPP_WARN(rclcpp::get_logger("FirmwareUpdater"), "Download stopped by master."); + break; + case STOPPED_BY_SLAVE: + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Download stopped by slave."); + break; + case STOPPED_BY_ERROR_TIMEOUT: + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Download failed: timeout."); + break; + case STOPPED_BY_ERROR_BLOCK_REPEAT: + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Download failed: block repeat error."); + break; + case STOPPED_BY_ERROR_IMAGE_INVALID: + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Download failed: invalid image."); + break; + default: + RCLCPP_ERROR(rclcpp::get_logger("FirmwareUpdater"), "Download failed: unknown error."); + break; + } +} diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/test/radar_node_test.launch.py b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/test/radar_node_test.launch.py new file mode 100644 index 0000000..bb4279c --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_driver/test/radar_node_test.launch.py @@ -0,0 +1,172 @@ +# Copyright (c) 2021, s.m.s, smart microwave sensors GmbH, Brunswick, Germany +# +# 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 time +import unittest + +import launch +import launch_testing +import launch_testing.actions +from launch.actions import ExecuteProcess + +import pytest +import rclpy +import sensor_msgs.msg as sensor_msgs +from ament_index_python import get_package_share_directory +from launch_ros.actions import Node + +PACKAGE_NAME = 'umrr_ros2_driver' + + +@pytest.mark.launch_test +def generate_test_description(): + + radar__params = os.path.join( + get_package_share_directory(PACKAGE_NAME), 'param/radar.params.template.yaml') + radar_node = Node( + package=PACKAGE_NAME, + executable='smartmicro_radar_node_exe', + name='smart_radar', + parameters=[radar__params] + ) + + set_frequency_sweep_service = ExecuteProcess( + cmd = [ + 'ros2', 'service', 'call', + '/smart_radar/set_radar_mode', + 'umrr_ros2_msgs/srv/SetMode', + '{section_name: auto_interface_0dim, sensor_id: 200, params: ["frequency_sweep_idx"], values: ["1"], value_types: [3]}' + ], + output='screen', + ) + + set_angular_separation_service = ExecuteProcess( + cmd=[ + 'ros2', 'service', 'call', + '/smart_radar/set_radar_mode', + 'umrr_ros2_msgs/srv/SetMode', + '{section_name: auto_interface_0dim, sensor_id: 100, params: ["angular_separation"], values: ["1"], value_types: [3]}' + ], + output='screen' + ) + + + get_range_toggle_mode_service = ExecuteProcess( + cmd = [ + 'ros2', 'service', 'call', + '/smart_radar/get_radar_mode', + 'umrr_ros2_msgs/srv/GetMode', + '{section_name: auto_interface_0dim, sensor_id: 300, params: ["range_toggle_mode"], param_types: [3]}' + ], + output='screen', + ) + + get_software_version_service = ExecuteProcess( + cmd = [ + 'ros2', 'service', 'call', + '/smart_radar/get_radar_status', + 'umrr_ros2_msgs/srv/GetStatus', + '{section_name: auto_interface, sensor_id: 400, statuses: ["sw_version_major", "sw_version_minor"], status_types: [1, 1]}' + ], + output='screen', + ) + + return ( + launch.LaunchDescription([ + radar_node, + set_frequency_sweep_service, + set_angular_separation_service, + get_range_toggle_mode_service, + get_software_version_service, + launch_testing.actions.ReadyToTest(), + ]), + { + 'radar_node': radar_node, + } + ) + + +class TestSmartNode(unittest.TestCase): + + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node('test_node') + + def tearDown(self): + self.test_node.destroy_node() + + def test_smart_node_publishes(self): + # Expect the smartnode to publish strings on 'smart_radar/port_targets_' + data_rx_s1 = [] + data_rx_s2 = [] + data_rx_s3 = [] + + def data_rx_s1_callback(msg): + data_rx_s1.append(msg) + + def data_rx_s2_callback(msg): + data_rx_s2.append(msg) + + def data_rx_s3_callback(msg): + data_rx_s3.append(msg) + + sub_s1 = self.test_node.create_subscription( + sensor_msgs.PointCloud2, + 'smart_radar/port_targets_3', + data_rx_s1_callback, + 10 + ) + sub_s2 = self.test_node.create_subscription( + sensor_msgs.PointCloud2, + 'smart_radar/port_targets_1', + data_rx_s2_callback, + 10 + ) + sub_s3 = self.test_node.create_subscription( + sensor_msgs.PointCloud2, + 'smart_radar/port_targets_2', + data_rx_s3_callback, + 10 + ) + try: + # Wait until the publisher publishes + end_time = time.time() + 20 + + while time.time() < end_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + if len(data_rx_s1) > 1: + print(f"Data from S1 received at {time.time()}") + if len(data_rx_s2) > 1: + print(f"Data from S2 received at {time.time()}") + if len(data_rx_s3) > 1: + print(f"Data from S3 received at {time.time()}") + self.assertGreater(len(data_rx_s1), 1) + self.assertGreater(len(data_rx_s2), 1) + self.assertGreater(len(data_rx_s3), 1) + + finally: + self.test_node.destroy_subscription(sub_s1) + self.test_node.destroy_subscription(sub_s2) + self.test_node.destroy_subscription(sub_s3) diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/CMakeLists.txt b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/CMakeLists.txt new file mode 100644 index 0000000..a21888d --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.11) +project(umrr_ros2_msgs) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SetMode.srv" + "srv/SetIp.srv" + "srv/SendCommand.srv" + "srv/FirmwareDownload.srv" + "srv/GetStatus.srv" + "srv/GetMode.srv" + "msg/CanTargetHeader.msg" + "msg/PortTargetHeader.msg" + "msg/PortObjectHeader.msg" + "msg/CanObjectHeader.msg" + DEPENDENCIES + "geometry_msgs" + "sensor_msgs" + "std_msgs" +) + +ament_export_dependencies(rosidl_default_runtime) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/CanObjectHeader.msg b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/CanObjectHeader.msg new file mode 100644 index 0000000..f32938e --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/CanObjectHeader.msg @@ -0,0 +1,9 @@ +string frame_id +float32 cycle_time +uint16 number_of_objects +float32 ego_speed +float32 ego_speed_quality +float32 ego_yaw_rate +float32 ego_yaw_rate_quality +uint32 cycle_count +uint8 dyn_source diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/CanTargetHeader.msg b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/CanTargetHeader.msg new file mode 100644 index 0000000..0d519e5 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/CanTargetHeader.msg @@ -0,0 +1,7 @@ +string frame_id +float32 cycle_time +uint8 number_of_targets +uint32 cycle_count +uint8 acquisition_setup +uint32 time_stamp +float32 acq_ts_fraction diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/PortObjectHeader.msg b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/PortObjectHeader.msg new file mode 100644 index 0000000..6b71d49 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/PortObjectHeader.msg @@ -0,0 +1,12 @@ +string frame_id +uint32 port_identifier +int16 port_ver_major +int16 port_ver_minor +uint32 port_size +uint8 body_endianness +uint8 port_index +uint8 header_ver_major +uint8 header_ver_minor +float32 cycle_time +uint16 number_of_objects +uint64 ts_measurement diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/PortTargetHeader.msg b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/PortTargetHeader.msg new file mode 100644 index 0000000..f82cdbf --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/msg/PortTargetHeader.msg @@ -0,0 +1,17 @@ +string frame_id +uint32 port_identifier +int16 port_ver_major +int16 port_ver_minor +uint32 port_size +uint8 body_endianness +uint8 port_index +uint8 header_ver_major +uint8 header_ver_minor +float32 cycle_time +uint16 number_of_targets +uint8 acquisition_tx_ant_idx +uint8 acquisition_sweep_idx +uint8 acquisition_cf_idx +uint8 prf +float32 umambiguous_speed +uint64 acquisition_start diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/package.xml b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/package.xml new file mode 100644 index 0000000..2b6bf47 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/package.xml @@ -0,0 +1,19 @@ + + + + umrr_ros2_msgs + 3.0.0 + A package for smartmicro radar messages + shah + Apache 2.0 + + ament_cmake + + rosidl_default_generators + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/FirmwareDownload.srv b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/FirmwareDownload.srv new file mode 100644 index 0000000..4557eff --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/FirmwareDownload.srv @@ -0,0 +1,4 @@ +uint32 sensor_id +string file_path +--- +string res diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/GetMode.srv b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/GetMode.srv new file mode 100644 index 0000000..bf0cd88 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/GetMode.srv @@ -0,0 +1,6 @@ +string section_name # name of the interface section +uint32 sensor_id # id of the sensor to send request +string[] params # list of parameters or parameter to read +uint8[] param_types # 0: float32, 1: uint32, 2: uint16, 3: uint8 +--- +string res # response from the sensor diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/GetStatus.srv b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/GetStatus.srv new file mode 100644 index 0000000..5d495b3 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/GetStatus.srv @@ -0,0 +1,6 @@ +string section_name # name of the interface section +uint32 sensor_id # id of the sensor to send request +string[] statuses # list of status or status to send +uint8[] status_types # 0: uint32, 1: uint16 +--- +string res # response from the sensor diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/SendCommand.srv b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/SendCommand.srv new file mode 100644 index 0000000..adb43cc --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/SendCommand.srv @@ -0,0 +1,6 @@ +string section_name # name of the interface section +string command # name of command to send +float32 value # value to send +uint32 sensor_id # id of the sensor to send request +--- +string res diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/SetIp.srv b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/SetIp.srv new file mode 100644 index 0000000..d21c0c2 --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/SetIp.srv @@ -0,0 +1,4 @@ +uint32 value_ip +uint32 sensor_id +--- +string res_ip diff --git a/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/SetMode.srv b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/SetMode.srv new file mode 100644 index 0000000..9f7549c --- /dev/null +++ b/src/drivers/smartmicro_ros2_radars/umrr_ros2_msgs/srv/SetMode.srv @@ -0,0 +1,7 @@ +string section_name # name of the interface section +uint32 sensor_id # id of the sensor to send request +string[] params # list of params or param to send +string[] values # list of values or value to send +uint8[] value_types # 0: float32, 1: uint32, 2: uint16, 3: uint8 +--- +string res # response from the sensor diff --git a/src/tools/point_cloud_msg_wrapper/CMakeLists.txt b/src/tools/point_cloud_msg_wrapper/CMakeLists.txt new file mode 100644 index 0000000..fd2f63d --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/CMakeLists.txt @@ -0,0 +1,44 @@ +# Copyright 2021 Apex.AI, Inc. +# All rights reserved. +cmake_minimum_required(VERSION 3.5) +project(point_cloud_msg_wrapper) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +add_library(${PROJECT_NAME} INTERFACE) +target_include_directories(${PROJECT_NAME} INTERFACE include/) + +if(BUILD_TESTING) + # run common linters + find_package(ament_lint_auto) + ament_lint_auto_find_test_dependencies() + ament_add_gtest(${PROJECT_NAME}_test + test/test_field_generators.cpp + test/test_point_cloud_msg_wrapper.cpp + ) + target_compile_options(${PROJECT_NAME}_test PRIVATE + -Wall + -Wextra + -Wpedantic + -Wall + -Wconversion + -Wsign-conversion + -Werror + -Wcast-align + -Wunused + -Wsign-conversion + -Wdouble-promotion) + + ament_target_dependencies(${PROJECT_NAME}_test sensor_msgs geometry_msgs) + target_include_directories(${PROJECT_NAME}_test PRIVATE include) + target_include_directories(${PROJECT_NAME}_test SYSTEM PRIVATE ${GTEST_INCLUDE_DIRS}) + +endif() + +ament_auto_package() diff --git a/src/tools/point_cloud_msg_wrapper/design/point_cloud_msg_wrapper-design.md b/src/tools/point_cloud_msg_wrapper/design/point_cloud_msg_wrapper-design.md new file mode 100644 index 0000000..cdd092f --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/design/point_cloud_msg_wrapper-design.md @@ -0,0 +1,86 @@ +Point Cloud Message Wrapper (#point-cloud-msg-wrapper) +=========== + +# Purpose / Use cases + +`PointCloudMsgWrapper` is a class that aims to be used instead of `PointCloud2Iterator` and +`PointCloud2Modifier`. The core idea is that it wraps a point cloud message and allows treating it +essentially as a vector of points. To create a wrapper the user needs to specify the type of points +they want to use and a message that the wrapper wraps. If the fields stored in the point cloud +message do not match the fields or methods of the point struct a runtime error is thrown. An array +of PointField objects can be generated from a given point type automatically through the mechanism +of field generators defined in file `field_generators.hpp` and passed as a tuple into the +`PointCloudMsgWrapper`. If a point has `float x`, `float y`, and `uint8_t intensity` members (or +functions that return non-const references to such fields, e.g. `float& x()`, `float& y()` and +`uint8_t& intensity()`), these generators must be generated (these particular ones _are_ generated +by default, see `default_field_generators.hpp`): + +```c++ +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(x); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(y); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(intensity); +``` + +These create classes `field_x_generator`, `field_y_generator`, `field_intensity_generator` that are +then passed as a tuple as a template parameter into the `PointCloudMsgWrapper`. + +Other field generators can be generated if needed by the user, but we strive to provide a sane +default value. It is expected that the user provides all needed generators that can be created with +the macros above into the wrapper class upon construction. If the user fails to provide the +appropriate generators an error will be thrown when wrapping an existing point cloud message as an +expected field from a point cloud message would not be matched by the generated ones. See the +default ones in the variable `detail::DefaultFieldGenerators` in `point_cloud_msg_wrapper.hpp` file. + +For convenience, there are two typedefs: `PointCloud2View` and `PointCloud2Modifier`. + +- `PointCloudView{msg}` wraps a constant point cloud and allows read-only access. + Modification through this view is impossible. +- `PointCloud2Modifier{msg}` wraps a mutable message and allows read-write access to the + underlying data. +- `PointCloud2Modifier{msg, new_frame_id}` initializes an empty mutable message. This + constructor is to be used solely to initialize a new message and will throw if the point cloud + message is already initialized (i.e., has non-zero number of fields). + +## Assumptions / Known limits + +### Points used with this class _must_ have an equality operator + +If a point has no equality operator, the user must define a free standing operator for it. Otherwise +a `static_assert` will be hit. + +### For non-common fields, the generators for these must be created by the user + +This package provides a set of generators for common field names, see `default_field_generators.hpp` +file. If other fields are required a custom generator must be created to work with these. We provide +a macro for these purposes: `LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER`, also see +`ProcessPointWithCustomField` unit test for an example. + +### Field offsets must match _exactly_ + +To offer safety, fields of the point struct/class must match those stored in the cloud message +_exactly_. That is, if an offset or a type is not strictly matching between the point and the field +stored in the message the wrapper cannot be created for such a combination and an error will be +thrown. If such a situation is encountered, a custom data structure can be created in order to +accomodate for these changes, see `CustomAlignedPoint` struct in the unit tests of this package for +an example. + +### Working with points that have virtual methods + +We allow point type to have any additional functions as long as it has either: +- public members with names matching the wanted fields, e.g. `float x;` +- public methods that return a reference to the needed field, e.g. `float& x() {return m_x;}`. + +That means that some methods can be virtual. In general it is not an issue to work with such types +with `PointCloudMsgWrapper` but one must be careful. It is _forbidden_ to call virtual methods when +reading/writing to a wrapper. The reason is that we memory-map the points into the message, +_including_ the vtable pointer. It is not guaranteed that the same vtable is present when reading +from such a point, so calling any of these methods is undefined behavior. As long as these are not +called, the system behavior is defined. + +## Inputs / Outputs / API + +See the API documentation. + +# Security considerations + +TBD by a security specialist. diff --git a/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/default_field_generators.hpp b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/default_field_generators.hpp new file mode 100644 index 0000000..f0275a2 --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/default_field_generators.hpp @@ -0,0 +1,36 @@ +// Copyright 2021 Apex.AI, 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. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + +#ifndef POINT_CLOUD_MSG_WRAPPER__DEFAULT_FIELD_GENERATORS_HPP_ +#define POINT_CLOUD_MSG_WRAPPER__DEFAULT_FIELD_GENERATORS_HPP_ + +#include + +namespace point_cloud_msg_wrapper +{ + +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(x); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(y); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(z); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(id); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(ring); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(intensity); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(timestamp); + +} // namespace point_cloud_msg_wrapper + +#endif // POINT_CLOUD_MSG_WRAPPER__DEFAULT_FIELD_GENERATORS_HPP_ diff --git a/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/field_generators.hpp b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/field_generators.hpp new file mode 100644 index 0000000..6b35b5f --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/field_generators.hpp @@ -0,0 +1,157 @@ +// Copyright 2021 Apex.AI, 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. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + +#ifndef POINT_CLOUD_MSG_WRAPPER__FIELD_GENERATORS_HPP_ +#define POINT_CLOUD_MSG_WRAPPER__FIELD_GENERATORS_HPP_ + +#include +#include + +namespace point_cloud_msg_wrapper +{ + +namespace detail +{ +template +inline std::size_t offset_of(ReturnT ClassT::* member) +{ + ClassT object{}; + return std::size_t(&(object.*member)) - std::size_t(&object); +} + +template +std::size_t offset_of_return_ref(ReturnT (ClassT::* member_func)(void)) +{ + ClassT object; + const auto & result_ref = (object.*member_func)(); + return std::size_t(&result_ref) - std::size_t(&object); +} + +template +void push_back_field( + sensor_msgs::msg::PointCloud2::_fields_type & fields, + const sensor_msgs::msg::PointField::_name_type & name, + MemberT ClassT::* member) +{ + fields.emplace_back(); + fields.back() + .set__name(name) + .set__offset(static_cast(offset_of(member))) + .set__datatype(get_field_datatype()) + .set__count(get_field_count()); +} + +template +void push_back_field( + sensor_msgs::msg::PointCloud2::_fields_type & fields, + const sensor_msgs::msg::PointField::_name_type & name, + ReturnT (ClassT::* member_func)(void)) +{ + using MemberT = std::decay_t; + fields.emplace_back(); + fields.back() + .set__name(name) + .set__offset(static_cast(offset_of_return_ref(member_func))) + .set__datatype(get_field_datatype()) + .set__count(get_field_count()); +} + +/// End of recursion for applying all field adders to a given point type. +template +constexpr std::enable_if_t < (N >= std::tuple_size{}) > +apply_field_adder_to_generate_field_if_needed(sensor_msgs::msg::PointCloud2::_fields_type &) { +} + +/// Standard case to recursively apply all field adders for a given point type. +template +constexpr std::enable_if_t < (N < std::tuple_size{}) > +apply_field_adder_to_generate_field_if_needed(sensor_msgs::msg::PointCloud2::_fields_type & fields) +{ + using CurrentFieldAdder = std::tuple_element_t; + CurrentFieldAdder::template push_back_field_if_needed(fields); + apply_field_adder_to_generate_field_if_needed(fields); +} + +} // namespace detail + + +/// A macro that defines a struct that allows to query for the MEMBER member in any given struct. +/// The intended usecase for this struct is (example for MEMBER 'x'): +/// has_member_x::value +#define IMPL__LIDAR_UTILS__DEFINE_MEMBER_CHECKER(MEMBER) \ + template \ + struct has_member_ ## MEMBER : std::false_type {}; \ + template \ + struct has_member_ ## MEMBER< \ + T, typename std::enable_if< \ + !std::is_same().MEMBER), void>::value, \ + bool>::type>: std::true_type {} + +#define IMPL__LIDAR_UTILS__DEFINE_MEMBER_FUNCTION_CHECKER(FUNCTION_NAME) \ + template \ + struct has_function_ ## FUNCTION_NAME ## _returning_non_const_ref : std::false_type {}; \ + template \ + struct has_function_ ## FUNCTION_NAME ## _returning_non_const_ref< \ + T, std::enable_if_t< \ + std::is_reference().FUNCTION_NAME())>::value && \ + !std::is_const().FUNCTION_NAME())>>::value>>: std::true_type {} + +/// A Macro that defines a generator struct for a given field name. +/// +/// The generator structs take a vector of PointFields and push_back a new +/// PointField into it if the underlying class has the specified field. The +/// existence of the field is checked by the has_member_FIELD::value. +#define LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(FIELD) \ + struct field_ ## FIELD ## _generator { \ + IMPL__LIDAR_UTILS__DEFINE_MEMBER_CHECKER(FIELD); \ + IMPL__LIDAR_UTILS__DEFINE_MEMBER_FUNCTION_CHECKER(FIELD); \ + template \ + static inline std::enable_if_t< \ + !has_member_ ## FIELD::value && \ + !has_function_ ## FIELD ## _returning_non_const_ref::value> \ + push_back_field_if_needed(sensor_msgs::msg::PointCloud2::_fields_type &) { \ + } \ + template \ + static inline std::enable_if_t< \ + has_member_ ## FIELD::value || \ + has_function_ ## FIELD ## _returning_non_const_ref::value> \ + push_back_field_if_needed(sensor_msgs::msg::PointCloud2::_fields_type & fields) { \ + point_cloud_msg_wrapper::detail::push_back_field(fields, #FIELD, &PointT::FIELD); \ + } \ + } + +/// A help function to start iterating over all field adders and return the appropritate fields +/// generated by those. +/// +/// @tparam PointT Type of point +/// @tparam FieldAdders A tuple of field adder types, generated by +/// LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER +/// +/// @return All generated PointField structs. +/// +template +inline sensor_msgs::msg::PointCloud2::_fields_type generate_fields_from_point() +{ + sensor_msgs::msg::PointCloud2::_fields_type fields; + detail::apply_field_adder_to_generate_field_if_needed(fields); + return fields; +} + +} // namespace point_cloud_msg_wrapper + +#endif // POINT_CLOUD_MSG_WRAPPER__FIELD_GENERATORS_HPP_ diff --git a/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/field_properties.hpp b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/field_properties.hpp new file mode 100644 index 0000000..c85709c --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/field_properties.hpp @@ -0,0 +1,135 @@ +// Copyright 2021 Apex.AI, 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. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + +#ifndef POINT_CLOUD_MSG_WRAPPER__FIELD_PROPERTIES_HPP_ +#define POINT_CLOUD_MSG_WRAPPER__FIELD_PROPERTIES_HPP_ + +#include + +#include + +namespace point_cloud_msg_wrapper +{ + +inline std::uint32_t sizeof_field(const std::uint8_t datatype) +{ + switch (datatype) { + case sensor_msgs::msg::PointField::INT8: + case sensor_msgs::msg::PointField::UINT8: + return 2U; + case sensor_msgs::msg::PointField::INT16: + case sensor_msgs::msg::PointField::UINT16: + return 2U; + case sensor_msgs::msg::PointField::INT32: + case sensor_msgs::msg::PointField::UINT32: + case sensor_msgs::msg::PointField::FLOAT32: + return 4U; + case sensor_msgs::msg::PointField::FLOAT64: + return 8U; + } + throw std::runtime_error("Unexpected datatype provided"); +} + +template +inline constexpr std::uint32_t get_field_count() +{ + static_assert(sizeof(T) == -1, "Only specializations of this function are expected"); + return 0U; +} + +template +inline constexpr std::uint8_t get_field_datatype() +{ + static_assert(sizeof(T) == -1, "Only specializations of this function are expected"); + return 0U; +} + +template<> +inline constexpr std::uint32_t get_field_count() {return 1U;} +template<> +inline constexpr std::uint32_t get_field_count() {return 1U;} +template<> +inline constexpr std::uint32_t get_field_count() {return 1U;} +template<> +inline constexpr std::uint32_t get_field_count() {return 1U;} +template<> +inline constexpr std::uint32_t get_field_count() {return 1U;} +template<> +inline constexpr std::uint32_t get_field_count() {return 1U;} +template<> +inline constexpr std::uint32_t get_field_count() {return 1U;} +template<> +inline constexpr std::uint32_t get_field_count() {return 1U;} +template<> +inline constexpr std::uint32_t get_field_count() {return 2U;} +template<> +inline constexpr std::uint32_t get_field_count() {return 2U;} + +template<> +inline constexpr std::uint8_t get_field_datatype() +{ + return sensor_msgs::msg::PointField::FLOAT32; +} +template<> +inline constexpr std::uint8_t get_field_datatype() +{ + return sensor_msgs::msg::PointField::FLOAT64; +} +template<> +inline constexpr std::uint8_t get_field_datatype() +{ + return sensor_msgs::msg::PointField::INT8; +} +template<> +inline constexpr std::uint8_t get_field_datatype() +{ + return sensor_msgs::msg::PointField::UINT8; +} +template<> +inline constexpr std::uint8_t get_field_datatype() +{ + return sensor_msgs::msg::PointField::INT16; +} +template<> +inline constexpr std::uint8_t get_field_datatype() +{ + return sensor_msgs::msg::PointField::UINT16; +} +template<> +inline constexpr std::uint8_t get_field_datatype() +{ + return sensor_msgs::msg::PointField::INT32; +} +template<> +inline constexpr std::uint8_t get_field_datatype() +{ + return sensor_msgs::msg::PointField::UINT32; +} +template<> +inline constexpr std::uint8_t get_field_datatype() +{ + return sensor_msgs::msg::PointField::INT32; +} +template<> +inline constexpr std::uint8_t get_field_datatype() +{ + return sensor_msgs::msg::PointField::UINT32; +} + +} // namespace point_cloud_msg_wrapper + +#endif // POINT_CLOUD_MSG_WRAPPER__FIELD_PROPERTIES_HPP_ diff --git a/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp new file mode 100644 index 0000000..22d1767 --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp @@ -0,0 +1,507 @@ +// Copyright 2021 Apex.AI, 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. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + + +#ifndef POINT_CLOUD_MSG_WRAPPER__POINT_CLOUD_MSG_WRAPPER_HPP_ +#define POINT_CLOUD_MSG_WRAPPER__POINT_CLOUD_MSG_WRAPPER_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +/// +/// @brief A help macro to simplify conditional compilation based on mutability. +/// +/// This macro just wraps the SFINAE paradigm and enables compilation only if the +/// QUERY_TYPE is mutable. This macro is replaced by RETURN_TYPE in this case. If +/// QUERY_TYPE is const, the code following this macro invocation will not be compiled. +/// +/// @param QUERY_TYPE The query type to be checked for mutability. +/// @param RETURN_TYPE The return type generated if compilation succeeds. +/// +#define COMPILE_IF_MUTABLE(QUERY_TYPE, RETURN_TYPE) \ + template \ + std::enable_if_t::value, RETURN_TYPE> \ + +namespace point_cloud_msg_wrapper +{ + +namespace detail +{ +using DefaultFieldGenerators = std::tuple< + field_x_generator, + field_y_generator, + field_z_generator, + field_id_generator, + field_ring_generator, + field_intensity_generator, + field_timestamp_generator>; +} // namespace detail + +/// +/// @brief This class implements a point cloud message wrapper. Unless otherwise required, use +/// the typedefs of this class: PointCloud2View and PointCloud2Modifier. Only use this +/// class directly if those typedefs do not provide enough flexibility. +/// +/// @details This class is designed to simplify working with point cloud messages. The idea is +/// that it wraps a point cloud message reference allowing for simple access and +/// modification. Upon creation, the wrapper checks if the fields of the message +/// correspond to the fields generated from the point type provided by the user. If this +/// check has passed, then the fields are identical (including the offsets of the +/// members) and it is safe to reinterpret the point cloud message as an array of PointT +/// points. Note that due to these checks it is relatively expensive (the complexity of +/// the checks is around O(n^2) where n is the number of field generators) to create +/// this wrapper, thus it should be avoided in very tight scopes. This operation is not +/// slow, but is significantly slower than a single element access. It is better to +/// create the wrapper once per message in a function scope and batch all the read/write +/// operations afterwards. +/// +/// For convenience, there are two typedefs: PointCloud2View and PointCloud2Modifier. +/// - PointCloudView{msg} wraps a constant point cloud and allows read-only +/// access. Modification through this view is impossible. +/// - PointCloud2Modifier{msg} wraps a mutable message and allows read-write +/// access to the underlying data. +/// - PointCloud2Modifier{msg, new_frame_id} initializes an empty mutable +/// message. This constructor is to be used solely to initialize a new message and +/// will throw if the point cloud message is already initialized (i.e., has non-zero +/// number of fields). +/// +/// @warning This class wraps a raw reference, so the user is responsible to use it in such a way +/// that the underlying point cloud message is not deleted before the wrapper. +/// +/// @tparam PointT Type of point to use for message reading/writing. +/// @tparam PointCloudMsgT Type of point cloud message. +/// @tparam FieldGenerators A tuple of all field generators that allow generating Field structs +/// from members of a PointT struct. See +/// LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER for more details. +/// The class provides a sane default value here, but a custom tuple +/// can be generated by the user by using +/// LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER if needed. +/// @tparam kIsMutable Define if the point cloud message is mutable. Used to conditionally +/// compile certain functions. +/// @tparam AllocatorT The message allocator type. This allocator type must be rebindable +/// to the PointT allocator through the use of +/// `std::allocator_traits::rebind_alloc`. +/// +template< + typename PointT, + template class PointCloudMsgT, + typename FieldGenerators, + const bool kIsMutable, + typename AllocatorT> +class PointCloudMsgWrapper +{ + /// Depending on the provided kIsMutable boolean use the const or a mutable point cloud msg type. + using CloudMsgT = + std::conditional_t, const PointCloudMsgT>; + + static_assert( + std::is_default_constructible::value, + "\n\nThe point type must satisfy is_default_constructible trait.\n\n"); + + static_assert( + detail::is_specialization::value, + "\n\nFieldGenerators must be an std::tuple.\n\n"); + + static_assert( + std::is_same, sensor_msgs::msg::PointCloud2_>::value, + "\n\nThis class is designed to work with PointCloud2 messages only for now.\n\n"); + + /// Type of the fields entry + using FieldNameT = typename sensor_msgs::msg::PointField_::_name_type; + /// Type of the data entry + using DataVectorT = typename sensor_msgs::msg::PointCloud2_::_data_type; + + /// Get the type of the allocator to use with points. + using PointAllocatorT = typename std::allocator_traits::template rebind_alloc; + + /// Derive a point vector type (usually std::vector). Needed in case a different (e.g. + /// bounded vector) type used instead and use it for the Point type with the msg Allocator. + using PointVectorType = + typename detail::derived_point_vector::template type; + + /// Size of the point. + using kSizeofPoint = + std::integral_constant(sizeof(PointT))>; + +public: + using value_type = PointT; + using iterator = typename PointVectorType::iterator; + using const_iterator = typename PointVectorType::const_iterator; + using reverse_iterator = typename PointVectorType::reverse_iterator; + using const_reverse_iterator = typename PointVectorType::const_reverse_iterator; + + /// + /// @brief Constructor that wraps a point cloud message. Checks if the message fields + /// correspond to the ones generated from the PointT type and throws in case of a + /// mismatch. + /// + /// @param cloud_ref A reference to the wrapped cloud. + /// + explicit PointCloudMsgWrapper(CloudMsgT & cloud_ref) + : m_cloud_ref{cloud_ref} + { + if (m_cloud_ref.fields.empty()) { + throw std::runtime_error( + "Trying to wrap a message with no fields. Reset this message with" + "'create_wrapper_from_empty_msg' function instead!"); + } + std::string error_msg{}; + if (!can_be_created_from(cloud_ref, &error_msg)) { + throw std::runtime_error(error_msg); + } + } + + /// + /// A constructor that initializes the message and sets a new frame_id. Note that this only + /// compiles for a mutable cloud. It also throws if the message is already initialized. + /// + /// @param cloud_ref A reference to the wrapped cloud + /// @param[in] frame_id A frame id to be set to the message. + /// + explicit PointCloudMsgWrapper(CloudMsgT & cloud_ref, const FieldNameT & frame_id) + : m_cloud_ref{cloud_ref} + { + static_assert( + !std::is_const::value, + "\n\nWe must be able to modify this point cloud.\n\n"); + static_assert( + detail::has_operator_equals::value, + "\n\nTo guarantee that all struct members are present in the fields of the message, " + "the point struct needs an equality operator defined.\n\n"); + static_assert( + sizeof(PointT) < static_cast(UINT32_MAX), + "\n\nOnly points with sizeof that fits in uint32_t are supported\n\n"); + + if (!m_cloud_ref.fields.empty()) { + throw std::runtime_error( + "Trying to reset a non-empty point cloud message." + " Use reset_msg with the correct wrapper instead."); + } + const auto generated_fields = generate_fields_from_point(); + if (!check_that_generated_fields_cover_all_point_members(generated_fields)) { + throw std::runtime_error( + "Generated fields don't match the members of the point struct. " + "Make sure you have correct generators tuple passed into this class."); + } + reset_msg(frame_id, generated_fields); + } + + + /// + /// @brief Determines ability to be created from a given cloud. + /// + /// @param[in] cloud_msg The cloud message + /// @param error_msg Optional error message + /// + /// @return True if able to be created for the provided cloud, False otherwise. + /// + static bool can_be_created_from(const CloudMsgT & cloud_msg, std::string * error_msg = nullptr) + { + const auto find_missing_field = []( + const auto & query_fields, + const auto & source_fields) -> const FieldNameT * { + for (const auto & query_field : query_fields) { + // Note that we use find on a vector here. This is intended. The number of fields is + // usually very limited, so the O(n^2) complexity is ok here. This operation also only + // takes place roughly once per message. The subsequent read/write operations are then + // appropriately fast. + const auto corresponding_field_iter = std::find_if( + source_fields.begin(), source_fields.end(), [&query_field](const auto & field) { + const bool equal{ + (field.name == query_field.name) && (field.count == query_field.count) && + (field.datatype == query_field.datatype) && (field.offset == query_field.offset)}; + return equal; + }); + const auto found_corresponding_field = (corresponding_field_iter != source_fields.end()); + if (!found_corresponding_field) {return &query_field.name;} + } + return nullptr; + }; + + const auto struct_fields = generate_fields_from_point(); + const auto missing_field_in_cloud = find_missing_field(struct_fields, cloud_msg.fields); + if (missing_field_in_cloud) { + if (error_msg) { + *error_msg = "Point struct has a field that the cloud does not! Field: " + + *missing_field_in_cloud; + } + return false; + } + const auto missing_field_in_struct = find_missing_field(cloud_msg.fields, struct_fields); + if (missing_field_in_struct) { + if (error_msg) { + *error_msg = "Cloud has a field that the point struct does not! Field: " + + *missing_field_in_struct; + } + return false; + } + if (cloud_msg.point_step != kSizeofPoint::value) { + if (error_msg) { + *error_msg = + "Point cloud was created with a point of different sizeof. " + "Are the point members in the same order? Cloud point step = " + std::to_string( + cloud_msg.point_step) + " while sizeof(PointT) = " + std::to_string(sizeof(PointT)); + } + return false; + } + if (sizeof(PointT) * cloud_msg.width != cloud_msg.data.size()) { + if (error_msg) { + *error_msg = + "Point cloud data size " + std::to_string(cloud_msg.data.size()) + + " does not match the data size derived from the point size of count, which is " + + std::to_string(sizeof(PointT) * cloud_msg.width); + } + return false; + } + return true; + } + + /// @brief Push a new point into the message. + COMPILE_IF_MUTABLE(CloudMsgT, void) push_back(const PointT & point) + { + PointT point_copy{point}; + extend_data_by(sizeof(PointT)); + m_cloud_ref.row_step += kSizeofPoint::value; + m_cloud_ref.width++; + this->operator[](m_cloud_ref.width - 1U) = point_copy; + } + + /// @brief Push a new point into the message. + COMPILE_IF_MUTABLE(CloudMsgT, void) push_back(PointT && point) + { + extend_data_by(sizeof(PointT)); + m_cloud_ref.row_step += kSizeofPoint::value; + m_cloud_ref.width++; + this->operator[](m_cloud_ref.width - 1U) = std::move(point); + } + + /// Get the number of points in the message. + std::size_t size() const noexcept {return m_cloud_ref.width;} + + /// Check if the point cloud message stores no points. + bool empty() const noexcept {return m_cloud_ref.width == 0U;} + + /// Get the first point. + COMPILE_IF_MUTABLE(CloudMsgT, PointT &) front() noexcept {return *begin();} + /// Get the first point. + const PointT & front() const noexcept {return *begin();} + /// Get the last point. + COMPILE_IF_MUTABLE(CloudMsgT, PointT &) back() noexcept {return *rbegin();} + /// Get the last point. + const PointT & back() const noexcept {return *rbegin();} + + /// Get a point reference at the specified index. + /// @throws std::runtime_error if the index is out of bounds. + const PointT & at(const std::size_t index) const + { + if (index >= size()) { + throw std::out_of_range( + "Index is out of bounds, " + + std::to_string(index) + " >= " + std::to_string(size())); + } + return reinterpret_cast( + *(m_cloud_ref.data.data() + index * sizeof(PointT))); + } + /// Get a point reference at the specified index. Only compiled if message is not const. + /// @throws std::runtime_error if the index is out of bounds. + COMPILE_IF_MUTABLE(CloudMsgT, PointT &) at(const std::size_t index) + { + if (index >= size()) { + throw std::out_of_range( + "Index is out of bounds, " + + std::to_string(index) + " >= " + std::to_string(size())); + } + return reinterpret_cast(*(m_cloud_ref.data.data() + index * sizeof(PointT))); + } + + /// Get a point reference at the specified index. + const PointT & operator[](const std::size_t index) const noexcept + { + return *reinterpret_cast( + m_cloud_ref.data.data() + index * sizeof(PointT)); + } + /// Get a point reference as a specified index. Only compiled if message type is not const. + COMPILE_IF_MUTABLE(CloudMsgT, PointT &) operator[](const std::size_t index) noexcept + { + return *reinterpret_cast(m_cloud_ref.data.data() + index * sizeof(PointT)); + } + + /// @brief Reset the message fields to match the members of the PointT struct. The point + /// cloud message is ready for modification after this operation. + COMPILE_IF_MUTABLE(CloudMsgT, void) reset_msg(const FieldNameT & frame_id) + { + reset_msg(frame_id, generate_fields_from_point()); + } + + /// @brief Clear the message. + COMPILE_IF_MUTABLE(CloudMsgT, void) clear() { + reset_msg(m_cloud_ref.header.frame_id); + } + + /// @brief Allocate memory to hold a specified number of points. + COMPILE_IF_MUTABLE(CloudMsgT, void) reserve(const size_t expected_number_of_points) { + m_cloud_ref.data.reserve(sizeof(PointT) * expected_number_of_points); + } + + /// @brief Resize the container to hold a given number of points. + COMPILE_IF_MUTABLE(CloudMsgT, void) resize(const std::uint32_t new_number_of_points) { + m_cloud_ref.width = new_number_of_points; + m_cloud_ref.row_step = m_cloud_ref.width * kSizeofPoint::value; + m_cloud_ref.data.resize(m_cloud_ref.row_step); + } + + /// An iterator to the beginning of data. Only compiled if point cloud message type is not const. + COMPILE_IF_MUTABLE(CloudMsgT, iterator) begin() noexcept { + return iterator{reinterpret_cast(&m_cloud_ref.data[0U])}; + } + /// An iterator to the end of data. Only compiled if point cloud message type is not const. + COMPILE_IF_MUTABLE(CloudMsgT, iterator) end() noexcept { + return iterator{reinterpret_cast(&m_cloud_ref.data[m_cloud_ref.data.size()])}; + } + /// A reverse iterator staring position. Only compiled if point cloud message type is not const. + COMPILE_IF_MUTABLE(CloudMsgT, reverse_iterator) rbegin() noexcept { + return std::make_reverse_iterator(end()); + } + /// A reverse iterator end position. Only compiled if point cloud message type is not const. + COMPILE_IF_MUTABLE(CloudMsgT, reverse_iterator) rend() noexcept { + return std::make_reverse_iterator(begin()); + } + /// A constant iterator to the beginning of data. + const_iterator begin() const noexcept {return cbegin();} + /// A constant iterator to the end of data. + const_iterator end() const noexcept {return cend();} + /// A constant reverse iterator to the beginning of the reversed data. + const_reverse_iterator rbegin() const noexcept {return crbegin();} + /// A constant reverse iterator to the end of reversed data. + const_reverse_iterator rend() const noexcept {return crend();} + /// A constant iterator to the beginning of data. + const_iterator cbegin() const noexcept + { + return const_iterator{reinterpret_cast(&m_cloud_ref.data[0U])}; + } + /// A constant iterator to the end of data. + const_iterator cend() const noexcept + { + return const_iterator{ + reinterpret_cast(&m_cloud_ref.data[m_cloud_ref.data.size()])}; + } + /// A constant reverse iterator to the reverse beginning of data. + const_reverse_iterator crbegin() const noexcept {return std::make_reverse_iterator(cend());} + /// A constant reverse iterator to the reverse end of data. + const_reverse_iterator crend() const noexcept {return std::make_reverse_iterator(cbegin());} + +private: + /// Allocate additional memory in the end of data field of the message. + COMPILE_IF_MUTABLE(CloudMsgT, void) extend_data_by(const std::size_t bytes_to_allocate) + { + const auto new_size = m_cloud_ref.data.size() + bytes_to_allocate; + m_cloud_ref.data.resize(new_size); + } + + /// @brief Reset the message fields to match the members of the PointT struct. The point + /// cloud message is ready for modification after this operation. + COMPILE_IF_MUTABLE(CloudMsgT, void) reset_msg( + const FieldNameT & frame_id, + const sensor_msgs::msg::PointCloud2::_fields_type & generated_fields) + { + // TODO(igor): All these settings can also be specified through some input struct if needed in + // the future. This is omitted for now. + m_cloud_ref.fields = generated_fields; + m_cloud_ref.height = 1; + m_cloud_ref.is_bigendian = false; + m_cloud_ref.is_dense = false; + m_cloud_ref.header.frame_id = frame_id; + m_cloud_ref.point_step = kSizeofPoint::value; + m_cloud_ref.data.clear(); + m_cloud_ref.width = 0U; + m_cloud_ref.row_step = 0U; + } + + bool check_that_generated_fields_cover_all_point_members( + const sensor_msgs::msg::PointCloud2::_fields_type & generated_fields) + { + std::uint32_t sum_of_all_fields_sizes{}; + for (const auto & field : generated_fields) { + sum_of_all_fields_sizes += field.count * sizeof_field(field.datatype); + } + if (sum_of_all_fields_sizes == kSizeofPoint::value) { + // The generated fields cover all members of the struct without gaps. + return true; + } + // The idea is the following: + // 1. Allocate storage as big as a point. + // 2. Fill the storage with zeros. + // 3. Iterate through all fields and set the appropriate memory to ones. + // 4. Flip all bits - now, if all fields are present, only the padding is non-zero. + // 5. Reinterpret this memory as a point. + // 6. Compare this point to a value initialized one. + // If these points are equal - we have all fields. Otherwise, we are missing a field. + alignas(PointT) std::uint8_t buffer[sizeof(PointT)]; + std::fill(buffer, buffer + sizeof(PointT), 0U); + for (const auto & field : generated_fields) { + const auto field_first_byte = field.offset; + const auto field_last_byte = + field.offset + field.count * static_cast(sizeof_field(field.datatype)); + std::fill( + &buffer[field_first_byte], + &buffer[field_last_byte], + std::numeric_limits::max()); + } + for (auto i = 0U; i < sizeof(PointT); ++i) { + buffer[i] = buffer[i] > 0 ? 0 : std::numeric_limits::max(); // Flip all bits. + } + const PointT * const point_from_flipped_memory{reinterpret_cast(buffer)}; + if (PointT{} == *point_from_flipped_memory) { + return true; + } + return false; + } + + /// A reference to the cloud message. + CloudMsgT & m_cloud_ref; +}; + +/// A typedef for the PointCloudMsgWrapper to represent a view that wraps a const cloud message. +template< + typename PointT, + typename FieldGeneratorsT = detail::DefaultFieldGenerators, + typename AllocatorT = std::allocator> +using PointCloud2View = PointCloudMsgWrapper< + PointT, sensor_msgs::msg::PointCloud2_, FieldGeneratorsT, false, AllocatorT>; + +/// A typedef for the PointCloudMsgWrapper to represent a view that wraps a mutable cloud message. +template< + typename PointT, + typename FieldGeneratorsT = detail::DefaultFieldGenerators, + typename AllocatorT = std::allocator> +using PointCloud2Modifier = PointCloudMsgWrapper< + PointT, sensor_msgs::msg::PointCloud2_, FieldGeneratorsT, true, AllocatorT>; + +} // namespace point_cloud_msg_wrapper + +#undef COMPILE_IF_MUTABLE + +#endif // POINT_CLOUD_MSG_WRAPPER__POINT_CLOUD_MSG_WRAPPER_HPP_ diff --git a/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/type_traits.hpp b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/type_traits.hpp new file mode 100644 index 0000000..ba13225 --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/type_traits.hpp @@ -0,0 +1,85 @@ +// Copyright 2021 Apex.AI, 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. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + +#ifndef POINT_CLOUD_MSG_WRAPPER__TYPE_TRAITS_HPP_ +#define POINT_CLOUD_MSG_WRAPPER__TYPE_TRAITS_HPP_ + +#include +#include + +namespace point_cloud_msg_wrapper +{ +namespace detail +{ + +// TODO(igor): this is a general trait, should it live elsewhere? +template class Ref> +struct is_specialization : std::false_type {}; + +template class Ref, typename ... Args> +struct is_specialization, Ref>: std::true_type {}; + +// Taken from https://stackoverflow.com/a/35207812/1763680 +template +struct has_operator_equals_impl +{ + template + static auto test(U *)->decltype(std::declval() == std::declval()); + template + static auto test(...)->std::false_type; + + using type = typename std::is_same(0))>::type; +}; + +template +struct has_operator_equals : has_operator_equals_impl::type {}; + +/// Detect the template type used under the hood. +/// Provided std::vector type will be std::vector. +template +struct derived_point_vector; + +/// An overload for a container like an std::vector>. +template class ContainerT, class PointT> +struct derived_point_vector, PointT> +{ + template + using type = ContainerT; +}; + +/// An overload for containers like a bounded_vector>. +template< + class ValueT, + std::size_t UPPER_BOUND, + class AllocatorT, + template class ContainerT, + class PointT> +struct derived_point_vector, PointT> +{ + using kPointCapacity = std::integral_constant< + std::uint32_t, ContainerT::capacity() / + static_cast(sizeof(PointT))>; + + template + using type = + ContainerT; +}; + +} // namespace detail +} // namespace point_cloud_msg_wrapper + +#endif // POINT_CLOUD_MSG_WRAPPER__TYPE_TRAITS_HPP_ diff --git a/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/visibility_control.hpp b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/visibility_control.hpp new file mode 100644 index 0000000..2b43d17 --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/include/point_cloud_msg_wrapper/visibility_control.hpp @@ -0,0 +1,40 @@ +// Copyright 2021 Apex.AI, 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. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + + +#ifndef POINT_CLOUD_MSG_WRAPPER__VISIBILITY_CONTROL_HPP_ +#define POINT_CLOUD_MSG_WRAPPER__VISIBILITY_CONTROL_HPP_ + +#if defined(_MSC_VER) && defined(_WIN64) + #if defined(POINT_CLOUD_MSG_WRAPPER_BUILDING_DLL) || defined(POINT_CLOUD_MSG_WRAPPER_EXPORTS) + #define POINT_CLOUD_MSG_WRAPPER_PUBLIC __declspec(dllexport) + #define POINT_CLOUD_MSG_WRAPPER_LOCAL + #else + #define POINT_CLOUD_MSG_WRAPPER_PUBLIC __declspec(dllimport) + #define POINT_CLOUD_MSG_WRAPPER_LOCAL + #endif +#elif defined(__GNUC__) && defined(__linux__) + #define POINT_CLOUD_MSG_WRAPPER_PUBLIC __attribute__((visibility("default"))) + #define POINT_CLOUD_MSG_WRAPPER_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__GNUC__) && defined(__APPLE__) + #define POINT_CLOUD_MSG_WRAPPER_PUBLIC __attribute__((visibility("default"))) + #define POINT_CLOUD_MSG_WRAPPER_LOCAL __attribute__((visibility("hidden"))) +#else // !(defined(__GNUC__) && defined(__APPLE__)) + #error "Unsupported Build Configuration" +#endif // _MSC_VER + +#endif // POINT_CLOUD_MSG_WRAPPER__VISIBILITY_CONTROL_HPP_ diff --git a/src/tools/point_cloud_msg_wrapper/package.xml b/src/tools/point_cloud_msg_wrapper/package.xml new file mode 100644 index 0000000..29713bd --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/package.xml @@ -0,0 +1,24 @@ + + + + point_cloud_msg_wrapper + 1.0.7 + A point cloud message wrapper that allows for simple and safe PointCloud2 msg usage + Apex.AI, Inc. + Apache 2.0 + Igor Bogoslavskyi + M. Fatih Cırıt + + ament_cmake_auto + + ament_cmake + + sensor_msgs + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + geometry_msgs + + ament_cmake + diff --git a/src/tools/point_cloud_msg_wrapper/test/test_field_generators.cpp b/src/tools/point_cloud_msg_wrapper/test/test_field_generators.cpp new file mode 100644 index 0000000..da4ee3b --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/test/test_field_generators.cpp @@ -0,0 +1,53 @@ +// Copyright 2021 Apex.AI, 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. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + +#include +#include + +#include + +#include +#include + +namespace +{ +struct PointXI +{ + std::int64_t id; + float x; +}; + +} // namespace + +TEST(FieldGeneratorsTest, init) { + sensor_msgs::msg::PointCloud2::_fields_type fields; + point_cloud_msg_wrapper::field_y_generator::push_back_field_if_needed(fields); + ASSERT_TRUE(fields.empty()); + point_cloud_msg_wrapper::field_x_generator::push_back_field_if_needed(fields); + ASSERT_EQ(1UL, fields.size()); + EXPECT_EQ("x", fields.back().name); + EXPECT_EQ(sensor_msgs::msg::PointField::FLOAT32, fields.back().datatype); + EXPECT_EQ(1U, fields.back().count); + EXPECT_EQ(8U, fields.back().offset); + + point_cloud_msg_wrapper::field_id_generator::push_back_field_if_needed(fields); + ASSERT_EQ(2UL, fields.size()); + EXPECT_EQ("id", fields.back().name); + EXPECT_EQ(sensor_msgs::msg::PointField::INT32, fields.back().datatype); + EXPECT_EQ(2U, fields.back().count); + EXPECT_EQ(0U, fields.back().offset); +} diff --git a/src/tools/point_cloud_msg_wrapper/test/test_point_cloud_msg_wrapper.cpp b/src/tools/point_cloud_msg_wrapper/test/test_point_cloud_msg_wrapper.cpp new file mode 100644 index 0000000..6e3c7e4 --- /dev/null +++ b/src/tools/point_cloud_msg_wrapper/test/test_point_cloud_msg_wrapper.cpp @@ -0,0 +1,364 @@ +// Copyright 2021 Apex.AI, 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. + +/// \copyright Copyright 2021 Apex.AI, Inc. +/// All rights reserved. + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace +{ +struct PointX; +struct PointXYZI; +struct PointXYWithVirtualDestructor; +class ClassPointXY; +using GeometryPointXYZ = geometry_msgs::msg::Point32; + +template::value> * = nullptr> +constexpr bool nearly_equal( + const T & a, + const T & b, + const T & epsilon = std::numeric_limits::epsilon()) noexcept +{ + return std::fabs(a - b) <= + (epsilon * std::max(std::fabs(a), std::fabs(b))); +} + + +struct PointWithCustomField +{ + float x; + double non_standard_test_field; + std::int32_t y; + friend bool operator==(const PointWithCustomField & p1, const PointWithCustomField & p2) noexcept + { + return + ::nearly_equal(p1.x, p2.x) && + ::nearly_equal(p1.non_standard_test_field, p2.non_standard_test_field) && + (p1.y == p2.y); + } +}; + +struct CustomAlignedPoint +{ + float x; + float y; + float z; + alignas(double) std::uint8_t intensity; + double timestamp; + friend bool operator==(const CustomAlignedPoint & p1, const CustomAlignedPoint & p2) noexcept + { + return + ::nearly_equal(p1.x, p2.x) && + ::nearly_equal(p1.y, p2.y) && + ::nearly_equal(p1.z, p2.z) && + ::nearly_equal(p1.timestamp, p2.timestamp) && + (p1.intensity == p2.intensity); + } +}; + +struct PointNotPresentInAllPointTypes {std::int8_t x;}; + +template +PointT create_point() {return PointT{};} +template<> +PointX create_point(); +template<> +PointXYZI create_point(); +template<> +ClassPointXY create_point(); +template<> +PointXYWithVirtualDestructor create_point(); +template<> +GeometryPointXYZ create_point(); +template<> +CustomAlignedPoint create_point(); + +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(non_standard_test_field); + +} // namespace + +template +class PointCloudMsgWrapperTest : public testing::Test +{ +public: + using PointT = T; +}; + +using AllPointTypes = ::testing::Types< + PointX, + PointXYZI, + ClassPointXY, + PointXYWithVirtualDestructor, + CustomAlignedPoint, + GeometryPointXYZ>; +// cppcheck-suppress syntaxError - trailing comma is the only way to remove the compiler warning. +TYPED_TEST_CASE(PointCloudMsgWrapperTest, AllPointTypes, ); + +using point_cloud_msg_wrapper::PointCloud2View; +using point_cloud_msg_wrapper::PointCloud2Modifier; + +/// @test Test that for any of the different types of points we can read and write them into msg. +TYPED_TEST(PointCloudMsgWrapperTest, ReadingAndWritingGenericPoints) +{ + using Point = typename TestFixture::PointT; + sensor_msgs::msg::PointCloud2 msg; + + // Cannot initialize a wrapper without resetting an empty message. + EXPECT_THROW(PointCloud2View{msg}, std::runtime_error); + EXPECT_THROW(PointCloud2Modifier{msg}, std::runtime_error); + + PointCloud2Modifier cloud_wrapper{msg, "some_frame_id"}; + EXPECT_EQ(msg.header.frame_id, "some_frame_id"); + + const auto point = create_point(); + + ASSERT_FALSE(msg.fields.empty()); + ASSERT_FALSE(msg.fields.front().name.empty()); + ASSERT_NO_THROW(cloud_wrapper.push_back(point)); + + ASSERT_THROW(PointCloud2View{msg}, std::runtime_error); + ASSERT_THROW(PointCloud2Modifier{msg}, std::runtime_error); + + ASSERT_TRUE(PointCloud2Modifier::can_be_created_from(msg)); + const auto initialized_wrapper = PointCloud2Modifier{msg}; + ASSERT_EQ(initialized_wrapper.size(), 1U); + EXPECT_EQ(initialized_wrapper.at(0U), point); + EXPECT_EQ(initialized_wrapper[0U], point); + EXPECT_NO_THROW(cloud_wrapper.push_back(initialized_wrapper[0U])); + EXPECT_EQ(initialized_wrapper.size(), 2U); + + const auto & const_cloud = msg; + const PointCloud2View const_wrapper{const_cloud}; + ASSERT_EQ(const_wrapper.size(), 2U); + EXPECT_EQ(const_wrapper.at(0U), point); + EXPECT_EQ(const_wrapper[0U], point); + EXPECT_EQ(const_wrapper.at(1U), point); + EXPECT_EQ(const_wrapper[1U], point); + + // Test that we can iterate over the message. + std::array points; + std::size_t index{}; + for (const auto & p : cloud_wrapper) { + points[index] = p; + index++; + } + ASSERT_EQ(points.size(), cloud_wrapper.size()); + for (auto i = 0U; i < points.size(); ++i) { + EXPECT_EQ(points[i], cloud_wrapper[i]); + } + EXPECT_EQ(points.front(), cloud_wrapper.front()); + EXPECT_EQ(points.back(), cloud_wrapper.back()); + + cloud_wrapper.resize(3U); + EXPECT_EQ(cloud_wrapper.size(), 3U); + // Check that the untouched points stayed the same. + for (auto i = 0U; i < points.size(); ++i) { + EXPECT_EQ(points[i], cloud_wrapper[i]); + } + + // Change values. + for (auto & p : cloud_wrapper) { + p = Point{}; + } + for (const auto & p : cloud_wrapper) { + EXPECT_EQ(Point{}, p); + } + + cloud_wrapper.clear(); + EXPECT_TRUE(cloud_wrapper.empty()); + + // Cannot reinitialize an already initialized message. + EXPECT_THROW(PointCloud2Modifier(msg, "some_new_frame_id"), std::runtime_error); + cloud_wrapper.reset_msg("some_new_frame_id"); + EXPECT_EQ(msg.header.frame_id, "some_new_frame_id"); + EXPECT_TRUE(cloud_wrapper.empty()); +} + +/// @test Check that using using iterator including std::back_inserter is possible with the wrapper. +TYPED_TEST(PointCloudMsgWrapperTest, Iterators) { + using Point = typename TestFixture::PointT; + sensor_msgs::msg::PointCloud2 msg; + PointCloud2Modifier cloud_wrapper{msg, "some_frame_id"}; + std::array points{{create_point(), Point{}}}; + // Fill the message from an array. + std::transform( + points.cbegin(), points.cend(), std::back_inserter(cloud_wrapper), + [](const Point & point) {return point;}); + ASSERT_EQ(points.size(), cloud_wrapper.size()); + for (auto i = 0U; i < points.size(); ++i) { + EXPECT_EQ(points[i], cloud_wrapper[i]); + } + // Change existing values. + for (auto & point : cloud_wrapper) { + point = Point{}; + } + const auto & const_cloud = msg; + const PointCloud2View const_wrapper{const_cloud}; + for (const auto & point : const_wrapper) { + EXPECT_EQ(Point{}, point); + } + + // Check the reverse iteration. + for (auto riter = const_wrapper.rbegin(); riter != const_wrapper.rend(); ++riter) { + EXPECT_EQ(Point{}, *riter); + } +} + +/// @test Check initialization with a complicated type with an additional custom field. +TEST(PointCloudMsgWrapperTest, ProcessPointWithCustomField) { + sensor_msgs::msg::PointCloud2 msg; + // There is no matching field generator for non_standard_test_field present. + EXPECT_THROW(PointCloud2Modifier(msg, "some_frame_id"), std::runtime_error); + EXPECT_THROW(PointCloud2View{msg}, std::runtime_error); + using Generators = std::tuple< + point_cloud_msg_wrapper::field_x_generator, + point_cloud_msg_wrapper::field_y_generator, + field_non_standard_test_field_generator>; + using CustomCloudModifier = PointCloud2Modifier; + // Cannot initialize message without a new frame id provided. + EXPECT_THROW(CustomCloudModifier{msg}, std::runtime_error); + + CustomCloudModifier cloud_wrapper{msg, "some_frame_id"}; + ASSERT_EQ(msg.fields.size(), 3U); + // Note that the order of fields is defined by the order of generators in the tuple, NOT by the + // order of members in the struct. However, the order of fields plays no role in point + // representation within the message. + EXPECT_EQ(msg.fields[0].name, "x"); + EXPECT_EQ(msg.fields[1].name, "y"); + EXPECT_EQ(msg.fields[2].name, "non_standard_test_field"); + EXPECT_NO_THROW(cloud_wrapper.push_back({42.0F, 42.0, 23})); + + const CustomCloudModifier initialized_wrapper{msg}; + ASSERT_FALSE(initialized_wrapper.empty()); + ASSERT_EQ(initialized_wrapper.size(), 1U); + EXPECT_FLOAT_EQ(initialized_wrapper.at(0U).x, 42.0F); + EXPECT_DOUBLE_EQ(initialized_wrapper.at(0U).non_standard_test_field, 42.0); + EXPECT_EQ(initialized_wrapper.at(0U).y, 23); + EXPECT_THROW(initialized_wrapper.at(1U), std::out_of_range); + EXPECT_FLOAT_EQ(initialized_wrapper[0U].x, 42.0F); + EXPECT_DOUBLE_EQ(initialized_wrapper[0U].non_standard_test_field, 42.0); + EXPECT_EQ(initialized_wrapper[0U].y, 23); + EXPECT_NO_THROW(cloud_wrapper.push_back(initialized_wrapper[0U])); + ASSERT_EQ(initialized_wrapper.size(), 2U); +} + +/// @test Check that a macro we use for readability is not leaking outside of the header file. +TEST(PointCloudMsgWrapperTest, CompilationMacroIsUnset) { + #ifdef COMPILE_IF_MUTABLE + FAIL() << "Compilation macro should not be available outside of point_cloud_msg_wrapper.hpp file"; + #endif +} + + +namespace +{ + +struct PointX +{ + float x; + friend bool operator==(const PointX & p1, const PointX & p2) noexcept + { + return ::nearly_equal(p1.x, p2.x); + } +}; + +class ClassPointXY +{ +public: + ClassPointXY() = default; + explicit ClassPointXY(float x, float y) + : x_{x}, y_{y} {} + + float & x() {return x_;} + const float & x() const {return x_;} + float & y() {return y_;} + const float & y() const {return y_;} + + friend bool operator==(const ClassPointXY & p1, const ClassPointXY & p2) noexcept + { + return ::nearly_equal(p1.x_, p2.x_) && + ::nearly_equal(p1.y_, p2.y_); + } + +private: + float x_; + float y_; +}; + +struct PointXYZI +{ + float x; + float y; + float z; + std::int64_t id; + friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept + { + return ::nearly_equal(p1.x, p2.x) && + ::nearly_equal(p1.y, p2.y) && + ::nearly_equal(p1.z, p2.z) && + (p1.id == p2.id); + } +}; + +struct PointXYWithVirtualDestructor +{ + PointXYWithVirtualDestructor() = default; + PointXYWithVirtualDestructor(float x_, float y_) + : x{x_}, y{y_} {} + + virtual ~PointXYWithVirtualDestructor() {} + + friend bool operator==( + const PointXYWithVirtualDestructor & p1, + const PointXYWithVirtualDestructor & p2) noexcept + { + return ::nearly_equal(p1.x, p2.x) && + ::nearly_equal(p1.y, p2.y); + } + + float x{}; + float y{}; +}; + +template<> +PointX create_point() {return PointX{42.0F};} +template<> +PointXYZI create_point() {return PointXYZI{42.0F, 23.0F, 13.0F, 42LL};} +template<> +ClassPointXY create_point() {return ClassPointXY{42.0F, 23.0F};} +template<> +PointXYWithVirtualDestructor create_point() {return PointXYWithVirtualDestructor{42.0F, 23.0F};} +template<> +GeometryPointXYZ create_point() +{ + GeometryPointXYZ point; + point.set__x(42.0F).set__y(23.0F).set__x(13.0F); + return point; +} +template<> +CustomAlignedPoint create_point() {return CustomAlignedPoint{42.0F, 23.0F, 4242.0F, 23, 2323.0};} +} // namespace