diff --git a/CHANGELOG.md b/CHANGELOG.md index 36c78a0..036247c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -34,4 +34,8 @@ This release fixes the offset which causes anomaly in pointclouds from DRVEGRD 1 ## v4.0.0 - 2023-02-06 -This release includes the new DRVEGRD 169 user-interface. The release includes new modes for DRVEGRD 169 and also introduces an additional ros2 service to save configurations. Previously used model 'UMRR9F' in radar parameters has been divided into two versions, the 'UMRR9F_V1_1_1' and 'UMRR9F_V2_0_0'. \ No newline at end of file +This release includes the new DRVEGRD 169 user-interface. The release includes new modes for DRVEGRD 169 and also introduces an additional ros2 service to save configurations. Previously used model 'UMRR9F' in radar parameters has been divided into two versions, the 'UMRR9F_V1_1_1' and 'UMRR9F_V2_0_0'. + +## 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 diff --git a/Readme.md b/Readme.md index 8976d81..322f3c9 100644 --- a/Readme.md +++ b/Readme.md @@ -22,17 +22,20 @@ ros2 launch umrr_ros2_driver radar.launch.py - ROS2 foxy ### UMRR radars and Smart Access API version -A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVEGRD 152 or DRVEGRD 169 radar are +A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVEGRD 171, DRVEGRD 152 or DRVEGRD 169 radar are required to run this node. This code is bundled with a version of Smart Access API. Please make sure the version used to publish the data is compatible with this version: -- Date of release: `February 06, 2023` -- Smart Access Automotive version: `v3.0.0` +- 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` - 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` ### Sensor Firmwares This ROS2 driver release is compatible with the following sensor firmwares: @@ -41,6 +44,8 @@ This ROS2 driver release is compatible with the following sensor firmwares: - UMRR9D Type 152: V2.1.0 - UMRR9F Type 169: V1.3.0 - UMRR9F Type 169: V2.0.2 +- UMRR9F Type 169: V2.2.0 +- UMRRA4 Type 171: V1.0.0 ### Point cloud message wrapper library To add targets to the point cloud in a safe and quick fashion a @@ -74,11 +79,11 @@ For more details, see the [`radar.template.yaml`](umrr_ros2_driver/param/radar.t - `iface_name`: name of the used network interface - `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('umrr11', 'umrr9d', 'umrr96', 'umrr9f_v1_1_1', 'umrr9f_v2_0_0') of the sensor being used +- `model`: the model('umrra4_v1_0_0', 'umrr11', 'umrr9d', 'umrr96', 'umrr9f_v1_1_1', 'umrr9f_v2_0_0') of the sensor being used ## Mode of operations of the sensors The smartmicro radars come equipped with numerous features and modes of operation. Using the ros2 services provided one -may access these modes. A list of available sensor modes is given in the [`sensor_params.json`](umrr_ros2_driver/config/sensor_params.json). +may access these modes and send commands to the sensor. A list of available sensor operations is given in the [`user_interfaces`](umrr_ros2_driver/smartmicro/user_interfaces/). A ros2 `SetMode` service should be called to implement these mode changes. There are three inputs to a ros2 service call: - `param`: name of the mode instruction (specific to the sensor) @@ -88,6 +93,13 @@ A ros2 `SetMode` service should be called to implement these mode changes. There For instance, changing the `Index of Transmit Antenna (tx_antenna_idx)` of a UMRR-11 sensor to `AEB (2)` mode would require the following call: `ros2 service call /smart_radar/set_radar_mode umrr_ros2_msgs/srv/SetMode "{param: "tx_antenna_idx", value: 2, sensor_id: 100}"` +Similarly, a ros2 `SendCommand` service could be used to send commands to the sensors. There are two inputs for sending a command: +- `command`: name of the command (specific to the sensor interface) +- `sensor_id`: the id of the sensor to which the service call should be sent. + +The call for such a service would be as follows: +`ros2 service call /smart_radar/send_command umrr_ros2_msgs/srv/SendCommand "{command: "comp_eeprom_ctrl_default_param_sec", sensor_id: 100}"` + ## Configuration of the sensors In order to use multiple sensors (maximum of up to eight sensors) with the node the sensors should be configured separately. The IP addresses of the sensors could be assigned using: @@ -108,15 +120,7 @@ The call for such a service would be as follows: Note: For successfull execution of this call it is important that the sensor is restarted, the ip address in the [`radar.template.yaml`](umrr_ros2_driver/param/radar.template.yaml) is updated and the driver is build again. -## Saving mode changes -In order to save the mode changes, an additional service if provided. This service offers different save options and also the possibility to -set the default values for the sensors. The list of all the options could be found in the [`sensor_commands.json`](umrr_ros2_driver/config/sensor_commands.json). - -The call for such a service would be as follows: -`ros2 service call /smart_radar/send_command umrr_ros2_msgs/srv/SendCommand "{command: "comp_eeprom_ctrl_default_param_sec", sensor_id: 100}"` - ## Sensor Service Responses - The sensor services respond with certain value codes. The following is a lookup table for the possible responses: **Value** | **Description** diff --git a/smart_extract.sh b/smart_extract.sh index 6034e6c..3c3f8a7 100755 --- a/smart_extract.sh +++ b/smart_extract.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -smart_pack=SmartAccessAutomotive_3_0_0.tgz +smart_pack=SmartAccessAutomotive_3_2_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 6820363..d6680e8 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -45,19 +45,24 @@ endif() ament_auto_find_build_dependencies() +set(SMARTMICRO_LIB_DIR "lib-linux-x86_64-gcc_9") + # specify the path to search for the libraries -link_directories(${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9) +link_directories(${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}) install(FILES - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libsmart_access.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libosal.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libcom_lib.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libbasev1.0.2_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr11_t132_automotivev1.1.1_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr96_t153_automotivev1.2.1_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr9f_t169_automotivev1.1.1_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr9f_t169_automotivev2.0.0_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr9d_t152_automotivev1.0.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libsmart_access.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libosal.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libcom_lib.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libbasev1.0.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr11_t132_automotivev1.1.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}/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" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -65,19 +70,11 @@ set(CONFIG_FOLDER_PATH "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/config") set(CONFIG_FILE_PATH "${CONFIG_FOLDER_PATH}/smart_access_config.json") set(HW_INVENTORY_FILE_PATH "${CONFIG_FOLDER_PATH}/hw_inventory.json") set(ROUTING_TABLE_FILE_PATH "${CONFIG_FOLDER_PATH}/routing_table.json") -set(SENSOR_PARAM_FILE_PATH "${PROJECT_SOURCE_DIR}/config/sensor_params.json") -set(SENSOR_COMMAND_FILE_PATH "${PROJECT_SOURCE_DIR}/config/sensor_commands.json") configure_file( "${PROJECT_SOURCE_DIR}/cmake/smart_access_config.json.in" "${CONFIG_FILE_PATH}" @ONLY) configure_file( "${PROJECT_SOURCE_DIR}/cmake/config_path.hpp.in" "${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}/config_path.hpp" @ONLY) -configure_file( - "${PROJECT_SOURCE_DIR}/cmake/sensor_params.hpp.in" - "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/sensor_params.hpp" @ONLY) -configure_file( - "${PROJECT_SOURCE_DIR}/cmake/sensor_commands.hpp.in" - "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/sensor_commands.hpp" @ONLY) ament_auto_add_library(smartmicro_radar_node SHARED "src/smartmicro_radar_node.cpp") @@ -87,7 +84,10 @@ smartmicro/include/umrr11_t132_automotive_v1_1_1 smartmicro/include/umrr96_t153_automotive_v1_2_1 smartmicro/include/umrr9f_t169_automotive_v1_1_1 smartmicro/include/umrr9f_t169_automotive_v2_0_0 -smartmicro/include/umrr9d_t152_automotive_v1_0_2) +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) # link smart_access_lib-linux-x86_64-gcc_9 to the node target_link_libraries(smartmicro_radar_node @@ -99,7 +99,10 @@ target_link_libraries(smartmicro_radar_node umrr96_t153_automotivev1.2.1_user_interface umrr9f_t169_automotivev1.1.1_user_interface umrr9f_t169_automotivev2.0.0_user_interface - umrr9d_t152_automotivev1.0.2_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) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" @@ -109,11 +112,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") - - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_smartmicro_radar_node test/test_smartmicro_radar_node.cpp) - target_link_libraries(test_smartmicro_radar_node smartmicro_radar_node) - + # Not applying ros2 linters on external sources list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright diff --git a/umrr_ros2_driver/config/sensor_commands.json b/umrr_ros2_driver/config/sensor_commands.json deleted file mode 100644 index f8a9919..0000000 --- a/umrr_ros2_driver/config/sensor_commands.json +++ /dev/null @@ -1,17 +0,0 @@ -{ - "commands": - [ - { - "name": "comp_eeprom_ctrl_save_param_sec", - "comment": "Save the parameter inside the EEPROM. (3102.3)" - }, - { - "name": "comp_eeprom_ctrl_reset_param_sec", - "comment": "Restore default values in RAM. EEPROM content is not changed. (3102.2)" - }, - { - "name": "comp_eeprom_ctrl_default_param_sec", - "comment": "Restore default values in RAM and EEPROM. (3102.1)" - } - ] -} diff --git a/umrr_ros2_driver/config/sensor_params.json b/umrr_ros2_driver/config/sensor_params.json deleted file mode 100644 index c930190..0000000 --- a/umrr_ros2_driver/config/sensor_params.json +++ /dev/null @@ -1,343 +0,0 @@ -{ - "parameters": [ - { - "name": "tx_antenna_idx", - "comment": "Index of Transmit Antenna", - "sensors": [ - { - "model": "umrr11", - "comment": "0 and 1 are ACC, 2 is AEB", - "min": 0, - "max": 2 - }, - { - "model": "umrr96", - "min": 0, - "max": 2, - "default": 0 - }, - { - "model": "umrr9f", - "min": 0, - "max": 2, - "default": 0 - } - ] - }, - { - "name": "center_frequency_idx", - "comment": "Index of center frequency", - "sensors": [ - { - "model": "umrr11", - "comment": " 0..3 in ACC selectable. In AEB mode values 2 and 3 are similar to 1.", - "min": 0, - "max": 3 - }, - { - "model": "umrr96", - "min": 0, - "max": 2, - "default": 0 - }, - { - "model": "umrr9f", - "min": 1, - "max": 2, - "default": 1 - }, - { - "model": "umrr9d", - "min": 0, - "max": 3, - "default": 1 - } - ] - }, - { - "name": "frequency_sweep_idx", - "comment": "Index of sweep", - "sensors": [ - { - "model": "umrr11", - "comment": " currently always 0 because only one sweep for every TX-antenna", - "min": 0, - "max": 0 - }, - { - "model": "umrr96", - "comment": "0=226MHz, 1=512MHz, 2=1536MHz", - "min": 0, - "max": 2, - "default": 2 - }, - { - "model": "umrr9f", - "comment": "0=226MHz, 1=512MHz, 2=1536MHz, 3=3072MHz", - "min": 0, - "max": 3, - "default": 1 - }, - { - "model": "umrr9d", - "comment": "0=medium-range, 1=long-range", - "min": 0, - "max": 1, - "default": 0 - } - ] - }, - { - "name": "enable_tx_ant_toggle", - "comment": "Automatic toggle of TX-Antenna 0->2->0 every radar cycle", - "sensors": [ - { - "model": "umrr11", - "min": 0, - "max": 1, - "default": 0 - } - ] - }, - { - "name": "angular_separation", - "comment": "0=Disable Angular Separation, 1=Enable Angular Separation", - "sensors": [ - { - "model": "umrr11", - "min": 0, - "max": 1, - "default": 0 - } - ] - }, - { - "name": "range_toggle_mode", - "comment": "Automatic toggle of range:", - "sensors": [ - { - "model": "umrr96", - "comment": "0=off, 1=Short-Med, 2=Short-Long, 3=Med-Long", - "min": 0, - "max": 3, - "default": 0 - }, - { - "model": "umrr9f", - "comment": " 0=off, 1=Short-Med, 2=Short-Long, 3=Med-Long, 4=Long-UltraShort, 5=Medium-UltraShort, 6=Short-UltraShort", - "min": 0, - "max": 6, - "default": 0 - }, - { - "model": "umrr9d", - "comment": " 0=off, 1=Med-Long", - "min": 0, - "max": 1, - "default": 0 - } - ] - }, - { - "name": "detection_sensitivity", - "comment": "Detection sensitivity: 0=low, 1=normal, 2=high", - "sensors": [ - { - "model": "umrr9d", - "min": 0, - "max": 2, - "default": 1 - }, - { - "model": "umrr9f_v2_0_0", - "min": 0, - "max": 2, - "default": 1 - } - ] - }, - { - "name": "tv_min_speed_sweep_idx_0", - "comment": "Target Validation: minimum speed of target at first sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": -112.0 - } - ] - }, - { - "name": "tv_min_speed_sweep_idx_1", - "comment": "Target Validation: minimum speed of target at second sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": -112.0 - } - ] - }, - { - "name": "tv_min_speed_sweep_idx_2", - "comment": "Target Validation: minimum speed of target at third sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": -20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": -112.0 - } - ] - }, - { - "name": "tv_min_speed_sweep_idx_3", - "comment": "Target Validation: minimum speed of target at fourth sweep", - "sensors": [ - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": -112.0 - } - ] - }, - { - "name": "tv_max_speed_sweep_idx_0", - "comment": "Target Validation: maximum speed of target at first sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": 56.0 - } - ] - }, - { - "name": "tv_max_speed_sweep_idx_1", - "comment": "Target Validation: maximum speed of target at second sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": 56.0 - } - ] - }, - { - "name": "tv_max_speed_sweep_idx_2", - "comment": "Target Validation: maximum speed of target at third sweep", - "sensors": [ - { - "model": "umrr96", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0, - "default": 20 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": 56.0 - } - ] - }, - { - "name": "tv_max_speed_sweep_idx_3", - "comment": "Target Validation: maximum speed of target at fourth sweep", - "sensors": [ - { - "model": "umrr9f", - "min": -150.0, - "max": 150.0 - }, - { - "model": "umrr9d", - "min": -112.0, - "max": 56.0, - "default": 56.0 - } - ] - }, - { - "name": "ip_source_address", - "comment": "IP source address (32bit)", - "default": 3232238347 - } - ] -} 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 dd5a7b2..24d63f8 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,11 +25,14 @@ #include #include +#include #include #include #include #include +#include #include +#include #include #include @@ -112,7 +115,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// /// @brief A callback that is called when a new target list port for - /// umrr9f arrives. + /// umrr9f_v1_1_1 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port The target list port @@ -127,7 +130,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node const com::types::ClientId client_id); /// /// @brief A callback that is called when a new target list port for - /// umrr9f arrives. + /// 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 @@ -143,18 +146,66 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// /// @brief A callback that is called when a new target list port for - /// umrr9d arrives. + /// umrr9f_v2_2_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_umrr9d( + 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_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, + targetlist_port_umrr9d_v1_0_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. + /// + /// @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_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 target list port 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 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); /// @@ -228,11 +279,14 @@ 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_umrr9f_v1_1_1{}; std::shared_ptr data_umrr9f_v2_0_0{}; -std::shared_ptr data_umrr9d{}; +std::shared_ptr data_umrr9f_v2_2_0{}; +std::shared_ptr data_umrr9d_v1_0_2{}; +std::shared_ptr data_umrr9d_v1_2_1{}; } // namespace radar } // namespace drivers diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml index c8f3107..bba5ec1 100644 --- a/umrr_ros2_driver/param/radar.template.yaml +++ b/umrr_ros2_driver/param/radar.template.yaml @@ -55,7 +55,7 @@ history_size: 10 sensor_3: # The model of the connected sensor. - model: "umrr9d" + 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. diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index ddc6fc6..42aed5f 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -19,12 +19,16 @@ #include #include +#include +#include #include #include #include #include #include #include +#include +#include #include #include #include @@ -47,8 +51,6 @@ #include #include "umrr_ros2_driver/config_path.hpp" -#include "umrr_ros2_driver/sensor_params.hpp" -#include "umrr_ros2_driver/sensor_commands.hpp" using com::common::Instruction; using com::master::CmdRequest; @@ -143,11 +145,14 @@ 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_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_umrr9d = com::master::umrr9d_t152_automotive_v1_0_2::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(); RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); // Wait init time @@ -155,6 +160,19 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option for (auto i = 0UL; i < m_number_of_sensors; ++i) { const auto & sensor = m_sensors[i]; + + m_publishers[i] = create_publisher( + "smart_radar/targets_" + std::to_string(i), sensor.history_size); + + if ( + sensor.model == "umrra4_v1_0_0" && + com::types::ERROR_CODE_OK != + data_umrra4_v1_0_0->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra4_v1_0_0, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrra4_v1_0_0" << std::endl; + } if ( sensor.model == "umrr11" && com::types::ERROR_CODE_OK != @@ -192,17 +210,32 @@ 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 == "umrr9d" && + sensor.model == "umrr9f_v2_2_0" && + com::types::ERROR_CODE_OK != + data_umrr9f_v2_2_0->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f_v2_2_0, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_2_0" << std::endl; + } + if ( + sensor.model == "umrr9d_v1_0_2" && com::types::ERROR_CODE_OK != - data_umrr9d->RegisterComTargetListPortReceiveCallback( + data_umrr9d_v1_0_2->RegisterComTargetListPortReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9d, this, i, + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_2, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr9d" << std::endl; + } + if ( + sensor.model == "umrr9d_v1_2_1" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_2_1->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_1, this, i, std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr9d" << std::endl; } - - m_publishers[i] = create_publisher( - "smart_radar/targets_" + std::to_string(i), sensor.history_size); } // create a ros2 service to change the radar parameters @@ -241,29 +274,11 @@ void SmartmicroRadarNode::radar_mode( std::shared_ptr result) { std::string instruction_name{}; - bool check_flag_param = false; bool check_flag_id = false; - std::ifstream instr_file(KSensorParamFilePath); - auto param_table = nlohmann::json::parse(instr_file); - instruction_name = request->param; client_id = request->sensor_id; - for (const auto & item : param_table.items()) { - for (const auto & param : item.value().items()) { - if (instruction_name == param.value()["name"]) { - check_flag_param = true; - break; - } - } - } - - if (!check_flag_param) { - result->res = "Invalid instruction name or value! "; - return; - } - for (auto & sensor : m_sensors) { if (client_id == sensor.id) { check_flag_id = true; @@ -374,33 +389,11 @@ void SmartmicroRadarNode::radar_command( std::shared_ptr result) { std::string command_name{}; - bool check_flag_command = false; bool check_flag_id = false; - std::ifstream command_file(KSensorCommandFilePath); - if (!command_file) - { - std::cout << "File is not present" << std::endl; - } - auto command_table = nlohmann::json::parse(command_file); - command_name = request->command; client_id = request->sensor_id; - for (const auto & item : command_table.items()) { - for (const auto & command : item.value().items()) { - if (command_name == command.value()["name"]) { - check_flag_command = true; - break; - } - } - } - - if (!check_flag_command) { - result->res = "Invalid command name! "; - return; - } - for (auto & sensor : m_sensors) { if (client_id == sensor.id) { check_flag_id = true; @@ -492,6 +485,42 @@ void SmartmicroRadarNode::command_response( } } +void SmartmicroRadarNode::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) +{ + std::cout << "Targetlist callback is being called for umrra4_v1_0_0" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrra4_automotive_v1_0_0::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrra4_v1_0_0->GetGenericPortHeader(); + 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_umrra4_v1_0_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetTgtNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRCS(), target->GetTgtNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + void SmartmicroRadarNode::targetlist_callback_umrr11( const std::uint32_t sensor_idx, const std::shared_ptr< @@ -636,19 +665,91 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_0_0( } } -void SmartmicroRadarNode::targetlist_callback_umrr9d( +void SmartmicroRadarNode::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) +{ + std::cout << "Targetlist callback is being called for umrr9f_v2_2_0" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr9f_t169_automotive_v2_2_0::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr9f_v2_2_0->GetGenericPortHeader(); + 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_0->GetTargetList()) { + const auto range = target->GetRange(); + const auto elevation_angle = target->GetElevationAngle(); + const auto range_2d = range * std::cos(elevation_angle); + const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetTgtNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRCS(), target->GetTgtNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::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, + targetlist_port_umrr9d_v1_0_2, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrr9d" << std::endl; + std::cout << "Targetlist callback is being called for umrr9d_v1_0_2" << std::endl; if (!check_signal) { std::shared_ptr< com::master::umrr9d_t152_automotive_v1_0_2::comtargetlistport::GenericPortHeader> port_header; - port_header = targetlist_port_umrr9d->GetGenericPortHeader(); + port_header = targetlist_port_umrr9d_v1_0_2->GetGenericPortHeader(); + 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_0_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->GetTgtNoise(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRCS(), target->GetTgtNoise(), snr}); + } + + m_publishers[sensor_idx]->publish(msg); + } +} + +void SmartmicroRadarNode::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) +{ + std::cout << "Targetlist callback is being called for umrr9d_v1_2_1" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_2_1::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr9d_v1_2_1->GetGenericPortHeader(); sensor_msgs::msg::PointCloud2 msg; RadarCloudModifier modifier{msg, m_sensors[sensor_idx].frame_id}; const auto timestamp = std::chrono::microseconds{port_header->GetTimestamp()}; @@ -656,7 +757,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d( 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->GetTargetList()) { + for (const auto & target : targetlist_port_umrr9d_v1_2_1->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index 230a3b2..6fe683f 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -108,7 +108,6 @@ class TestSmartNode(unittest.TestCase): @classmethod def setUpClass(cls): # Initialize the ROS context for the test node - print("Initialize") rclpy.init() @classmethod @@ -118,11 +117,9 @@ def tearDownClass(cls): def setUp(self): # Create a ROS node for tests - print("setUp") self.test_node = rclpy.create_node('test_node') def tearDown(self): - print("Destroy node") self.test_node.destroy_node() def test_smart_node_publishes(self): diff --git a/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp b/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp deleted file mode 100644 index 7f61f77..0000000 --- a/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2021 Apex.AI, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -using smartmicro::drivers::radar::SmartmicroRadarNode; - -/// @test Test that if we publish one message, it generates a state estimate which is sent out. -TEST(SmartmicroRadarNodeTest, Create) -{ - rclcpp::init(0, nullptr); - ASSERT_TRUE(rclcpp::ok()); - rclcpp::NodeOptions node_options{}; - EXPECT_THROW(std::make_shared(node_options), std::runtime_error); - node_options.append_parameter_override("master_client_id", 1); - node_options.append_parameter_override("hw_dev_id", 2); - node_options.append_parameter_override("hw_port", 55555); - node_options.append_parameter_override("hw_iface_name", "eth0"); - node_options.append_parameter_override("sensors.sensor_0.id", 10); - node_options.append_parameter_override("sensors.sensor_0.ip", "172.22.10.102"); - node_options.append_parameter_override("sensors.sensor_0.model", "umrr96"); - auto node = std::make_shared(node_options); - ASSERT_TRUE(node != nullptr); - rclcpp::Rate sleepRate(std::chrono::seconds(5)); - sleepRate.sleep(); - rclcpp::shutdown(); -}