From 587467f78bc2e64422d4989fc89c025409af8f01 Mon Sep 17 00:00:00 2001 From: Shahrukh <82996646+smartSRA@users.noreply.github.com> Date: Fri, 22 Sep 2023 16:33:47 +0200 Subject: [PATCH] #6 Enable communication via CAN (#36) 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. --- CHANGELOG.md | 16 +- Readme.md | 61 +- docker-compose.yml | 2 +- simulator/config_umrr9d/com_lib_config.json | 2 +- simulator/config_umrr9f/com_lib_config.json | 2 +- simulator/simulation/CMakeLists.txt | 16 +- simulator/simulation/src/simulator.cpp | 2 +- smart_extract.sh | 2 +- umrr_ros2_driver/CMakeLists.txt | 41 +- .../cmake/smart_access_config.json.in | 4 +- umrr_ros2_driver/config/hw_inventory.json | 11 +- umrr_ros2_driver/config/routing_table.json | 13 +- .../smartmicro_radar_node.hpp | 350 ++++++--- umrr_ros2_driver/launch/radar.launch.py | 18 +- .../param/example/radar.adapter.example.yaml | 32 + .../param/example/radar.sensor.example.yaml | 122 +++ .../param/radar.adapter.template.yaml | 14 + .../param/radar.sensor.template.yaml | 114 +++ umrr_ros2_driver/param/radar.template.yaml | 68 -- .../src/smartmicro_radar_node.cpp | 720 ++++++++++++++---- .../test/radar_node_test.launch.py | 14 +- 21 files changed, 1196 insertions(+), 428 deletions(-) create mode 100644 umrr_ros2_driver/param/example/radar.adapter.example.yaml create mode 100644 umrr_ros2_driver/param/example/radar.sensor.example.yaml create mode 100644 umrr_ros2_driver/param/radar.adapter.template.yaml create mode 100644 umrr_ros2_driver/param/radar.sensor.template.yaml delete mode 100644 umrr_ros2_driver/param/radar.template.yaml diff --git a/CHANGELOG.md b/CHANGELOG.md index 036247c..f7e0fc2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,23 +14,23 @@ Requires new sensor firmware, if using UMRR11-T132: V5.1.4 and if using UMRR96-T ## 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 possibilty of configuring the ip addresses of the sensor using ros2 services. The tests are further extended to inlcude ros2 services check. Requires new sensor firmware, if using UMRR11-T132: V5.1.4 and if using UMRR96-T153: V5.2.4. +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. ## 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 pointcloud data. The callbacks for datastream now require a clientID. +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. ## 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 pointcloud data. The callbacks for datastream now require a clientID. +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. ## v3.2.0 - 2022-11-11 -This minor release introduces signal-to-noise field in the pointclouds and also fixes the max number of sensors that could be connected at once. +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. ## v3.2.1 - 2022-12-16 -This release fixes the offset which causes anomaly in pointclouds from DRVEGRD 152 sensor. It also fixes the timestamp calculation bug which causes rviz to crash and updates the simulator source files. +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. ## v4.0.0 - 2023-02-06 @@ -38,4 +38,8 @@ This release includes the new DRVEGRD 169 user-interface. The release includes n ## 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. \ No newline at end of file +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. + +## 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. \ No newline at end of file diff --git a/Readme.md b/Readme.md index 322f3c9..fdd5f7d 100644 --- a/Readme.md +++ b/Readme.md @@ -26,22 +26,24 @@ A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVE 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: `August 21, 2023` -- Smart Access Automotive version: `v3.2.0` -- User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.1` -- User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.1` +- Date of release: `September 22, 2023` +- Smart Access Automotive version: `v3.4.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.2.0` -- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.2` -- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.2.1` -- User interface version: `UMRRA4 Type 171 AUTOMOTIVE v1.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: `UMRR9D Type 152 AUTOMOTIVE v1.0.3` +- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.2.2` +- User interface version: `UMRRA4 Type 171 AUTOMOTIVE v1.0.1` ### 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 - UMRR9F Type 169: V1.3.0 - UMRR9F Type 169: V2.0.2 - UMRR9F Type 169: V2.2.0 @@ -70,16 +72,45 @@ callback is triggered a new point cloud message is created and published. The driver publishes `sensor_msgs::msg::PointCloud2` messages with the radar targets on the topic `umrr/targets` which can be remapped through the parameters. -### Configuration: +### Interface Configuration: +For setting up a sensor with ethernet or can, the interfaces of the should be set properly prior to configuring the node. +The sensor is equipped with three physical layers (RS485, CAN and ethernet) however the driver uses only ethernet and can: +- ethernet: to set-up an ethernet interface the following command could be used `ifconfig my_interface_name 192.168.11.17 netmask 255.255.255.0`. +The command above uses the default source ip address used by the sensors. +- can: if using LAWICEL to set-up a can interface the following commands could be used `slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx`and than `ip link set up my_interface_name`. +This uses the default baudrate of _500000_. When using Peak CAN the interfaces are recognized by linux and is only needed to set the baudrate. + +### Node Configuration: The node is configured through the parameters. Here is a short recap of the most important parts. -For more details, see the [`radar.template.yaml`](umrr_ros2_driver/param/radar.template.yaml) file. -- `client_id`: the id of the client, must be a unique integer -- `ip`: the IP of the used sensor or the source -- `port`: port to be used to receive the packets -- `iface_name`: name of the used network interface +For more details, see the [`radar.sensor.example.yaml`](umrr_ros2_driver/param/radar.sensor.example.yaml) and +[`radar.adapter.example.yaml`](umrr_ros2_driver/param/radar.adapter.example.yaml) files. + +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', +- `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_. +- `port`: port to be used to receive the packets, default is _55555_ - `frame_id`: name of the frame in which the messages will be published - `history_size`: size of history for the message publisher -- `model`: the model('umrra4_v1_0_0', 'umrr11', 'umrr9d', 'umrr96', 'umrr9f_v1_1_1', 'umrr9f_v2_0_0') of the sensor being used +- `inst_type`: the type of instruction serialization type, relevant to sensors using ethernet and should be 'port_based' +- `data_type`: the type of data serialization type, relevant to sensors using ethernet and should be 'port_based' +- `uifname`: the user interface name of the sensor +- `uifmajorv`: the major version of the sensor user interface +- `uifminorv`: the minor version of the sensor user interface +- `uifpatchv`: the patch version of the sensor user interface + +For setting up the ***adapters***: +- `master_inst_serial_type`: the instruction serilization type of the master. When using a hybrid of 'can' and 'port' use 'can_based' +- `master_data_serial_type`: the data serilization type of the master. When using a hybrid of 'can' and 'port' use 'can_based' +- `hw_type`: the type of the hardware connection +- `hw_dev_id`: adapter id of the hardware, ***the sensor and adapter should use the same id*** +- `hw_iface_name`: name of the used network interface +- `baudrate`: the baudrate of the sensor connected with can, default is _500000_ +- `port`: port to be used to receive the packets, default is _55555_ ## Mode of operations of the sensors The smartmicro radars come equipped with numerous features and modes of operation. Using the ros2 services provided one diff --git a/docker-compose.yml b/docker-compose.yml index f3c20a2..2799005 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -60,7 +60,7 @@ services: - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr9f/com_lib_config.json - LD_LIBRARY_PATH=/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 entrypoint: ["/code/simulator/simulation/out/bin/simulator"] - command: ["9", "1", "1", "B"] + command: ["9", "2", "1", "C"] networks: device_network: ipv4_address: 172.22.10.103 diff --git a/simulator/config_umrr9d/com_lib_config.json b/simulator/config_umrr9d/com_lib_config.json index d71df6d..efebc3a 100644 --- a/simulator/config_umrr9d/com_lib_config.json +++ b/simulator/config_umrr9d/com_lib_config.json @@ -8,7 +8,7 @@ "user_interface_name": "umrr9d_t152_automotive", "user_interface_major_v": 1, "user_interface_minor_v": 0, - "user_interface_patch_v": 2, + "user_interface_patch_v": 3, "download_path": "", "instruction_serialization_type": "port_based", "data_serialization_type": "port_based", diff --git a/simulator/config_umrr9f/com_lib_config.json b/simulator/config_umrr9f/com_lib_config.json index caaff38..5a444bb 100644 --- a/simulator/config_umrr9f/com_lib_config.json +++ b/simulator/config_umrr9f/com_lib_config.json @@ -6,7 +6,7 @@ "shared_lib_path": "/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9", "config_path": "/code/simulator/config_umrr9f", "user_interface_name": "umrr9f_t169_automotive", - "user_interface_major_v": 1, + "user_interface_major_v": 2, "user_interface_minor_v": 1, "user_interface_patch_v": 1, "download_path": "", diff --git a/simulator/simulation/CMakeLists.txt b/simulator/simulation/CMakeLists.txt index fbd282d..5bfab9b 100644 --- a/simulator/simulation/CMakeLists.txt +++ b/simulator/simulation/CMakeLists.txt @@ -16,18 +16,18 @@ add_executable(simulator target_include_directories(simulator PUBLIC ../../umrr_ros2_driver/smartmicro/include - umrr_ros2_driver/smartmicro/include/umrr11_t132_automotive_v1_1_1 - umrr_ros2_driver/smartmicro/include/umrr96_t153_automotive_v1_2_1 - umrr_ros2_driver/smartmicro/include/umrr9f_t169_automotive_v1_1_1 - umrr_ros2_driver/smartmicro/include/umrr9d_t152_automotive_v1_0_2) + umrr_ros2_driver/smartmicro/include/umrr11_t132_automotive_v1_1_2 + umrr_ros2_driver/smartmicro/include/umrr96_t153_automotive_v1_2_2 + umrr_ros2_driver/smartmicro/include/umrr9f_t169_automotive_v2_1_1 + umrr_ros2_driver/smartmicro/include/umrr9d_t152_automotive_v1_0_3) target_link_libraries(simulator com_lib osal - umrr11_t132_automotivev1.1.1_user_interface - umrr96_t153_automotivev1.2.1_user_interface - umrr9f_t169_automotivev1.1.1_user_interface - umrr9d_t152_automotivev1.0.2_user_interface + umrr11_t132_automotivev1.1.2_user_interface + umrr96_t153_automotivev1.2.2_user_interface + umrr9f_t169_automotivev2.1.1_user_interface + umrr9d_t152_automotivev1.0.3_user_interface dl) install(TARGETS simulator DESTINATION bin) diff --git a/simulator/simulation/src/simulator.cpp b/simulator/simulation/src/simulator.cpp index 39003fd..750445d 100644 --- a/simulator/simulation/src/simulator.cpp +++ b/simulator/simulation/src/simulator.cpp @@ -155,7 +155,7 @@ int main(int argc, char * argv[]) } else { std::cout << "Invalid input!" << std::endl; } - if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(30)) { + if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(15)) { break; } } diff --git a/smart_extract.sh b/smart_extract.sh index 3c3f8a7..0895494 100755 --- a/smart_extract.sh +++ b/smart_extract.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -smart_pack=SmartAccessAutomotive_3_2_0.tgz +smart_pack=SmartAccessAutomotive_3_4_0.tgz URL_smartbinaries=https://www.smartmicro.com/fileadmin/media/Downloads/Automotive_Radar/Software/${smart_pack} cat << EOF diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index d6680e8..04f4e1b 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -55,14 +55,15 @@ install(FILES "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libosal.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libcom_lib.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libbasev1.0.2_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr11_t132_automotivev1.1.1_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr96_t153_automotivev1.2.1_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr11_t132_automotivev1.1.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr96_t153_automotivev1.2.2_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev1.1.1_user_interface.so" "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.0.0_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.2.0_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.0.2_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.2.1_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_automotivev1.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}/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}/libumrra4_automotivev1.0.1_user_interface.so" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -80,14 +81,15 @@ ament_auto_add_library(smartmicro_radar_node SHARED "src/smartmicro_radar_node.c target_include_directories(smartmicro_radar_node PUBLIC ${CMAKE_INSTALL_PREFIX}/include/ smartmicro/include/ -smartmicro/include/umrr11_t132_automotive_v1_1_1 -smartmicro/include/umrr96_t153_automotive_v1_2_1 +smartmicro/include/umrr11_t132_automotive_v1_1_2 +smartmicro/include/umrr96_t153_automotive_v1_2_2 smartmicro/include/umrr9f_t169_automotive_v1_1_1 smartmicro/include/umrr9f_t169_automotive_v2_0_0 -smartmicro/include/umrr9f_t169_automotive_v2_2_0 -smartmicro/include/umrr9d_t152_automotive_v1_0_2 -smartmicro/include/umrr9d_t152_automotive_v1_2_1 -smartmicro/include/umrra4_automotive_v1_0_0) +smartmicro/include/umrr9f_t169_automotive_v2_1_1 +smartmicro/include/umrr9f_t169_automotive_v2_2_1 +smartmicro/include/umrr9d_t152_automotive_v1_0_3 +smartmicro/include/umrr9d_t152_automotive_v1_2_2 +smartmicro/include/umrra4_automotive_v1_0_1) # link smart_access_lib-linux-x86_64-gcc_9 to the node target_link_libraries(smartmicro_radar_node @@ -95,14 +97,15 @@ target_link_libraries(smartmicro_radar_node nlohmann_json::nlohmann_json com_lib osal - umrr11_t132_automotivev1.1.1_user_interface - umrr96_t153_automotivev1.2.1_user_interface + umrr11_t132_automotivev1.1.2_user_interface + umrr96_t153_automotivev1.2.2_user_interface umrr9f_t169_automotivev1.1.1_user_interface umrr9f_t169_automotivev2.0.0_user_interface - umrr9f_t169_automotivev2.2.0_user_interface - umrr9d_t152_automotivev1.0.2_user_interface - umrr9d_t152_automotivev1.2.1_user_interface - umrra4_automotivev1.0.0_user_interface) + umrr9f_t169_automotivev2.1.1_user_interface + umrr9f_t169_automotivev2.2.1_user_interface + umrr9d_t152_automotivev1.0.3_user_interface + umrr9d_t152_automotivev1.2.2_user_interface + umrra4_automotivev1.0.1_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" @@ -111,7 +114,7 @@ rclcpp_components_register_node(smartmicro_radar_node if(BUILD_TESTING) find_package(launch_testing_ament_cmake) - add_launch_test(test/radar_node_test.launch.py TIMEOUT "150") + add_launch_test(test/radar_node_test.launch.py TIMEOUT "100") # Not applying ros2 linters on external sources list(APPEND AMENT_LINT_AUTO_EXCLUDE diff --git a/umrr_ros2_driver/cmake/smart_access_config.json.in b/umrr_ros2_driver/cmake/smart_access_config.json.in index 2b566d0..644827d 100644 --- a/umrr_ros2_driver/cmake/smart_access_config.json.in +++ b/umrr_ros2_driver/cmake/smart_access_config.json.in @@ -10,7 +10,7 @@ "user_interface_minor_v": 0, "user_interface_patch_v": 2, "download_path": "", - "instruction_serialization_type": "port_based", - "data_serialization_type": "port_based", + "instruction_serialization_type": "can_based", + "data_serialization_type": "can_based", "alive": false } diff --git a/umrr_ros2_driver/config/hw_inventory.json b/umrr_ros2_driver/config/hw_inventory.json index cae6c8f..48d7e7a 100644 --- a/umrr_ros2_driver/config/hw_inventory.json +++ b/umrr_ros2_driver/config/hw_inventory.json @@ -1,12 +1,9 @@ { - "name": "HW Inventory List", - "version": "1.1.0", "hwItems": [ { - "type": "eth", - "dev_id": 2, - "iface_name": "eth0", - "port": 55555 + } - ] + ], + "name": "HW Inventory List", + "version": "1.1.0" } diff --git a/umrr_ros2_driver/config/routing_table.json b/umrr_ros2_driver/config/routing_table.json index 28d5870..bc6cee3 100644 --- a/umrr_ros2_driver/config/routing_table.json +++ b/umrr_ros2_driver/config/routing_table.json @@ -1,14 +1,9 @@ { - "name": "Client Routing Table", - "version": "1.0.0", "clients": [ { - "client_id": 111, - "link_type": "eth", - "ip": "172.22.10.101", - "port": 55555, - "instruction_serialization_type": "port_based", - "data_serialization_type": "port_based" + "can_network_id": 0 } - ] + ], + "name": "Client Routing Table", + "version": "1.0.0" } 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 24d63f8..d9e3d4a 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 @@ -25,188 +25,314 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include -#include -#include -#include +#include +#include +#include +#include #include #include #include +#include "umrr_ros2_msgs/srv/send_command.hpp" #include "umrr_ros2_msgs/srv/set_ip.hpp" #include "umrr_ros2_msgs/srv/set_mode.hpp" -#include "umrr_ros2_msgs/srv/send_command.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::string ip{}; - std::uint32_t port{}; + std::uint32_t dev_id{}; std::string frame_id{}; std::uint32_t history_size{}; + std::string ip{}; + std::string link_type{}; std::string model{}; + std::uint32_t port{}; + std::string inst_type{}; + std::string data_type{}; + std::string uifname{}; + std::uint32_t uifmajorv{}; + std::uint32_t uifminorv{}; + std::uint32_t uifpatchv{}; +}; + +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. /// void my_timer_callback() { timer->cancel(); } + void on_shutdown_callback(); private: /// /// @brief A callback that is called when a new target list port for - /// umrr11 arrives. + /// 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] client_id The client_id of the sensor /// void targetlist_callback_umrr11( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr11_t132_automotive_v1_1_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr11, - const com::types::ClientId client_id); + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr11, + const com::types::ClientId client_id); /// /// @brief A callback that is called when a new target list port for - /// umrr96 arrives. + /// 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] 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); + + /// + /// @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] client_id The client_id of the sensor + /// + void targetlist_callback_umrr9f_v2_0_0( const std::uint32_t sensor_idx, const std::shared_ptr< - com::master::umrr96_t153_automotive_v1_2_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr96, + com::master::umrr9f_t169_automotive_v2_0_0::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9f_v2_0_0, const com::types::ClientId client_id); /// /// @brief A callback that is called when a new target list port for - /// umrr9f_v1_1_1 arrives. + /// 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] client_id The client_id of the sensor /// - void targetlist_callback_umrr9f_v1_1_1( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9f, - const com::types::ClientId client_id); + const std::uint32_t sensor_idx, + const std::shared_ptr + &targetlist_port_umrr9f, + const com::types::ClientId client_id); + /// /// @brief A callback that is called when a new target list port for - /// umrr9f_v2_0_0 arrives. + /// 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] client_id The client_id of the sensor /// - - void targetlist_callback_umrr9f_v2_0_0( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr9f_t169_automotive_v2_0_0::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9f_v2_0_0, - const com::types::ClientId client_id); + 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); /// /// @brief A callback that is called when a new target list port for - /// umrr9f_v2_2_0 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] 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); - void targetlist_callback_umrr9f_v2_2_0( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr9f_t169_automotive_v2_2_0::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9f_v2_2_0, - const com::types::ClientId client_id); + /// + /// @brief A callback that is called when a new target list port for + /// umrr9d_v1_0_3 T152 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] target_list_port 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); /// /// @brief A callback that is called when a new target list port for - /// umrr9d_v1_0_2 arrives. + /// umrr9d_v1_2_2 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port The target list port /// @param[in] client_id The client_id of the sensor /// - - void targetlist_callback_umrr9d_v1_0_2( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_0_2::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9d_v1_0_2, - const com::types::ClientId client_id); + 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); /// /// @brief A callback that is called when a new target list port for - /// umrr9d_v1_2_1 arrives. + /// umrra4 T171_v1_0_1 arrives. + /// @param[in] sensor_idx The sensor id for the respected published topic. + /// @param[in] targetlist_port_umrra4 The targetlist port + /// @param[in] client_id The client_id of the sensor + /// + void targetlist_callback_umrra4_v1_0_1( + const std::uint32_t sensor_idx, + const std::shared_ptr &targetlist_port_umrra4_v1_0_1, + const com::types::ClientId client_id + ); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr96_v1_0_3 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port 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); - void targetlist_callback_umrr9d_v1_2_1( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_2_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9d_v1_2_1, - const com::types::ClientId client_id); + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr11 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] target_list_port 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); /// - /// @brief A callback that is called when a new target list port for - /// umrra4 arrives. + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_v2_1_1 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port 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); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9f_v2_2_1 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] target_list_port 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); - void targetlist_callback_umrra4_v1_0_0( - const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrra4_automotive_v1_0_0::comtargetlistport::ComTargetListPort> & - targetlist_port_umrra4_v1_0_0, - const com::types::ClientId client_id); + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9d_v1_=_ยง arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] target_list_port 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); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrr9d_v1_2_2 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] target_list_port 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); + + /// + /// @brief A callback that is called when a new CAN target list for + /// umrra4 arrives. + /// + /// @param[in] sensor_idx The sensor id for respective published topic. + /// @param[in] target_list_port 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); /// /// @brief Read parameters and update the json config files required by @@ -217,79 +343,77 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// /// @brief Callaback for getting the parameter response. /// - void mode_response( - const com::types::ClientId client_id, - const std::shared_ptr & response, - const std::string instruction_name); + void + mode_response(const com::types::ClientId client_id, + const std::shared_ptr &response, + const std::string instruction_name); /// /// @brief Callaback for getting the command response. /// - void command_response( - const com::types::ClientId client_id, - const std::shared_ptr & response, - const std::string 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. /// 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. /// 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. /// - 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. /// void radar_command( - const std::shared_ptr request, - std::shared_ptr response); - + const std::shared_ptr request, + std::shared_ptr response); + rclcpp::Service::SharedPtr mode_srv_; rclcpp::Service::SharedPtr ip_addr_srv_; rclcpp::Service::SharedPtr command_srv_; std::array m_sensors{}; + std::array m_adapters{}; - std::array::SharedPtr, detail::kMaxSensorCount> - m_publishers{}; + std::array::SharedPtr, + detail::kMaxSensorCount> + m_publishers{}; std::size_t m_number_of_sensors{}; + std::size_t m_number_of_adapters{}; rclcpp::TimerBase::SharedPtr timer; com::types::ClientId client_id; std::uint64_t response_type{}; }; -/// -/// @brief Set flag used in callbacks and call ros2 shutdown. -/// -void terminate_on_receive(int signal); - bool check_signal = false; std::shared_ptr m_services{}; -std::shared_ptr data_umrra4_v1_0_0{}; -std::shared_ptr data_umrr11{}; -std::shared_ptr data_umrr96{}; +std::shared_ptr data_umrra4_v1_0_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_2_0{}; -std::shared_ptr data_umrr9d_v1_0_2{}; -std::shared_ptr data_umrr9d_v1_2_1{}; +std::shared_ptr data_umrr9f_v2_1_1{}; +std::shared_ptr data_umrr9f_v2_2_1{}; +std::shared_ptr data_umrr9d_v1_0_3{}; +std::shared_ptr data_umrr9d_v1_2_2{}; -} // namespace radar -} // namespace drivers -} // namespace smartmicro +} // namespace radar +} // namespace drivers +} // namespace smartmicro -#endif // UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ +#endif // UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ diff --git a/umrr_ros2_driver/launch/radar.launch.py b/umrr_ros2_driver/launch/radar.launch.py index afbcc45..23946f3 100644 --- a/umrr_ros2_driver/launch/radar.launch.py +++ b/umrr_ros2_driver/launch/radar.launch.py @@ -27,24 +27,20 @@ def generate_launch_description(): """Generate the launch description.""" - radar_param_file = os.path.join( - get_package_share_directory(PACKAGE_NAME), 'param/radar.template.yaml') - - radar_params = DeclareLaunchArgument( - 'radar_param_file', - default_value=radar_param_file, - description='Path to param file for the radar node.' - ) + + radar_sensor_params = os.path.join( + get_package_share_directory(PACKAGE_NAME), 'param/radar.sensor.template.yaml') + + radar_adapter_params = os.path.join( + get_package_share_directory(PACKAGE_NAME), 'param/radar.adapter.template.yaml') radar_node = Node( package=PACKAGE_NAME, executable='smartmicro_radar_node_exe', name='smart_radar', - parameters=[LaunchConfiguration('radar_param_file')] + parameters=[radar_sensor_params, radar_adapter_params] ) - return LaunchDescription([ - radar_params, radar_node ]) diff --git a/umrr_ros2_driver/param/example/radar.adapter.example.yaml b/umrr_ros2_driver/param/example/radar.adapter.example.yaml new file mode 100644 index 0000000..8d238f2 --- /dev/null +++ b/umrr_ros2_driver/param/example/radar.adapter.example.yaml @@ -0,0 +1,32 @@ +--- +/**: + ros__parameters: + # This sets the iface_name in the hw_inventory.json + # sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyUSBx + # sudo ip link set up slcan0 + # For peak CAN adapters, only required to set baudrate + # One sensor per interface for can, for ethernet we can use one interface/dev for multiple sensors + master_inst_serial_type: "can_based" + master_data_serial_type: "can_based" + adapters: + adapter_0: + hw_type: "can" + hw_dev_id: 2 + hw_iface_name: "slcan0" + baudrate: 500000 + adapter_1: + hw_type: "eth" + hw_dev_id: 3 + hw_iface_name: "eth0" + port: 55555 + adapter_2: + hw_type: "can" + hw_dev_id: 4 + hw_iface_name: "slcan1" + baudrate: 500000 + adapter_3: + hw_type: "can" + hw_dev_id: 5 + hw_iface_name: "can0" + baudrate: 500000 + diff --git a/umrr_ros2_driver/param/example/radar.sensor.example.yaml b/umrr_ros2_driver/param/example/radar.sensor.example.yaml new file mode 100644 index 0000000..7a38608 --- /dev/null +++ b/umrr_ros2_driver/param/example/radar.sensor.example.yaml @@ -0,0 +1,122 @@ +--- +/**: + ros__parameters: +# An array of sensors to subscribe to. + sensors: + # As many as 10 sensors all named as "sensor_" in increasing order of numbers, + # e.g., sensor_0, sensor_1, etc. The list must start with sensor_0. + sensor_0: + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr9d_can" + # The client_id of the sensor/source, must be a unique integer. + id: 500 + # Adapter id to which sensor is connected + dev_id: 2 + # The interface name of the sensor + uifname: "umrr9d_t152_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 0 + # The pathc version of the interface + uifpatchv: 3 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + sensor_1: + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr9f_can_v2_1_1" + # The client_id of the sensor/source, must be a unique integer. + id: 600 + # Adapter id to which sensor is connected + dev_id: 4 + # The interface name of the sensor + uifname: "umrr9f_t169_automotive" + # The major version of the interface + uifmajorv: 2 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 1 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + sensor_2: + link_type: "eth" + # The model of the connected sensor. + model: "umrr9f_v2_1_1" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 700 + # The ip address of the sensor or of the source acting as a sensor. + ip: "192.168.11.11" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr9f_t169_automotive" + # The major version of the interface + uifmajorv: 2 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 1 + sensor_3: + link_type: "eth" + # The model of the connected sensor. + model: "umrr11" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 800 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.101" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr11_t132_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 2 + sensor_4: + # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, + # umrr9f_v1_1_1, umrr9f_v2_0_0 + link_type: "can" + model: "umrr9d_can" + # The client_id of the sensor/source, must be a unique integer. + id: 900 + # Adapter id to which sensor is connected + dev_id: 5 + # The interface name of the sensor + uifname: "umrr9d_t152_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 0 + # The pathc version of the interface + uifpatchv: 3 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 diff --git a/umrr_ros2_driver/param/radar.adapter.template.yaml b/umrr_ros2_driver/param/radar.adapter.template.yaml new file mode 100644 index 0000000..f1c3e36 --- /dev/null +++ b/umrr_ros2_driver/param/radar.adapter.template.yaml @@ -0,0 +1,14 @@ +--- +/**: + ros__parameters: + # This sets the iface_name in the hw_inventory.json + # One sensor per adapter in case of CAN + # For ethernet it is possible to have one adapter/interface for multiple sensors + master_inst_serial_type: "port_based" + master_data_serial_type: "port_based" + adapters: + adapter_0: + hw_type: "eth" + hw_dev_id: 3 + hw_iface_name: "eth0" + port: 55555 diff --git a/umrr_ros2_driver/param/radar.sensor.template.yaml b/umrr_ros2_driver/param/radar.sensor.template.yaml new file mode 100644 index 0000000..98e5634 --- /dev/null +++ b/umrr_ros2_driver/param/radar.sensor.template.yaml @@ -0,0 +1,114 @@ +--- +/**: + ros__parameters: +# An array of sensors to subscribe to. + sensors: + # As many as 10 sensors all named as "sensor_" in increasing order of numbers, + # e.g., sensor_0, sensor_1, etc. The list must start with sensor_0. + # 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_0: + link_type: "eth" + # The model of the connected sensor. + model: "umrr11" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 100 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.101" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr11_t132_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 2 + sensor_1: + link_type: "eth" + # The model of the connected sensor. + model: "umrr96" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 200 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.102" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr96_t153_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 2 + # The pathc version of the interface + uifpatchv: 2 + sensor_2: + link_type: "eth" + # The model of the connected sensor. + model: "umrr9f_v2_1_1" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 300 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.103" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr9f_t169_automotive" + # The major version of the interface + uifmajorv: 2 + # The minor version of the interface + uifminorv: 1 + # The pathc version of the interface + uifpatchv: 1 + sensor_3: + link_type: "eth" + # The model of the connected sensor. + model: "umrr9d_v1_0_3" + # Adapter id to which sensor is connected + dev_id: 3 + # The client_id of the sensor/source, must be a unique integer. + id: 400 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.104" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + inst_type: "port_based" + data_type: "port_based" + # The interface name of the sensor + uifname: "umrr9d_t152_automotive" + # The major version of the interface + uifmajorv: 1 + # The minor version of the interface + uifminorv: 0 + # The pathc version of the interface + uifpatchv: 3 diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml deleted file mode 100644 index bba5ec1..0000000 --- a/umrr_ros2_driver/param/radar.template.yaml +++ /dev/null @@ -1,68 +0,0 @@ ---- -/**: - ros__parameters: - # The client_id to be set in smart_access_config.json, must be a unique integer. - master_client_id: 1000 - # This sets the dev_id in the hw_inventory.json - hw_dev_id: 2 - # This sets the port in the hw_inventory.json - hw_port: 55555 - # This sets the iface_name in the hw_inventory.json - hw_iface_name: "eth0" - # An array of sensors to subscribe to. - sensors: - # As many as 10 sensors all named as "sensor_" in increasing order of numbers, - # e.g., sensor_0, sensor_1, etc. The list must start with sensor_0. - sensor_0: - # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, - # umrr9f_v1_1_1, umrr9f_v2_0_0 - model: "umrr11" - # The client_id of the sensor/source, must be a unique integer. - id: 100 - # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.101" - # The port to be used. - port: 55555 - # The frame_id to be set to the published messages. - frame_id: "umrr" - # Specify the history size. - history_size: 10 - sensor_1: - # The model of the connected sensor. - model: "umrr96" - # The client_id of the sensor/source, must be a unique integer. - id: 200 - # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.102" - # The port to be used. - port: 55555 - # The frame_id to be set to the published messages. - frame_id: "umrr" - # Specify the history size. - history_size: 10 - sensor_2: - # The model of the connected sensor. - model: "umrr9f_v1_1_1" - # The client_id of the sensor/source, must be a unique integer. - id: 300 - # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.103" - # The port to be used. - port: 55555 - # The frame_id to be set to the published messages. - frame_id: "umrr" - # Specify the history size. - history_size: 10 - sensor_3: - # The model of the connected sensor. - model: "umrr9d_v1_0_2" - # The client_id of the sensor/source, must be a unique integer. - id: 400 - # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.104" - # The port to be used. - port: 55555 - # The frame_id to be set to the published messages. - frame_id: "umrr" - # Specify the history size. - history_size: 10 diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 42aed5f..6bc38f9 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -19,20 +19,24 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include +#include +#include +#include +#include #include @@ -73,19 +77,37 @@ constexpr auto kDefaultPort = 55555; constexpr auto kDefaultHistorySize = 10; constexpr auto kDefaultFrameId = "umrr"; constexpr auto kDefaultSensorType = "umrr11"; +constexpr auto kDefaultInstType = "port_based"; +constexpr auto kDefaultDataType = "port_based"; + +constexpr auto kDefaultHwDevId = 1; +constexpr auto kDefaultHwDevIface = "slcan"; +constexpr auto kDefaultHwLinkType = "can"; + +constexpr auto kHwDevLinkTag = "type"; +constexpr auto kClientLinkTag = "link_type"; +constexpr auto kHwDevIdTag = "dev_id"; +constexpr auto kHwDevIfaceNameTag = "iface_name"; + +constexpr auto kHwDevPortTag = "hw_port"; constexpr auto kClientIdTag = "client_id"; constexpr auto kPortTag = "port"; +constexpr auto kBaudRateTag = "baudrate"; constexpr auto kIpTag = "ip"; -constexpr auto kIfaceNameTag = "iface_name"; -constexpr auto kMasterClientIdTag = "master_client_id"; -constexpr auto kDevIdTag = "hw_dev_id"; -constexpr auto kDevIfaceNameTag = "hw_iface_name"; -constexpr auto kDevPortTag = "hw_port"; -constexpr auto kDevIdJsonTag = "dev_id"; +constexpr auto kInstSerialTypeTag = "master_inst_serial_type"; +constexpr auto kDataSerialTypeTag = "master_data_serial_type"; + +constexpr auto kInstSerialTypeJsonTag = "instruction_serialization_type"; +constexpr auto kDataSerialTypeJsonTag = "data_serialization_type"; + constexpr auto kClientsJsonTag = "clients"; constexpr auto kHwItemsJsonTag = "hwItems"; +constexpr auto kUINameTag = "user_interface_name"; +constexpr auto kUIMajorVTag = "user_interface_major_v"; +constexpr auto kUIMinorVTag = "user_interface_minor_v"; +constexpr auto kUIPatchVTag = "user_interface_patch_v"; constexpr bool float_eq(const float a, const float b) noexcept { @@ -100,26 +122,26 @@ struct RadarPoint float z{}; float radial_speed{}; float power{}; - float RCS{}; - float Noise{}; + float rcs{}; + float noise{}; float snr{}; constexpr friend bool operator==(const RadarPoint & p1, const RadarPoint & p2) noexcept { return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && float_eq(p1.z, p2.z) && float_eq(p1.radial_speed, p2.radial_speed) && float_eq(p1.power, p2.power) && - float_eq(p1.RCS, p2.RCS) && float_eq(p1.Noise, p2.Noise) && float_eq(p1.snr, p2.snr); + float_eq(p1.rcs, p2.rcs) && float_eq(p1.noise, p2.noise) && float_eq(p1.snr, p2.snr); } }; LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(radial_speed); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(power); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(RCS); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(Noise); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(rcs); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(noise); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(snr); using Generators = std::tuple< point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, field_radial_speed_generator, field_RCS_generator, - field_Noise_generator, field_power_generator, field_snr_generator>; + point_cloud_msg_wrapper::field_z_generator, field_radial_speed_generator, field_rcs_generator, + field_noise_generator, field_power_generator, field_snr_generator>; using RadarCloudModifier = PointCloud2Modifier; } // namespace @@ -145,18 +167,19 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option } // Getting the data stream service - data_umrra4_v1_0_0 = com::master::umrra4_automotive_v1_0_0::DataStreamServiceIface::Get(); - data_umrr11 = com::master::umrr11_t132_automotive_v1_1_1::DataStreamServiceIface::Get(); - data_umrr96 = com::master::umrr96_t153_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(); data_umrr9f_v1_1_1 = com::master::umrr9f_t169_automotive_v1_1_1::DataStreamServiceIface::Get(); data_umrr9f_v2_0_0 = com::master::umrr9f_t169_automotive_v2_0_0::DataStreamServiceIface::Get(); - data_umrr9f_v2_2_0 = com::master::umrr9f_t169_automotive_v2_2_0::DataStreamServiceIface::Get(); - data_umrr9d_v1_0_2 = com::master::umrr9d_t152_automotive_v1_0_2::DataStreamServiceIface::Get(); - data_umrr9d_v1_2_1 = com::master::umrr9d_t152_automotive_v1_2_1::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_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(); - RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); - // Wait init time + // 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]; @@ -165,31 +188,31 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option "smart_radar/targets_" + std::to_string(i), sensor.history_size); if ( - sensor.model == "umrra4_v1_0_0" && + sensor.model == "umrra4_v1_0_1" && com::types::ERROR_CODE_OK != - data_umrra4_v1_0_0->RegisterComTargetListPortReceiveCallback( + data_umrra4_v1_0_1->RegisterComTargetListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_0, this, i, + &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_0" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrra4" << std::endl; } if ( - sensor.model == "umrr11" && + sensor.model == "umrr96" && com::types::ERROR_CODE_OK != - data_umrr11->RegisterComTargetListPortReceiveCallback( + data_umrr96->RegisterComTargetListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr11, this, i, + &SmartmicroRadarNode::targetlist_callback_umrr96, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; } if ( - sensor.model == "umrr96" && + sensor.model == "umrr11" && com::types::ERROR_CODE_OK != - data_umrr96->RegisterComTargetListPortReceiveCallback( + data_umrr11->RegisterComTargetListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr96, this, i, + &SmartmicroRadarNode::targetlist_callback_umrr11, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; } if ( sensor.model == "umrr9f_v1_1_1" && @@ -210,31 +233,103 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_0_0" << std::endl; } if ( - sensor.model == "umrr9f_v2_2_0" && + 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 == "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" << 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_umrr9f_v2_2_0->RegisterComTargetListPortReceiveCallback( + data_umrr9d_v1_2_2->RegisterComTargetBaseListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_0, this, i, + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_2_0" << std::endl; + std::cout << "Failed to register CAN targetlist for sensor umrr9d_v1_2_2" << std::endl; } if ( - sensor.model == "umrr9d_v1_0_2" && + sensor.model == "umrr9f_can_v2_1_1" && com::types::ERROR_CODE_OK != - data_umrr9d_v1_0_2->RegisterComTargetListPortReceiveCallback( + data_umrr9f_v2_1_1->RegisterComTargetBaseListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_2, this, i, + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d" << std::endl; + std::cout << "Failed to register CAN targetlist for sensor umrr9f_v2_1_1" << std::endl; } if ( - sensor.model == "umrr9d_v1_2_1" && + sensor.model == "umrr9f_can_v2_2_1" && com::types::ERROR_CODE_OK != - data_umrr9d_v1_2_1->RegisterComTargetListPortReceiveCallback( + data_umrr9f_v2_2_1->RegisterComTargetBaseListReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_1, this, i, + &SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1, this, i, std::placeholders::_1, std::placeholders::_2))) { - std::cout << "Failed to register targetlist callback for sensor umrr9d" << std::endl; + std::cout << "Failed to register CAN targetlist for sensor umrr9f_v2_2_1" << std::endl; } } @@ -255,18 +350,19 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option "smart_radar/send_command", std::bind( &SmartmicroRadarNode::radar_command, this, std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO(this->get_logger(), "Radar services are ready."); - std::signal(SIGINT, [](int signal) { return terminate_on_receive(signal); }); + rclcpp::on_shutdown(std::bind(&SmartmicroRadarNode::on_shutdown_callback, this)); } -void terminate_on_receive(int signal) +void SmartmicroRadarNode::on_shutdown_callback() { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "SIGINT has been received"); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Shutdown called!!!"); check_signal = true; rclcpp::Rate sleepRate(std::chrono::milliseconds(100)); sleepRate.sleep(); - rclcpp::shutdown(); + m_services.reset(); } void SmartmicroRadarNode::radar_mode( @@ -416,8 +512,8 @@ void SmartmicroRadarNode::radar_command( return; } - std::shared_ptr radar_command = std::make_shared( - "auto_interface_command", request->command, 2010); + std::shared_ptr radar_command = + std::make_shared("auto_interface_command", request->command, 2010); if (!batch->AddRequest(radar_command)) { result->res = "Failed to add instruction to the batch! "; @@ -476,8 +572,8 @@ void SmartmicroRadarNode::command_response( const std::shared_ptr & response, const std::string command_name) { std::vector>> command_resp; - if (response->GetResponse("auto_interface_command", command_name.c_str(), command_resp)) - { + if (response->GetResponse( + "auto_interface_command", command_name.c_str(), command_resp)) { for (auto & resp : command_resp) { response_type = resp->GetResponseType(); RCLCPP_INFO(this->get_logger(), "Response from sensor to command: %i", response_type); @@ -485,19 +581,17 @@ void SmartmicroRadarNode::command_response( } } -void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_0( +void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_1( const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrra4_automotive_v1_0_0::comtargetlistport::ComTargetListPort> & - targetlist_port_umrra4_v1_0_0, + const std::shared_ptr & + targetlist_port_umrra4_v1_0_1, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrra4_v1_0_0" << std::endl; + std::cout << "Targetlist for umrra4_v1_0_1" << std::endl; if (!check_signal) { - std::shared_ptr< - com::master::umrra4_automotive_v1_0_0::comtargetlistport::GenericPortHeader> + std::shared_ptr port_header; - port_header = targetlist_port_umrra4_v1_0_0->GetGenericPortHeader(); + port_header = targetlist_port_umrra4_v1_0_1->GetPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -505,16 +599,50 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_0( 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_0_0->GetTargetList()) { + for (const auto & target : targetlist_port_umrra4_v1_0_1->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); const auto azimuth_angle = target->GetAzimuthAngle(); - const auto snr = target->GetPower() - target->GetTgtNoise(); + 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->GetTgtNoise(), snr}); + target->GetRcs(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr96( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr96, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr96_v1_2_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr96->GetPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + 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_umrr96->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRCS(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); @@ -523,17 +651,15 @@ void SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_0( void SmartmicroRadarNode::targetlist_callback_umrr11( const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr11_t132_automotive_v1_1_1::comtargetlistport::ComTargetListPort> & + const std::shared_ptr & targetlist_port_umrr11, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr11" << std::endl; + std::cout << "Targetlist for umrr11_v1_1_2" << std::endl; if (!check_signal) { - std::shared_ptr< - com::master::umrr11_t132_automotive_v1_1_1::comtargetlistport::GenericPortHeader> + std::shared_ptr port_header; - port_header = targetlist_port_umrr11->GetGenericPortHeader(); + port_header = targetlist_port_umrr11->GetPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -546,30 +672,28 @@ void SmartmicroRadarNode::targetlist_callback_umrr11( const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); const auto azimuth_angle = target->GetAzimuthAngle(); - const auto snr = target->GetPower() - target->GetTgtNoise(); + 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->GetTgtNoise(), snr}); + target->GetRCS(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); } } -void SmartmicroRadarNode::targetlist_callback_umrr96( +void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_3( const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr96_t153_automotive_v1_2_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr96, + const std::shared_ptr & + targetlist_port_umrr9d_v1_0_3, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr96" << std::endl; + std::cout << "Targetlist for umrr9d_v1_0_3" << std::endl; if (!check_signal) { - std::shared_ptr< - com::master::umrr96_t153_automotive_v1_2_1::comtargetlistport::GenericPortHeader> + std::shared_ptr port_header; - port_header = targetlist_port_umrr96->GetGenericPortHeader(); + port_header = targetlist_port_umrr9d_v1_0_3->GetPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -577,16 +701,50 @@ void SmartmicroRadarNode::targetlist_callback_umrr96( 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_umrr96->GetTargetList()) { + for (const auto & target : targetlist_port_umrr9d_v1_0_3->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); const auto azimuth_angle = target->GetAzimuthAngle(); - const auto snr = target->GetPower() - target->GetTgtNoise(); + 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->GetTgtNoise(), snr}); + target->GetRcs(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_2( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9d_v1_2_2, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_2_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9d_v1_2_2->GetPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + 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_2_2->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); @@ -600,7 +758,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v1_1_1( targetlist_port_umrr9f_v1_1_1, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9f_v1_1_1" << std::endl; + std::cout << "Targetlist for umrr9f_v1_1_1" << std::endl; if (!check_signal) { std::shared_ptr< com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::GenericPortHeader> @@ -665,19 +823,17 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_0_0( } } -void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_0( +void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_1_1( const std::uint32_t sensor_idx, - const std::shared_ptr< - com::master::umrr9f_t169_automotive_v2_2_0::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9f_v2_2_0, + const std::shared_ptr & + targetlist_port_umrr9f_v2_1_1, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9f_v2_2_0" << std::endl; + std::cout << "Targetlist for umrr9f_v2_1_1" << std::endl; if (!check_signal) { - std::shared_ptr< - com::master::umrr9f_t169_automotive_v2_2_0::comtargetlistport::GenericPortHeader> + std::shared_ptr port_header; - port_header = targetlist_port_umrr9f_v2_2_0->GetGenericPortHeader(); + port_header = targetlist_port_umrr9f_v2_1_1->GetPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -685,35 +841,68 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_0( 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_2_0->GetTargetList()) { + for (const auto & target : targetlist_port_umrr9f_v2_1_1->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); const auto azimuth_angle = target->GetAzimuthAngle(); - const auto snr = target->GetPower() - target->GetTgtNoise(); + 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->GetTgtNoise(), snr}); + target->GetRcs(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); } } -void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_2( +void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr & + targetlist_port_umrr9f_v2_2_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f v2_2_1" << std::endl; + if(!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_port_umrr9f_v2_2_1->GetPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + 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_2_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRcs(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrra4_v1_0_1( const std::uint32_t sensor_idx, const std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_0_2::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9d_v1_0_2, + com::master::umrra4_automotive_v1_0_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrra4_v1_0_1, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9d_v1_0_2" << std::endl; + std::cout << "CAN Targetlist for umrra4_v1_0_1" << std::endl; if (!check_signal) { - std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_0_2::comtargetlistport::GenericPortHeader> + std::shared_ptr port_header; - port_header = targetlist_port_umrr9d_v1_0_2->GetGenericPortHeader(); + port_header = targetlist_can_umrra4_v1_0_1->GetPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -721,35 +910,34 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_2( 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_0_2->GetTargetList()) { + for (const auto & target : targetlist_can_umrra4_v1_0_1->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); const auto azimuth_angle = target->GetAzimuthAngle(); - const auto snr = target->GetPower() - target->GetTgtNoise(); + 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->GetPower(), - target->GetRCS(), target->GetTgtNoise(), snr}); + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); } } -void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_1( +void SmartmicroRadarNode::CAN_targetlist_callback_umrr96( const std::uint32_t sensor_idx, const std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_2_1::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9d_v1_2_1, + com::master::umrr96_t153_automotive_v1_2_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr96, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9d_v1_2_1" << std::endl; + std::cout << "CAN Targetlist for umrr96_v1_2_2" << std::endl; if (!check_signal) { - std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_2_1::comtargetlistport::GenericPortHeader> + std::shared_ptr port_header; - port_header = targetlist_port_umrr9d_v1_2_1->GetGenericPortHeader(); + port_header = targetlist_can_umrr96->GetPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -757,73 +945,271 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_1( 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_2_1->GetTargetList()) { + for (const auto & target : targetlist_can_umrr96->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); const auto azimuth_angle = target->GetAzimuthAngle(); - const auto snr = target->GetPower() - target->GetTgtNoise(); + 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->GetPower(), - target->GetRCS(), target->GetTgtNoise(), snr}); + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); } } -void SmartmicroRadarNode::update_config_files_from_params() +void SmartmicroRadarNode::CAN_targetlist_callback_umrr11( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr11_t132_automotive_v1_1_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr11, + const com::types::ClientId client_id) { - const auto dev_id = declare_parameter(kDevIdTag, 0); - if (!dev_id) { - throw std::runtime_error("Parameter dev_id not set."); + std::cout << "CAN Targetlist for umrr11_v1_1_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr11->GetPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + 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_umrr11->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); } - const auto dev_port = declare_parameter(kDevPortTag, 0); - if (!dev_port) { - throw std::runtime_error("Parameter dev_port not set."); +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_0_3( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_0_3::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_0_3, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_0_3" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9d_v1_0_3->GetPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + 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_0_3->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); } - const auto dev_iface_name = declare_parameter(kDevIfaceNameTag, std::string{}); - if (dev_iface_name.empty()) { - throw std::runtime_error("Parameter dev_iface_name not set."); +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9d_v1_2_2( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_2_2::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9d_v1_2_2, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9d_v1_2_2" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9d_v1_2_2->GetPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + 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_2_2->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); } - const auto master_client_id = declare_parameter(kMasterClientIdTag, 0); - if (!master_client_id) { - throw std::runtime_error("Parameter master_client_id not set."); +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_1_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_1_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v2_1_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_v2_1_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9f_v2_1_1->GetPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + 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_1_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); } +} + +void SmartmicroRadarNode::CAN_targetlist_callback_umrr9f_v2_2_1( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_2_1::comtargetbaselist::ComTargetBaseList> & + targetlist_can_umrr9f_v2_2_1, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist for umrr9f_v2_2_1" << std::endl; + if (!check_signal) { + std::shared_ptr + port_header; + port_header = targetlist_can_umrr9f_v2_2_1->GetPortHeader(); + sensor_msgs::msg::PointCloud2 msg; + 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_2_1->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetSignalLevel() - target->GetNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetSignalLevel(), + target->GetRCS(), target->GetNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::update_config_files_from_params() +{ + const auto master_inst_serial_type = declare_parameter(kInstSerialTypeTag, std::string{}); + const auto master_data_serial_type = declare_parameter(kDataSerialTypeTag, std::string{}); + + auto read_adapter_params_if_possible = [&](const std::uint32_t index) { + RCLCPP_INFO(this->get_logger(), "Updating ADAPTER params."); + auto & current_adapter = m_adapters[index]; + const auto prefix_2 = "adapters.adapter_" + std::to_string(index); + current_adapter.hw_dev_id = this->declare_parameter(prefix_2 + ".hw_dev_id", kDefaultHwDevId); + if (current_adapter.hw_dev_id == kDefaultHwDevId) { + // The id was not set, so the adapter with this index was not defined. + // Stop here. + return false; + } + current_adapter.hw_iface_name = + this->declare_parameter(prefix_2 + ".hw_iface_name", kDefaultHwDevIface); + current_adapter.hw_type = this->declare_parameter(prefix_2 + ".hw_type", kDefaultHwLinkType); + current_adapter.baudrate = this->declare_parameter(prefix_2 + ".baudrate", 500000); + current_adapter.port = this->declare_parameter(prefix_2 + ".port", kDefaultPort); + + return true; + }; auto read_sensor_params_if_possible = [&](const std::uint32_t index) { - auto & current_sensor = m_sensors[index]; - const auto prefix = "sensors.sensor_" + std::to_string(index); - current_sensor.model = this->declare_parameter(prefix + ".model", kDefaultSensorType); - current_sensor.id = this->declare_parameter(prefix + ".id", kDefaultClientId); - if (current_sensor.id == kDefaultClientId) { + RCLCPP_INFO(this->get_logger(), "Updating SENSOR params."); + auto & sensor = m_sensors[index]; + const auto prefix_3 = "sensors.sensor_" + std::to_string(index); + sensor.dev_id = this->declare_parameter(prefix_3 + ".dev_id", kDefaultHwDevId); + sensor.uifname = this->declare_parameter(prefix_3 + ".uifname", ""); + sensor.uifmajorv = this->declare_parameter(prefix_3 + ".uifmajorv", 0); + sensor.uifminorv = this->declare_parameter(prefix_3 + ".uifminorv", 0); + sensor.uifpatchv = this->declare_parameter(prefix_3 + ".uifpatchv", 0); + sensor.model = this->declare_parameter(prefix_3 + ".model", kDefaultSensorType); + sensor.id = this->declare_parameter(prefix_3 + ".id", kDefaultClientId); + if (sensor.id == kDefaultClientId) { // The id was not set, so the sensor with this index was not defined. Stop // here. return false; } - current_sensor.ip = this->declare_parameter(prefix + ".ip", kDefaultIp); - current_sensor.port = this->declare_parameter(prefix + ".port", kDefaultPort); - current_sensor.frame_id = this->declare_parameter(prefix + ".frame_id", kDefaultFrameId); - current_sensor.history_size = - this->declare_parameter(prefix + ".history_size", kDefaultHistorySize); - + sensor.ip = this->declare_parameter(prefix_3 + ".ip", ""); + sensor.port = this->declare_parameter(prefix_3 + ".port", 0); + sensor.frame_id = this->declare_parameter(prefix_3 + ".frame_id", kDefaultFrameId); + sensor.history_size = this->declare_parameter(prefix_3 + ".history_size", kDefaultHistorySize); + sensor.inst_type = this->declare_parameter(prefix_3 + ".inst_type", ""); + sensor.data_type = this->declare_parameter(prefix_3 + ".data_type", ""); + sensor.link_type = this->declare_parameter(prefix_3 + ".link_type", kDefaultHwLinkType); return true; }; + for (auto j = 0UL; j < m_adapters.size(); ++j) { + if (!read_adapter_params_if_possible(j)) { + m_number_of_adapters = j; + break; + } + } + + if (!m_number_of_adapters) { + throw std::runtime_error("At least one adapter must be configured."); + } + for (auto i = 0UL; i < m_sensors.size(); ++i) { if (!read_sensor_params_if_possible(i)) { m_number_of_sensors = i; break; } } - if (!m_number_of_sensors) { throw std::runtime_error("At least one sensor must be configured."); } auto config = nlohmann::json::parse(std::ifstream{kConfigFilePath}); - config[kClientIdTag] = master_client_id; + config[kDataSerialTypeJsonTag] = master_data_serial_type; + config[kInstSerialTypeJsonTag] = master_inst_serial_type; std::ofstream{kConfigFilePath, std::ios::trunc} << config; auto hw_inventory = nlohmann::json::parse(std::ifstream{kHwInventoryFilePath}); @@ -831,10 +1217,17 @@ void SmartmicroRadarNode::update_config_files_from_params() if (hw_items.empty()) { throw std::runtime_error("There are no 'hwItems' defined in the hw_inventory.json file."); } - auto & hw_item = hw_items.front(); - hw_item[kDevIdJsonTag] = dev_id; - hw_item[kPortTag] = dev_port; - hw_item[kIfaceNameTag] = dev_iface_name; + auto hw_item = hw_items.front(); + hw_items.clear(); + for (auto j = 0UL; j < m_number_of_adapters; ++j) { + const auto & adapter = m_adapters[j]; + hw_item[kPortTag] = adapter.port; + hw_item[kHwDevLinkTag] = adapter.hw_type; + hw_item[kHwDevIdTag] = adapter.hw_dev_id; + hw_item[kHwDevIfaceNameTag] = adapter.hw_iface_name; + hw_item[kBaudRateTag] = adapter.baudrate; + hw_items.push_back(hw_item); + } std::ofstream{kHwInventoryFilePath, std::ios::trunc} << hw_inventory; auto routing_table = nlohmann::json::parse(std::ifstream{kRoutingTableFilePath}); @@ -846,12 +1239,21 @@ void SmartmicroRadarNode::update_config_files_from_params() clients.clear(); for (auto i = 0UL; i < m_number_of_sensors; ++i) { const auto & sensor = m_sensors[i]; - client[kClientIdTag] = sensor.id; + client[kClientLinkTag] = sensor.link_type; + client[kClientIdTag] = sensor.id; // + client[kHwDevIdTag] = sensor.dev_id; client[kPortTag] = sensor.port; client[kIpTag] = sensor.ip; + client[kInstSerialTypeJsonTag] = sensor.inst_type; + client[kDataSerialTypeJsonTag] = sensor.data_type; + client[kUINameTag] = sensor.uifname; + client[kUIMajorVTag] = sensor.uifmajorv; + client[kUIMinorVTag] = sensor.uifminorv; + client[kUIPatchVTag] = sensor.uifpatchv; clients.push_back(client); } - std::ofstream{kRoutingTableFilePath, std::ios::trunc} << routing_table; + + std::ofstream{kRoutingTableFilePath, std::ios::trunc} << std::setw(4) << routing_table; } } // namespace radar diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index 6fe683f..16c4b14 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -33,15 +33,17 @@ @pytest.mark.launch_test def generate_test_description(): + radar_sensor_params = os.path.join( + get_package_share_directory(PACKAGE_NAME), 'param/radar.sensor.template.yaml') + + radar_adapter_params = os.path.join( + get_package_share_directory(PACKAGE_NAME), 'param/radar.adapter.template.yaml') + radar_node = Node( package=PACKAGE_NAME, executable='smartmicro_radar_node_exe', name='smart_radar', - output='screen', - parameters=[ - os.path.join(get_package_share_directory(PACKAGE_NAME), - 'param/radar.template.yaml'), - ], + parameters=[radar_sensor_params, radar_adapter_params] ) frequency_sweep_service = ExecuteProcess( @@ -123,7 +125,7 @@ def tearDown(self): self.test_node.destroy_node() def test_smart_node_publishes(self): - # Expect the smartnode to publish strings on '/umrr/targets_' + # Expect the smartnode to publish strings on '/smart_radar/targets_' data_rx_s1 = [] data_rx_s2 = [] data_rx_s3 = []