diff --git a/.github/workflows/dockerbuild.yml b/.github/workflows/dockerbuild.yml index 45bd366..2bd920c 100644 --- a/.github/workflows/dockerbuild.yml +++ b/.github/workflows/dockerbuild.yml @@ -21,7 +21,7 @@ jobs: run: docker build . -t umrr-ros:latest - name: Building the driver with the docker container - run: docker run --rm -v`pwd`:/code umrr-ros colcon build + run: docker run --rm -v`pwd`:/code umrr-ros colcon build --packages-skip smart_rviz_plugin - name: Running the unit/integration tests via the docker container and exit run: docker-compose up diff --git a/CHANGELOG.md b/CHANGELOG.md index ee8dcb9..f6de3e3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,51 +3,108 @@ All notable changes to this project will be documented in this file. This projec ## v1.0.0 - 2021-11-01 -This is the first official versioned release of the `smartmicro_ros2_radars`. It provides a ros2 node that is wrapped around `Smart Access C++ API v3.3.15`, interfacing with smartmicro automotive radars and publishing the incoming data as point cloud. +### 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 -This major release of the driver includes multi-user interfaces. The driver now supports and publishes data from UMRR96 and UMRR11. -A new test approach has been implemented which simulates the sensors and interfaces with the node hence making the tests more robust. -Requires new sensor firmware, if using UMRR11-T132: V5.1.4 and if using UMRR96-T153: V5.2.4. +### 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 +# v2.1.0 - 2022-06-02 -This minor release of the driver offer ros2 services to communicate with the sensor. The driver now supports mode changes for UMRR96 and UMRR11. -This release also offers the possibility of configuring the ip addresses of the sensor using ros2 services. The tests are further extended to include ros2 services check. Requires new sensor firmware, if using UMRR11-T132: V5.1.4 and if using UMRR96-T153: V5.2.4. +### 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 release includes the new smartmicro sensor DRVEGRD 169. The driver offers mode changes and configuration of the DRVEGRD 169 along with publishing the radar targets as point cloud data. The callbacks for datastream now require a clientID. +### 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 -This release includes the new smartmicro sensor DRVEGRD 152. The driver offers mode changes and configuration of the DRVEGRD 152 along with publishing the radar targets as point cloud data. The callbacks for datastream now require a clientID. +### 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 -This minor release introduces signal-to-noise field in the point clouds and also fixes the max number of sensors that could be connected at once. +### 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 -This release fixes the offset which causes anomaly in point clouds from DRVEGRD 152 sensor. It also fixes the timestamp calculation bug which causes rviz to crash and updates the simulator source files. +### 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 -This release includes the new DRVEGRD 169 user-interface. The release includes new modes for DRVEGRD 169 and also introduces an additional ros2 service to save configurations. Previously used model 'UMRR9F' in radar parameters has been divided into two versions, the 'UMRR9F_V1_1_1' and 'UMRR9F_V2_0_0'. +### 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 -This release includes the new DRVEGRD 171 and DRVEGRD 152 user-interface. The release includes a complete list of all params and commands for all sensors which are accessed using the ros2 services. +### 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 -This release includes CAN communication for all the provided sensor types and sensors interfaces. The params are now extended to include the params/setting for the connected adapters along with the sensor params. +### 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 -This release includes features for downloading sensor firmware on to the sensors. It also provides custom RVIZ plugins to log the target list data, record and save it. It has a plugin to send instructions to the sensors and also it is possible to dowload the firmware too. This release also includes a python GUI to send custom CAN messages. The radar param templates have now been merged into one param file. The pointcloud has been extended to also include the polar coordinates. Additionally, the release also includes new user interface for sensor A4 T171. +### 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 -This release includes the new user interfaces for DRVEGRD 169 and DRVEGRD 152. This releaase also fixes some issues with the smart record plugin and offers now azimuth and elevation angles in degrees. +### 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. diff --git a/Readme.md b/Readme.md index cc9ac88..8c2f121 100644 --- a/Readme.md +++ b/Readme.md @@ -38,39 +38,36 @@ python custom_can_sender.py - ROS2 foxy ### UMRR radars and Smart Access API version -A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVEGRD 171, DRVEGRD 152 or DRVEGRD 169 radar are +A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVEGRD 171, DRVEGRD 152, DRVEGRD 169 or DRVEGRD 169 MSE radar are 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: `January 26, 2023` -- Smart Access Automotive version: `v3.7.0` -- User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.2` -- User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.2` -- User interface version: `UMRR9F Type 169 AUTOMOTIVE v1.1.1` -- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.0.0` -- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.1.1` -- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.2.1` -- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.5.0` -- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.3` -- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.2.2` -- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.5.0` -- User interface version: `UMRRA4 Type 171 AUTOMOTIVE v1.0.1` -- User interface version: `UMRRA4 Type 171 AUTOMOTIVE v1.3.0` - -### Sensor Firmwares -This ROS2 driver release is compatible with the following sensor firmwares: -- UMRR11 Type 132: V5.1.4 -- UMRR96 Type 153: V5.2.4 -- UMRR9D Type 152: V2.1.0 -- UMRR9D Type 152: V2.5.0 -- UMRR9D Type 152: V2.7.0 -- UMRR9F Type 169: V1.3.0 -- UMRR9F Type 169: V2.0.2 -- UMRR9F Type 169: V2.2.0 -- UMRR9F Type 169: V2.4.0 -- UMRRA4 Type 171: V1.0.0 -- UMRRA4 Type 171: V1.2.1 -- UMRRA4 Type 171: V1.3.0 +- Date of release: `June 07, 2024` +- Smart Access Automotive version: `v3.8.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 | +| 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 | +| 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 | ### Point cloud message wrapper library To add targets to the point cloud in a safe and quick fashion a @@ -116,8 +113,8 @@ For more details, see the [`radar.sensor.example.yaml`](umrr_ros2_driver/param/r For the setting up the ***sensors***: - `link_type`: the type of hardware connection - `model`: the model of the sensor being used - - can: 'umrra4_can_v1_0_1', 'umrr96_can', 'umrr11_can', 'umrr9d_can_v1_0_3', 'umrr9d_can_v1_2_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1' - - port: 'umrra4_v1_0_1', 'umrr96', 'umrr11', 'umrr9d_v1_0_3', 'umrr9d_v1_2_2', 'umrr9f_v1_1_1', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1', + - can: 'umrr9f_can_mse_v1_0_0', 'umrra4_can_v1_0_1', 'umrr96_can_v1_2_2', 'umrr11_can_v1_1_2', 'umrr9d_can_v1_0_3', 'umrr9d_can_v1_2_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1' + - port: 'umrr9f_mse_v1_0_0', 'umrra4_v1_0_1', 'umrr96_v1_2_2', 'umrr11_v1_1_2', 'umrr9d_v1_0_3', 'umrr9d_v1_2_2', 'umrr9f_v1_1_1', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1' - `dev_id`: adapter id to which sensor is connected. ***The adapter and sensor should have the same dev_id*** - `id`: the client_id of the sensor/source, ***must be a _unique_ integer and non-zero***. - `ip`: the ***_unique_*** ip address of the sensor or of the source acting as a sensor, required only for sensors using _ethernet_. @@ -234,7 +231,7 @@ docker build . -t umrr-ros:latest Building the driver with the docker container ```bash -docker run --rm -v`pwd`:/code umrr-ros colcon build +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 diff --git a/smart_extract.sh b/smart_extract.sh index 1af7f6e..108c42a 100755 --- a/smart_extract.sh +++ b/smart_extract.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -smart_pack=SmartAccessAutomotive_3_7_0.tgz +smart_pack=SmartAccessAutomotive_3_8_0.tgz URL_smartbinaries=https://www.smartmicro.com/fileadmin/media/Downloads/Automotive_Radar/Software/${smart_pack} cat << EOF diff --git a/smart_rviz_plugin/CMakeLists.txt b/smart_rviz_plugin/CMakeLists.txt index 3e20bc9..50e5cf8 100644 --- a/smart_rviz_plugin/CMakeLists.txt +++ b/smart_rviz_plugin/CMakeLists.txt @@ -23,7 +23,8 @@ 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) @@ -31,12 +32,14 @@ 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 @@ -54,6 +57,8 @@ set(dependencies rviz_ogre_vendor rviz_rendering std_msgs + OpenCV + cv_bridge ) ament_target_dependencies(smart_rviz_plugin @@ -64,6 +69,7 @@ target_include_directories(smart_rviz_plugin PUBLIC $ $ ${Qt5Widgets_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) diff --git a/smart_rviz_plugin/config/images/recorder_rviz.png b/smart_rviz_plugin/config/images/recorder_rviz.png index dd45f91..3f73ca3 100644 Binary files a/smart_rviz_plugin/config/images/recorder_rviz.png and b/smart_rviz_plugin/config/images/recorder_rviz.png differ diff --git a/smart_rviz_plugin/config/rviz/recorder.rviz b/smart_rviz_plugin/config/rviz/recorder.rviz index 7032545..a99da53 100644 --- a/smart_rviz_plugin/config/rviz/recorder.rviz +++ b/smart_rviz_plugin/config/rviz/recorder.rviz @@ -1,17 +1,21 @@ Panels: - Class: rviz_common/Displays - Help Height: 0 + Help Height: 81 Name: Displays Property Tree Widget: Expanded: - - /Grid1/Offset1 + - /Global Options1 + - /Status1 - /PointCloud21 + - /PointCloud22 + - /PointCloud23 Splitter Ratio: 0.5 - Tree Height: 370 + Tree Height: 1140 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 @@ -20,15 +24,23 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: smart_rviz_plugin/TargetList Recorder - Name: TargetList Recorder + - 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 Firmware Download + Name: Smart Firmware Download + - Class: smart_rviz_plugin/Smart Status + Name: Smart Status + - Class: smart_rviz_plugin/Smart Recorder + Name: Smart Recorder Visualization Manager: Class: "" Displays: - - Alpha: 0.10000000149011612 - Cell Size: 10 + - Alpha: 0.5 + Cell Size: 1 Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true @@ -38,21 +50,54 @@ Visualization Manager: Name: Grid Normal Cell Count: 0 Offset: - X: 50 + X: 0 Y: 0 - Z: -1.5 + Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Alpha: 1 - Autocompute Intensity Bounds: false + Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z - Channel Name: radial_speed + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 204; 0; 0 + 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: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 15 + 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 + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: heading Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity @@ -60,30 +105,56 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 20 + Max Intensity: -999999 Min Color: 0; 0; 0 - Min Intensity: -20 + Min Intensity: 999999 Name: PointCloud2 Position Transformer: XYZ - Selectable: false - Size (Pixels): 5 - Size (m): 0.20000000298023224 - Style: Points + Selectable: true + Size (Pixels): 3 + Size (m): 1 + Style: Boxes Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /smart_radar/targets_0 + Value: /smart_radar/port_objects_0 Use Fixed Frame: true Use rainbow: true Value: true - - Class: rviz_default_plugins/Axes + - 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: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 Enabled: true - Length: 5 - Name: Axes - Radius: 0.5 - Reference Frame: + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 1 + Style: Tiles + 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 Enabled: true Global Options: @@ -92,6 +163,8 @@ Visualization Manager: 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 @@ -110,7 +183,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /move_base_simple/goal + Value: /goal_pose - Class: rviz_default_plugins/PublishPoint Single click: true Topic: @@ -126,43 +199,47 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 66.64474487304688 + Distance: 197.11245727539062 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 29.421253204345703 - Y: 3.1301658153533936 - Z: 12.849743843078613 + X: 33.84825134277344 + Y: -4.720839023590088 + Z: 7.3445963859558105 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 + Pitch: 1.5047965049743652 Target Frame: Value: Orbit (rviz) - Yaw: 3.140397071838379 + Yaw: 3.1354000568389893 Saved: ~ Window Geometry: Displays: collapsed: false Height: 2090 Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000007d7000007c4fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006a00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000045000001b8000000e800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000026005400610072006700650074004c0069007300740020005200650063006f007200640065007201000002040000049c0000010e00fffffffb000000340053006d00610072007400200043006f006d006d0061006e006400200043006f006e0066006900670075007200610074006f007201000006a7000001620000016200ffffff000000010000010f00000383fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000383000000be00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000722000007c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + QMainWindow State: 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 Selection: collapsed: false Smart Command Configurator: collapsed: false - TargetList Recorder: + Smart Firmware Download: + collapsed: false + Smart Recorder: + collapsed: false + Smart Status: collapsed: false Tool Properties: collapsed: false Views: - collapsed: true + collapsed: false Width: 3840 X: 0 Y: 33 diff --git a/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp b/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp index 6622182..7b2c464 100644 --- a/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp +++ b/smart_rviz_plugin/include/smart_rviz_plugin/smart_download.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -16,21 +17,55 @@ 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: + /// + /// @brief Constructor for the SmartDownloadService class. + /// + /// @param parent The parent widget. Defaults to nullptr. + /// SmartDownloadService(QWidget * parent = nullptr); private slots: - void onDownload(); - void onBrowse(); + /// + /// @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 panel's components and ROS2 client. + /// + /// This function sets up the GUI elements, initializes the ROS2 node and + /// client for the firmware download service, and connects the signals and + /// slots. + /// void initialize(); -private: rclcpp::Node::SharedPtr download_node; rclcpp::Client::SharedPtr download_client; @@ -38,6 +73,7 @@ private slots: QLineEdit * sensor_id_input; QPushButton * start_download_button; QPushButton * browse_button; + QTextEdit * response_text_edit; }; } // namespace smart_rviz_plugin diff --git a/smart_rviz_plugin/include/smart_rviz_plugin/smart_recorder.hpp b/smart_rviz_plugin/include/smart_rviz_plugin/smart_recorder.hpp index 84e9494..3b019fd 100644 --- a/smart_rviz_plugin/include/smart_rviz_plugin/smart_recorder.hpp +++ b/smart_rviz_plugin/include/smart_rviz_plugin/smart_recorder.hpp @@ -1,8 +1,11 @@ #ifndef SMART_RVIZ_PLUGIN__SMART_RECORDER_HPP_ #define SMART_RVIZ_PLUGIN__SMART_RECORDER_HPP_ +#include + #include #include +#include #include #include #include @@ -12,17 +15,19 @@ #include #include #include -#include #include #include +#include +#include #include +#include #include "std_msgs/msg/string.hpp" namespace smart_rviz_plugin { - -struct RecordedData { +struct TargetData +{ float range; float power; float azimuth_deg; @@ -37,44 +42,152 @@ struct RecordedData { 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: - void startRecording(); - void stopRecording(); - void saveDataToCSV(); - void checkForData(); - void updateTable(); + /// + /// @brief Slot function to start recording the sensor data being viewed. + /// + void start_recording(); -private: - void initializeRecorder(); - void callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name); - void updateRecordedData( - 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 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::vector recorded_data; + 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}; }; diff --git a/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp b/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp index 9abebe1..40d30b9 100644 --- a/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp +++ b/smart_rviz_plugin/include/smart_rviz_plugin/smart_services.hpp @@ -1,11 +1,19 @@ #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 @@ -16,39 +24,98 @@ 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 selecting, viewing +/// and sending the sensor instructions. +/// class SmartRadarService : public rviz_common::Panel { Q_OBJECT public: + /// + /// @brief Constructor for the SmartRadarService class. + /// + /// @param parent The parent widget. Defaults to nullptr. + /// SmartRadarService(QWidget * parent = nullptr); private slots: - void onSendParam(); - void onSendCommand(); + /// + /// @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 display instruction set of selected sensor model. + /// + void on_file_selected(int index); 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 instruction services, and connects the signals and + /// slots. + /// void initialize(); -private: - rclcpp::Node::SharedPtr client_node; - QLineEdit * param_name; - QLineEdit * param_value; - QLineEdit * sensor_id_value; - QLineEdit * command_name; - QLineEdit * command_value; - QLineEdit * command_sensor_id_value; + /// + /// @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 Populate the table with the read json files. + /// + void populate_file_menu(); + + const QString current_directory = QDir::currentPath(); + QString param_json_file_path; + QString command_json_file_path; + + QLineEdit * command_value_line_edit; + QLineEdit * command_argument_line_edit; + + QVBoxLayout * main_layout; + QTabWidget * tab_widget; + QWidget * param_tab; + QLineEdit * param_name_line_edit; + QLineEdit * param_value_line_edit; + QLineEdit * param_min_line_edit; + QLineEdit * param_max_line_edit; + QLineEdit * param_sensor_id; + QLineEdit * command_name_line_edit; + QLineEdit * command_comment_line_edit; + QLineEdit * command_sensor_id; + QPushButton * send_param_button; QPushButton * send_command_button; - QTabWidget * tab_widget; - QWidget * set_mode_tab; - QWidget * send_command_tab; - QVBoxLayout * send_mode_layout; - QVBoxLayout * send_command_layout; + QTableWidget * param_table_widget; + QTableWidget * command_table_widget; + QWidget * command_tab; + QComboBox * file_selector_combo_box; + QTextEdit * response_text_edit; + + rclcpp::Node::SharedPtr client_node; rclcpp::Client::SharedPtr mode_client; rclcpp::Client::SharedPtr command_client; }; - } // namespace smart_rviz_plugin #endif // SMART_RVIZ_PLUGIN__SMART_SERVICES_HPP_ diff --git a/smart_rviz_plugin/include/smart_rviz_plugin/smart_status.hpp b/smart_rviz_plugin/include/smart_rviz_plugin/smart_status.hpp new file mode 100644 index 0000000..4ab2e1b --- /dev/null +++ b/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/smart_rviz_plugin/package.xml b/smart_rviz_plugin/package.xml index db9598a..0c7949a 100644 --- a/smart_rviz_plugin/package.xml +++ b/smart_rviz_plugin/package.xml @@ -2,7 +2,7 @@ smart_rviz_plugin - 1.0.0 + 2.0.0 TargetList Recorder shahrukh Apache 2.0 License diff --git a/smart_rviz_plugin/plugins_description.xml b/smart_rviz_plugin/plugins_description.xml index 21941a5..d7a4f89 100644 --- a/smart_rviz_plugin/plugins_description.xml +++ b/smart_rviz_plugin/plugins_description.xml @@ -1,6 +1,6 @@ The Smart TargetList Recorder. @@ -17,4 +17,10 @@ base_class_type="rviz_common::Panel"> The Smart Download. + + The Smart Status. + diff --git a/smart_rviz_plugin/src/smart_download.cpp b/smart_rviz_plugin/src/smart_download.cpp index 226b7a6..e776f2f 100644 --- a/smart_rviz_plugin/src/smart_download.cpp +++ b/smart_rviz_plugin/src/smart_download.cpp @@ -27,6 +27,10 @@ void SmartDownloadService::initialize() start_download_button = new QPushButton("Start Download", this); browse_button = new QPushButton("Browse", this); + response_text_edit = new QTextEdit(this); + response_text_edit->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + response_text_edit->setFixedSize(1200, 100); + QVBoxLayout * layout = new QVBoxLayout(this); layout->addWidget(new QLabel("File Path:")); layout->addWidget(file_path_input); @@ -34,13 +38,14 @@ void SmartDownloadService::initialize() layout->addWidget(new QLabel("Sensor ID:")); layout->addWidget(sensor_id_input); layout->addWidget(start_download_button); + layout->addWidget(response_text_edit); // Connect signals and slots - connect(start_download_button, &QPushButton::clicked, this, &SmartDownloadService::onDownload); - connect(browse_button, &QPushButton::clicked, this, &SmartDownloadService::onBrowse); + connect(start_download_button, SIGNAL(clicked()), this, SLOT(download_firmware())); + connect(browse_button, SIGNAL(clicked()), this, SLOT(browse_file())); } -void SmartDownloadService::onDownload() +void SmartDownloadService::download_firmware() { if (!download_client) { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to create download_client"); @@ -56,6 +61,8 @@ void SmartDownloadService::onDownload() return; } + start_download_button->setText("Downloading..."); + // Set up the request auto request = std::make_shared(); request->file_path = filePath.toStdString(); @@ -67,14 +74,20 @@ void SmartDownloadService::onDownload() if ( rclcpp::spin_until_future_complete(download_node, result) == rclcpp::FutureReturnCode::SUCCESS) { + QString response_msg = QString::fromStdString(result.get()->res); + response_text_edit->append("Download request sent.\n Response from sensor: " + response_msg); RCLCPP_INFO( - rclcpp::get_logger("rclcpp"), "Request successful. Response: %s", result.get()->res.c_str()); + rclcpp::get_logger("rclcpp"), "Request successful\n. Response: %s ", + result.get()->res.c_str()); + start_download_button->setText("Start Download"); } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service firmware_download"); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to download sensor firmware"); + response_text_edit->append("Failed to downlaod sensor firmware!"); + start_download_button->setText("Start Download"); } } -void SmartDownloadService::onBrowse() +void SmartDownloadService::browse_file() { QString filePath = QFileDialog::getOpenFileName(this, tr("Open File"), "", tr("All Files (*)")); if (!filePath.isEmpty()) { diff --git a/smart_rviz_plugin/src/smart_recorder.cpp b/smart_rviz_plugin/src/smart_recorder.cpp index cca346e..2b8acbf 100644 --- a/smart_rviz_plugin/src/smart_recorder.cpp +++ b/smart_rviz_plugin/src/smart_recorder.cpp @@ -1,46 +1,77 @@ #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) { - initializeRecorder(); + initialize(); } -void SmartRadarRecorder::initializeRecorder() +void SmartRadarRecorder::initialize() { node_ = rclcpp::Node::make_shared("smart_radar_gui_node"); - // Retrieve available topics - auto topic_names_and_types = node_->get_topic_names_and_types(); - std::vector available_topics; - for (const auto & topic : topic_names_and_types) { - available_topics.push_back(topic.first); - } + 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"); - // Create subscribers for selected topics - for (const auto & topic : available_topics) { - subscribers_[topic] = node_->create_subscription( - topic, 10, - [this, topic](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { callback(msg, topic); }); - topic_dropdown_->addItem(QString::fromStdString(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(updateTable())); + connect(topic_dropdown_, SIGNAL(currentIndexChanged(int)), this, SLOT(update_table())); + + // Table one layout table_data_ = new QTableWidget(); - table_data_->setColumnCount(10); + table_data_->setColumnCount(13); + table_data_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); table_data_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); - table_data_->setHorizontalHeaderLabels( - {"Range [m]", "Power [dB]", "AzimuthAngle [Deg]", "ElevationAngle [Deg]", "RCS [dB]", - "Noise [dB]", "SNR [dB]", "RadialSpeed [m/s]", "AzimuthAngle [rad]", "ElevationAngle [rad]"}); + table_timestamps_ = new QTableWidget(); table_timestamps_->setColumnCount(2); table_timestamps_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); @@ -58,31 +89,56 @@ void SmartRadarRecorder::initializeRecorder() gui_layout_->addWidget(horiz_splitter_); timer_ = new QTimer(); - connect(timer_, SIGNAL(timeout()), this, SLOT(checkForData())); + connect(timer_, SIGNAL(timeout()), this, SLOT(check_data())); timer_->start(20); start_button_ = new QPushButton("Record"); - connect(start_button_, SIGNAL(clicked()), this, SLOT(startRecording())); + connect(start_button_, SIGNAL(clicked()), this, SLOT(start_recording())); stop_button_ = new QPushButton("Stop Recording"); - connect(stop_button_, SIGNAL(clicked()), this, SLOT(stopRecording())); + connect(stop_button_, SIGNAL(clicked()), this, SLOT(stop_recording())); save_button_ = new QPushButton("Save Data as CSV"); - connect(save_button_, SIGNAL(clicked()), this, SLOT(saveDataToCSV())); + 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::updateRecordedData( +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) + float snr, float radial_speed, float azimuth_angle, float elevation_angle, uint32_t timestamp_sec, + uint32_t timestamp_nanosec) { - RecordedData data; + TargetData data; data.range = range; data.power = power; data.azimuth_deg = azimuth_deg; @@ -96,10 +152,31 @@ void SmartRadarRecorder::updateRecordedData( data.timestamp_sec = timestamp_sec; data.timestamp_nanosec = timestamp_nanosec; - recorded_data.push_back(data); + target_recorded_data.push_back(data); } -void SmartRadarRecorder::callback( +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_) { @@ -107,48 +184,195 @@ void SmartRadarRecorder::callback( auto timestamp_nanosec = msg->header.stamp.nanosec; // Create iterators for the pc2 fields - sensor_msgs::PointCloud2ConstIterator iter_range(*msg, "range"); + 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_radial_speed(*msg, "radial_speed"); + 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); - // Conversion from radians to degrees - const double radToDeg = 180.0 / M_PI; - double azimuth_deg, elevation_deg; - - for (size_t i = 0; i < msg->height * msg->width; ++i, ++iter_range, ++iter_power, - ++iter_azimuth_angle, ++iter_elevation_angle, ++iter_rcs, ++iter_noise, - ++iter_snr) { - azimuth_deg = *iter_azimuth_angle * radToDeg; - elevation_deg = *iter_elevation_angle * radToDeg; + 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_) { - updateRecordedData( + 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 - table_data_->insertRow(0); - table_data_->setItem(0, 0, new QTableWidgetItem(QString::number(*iter_range, 'f', 2))); - table_data_->setItem(0, 1, new QTableWidgetItem(QString::number(*iter_power, 'f', 2))); - table_data_->setItem(0, 2, new QTableWidgetItem(QString::number(azimuth_deg, 'f', 2))); - table_data_->setItem(0, 3, new QTableWidgetItem(QString::number(elevation_deg, 'f', 2))); - table_data_->setItem(0, 4, new QTableWidgetItem(QString::number(*iter_rcs, 'f', 2))); - table_data_->setItem(0, 5, new QTableWidgetItem(QString::number(*iter_noise, 'f', 2))); - table_data_->setItem(0, 6, new QTableWidgetItem(QString::number(*iter_snr, 'f', 2))); - table_data_->setItem(0, 7, new QTableWidgetItem(QString::number(*iter_radial_speed, 'f', 2))); - table_data_->setItem( - 0, 8, new QTableWidgetItem(QString::number(*iter_azimuth_angle, 'f', 2))); - table_data_->setItem( - 0, 9, new QTableWidgetItem(QString::number(*iter_elevation_angle, 'f', 2))); + 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 @@ -159,31 +383,112 @@ void SmartRadarRecorder::callback( } } -void SmartRadarRecorder::updateTable() +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() { - // Clear the table when a new topic is selected 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::startRecording() +void SmartRadarRecorder::start_recording() { qDebug() << "Recording started!"; recording_active_ = true; start_button_->setText("Recording..."); } -void SmartRadarRecorder::stopRecording() +void SmartRadarRecorder::stop_recording() { qDebug() << "Recording stopped!"; recording_active_ = false; start_button_->setText("Record"); } -void SmartRadarRecorder::saveDataToCSV() +void SmartRadarRecorder::save_data() { qDebug() << "Saving data to CSV!"; - if (!recorded_data.empty()) { + 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 (*)"); @@ -192,13 +497,15 @@ void SmartRadarRecorder::saveDataToCSV() QFile csvfile(file_path); if (csvfile.open(QIODevice::WriteOnly | QIODevice::Text)) { QTextStream csv_writer(&csvfile); - csv_writer << "Range [m], Power [dB], AzimuthAngle [Deg], ElevationAngle [Deg], RCS [dB], " - "Noise [dB], SNR [dB], " - "RadialSpeed [m/s], ElevationAngle [rad], AzimuthAngle [rad], " - "TimestampSec, TimestampNanoSec\n"; + 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 : recorded_data) { + 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); @@ -215,6 +522,28 @@ void SmartRadarRecorder::saveDataToCSV() 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."; @@ -224,10 +553,11 @@ void SmartRadarRecorder::saveDataToCSV() qDebug() << "No recorded data to save."; } // Clear recorded_data after saving - recorded_data.clear(); + target_recorded_data.clear(); + object_recorded_data.clear(); } -void SmartRadarRecorder::checkForData() +void SmartRadarRecorder::check_data() { if (rclcpp::ok()) // Check if ROS2 is still running { @@ -238,4 +568,4 @@ void SmartRadarRecorder::checkForData() } // namespace smart_rviz_plugin #include -PLUGINLIB_EXPORT_CLASS(smart_rviz_plugin::SmartRadarRecorder, rviz_common::Panel) +PLUGINLIB_EXPORT_CLASS(smart_rviz_plugin::SmartRadarRecorder, rviz_common::Panel) \ No newline at end of file diff --git a/smart_rviz_plugin/src/smart_services.cpp b/smart_rviz_plugin/src/smart_services.cpp index c4c456a..ca5449a 100644 --- a/smart_rviz_plugin/src/smart_services.cpp +++ b/smart_rviz_plugin/src/smart_services.cpp @@ -18,7 +18,6 @@ void SmartRadarService::initialize() rclcpp::init(0, nullptr); } - // Declare node_ and clients client_node = rclcpp::Node::make_shared("smart_service_gui"); mode_client = client_node->create_client("smart_radar/set_radar_mode"); @@ -27,64 +26,219 @@ void SmartRadarService::initialize() RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Client node created!"); - // GUI setup - param_name = new QLineEdit(this); - param_value = new QLineEdit(this); - sensor_id_value = new QLineEdit(this); - command_name = new QLineEdit(this); - command_value = new QLineEdit(this); - command_sensor_id_value = new QLineEdit(this); + param_name_line_edit = new QLineEdit(this); + param_value_line_edit = new QLineEdit(this); + param_sensor_id = new QLineEdit(this); send_param_button = new QPushButton("Send Param", this); + param_table_widget = new QTableWidget(this); + + command_name_line_edit = new QLineEdit(this); + command_comment_line_edit = new QLineEdit(this); + command_value_line_edit = new QLineEdit(this); + command_sensor_id = new QLineEdit(this); send_command_button = new QPushButton("Send Command", this); + command_table_widget = new QTableWidget(this); tab_widget = new QTabWidget(this); - set_mode_tab = new QWidget(tab_widget); - send_command_tab = new QWidget(tab_widget); - - // Create layouts for each tab - send_mode_layout = new QVBoxLayout(set_mode_tab); - send_command_layout = new QVBoxLayout(send_command_tab); - - // Add widgets to the layouts - send_mode_layout->addWidget(new QLabel("Radar Params:")); - send_mode_layout->addWidget(new QLabel("Param:")); - send_mode_layout->addWidget(param_name); - send_mode_layout->addWidget(new QLabel("Value:")); - send_mode_layout->addWidget(param_value); - send_mode_layout->addWidget(new QLabel("Sensor ID:")); - send_mode_layout->addWidget(sensor_id_value); - send_mode_layout->addWidget(send_param_button); - - send_command_layout->addWidget(new QLabel("Radar Commands:")); - send_command_layout->addWidget(new QLabel("Command:")); - send_command_layout->addWidget(command_name); - send_command_layout->addWidget(new QLabel("Value:")); - send_command_layout->addWidget(command_value); - send_command_layout->addWidget(new QLabel("Sensor ID:")); - send_command_layout->addWidget(command_sensor_id_value); - send_command_layout->addWidget(send_command_button); - - // Set default values - param_name->setText("dummy_check_interface_file"); - param_value->setText("1"); - sensor_id_value->setText("100"); - - command_name->setText("dummy_check_interface_file"); - command_value->setText("1"); - command_sensor_id_value->setText("100"); - - tab_widget->addTab(set_mode_tab, "Set Radar Mode"); - tab_widget->addTab(send_command_tab, "Send Command"); - - QHBoxLayout * mainLayout = new QHBoxLayout(this); - mainLayout->addWidget(tab_widget); - - // Connect signals and slots - connect(send_param_button, &QPushButton::clicked, this, &SmartRadarService::onSendParam); - connect(send_command_button, &QPushButton::clicked, this, &SmartRadarService::onSendCommand); + param_tab = new QWidget(tab_widget); + command_tab = new QWidget(tab_widget); + + QVBoxLayout * param_layout = new QVBoxLayout(param_tab); + QVBoxLayout * command_layout = new QVBoxLayout(command_tab); + + param_layout->addWidget(param_table_widget); + param_layout->addWidget(new QLabel("Enter Param:")); + param_layout->addWidget(param_name_line_edit); + param_layout->addWidget(new QLabel("Enter Value:")); + param_layout->addWidget(param_value_line_edit); + param_layout->addWidget(new QLabel("Enter Sensor ID:")); + param_layout->addWidget(param_sensor_id); + param_layout->addWidget(send_param_button); + + command_layout->addWidget(command_table_widget); + command_layout->addWidget(new QLabel("Enter Command:")); + command_layout->addWidget(command_name_line_edit); + command_layout->addWidget(new QLabel("Enter Value:")); + command_layout->addWidget(command_value_line_edit); + command_layout->addWidget(new QLabel("Enter Sensor ID:")); + command_layout->addWidget(command_sensor_id); + command_layout->addWidget(send_command_button); + + tab_widget->addTab(param_tab, "Parameter"); + tab_widget->addTab(command_tab, "Command"); + + // Add dropdown menu for file selection + file_selector_combo_box = new QComboBox(this); + populate_file_menu(); + + QVBoxLayout * main_layout = new QVBoxLayout(this); + main_layout->addWidget(file_selector_combo_box); + main_layout->addWidget(tab_widget); + + response_text_edit = new QTextEdit(this); + response_text_edit->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + response_text_edit->setFixedSize(1200, 100); + main_layout->addWidget(response_text_edit); + + // Connect dropdown menu signal to slot + connect( + file_selector_combo_box, QOverload::of(&QComboBox::activated), this, + &SmartRadarService::on_file_selected); + + read_param_json_data(); + read_command_json_data(); + connect(send_param_button, SIGNAL(clicked()), this, SLOT(on_send_param())); + connect(send_command_button, SIGNAL(clicked()), this, SLOT(on_send_command())); +} + +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"); +} + +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(); + + // Clear command tab + command_table_widget->setRowCount(0); + command_name_line_edit->clear(); + command_argument_line_edit->clear(); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Index Zero!"); + } else if (index == 1) { + param_json_file_path = + current_directory + + "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" + "UserInterfaceUmrr9f_t169_mseV1.0.0/instructions/params/auto_interface_0dim.param"; + command_json_file_path = + current_directory + + "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" + "UserInterfaceUmrr9f_t169_mseV1.0.0/instructions/command/auto_interface.command"; + } else if (index == 2) { + param_json_file_path = + current_directory + + "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" + "UserInterfaceUmrr9f_t169_automotiveV2.4.1/instructions/params/auto_interface_0dim.param"; + command_json_file_path = + current_directory + + "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" + "UserInterfaceUmrr9f_t169_automotiveV2.4.1/instructions/command/auto_interface.command"; + } else if (index == 3) { + param_json_file_path = + current_directory + + "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" + "UserInterfaceUmrr9d_t152_automotiveV1.4.1/instructions/params/auto_interface_0dim.param"; + command_json_file_path = + current_directory + + "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" + "UserInterfaceUmrr9d_t152_automotiveV1.4.1/instructions/command/auto_interface.command"; + } else if (index == 4) { + param_json_file_path = + current_directory + + "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" + "UserInterfaceUmrra4_automotiveV1.2.1/instructions/params/auto_interface_0dim.param"; + command_json_file_path = + current_directory + + "/src/smartmicro_ros2_radars/umrr_ros2_driver/smartmicro/user_interfaces/" + "UserInterfaceUmrra4_automotiveV1.2.1/instructions/command/auto_interface.command"; + } + + // Update tables with data from selected files + if (index != 0) { + read_param_json_data(); + read_command_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 the "commands" array + QJsonArray commands_array = json_object["commands"].toArray(); + + // Set up table headers + QStringList command_header_labels = {"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 * name_item = new QTableWidgetItem(name); + QTableWidgetItem * argument_item = new QTableWidgetItem(argument); + QTableWidgetItem * comment_item = new QTableWidgetItem(comment); + + command_table_widget->setItem(i, 0, name_item); + command_table_widget->setItem(i, 1, argument_item); + command_table_widget->setItem(i, 2, comment_item); + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Commands parsed successfully!"); +} + +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 the "parameters" array + QJsonArray params_array = json_object["parameters"].toArray(); + + // Set up table headers + QStringList param_header_labels = {"Name", "Comment"}; + param_table_widget->setColumnCount(2); + 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(); + + QTableWidgetItem * name_item = new QTableWidgetItem(name); + QTableWidgetItem * comment_item = new QTableWidgetItem(comment); + + param_table_widget->setItem(i, 0, name_item); + param_table_widget->setItem(i, 1, comment_item); + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Parameters parsed successfully!"); } -void SmartRadarService::onSendParam() +void SmartRadarService::on_send_param() { if (!mode_client) { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to create mode_client"); @@ -92,23 +246,27 @@ void SmartRadarService::onSendParam() } auto request = std::make_shared(); - request->param = param_name->text().toStdString(); - request->value = std::stoi(param_value->text().toUtf8().constData()); - request->sensor_id = std::stoi(sensor_id_value->text().toUtf8().constData()); + request->param = param_name_line_edit->text().toStdString(); + request->value = std::stoi(param_value_line_edit->text().toUtf8().constData()); + request->sensor_id = std::stoi(param_sensor_id->text().toUtf8().constData()); auto result = mode_client->async_send_request(request); // Wait for the result. if ( rclcpp::spin_until_future_complete(client_node, result) == rclcpp::FutureReturnCode::SUCCESS) { + QString response_msg = QString::fromStdString(result.get()->res); + response_text_edit->append("Request sent. Response from sensor: " + response_msg); RCLCPP_INFO( - rclcpp::get_logger("rclcpp"), "Request successful. Response: %s", result.get()->res.c_str()); + rclcpp::get_logger("rclcpp"), "Request sent. Response from sensor: %s", + result.get()->res.c_str()); } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints"); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service!"); + response_text_edit->append("Failed to call service!"); } } -void SmartRadarService::onSendCommand() +void SmartRadarService::on_send_command() { if (!command_client) { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to create command client"); @@ -116,19 +274,22 @@ void SmartRadarService::onSendCommand() } auto request = std::make_shared(); - request->command = command_name->text().toStdString(); - request->value = std::stoi(command_value->text().toUtf8().constData()); - request->sensor_id = std::stoi(command_sensor_id_value->text().toUtf8().constData()); + 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); // Wait for the result. if ( rclcpp::spin_until_future_complete(client_node, result) == rclcpp::FutureReturnCode::SUCCESS) { + QString response_msg = QString::fromStdString(result.get()->res); + response_text_edit->append("Request sent. Response from sensor: " + response_msg); RCLCPP_INFO( - rclcpp::get_logger("rclcpp"), "Request successful. Response: %s", result.get()->res.c_str()); + rclcpp::get_logger("rclcpp"), "Request sent. Response: %s", result.get()->res.c_str()); } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints"); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service!"); + response_text_edit->append("Failed to call service!"); } } diff --git a/smart_rviz_plugin/src/smart_status.cpp b/smart_rviz_plugin/src/smart_status.cpp new file mode 100644 index 0000000..73237c0 --- /dev/null +++ b/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/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index 2c1609b..1958e2a 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -26,11 +26,12 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -g) + 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 GIT_REPOSITORY https://github.com/nlohmann/json.git @@ -61,12 +62,13 @@ install(FILES "${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.5.0_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}/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.5.0_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}/libumrra4_automotivev1.0.1_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_automotivev1.3.0_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_automotivev1.2.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_msev1.0.0_user_interface.so" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -93,12 +95,13 @@ 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_5_0 +smartmicro/include/umrr9f_t169_automotive_v2_4_1 smartmicro/include/umrr9d_t152_automotive_v1_0_3 smartmicro/include/umrr9d_t152_automotive_v1_2_2 -smartmicro/include/umrr9d_t152_automotive_v1_5_0 +smartmicro/include/umrr9d_t152_automotive_v1_4_1 smartmicro/include/umrra4_automotive_v1_0_1 -smartmicro/include/umrra4_automotive_v1_3_0) +smartmicro/include/umrra4_automotive_v1_2_1 +smartmicro/include/umrr9f_t169_mse_v1_0_0) # link smart_access_lib-linux-x86_64-gcc_9 to the node target_link_libraries(smartmicro_radar_node @@ -112,12 +115,13 @@ target_link_libraries(smartmicro_radar_node umrr9f_t169_automotivev2.0.0_user_interface umrr9f_t169_automotivev2.1.1_user_interface umrr9f_t169_automotivev2.2.1_user_interface - umrr9f_t169_automotivev2.5.0_user_interface + umrr9f_t169_automotivev2.4.1_user_interface umrr9d_t152_automotivev1.0.3_user_interface umrr9d_t152_automotivev1.2.2_user_interface - umrr9d_t152_automotivev1.5.0_user_interface + umrr9d_t152_automotivev1.4.1_user_interface umrra4_automotivev1.0.1_user_interface - umrra4_automotivev1.3.0_user_interface) + umrra4_automotivev1.2.1_user_interface + umrr9f_t169_msev1.0.0_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp index 4e733d3..f5cdcc6 100644 --- a/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp +++ b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp @@ -17,7 +17,6 @@ #ifndef UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ #define UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ - #include #include #include @@ -25,36 +24,46 @@ #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/firmware_download.hpp" -namespace smartmicro { -namespace drivers { -namespace radar { -namespace detail { +namespace smartmicro +{ +namespace drivers +{ +namespace radar +{ +namespace detail +{ constexpr auto kMaxSensorCount = 10UL; constexpr auto kMaxHwCount = 6UL; -struct SensorConfig { +struct SensorConfig +{ std::uint32_t id{}; std::uint32_t dev_id{}; std::string frame_id{}; @@ -69,29 +78,32 @@ struct SensorConfig { std::uint32_t uifmajorv{}; std::uint32_t uifminorv{}; std::uint32_t uifpatchv{}; + std::string pub_type{}; }; -struct HWConfig { +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 +} // namespace detail /// /// @brief The class for the Smartmicro radar node. /// -class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::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); - + explicit SmartmicroRadarNode(const rclcpp::NodeOptions & node_options); + protected: /// /// @brief A timer to handle the services. @@ -100,42 +112,70 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { void on_shutdown_callback(); private: + /// + /// @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 target list port for /// umrr11 T132 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_port_umrr11, - const com::types::ClientId client_id); + 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] target_list_port The target list port + /// @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 - &targetlist_port_umrr96, - const com::types::ClientId client_id); - + 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_v2_0_0 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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( @@ -150,283 +190,310 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// umrr9f_v1_1_1 T169 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_port_umrr9f, - const com::types::ClientId client_id); + 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_1_1 T169 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_port_umrr9f_v2_1_1, - const com::types::ClientId client_id); - + 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] target_list_port The target list port + /// @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 - &targetlist_port_umrr9f_v2_2_1, - const com::types::ClientId client_id); + 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 /// umrr9d_v1_0_3 T152 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_port_umrr9d_v1_0_3, - const com::types::ClientId client_id); - + 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 arrives. + /// umrr9d_v1_2_2 T152 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_port_umrr9d_v1_2_2, - const com::types::ClientId client_id); - + 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 - /// umrra4 T171_v1_0_1 arrives. + /// umrra4_v1_0_1 T171 arrives. /// @param[in] sensor_idx The sensor id for the respected published topic. - /// @param[in] targetlist_port_umrra4 The targetlist port + /// @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 - ); + 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 T171_v1_3_0 arrives. + /// umrra4_v1_2_1 T171 arrives. /// @param[in] sensor_idx The sensor id for the respected published topic. - /// @param[in] target_list_port The target list port + /// @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_3_0( + void targetlist_callback_umrra4_v1_2_1( const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_port_umrra4_v1_3_0, - const com::types::ClientId client_id - ); + 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 - /// umrr9d T152 v1_5_0 arrives. + /// umrr9d_v1_4_1 T152 arrives. /// @param[in] sensor_idx The sensor id for the respected published topic. - /// @param[in] target_list_port The target list port + /// @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_5_0( + void targetlist_callback_umrr9d_v1_4_1( const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_port_umrr9d_v1_5_0, - const com::types::ClientId client_id - ); + 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 - /// umrr9f_v2_5_0 T169 arrives. + /// umrr9f_v2_4_1 T169 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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_5_0( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_port_umrr9f_v2_5_0, - const com::types::ClientId client_id); + 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 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 target list for - /// umrr96_v1_0_3 arrives. + /// umrr96_v1_2_2 T153 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_can_umrr96, - const com::types::ClientId client_id); + 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 arrives. + /// umrr11_v1_1_2 T132 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_can_umrr11, - const com::types::ClientId client_id); - + 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 arrives. + /// umrr9f_v2_1_1 T169arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_can_umrr9f_v2_1_1, - const com::types::ClientId client_id); - + 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 arrives. + /// umrr9f_v2_2_1 T169 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_can_umrr9f_v2_2_1, - const com::types::ClientId client_id); + 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 - /// umrr9d_v1_=_§ arrives. + /// umrr9d_v1_0_3 T152 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_can_umrr9d_v1_0_3, - const com::types::ClientId client_id); + 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 arrives. + /// umrr9d_v1_2_2 T152 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_can_umrr9d_v1_2_2, - const com::types::ClientId client_id); + 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 - /// umrra4 arrives. + /// umrra4_v1_0_1 T171 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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 - &targetlist_can_umrra4_v1_0_1, - const com::types::ClientId client_id); - + 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 arrives. + /// umrra4_v1_2_1 T171 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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_3_0( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_can_umrra4_v1_3_0, - const com::types::ClientId client_id); + 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 - /// umrr9d_v1_5_0 arrives. + /// umrr9d_v1_4_1 T152 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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_5_0( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_can_umrr9d_v1_5_0, - const com::types::ClientId client_id); + 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 - /// umrr9f_v2_5_0 arrives. + /// umrr9f_v2_4_1 T169 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. - /// @param[in] target_list_port The target list port + /// @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_5_0( - const std::uint32_t sensor_idx, - const std::shared_ptr - &targetlist_can_umrr9f_v2_5_0, - const com::types::ClientId client_id); + 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 Read parameters and update the json config files required by @@ -435,54 +502,93 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { void update_config_files_from_params(); /// - /// @brief Callaback for getting the parameter response. + /// @brief Creates publishers for sensors using ports. + /// + /// @param[in] sensor The sensor configuration. + /// @param[in] sensor_idx The sensor index. /// - void - mode_response(const com::types::ClientId client_id, - const std::shared_ptr &response, - const std::string instruction_name); + void port_publishers(const detail::SensorConfig & sensor, size_t sensor_idx); /// - /// @brief Callaback for getting the command response. + /// @brief Creates publishers for sensors using CAN. /// - void - command_response(const com::types::ClientId client_id, - const std::shared_ptr &response, - const std::string command_name); + /// @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::string instruction_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); /// /// @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); + const com::types::ClientId client_id, + const std::shared_ptr & response); /// - /// @brief Send instructions to the sensor. + /// @brief Sends instructions to the sensor. + /// + /// @param[in] request The request. + /// @param[out] response The response. /// void radar_mode( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); /// - /// @brief Configure the sensor ip address. + /// @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); + void ip_address( + const std::shared_ptr request, + std::shared_ptr response); /// - /// @brief Send command to the sensor. + /// @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); + 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); + const std::shared_ptr request, + std::shared_ptr result); rclcpp::Service::SharedPtr mode_srv_; rclcpp::Service::SharedPtr ip_addr_srv_; @@ -492,9 +598,28 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { std::array m_sensors{}; std::array m_adapters{}; - std::array::SharedPtr, - detail::kMaxSensorCount> - m_publishers{}; + 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; @@ -506,20 +631,30 @@ 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_3_0{}; +std::shared_ptr data_umrra4_v1_2_1{}; 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_5_0{}; -std::shared_ptr data_umrr9d_v1_0_3{}; -std::shared_ptr data_umrr9d_v1_2_2{}; -std::shared_ptr data_umrr9d_v1_5_0{}; - -} // namespace radar -} // namespace drivers -} // namespace smartmicro - -#endif // UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ +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_umrr9d_v1_0_3{}; +std::shared_ptr + data_umrr9d_v1_2_2{}; +std::shared_ptr + data_umrr9d_v1_4_1{}; +std::shared_ptr + data_umrr9f_mse_v1_0_0{}; + +} // namespace radar +} // namespace drivers +} // namespace smartmicro + +#endif // UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ diff --git a/umrr_ros2_driver/package.xml b/umrr_ros2_driver/package.xml index 511291e..cd8eee0 100644 --- a/umrr_ros2_driver/package.xml +++ b/umrr_ros2_driver/package.xml @@ -2,7 +2,7 @@ umrr_ros2_driver - 6.1.0 + 7.0.0 A node to publish data from a smartmicro radar. s.m.s, smart microwave sensors GmbH. Apache 2.0 License @@ -14,6 +14,7 @@ rclcpp_components sensor_msgs point_cloud_msg_wrapper + visualization_msgs umrr_ros2_msgs ament_lint_auto diff --git a/umrr_ros2_driver/param/radar.params.template.yaml b/umrr_ros2_driver/param/radar.params.template.yaml index 501d612..35ad64a 100644 --- a/umrr_ros2_driver/param/radar.params.template.yaml +++ b/umrr_ros2_driver/param/radar.params.template.yaml @@ -11,13 +11,17 @@ 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. - # For sensor `model`if using : - # can: 'umrra4_can_v1_0_1', 'umrr96_can', 'umrr11_can', 'umrr9d_can_v1_0_3', 'umrr9d_can_v1_2_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1' - # port: 'umrra4_v1_0_1', 'umrr96', 'umrr11', 'umrr9d_v1_0_3', 'umrr9d_v1_2_2', 'umrr9f_v1_1_1', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1' + # + # Sensor `model` names if using link type: + # can: 'umrr9f_can_mse_v1_0_0', 'umrra4_can_v1_0_1', 'umrr96_can_v1_2_2', 'umrr11_can_v1_1_2', 'umrr9d_can_v1_0_3', 'umrr9d_can_v1_2_2', 'umrr9f_can_v2_1_1', 'umrr9f_can_v2_2_1' + # port: 'umrr9f_mse_v1_0_0', 'umrra4_v1_0_1', 'umrr96_v1_2_2', 'umrr11_v1_1_2', 'umrr9d_v1_0_3', 'umrr9d_v1_2_2', 'umrr9f_v1_1_1', 'umrr9f_v2_1_1', 'umrr9f_v2_2_1' 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" + model: "umrr11_v1_1_2" # Adapter id to which sensor is connected dev_id: 3 # The client_id of the sensor/source, must be a unique integer. @@ -41,9 +45,12 @@ # 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" + model: "umrr96_v1_2_2" # Adapter id to which sensor is connected dev_id: 3 # The client_id of the sensor/source, must be a unique integer. @@ -67,7 +74,10 @@ # 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 @@ -93,7 +103,10 @@ # 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_0_3" # Adapter id to which sensor is connected diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 9b6489b..2f5384f 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -27,8 +27,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -37,12 +37,14 @@ #include #include #include -#include -#include +#include +#include +#include +#include #include #include -#include -#include +#include +#include #include @@ -54,6 +56,7 @@ #include #include + #include #include #include @@ -121,7 +124,6 @@ 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{}; @@ -160,6 +162,51 @@ using Generators = std::tuple< 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 @@ -183,7 +230,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option } // Getting the data stream service - data_umrra4_v1_3_0 = com::master::umrra4_automotive_v1_3_0::DataStreamServiceIface::Get(); + data_umrra4_v1_2_1 = com::master::umrra4_automotive_v1_2_1::DataStreamServiceIface::Get(); data_umrra4_v1_0_1 = com::master::umrra4_automotive_v1_0_1::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(); @@ -191,221 +238,16 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option 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_5_0 = com::master::umrr9f_t169_automotive_v2_5_0::DataStreamServiceIface::Get(); + data_umrr9f_v2_4_1 = com::master::umrr9f_t169_automotive_v2_4_1::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_5_0 = com::master::umrr9d_t152_automotive_v1_5_0::DataStreamServiceIface::Get(); + data_umrr9d_v1_4_1 = com::master::umrr9d_t152_automotive_v1_4_1::DataStreamServiceIface::Get(); + data_umrr9f_mse_v1_0_0 = com::master::umrr9f_t169_mse_v1_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!"); - for (auto i = 0UL; i < m_number_of_sensors; ++i) { - const auto & sensor = m_sensors[i]; - - m_publishers[i] = create_publisher( - "smart_radar/targets_" + std::to_string(i), sensor.history_size); - - if ( - sensor.model == "umrra4_v1_3_0" && - com::types::ERROR_CODE_OK != - data_umrra4_v1_3_0->RegisterComTargetListReceiveCallback( - sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrra4_v1_3_0, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrra4_v1_3_0" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrra4_v1_0_1" << std::endl; - } - if ( - sensor.model == "umrr96" && - com::types::ERROR_CODE_OK != - data_umrr96->RegisterComTargetListReceiveCallback( - sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr96, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; - } - if ( - sensor.model == "umrr11" && - com::types::ERROR_CODE_OK != - data_umrr11->RegisterComTargetListReceiveCallback( - sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr11, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f_v1_1_1" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_0_0" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_2_1" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_0_3" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_2_2" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_5_0" << std::endl; - } - if ( - sensor.model == "umrr9f_v2_5_0" && - com::types::ERROR_CODE_OK != - data_umrr9f_v2_5_0->RegisterComTargetListReceiveCallback( - sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_5_0, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_5_0" << std::endl; - } - if ( - sensor.model == "umrra4_can_v1_3_0" && - com::types::ERROR_CODE_OK != - data_umrra4_v1_3_0->RegisterComTargetBaseListReceiveCallback( - sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_3_0, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist for sensor umrra4_v1_3_0" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist for sensor umrra4_v1_0_1" << std::endl; - } - if ( - sensor.model == "umrr96_can" && - com::types::ERROR_CODE_OK != - data_umrr96->RegisterComTargetBaseListReceiveCallback( - sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrr96, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist for sensor umrr96" << std::endl; - } - if ( - sensor.model == "umrr11_can" && - com::types::ERROR_CODE_OK != - data_umrr11->RegisterComTargetBaseListReceiveCallback( - sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrr11, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist for sensor umrr11" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist for sensor umrr9d_v1_0_3" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist for sensor umrr9d_v1_2_2" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist for sensor umrr9f_v2_1_1" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist for sensor umrr9f_v2_2_1" << std::endl; - } - 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, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist for sensor umrr9d_v1_5_0" << std::endl; - } - if ( - sensor.model == "umrr9f_can_v2_5_0" && - com::types::ERROR_CODE_OK != - data_umrr9f_v2_5_0->RegisterComTargetBaseListReceiveCallback( - sensor.id, std::bind( - &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_5_0, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register CAN targetlist for sensor umrr9f_v2_5_0" << std::endl; - } - } - // create a ros2 service to change the radar parameters mode_srv_ = create_service( "smart_radar/set_radar_mode", @@ -432,12 +274,327 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option RCLCPP_INFO(this->get_logger(), "Radar services are ready."); + for (auto i = 0UL; i < m_number_of_sensors; ++i) { + const auto & sensor = m_sensors[i]; + + if (sensor.pub_type == "mse") { + if (sensor.model.find("mse") == std::string::npos) { + throw std::runtime_error("Model name must contain 'mse' when pub_type is 'mse'"); + } + } else if (sensor.pub_type == "target") { + if (sensor.model.find("mse") != std::string::npos) { + throw std::runtime_error("Model name must not contain 'mse' when pub_type is 'target'"); + } + } + + if (sensor.link_type == "eth") { + port_publishers(sensor, i); + } else if (sensor.link_type == "can") { + can_publishers(sensor, i); + } else { + RCLCPP_INFO(this->get_logger(), "Link type for sensor unknown!"); + } + } rclcpp::on_shutdown(std::bind(&SmartmicroRadarNode::on_shutdown_callback, this)); } +void SmartmicroRadarNode::port_publishers(const detail::SensorConfig & sensor, size_t sensor_idx) +{ + if (m_sensors[sensor_idx].pub_type == "mse") { + 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 (m_sensors[sensor_idx].pub_type == "target") { + 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_INFO(this->get_logger(), "Unkwon publish type!"); + } + + RCLCPP_INFO(this->get_logger(), "Inside PORT publishers"); + + 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))) { + std::cout << "Failed to register objectlist callback for sensor umrr9f_mse" << std::endl; + } + + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f_mse" << std::endl; + } + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f_v1_1_1" << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_0_0" << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f" << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_2_1" << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_4_1" << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_0_3" << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_2_2" << std::endl; + } + 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"); + std::cout << "Failed to register targetlist callback for sensor umrr9d_v1_4_1" << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrra4_v1_0_1" << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrra4_v1_2_1" << std::endl; + } +} + +void SmartmicroRadarNode::can_publishers(const detail::SensorConfig & sensor, size_t sensor_idx) +{ + if (m_sensors[sensor_idx].pub_type == "mse") { + 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 (m_sensors[sensor_idx].pub_type == "target") { + 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(this->get_logger(), "Inside CAN publisher"); + + 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))) { + std::cout << "Failed to register objectlist callback for sensor umrr9f_can_mse_v1_0_0" + << std::endl; + } + 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))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f_can_mse_v1_0_0" + << std::endl; + } + } + 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))) { + std::cout << "Failed to register CAN targetlist for sensor umrr96" << std::endl; + } + 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))) { + std::cout << "Failed to register CAN targetlist for sensor umrr11" << std::endl; + } + 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))) { + std::cout << "Failed to register CAN targetlist for sensor umrr9f_v2_1_1" << std::endl; + } + 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))) { + std::cout << "Failed to register CAN targetlist for sensor umrr9f_v2_2_1" << std::endl; + } + 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))) { + std::cout << "Failed to register CAN targetlist for sensor umrr9f_v2_4_1" << std::endl; + } + 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))) { + std::cout << "Failed to register CAN targetlist for sensor umrr9d_v1_0_3" << std::endl; + } + 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))) { + std::cout << "Failed to register CAN targetlist for sensor umrr9d_v1_2_2" << std::endl; + } + 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))) { + std::cout << "Failed to register CAN targetlist for sensor umrr9d_v1_4_1" << std::endl; + } + 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))) { + std::cout << "Failed to register CAN targetlist for sensor umrra4_v1_0_1" << std::endl; + } + 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))) { + std::cout << "Failed to register CAN targetlist for sensor umrra4_v1_2_1" << std::endl; + } +} + void SmartmicroRadarNode::on_shutdown_callback() { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Shutdown called!!!"); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Shutdown called!"); check_signal = true; rclcpp::Rate sleepRate(std::chrono::milliseconds(100)); sleepRate.sleep(); @@ -679,28 +836,164 @@ void SmartmicroRadarNode::command_response( for (auto & resp : command_resp) { response_type = resp->GetResponseType(); RCLCPP_INFO(this->get_logger(), "Response from sensor to command: %i", response_type); + uint32_t value = resp->GetValue(); + RCLCPP_INFO(this->get_logger(), "Response from sensor to command: %i", value); + } + } +} + +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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; + const auto sec = std::chrono::duration_cast(timestamp); + const auto nanosec = std::chrono::duration_cast(timestamp - sec); + msg.header.stamp.sec = sec.count(); + msg.header.stamp.nanosec = nanosec.count(); + 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; + const auto sec = std::chrono::duration_cast(timestamp); + const auto nanosec = std::chrono::duration_cast(timestamp - sec); + msg.header.stamp.sec = sec.count(); + msg.header.stamp.nanosec = nanosec.count(); + 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_umrra4_v1_3_0( +void SmartmicroRadarNode::targetlist_callback_umrra4_v1_2_1( const std::uint32_t sensor_idx, - const std::shared_ptr & - targetlist_port_umrra4_v1_3_0, + const std::shared_ptr & + targetlist_port_umrra4_v1_2_1, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrra4_v1_3_0" << std::endl; + std::cout << "Targetlist for umrra4_v1_2_1" << std::endl; if (!check_signal) { - std::shared_ptr port_header; - port_header = targetlist_port_umrra4_v1_3_0->GetPortHeader(); + 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); - for (const auto & target : targetlist_port_umrra4_v1_3_0->GetTargetList()) { + 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); @@ -713,6 +1006,7 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_3_0( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } @@ -726,13 +1020,36 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -746,6 +1063,7 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } @@ -760,13 +1078,30 @@ void SmartmicroRadarNode::targetlist_callback_umrr96( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -780,6 +1115,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr96( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } @@ -794,13 +1130,30 @@ void SmartmicroRadarNode::targetlist_callback_umrr11( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -814,6 +1167,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr11( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } @@ -828,13 +1182,34 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_3( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -848,6 +1223,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_3( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } @@ -862,13 +1238,36 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_2( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -882,6 +1281,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_2( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } @@ -898,13 +1298,30 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -918,6 +1335,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } @@ -934,13 +1352,34 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_0_0( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -954,6 +1393,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_0_0( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } @@ -968,6 +1408,10 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -975,6 +1419,23 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1( const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -988,6 +1449,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } @@ -1002,13 +1464,36 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_1( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -1022,28 +1507,52 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_1( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } -void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_5_0( +void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_4_1( const std::uint32_t sensor_idx, - const std::shared_ptr & - targetlist_port_umrr9d_v1_5_0, + const std::shared_ptr & + targetlist_port_umrr9d_v1_4_1, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrr9d_v1_5_0" << std::endl; + std::cout << "Targetlist for umrr9d_v1_4_1" << std::endl; if (!check_signal) { - std::shared_ptr + std::shared_ptr port_header; - port_header = targetlist_port_umrr9d_v1_5_0->GetPortHeader(); + 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); - for (const auto & target : targetlist_port_umrr9d_v1_5_0->GetTargetList()) { + 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); @@ -1056,28 +1565,52 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_5_0( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_target_header[sensor_idx]->publish(header); } } -void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_5_0( +void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_4_1( const std::uint32_t sensor_idx, - const std::shared_ptr & - targetlist_port_umrr9f_v2_5_0, + const std::shared_ptr & + targetlist_port_umrr9f_v2_4_1, const com::types::ClientId client_id) { - std::cout << "Targetlist for umrr9f v2_5_0" << std::endl; + std::cout << "Targetlist for umrr9f v2_4_1" << std::endl; if (!check_signal) { - std::shared_ptr + std::shared_ptr port_header; - port_header = targetlist_port_umrr9f_v2_5_0->GetPortHeader(); + 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); - for (const auto & target : targetlist_port_umrr9f_v2_5_0->GetTargetList()) { + 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); @@ -1090,29 +1623,135 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_5_0( } m_publishers[sensor_idx]->publish(msg); + m_publishers_port_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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; + const auto sec = std::chrono::duration_cast(timestamp); + const auto nanosec = std::chrono::duration_cast(timestamp - sec); + msg.header.stamp.sec = sec.count(); + msg.header.stamp.nanosec = nanosec.count(); + 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; + const auto sec = std::chrono::duration_cast(timestamp); + const auto nanosec = std::chrono::duration_cast(timestamp - sec); + msg.header.stamp.sec = sec.count(); + msg.header.stamp.nanosec = nanosec.count(); + 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_targetlist_callback_umrra4_v1_3_0( +void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_2_1( const std::uint32_t sensor_idx, const std::shared_ptr< - com::master::umrra4_automotive_v1_3_0::comtargetbaselist::ComTargetBaseList> & - targetlist_can_umrra4_v1_3_0, + 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_3_0" << std::endl; + std::cout << "CAN Targetlist for umrra4_v1_2_1" << std::endl; if (!check_signal) { - std::shared_ptr + std::shared_ptr port_header; - port_header = targetlist_can_umrra4_v1_3_0->GetPortHeader(); + 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); - for (const auto & target : targetlist_can_umrra4_v1_3_0->GetTargetList()) { + 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); @@ -1125,6 +1764,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_3_0( } m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); } } @@ -1140,13 +1780,24 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -1160,6 +1811,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1( } m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); } } @@ -1175,13 +1827,24 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr96( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -1195,6 +1858,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr96( } m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); } } @@ -1210,13 +1874,24 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr11( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -1230,6 +1905,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr11( } m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); } } @@ -1245,13 +1921,24 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -1265,6 +1952,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3( } m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); } } @@ -1280,13 +1968,24 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -1300,6 +1999,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2( } m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); } } @@ -1315,13 +2015,24 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -1335,6 +2046,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1( } m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); } } @@ -1350,13 +2062,24 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1( 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); + 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(); @@ -1370,29 +2093,41 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1( } m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); } } -void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_5_0( +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_5_0::comtargetbaselist::ComTargetBaseList> & - targetlist_can_umrr9d_v1_5_0, + 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_5_0" << std::endl; + std::cout << "Targetlist for umrr9d_v1_4_1" << std::endl; if (!check_signal) { - std::shared_ptr + std::shared_ptr port_header; - port_header = targetlist_can_umrr9d_v1_5_0->GetPortHeader(); + 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); - for (const auto & target : targetlist_can_umrr9d_v1_5_0->GetTargetList()) { + 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); @@ -1405,29 +2140,41 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_5_0( } m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); } } -void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_5_0( +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_5_0::comtargetbaselist::ComTargetBaseList> & - targetlist_can_umrr9f_v2_5_0, + 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_5_0" << std::endl; + std::cout << "Targetlist for umrr9f_v2_4_1" << std::endl; if (!check_signal) { - std::shared_ptr + std::shared_ptr port_header; - port_header = targetlist_can_umrr9f_v2_5_0->GetPortHeader(); + 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 timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; const auto sec = std::chrono::duration_cast(timestamp); const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); - for (const auto & target : targetlist_can_umrr9f_v2_5_0->GetTargetList()) { + 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); @@ -1440,6 +2187,7 @@ void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_5_0( } m_publishers[sensor_idx]->publish(msg); + m_publishers_can_target_header[sensor_idx]->publish(header); } } @@ -1490,6 +2238,7 @@ void SmartmicroRadarNode::update_config_files_from_params() 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; }; diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index 52ad86a..7fcab34 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -121,7 +121,7 @@ def tearDown(self): self.test_node.destroy_node() def test_smart_node_publishes(self): - # Expect the smartnode to publish strings on '/smart_radar/targets_' + # Expect the smartnode to publish strings on 'smart_radar/port_targets_' data_rx_s1 = [] data_rx_s2 = [] data_rx_s3 = [] @@ -137,19 +137,19 @@ def data_rx_s3_callback(msg): sub_s1 = self.test_node.create_subscription( sensor_msgs.PointCloud2, - '/smart_radar/targets_0', + 'smart_radar/port_targets_0', data_rx_s1_callback, 10 ) sub_s2 = self.test_node.create_subscription( sensor_msgs.PointCloud2, - '/smart_radar/targets_1', + 'smart_radar/port_targets_1', data_rx_s2_callback, 10 ) sub_s3 = self.test_node.create_subscription( sensor_msgs.PointCloud2, - '/smart_radar/targets_2', + 'smart_radar/port_targets_2', data_rx_s3_callback, 10 ) diff --git a/umrr_ros2_msgs/CMakeLists.txt b/umrr_ros2_msgs/CMakeLists.txt index a6f7494..69e4984 100644 --- a/umrr_ros2_msgs/CMakeLists.txt +++ b/umrr_ros2_msgs/CMakeLists.txt @@ -15,12 +15,22 @@ endif() 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" + "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) diff --git a/umrr_ros2_msgs/msg/CanObjectHeader.msg b/umrr_ros2_msgs/msg/CanObjectHeader.msg new file mode 100644 index 0000000..f32938e --- /dev/null +++ b/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/umrr_ros2_msgs/msg/CanTargetHeader.msg b/umrr_ros2_msgs/msg/CanTargetHeader.msg new file mode 100644 index 0000000..0d519e5 --- /dev/null +++ b/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/umrr_ros2_msgs/msg/PortObjectHeader.msg b/umrr_ros2_msgs/msg/PortObjectHeader.msg new file mode 100644 index 0000000..6b71d49 --- /dev/null +++ b/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/umrr_ros2_msgs/msg/PortTargetHeader.msg b/umrr_ros2_msgs/msg/PortTargetHeader.msg new file mode 100644 index 0000000..f82cdbf --- /dev/null +++ b/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/umrr_ros2_msgs/package.xml b/umrr_ros2_msgs/package.xml index b4bc9b1..3bc3fe0 100644 --- a/umrr_ros2_msgs/package.xml +++ b/umrr_ros2_msgs/package.xml @@ -2,7 +2,7 @@ umrr_ros2_msgs - 1.0.0 + 2.0.0 A package for smartmicro radar messages shah Apache 2.0