diff --git a/.github/workflows/dockerbuild.yml b/.github/workflows/dockerbuild.yml index 5412e15..45bd366 100644 --- a/.github/workflows/dockerbuild.yml +++ b/.github/workflows/dockerbuild.yml @@ -21,7 +21,7 @@ jobs: run: docker build . -t umrr-ros:latest - name: Building the driver with the docker container - run: docker run --rm -v`pwd`:/code umrr-ros colcon build --packages-select umrr_ros2_driver umrr_ros2_msgs + run: docker run --rm -v`pwd`:/code umrr-ros colcon build - name: Running the unit/integration tests via the docker container and exit run: docker-compose up diff --git a/CHANGELOG.md b/CHANGELOG.md index b44135f..bf8f542 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,8 +9,13 @@ This is the first official versioned release of the `smartmicro_ros2_radars`. It This major release of the driver includes multi-user interfaces. The driver now supports and publishes data from UMRR96 and UMRR11. A new test approach has been implemented which simulates the sensors and interfaces with the node hence making the tests more robust. +Requires new sensor firmware, if using UMRR11-T132: V5.1.4 and if using UMRR96-T153: V5.2.4. ## 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. \ No newline at end of file +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. + +## 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. diff --git a/Readme.md b/Readme.md index 2a0d8e5..218cd02 100644 --- a/Readme.md +++ b/Readme.md @@ -21,15 +21,21 @@ ros2 launch umrr_ros2_driver radar.launch.py ### Supported ROS distributions: - ROS2 foxy -### UMRR-96 radar and Smart Access API version -A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96 radar, UMRR11 radar or both are +### UMRR radars and Smart Access API version +A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96 radar, UMRR11 radar or UMRR9F 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: `June 02, 2022` -- Smart Access Automotive version: `v1.1.0` +- Date of release: `September 22, 2022` +- Smart Access Automotive version: `v2.0.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` + +### 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 ### Point cloud message wrapper library To add targets to the point cloud in a safe and quick fashion a @@ -45,6 +51,7 @@ sudo apt install ros-foxy-point-cloud-msg-wrapper The inputs are coming as network packages generated in either of the following two ways: - Through directly interfacing with the sensor - Through a provided pcap file +- Through using the sensor simulators These inputs are processed through the Smart Access C++ API and trigger a callback. Every time this callback is triggered a new point cloud message is created and published. @@ -77,7 +84,7 @@ For instance, changing the `Index of Transmit Antenna (tx_antenna_idx)` of a UMR `ros2 service call /smartmicro_radar_node/set_radar_mode umrr_ros2_msgs/srv/SetMode "{param: "tx_antenna_idx", value: 2, sensor_id: 100}"` ## Configuration of the sensors -In order to use multiple sensors (maximum of up to ten sensors) with the node the sensors should be configured separately. +In order to use multiple sensors (maximum of up to eight sensors) with the node the sensors should be configured separately. The IP addresses of the sensors could be assigned using: - The smartmicro tool `DriveRecorder`. - Using the `Smart Access C++ API` @@ -112,6 +119,12 @@ The sensor services respond with certain value codes. The following is a lookup ## Development The dockerfile can be used to build and test the ros driver. +### Prerequisites + +- Docker version >= 20.10.14 +- Docker compose version >= 1.29.2 + +## Building and Testing Accept the agreement and get the smartaccess release ```bash ./smart_extract.sh @@ -124,7 +137,7 @@ docker build . -t umrr-ros:latest Building the driver with the docker container ```bash -docker run --rm -v`pwd`:/code umrr-ros colcon build --packages-select umrr_ros2_driver umrr_ros2_msgs +docker run --rm -v`pwd`:/code umrr-ros colcon build ``` Running the unit and integration tests via the docker compose diff --git a/docker-compose.yml b/docker-compose.yml index 7679d95..64e2208 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -30,7 +30,7 @@ services: - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr11/com_lib_config.json - LD_LIBRARY_PATH=/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 entrypoint: ["/code/simulator/simulation/out/bin/simulator"] - command: ["4", "1", "1"] + command: ["4", "1", "1", "A"] networks: device_network: ipv4_address: 172.22.10.101 @@ -45,15 +45,31 @@ services: - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr96/com_lib_config.json - LD_LIBRARY_PATH=/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 entrypoint: ["/code/simulator/simulation/out/bin/simulator"] - command: ["5", "1", "2"] + command: ["5", "1", "2", "A"] networks: device_network: ipv4_address: 172.22.10.102 + sensor_2: + depends_on: + build_simulator: + condition: service_completed_successfully + image: umrr-ros:latest + volumes: + - .:/code + environment: + - SMART_ACCESS_CFG_FILE_PATH=/code/simulator/config_umrr9f/com_lib_config.json + - LD_LIBRARY_PATH=/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9 + entrypoint: ["/code/simulator/simulation/out/bin/simulator"] + command: ["9", "1", "1", "B"] + networks: + device_network: + ipv4_address: 172.22.10.103 ros_node: image: umrr-ros:latest depends_on: - "sensor_0" - "sensor_1" + - "sensor_2" volumes: - .:/code command: ["colcon", "test", "--packages-above", "umrr_ros2_driver"] diff --git a/simulator/COLCON_IGNORE b/simulator/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/simulator/config_umrr9f/com_lib_config.json b/simulator/config_umrr9f/com_lib_config.json new file mode 100644 index 0000000..caaff38 --- /dev/null +++ b/simulator/config_umrr9f/com_lib_config.json @@ -0,0 +1,16 @@ +{ + "name": "Com Lib Config", + "version": "1.0.0", + "client_id": 300, + "role": "slave", + "shared_lib_path": "/code/umrr_ros2_driver/smartmicro/lib-linux-x86_64-gcc_9", + "config_path": "/code/simulator/config_umrr9f", + "user_interface_name": "umrr9f_t169_automotive", + "user_interface_major_v": 1, + "user_interface_minor_v": 1, + "user_interface_patch_v": 1, + "download_path": "", + "instruction_serialization_type": "port_based", + "data_serialization_type": "port_based", + "alive": false +} diff --git a/simulator/config_umrr9f/hw_inventory.json b/simulator/config_umrr9f/hw_inventory.json new file mode 100644 index 0000000..94f370a --- /dev/null +++ b/simulator/config_umrr9f/hw_inventory.json @@ -0,0 +1,12 @@ +{ + "hwItems": [ + { + "type": "eth", + "dev_id": 1, + "iface_name": "eth0", + "port": 55555 + } + ], + "name": "HW inventory List", + "version": "1.1.0" +} diff --git a/simulator/config_umrr9f/routing_table.json b/simulator/config_umrr9f/routing_table.json new file mode 100644 index 0000000..1eaffd3 --- /dev/null +++ b/simulator/config_umrr9f/routing_table.json @@ -0,0 +1,14 @@ +{ + "clients": [ + { + "client_id": 1, + "data_serialization_type": "port_based", + "instruction_serialization_type": "port_based", + "ip": "172.22.10.100", + "link_type": "eth", + "port": 55555 + } + ], + "name": "Client Routing Table", + "version": "1.0.0" +} diff --git a/simulator/simulation/CMakeLists.txt b/simulator/simulation/CMakeLists.txt index bd165f3..beb6a47 100644 --- a/simulator/simulation/CMakeLists.txt +++ b/simulator/simulation/CMakeLists.txt @@ -17,13 +17,15 @@ 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/umrr96_t153_automotive_v1_2_1 + umrr_ros2_driver/smartmicro/include/umrr9f_t169_automotive_v1_1_1) 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 dl) install(TARGETS simulator DESTINATION bin) diff --git a/simulator/simulation/src/simulator.cpp b/simulator/simulation/src/simulator.cpp index 7ecb783..386cdf9 100644 --- a/simulator/simulation/src/simulator.cpp +++ b/simulator/simulation/src/simulator.cpp @@ -22,6 +22,9 @@ using namespace com::types; uint64_t identifier; uint64_t majorVersion; uint64_t minorVersion; +std::string port; +std::string portFile; + std::shared_ptr dataServices = com::common::DataServicesIface::Get(); @@ -59,13 +62,18 @@ void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) { instruction->SetResponse(COM_INSTR_PORT_SUCCESS); instruction->SetValue(minorVersion); } else if (instruction->GetSectionId() == 2010 && - instruction->GetId() == 0) { - std::cout << "Instruction for mode change tx_antenna answered!" + instruction->GetId() == 2) { + std::cout << "UMRR96 mode frequency_sweep set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); } else if (instruction->GetSectionId() == 2010 && - instruction->GetId() == 2) { - std::cout << "Instruction for mode change sweep_idx answered!" + instruction->GetId() == 4) { + std::cout << "UMRR11 mode angular_separation set!" + << std::endl; + instruction->SetResponse(COM_INSTR_PORT_SUCCESS); + } else if (instruction->GetSectionId() == 2010 && + instruction->GetId() == 5) { + std::cout << "UMRR9F mode range_toggle_mode set!" << std::endl; instruction->SetResponse(COM_INSTR_PORT_SUCCESS); } else { @@ -76,8 +84,39 @@ void slave_callback(ClientId clientId, PortId, BufferDescriptor buffer) { dataServices->SetInstructionBuffer(clientId, *receive, nullptr); } +void stream_port(std::string portFile) { + + dataServices->RegisterInstRecvCallback(slave_callback); + + ClientId masterId = 1; + PortId portTargetListId = 66; + std::ifstream ifs(portFile, std::ifstream::binary | std::ios::binary); + std::filebuf *pbuf = ifs.rdbuf(); + int size = pbuf->pubseekoff(0, ifs.end, ifs.in); + pbuf->pubseekpos(0, ifs.in); + char *filebuffer = new (std::nothrow) char[size]; + + if (filebuffer == nullptr) { + std::cout << "error assigning memory!" << std::endl; + } + + pbuf->sgetn(filebuffer, size); + BufferDescriptor bufferdesc((uint8_t *)filebuffer, size); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + if (ERROR_CODE_OK != + dataServices->StreamDataPort(masterId, portTargetListId, bufferdesc)) { + return; + } + + std::cout << "sensor is transmitting data! " << std::endl; + ifs.close(); + delete[] filebuffer; + +} + int main(int argc, char *argv[]) { - if (argc != 4) { + if (argc != 5) { std::cout << "Specifiy User Interface for the sensor" << std::endl; return 1; } @@ -85,42 +124,28 @@ int main(int argc, char *argv[]) { identifier = strtoll(argv[1], nullptr, 10); majorVersion = strtoll(argv[2], nullptr, 10); minorVersion = strtoll(argv[3], nullptr, 10); - + port = argv[4]; + if (!dataServices->Init()) { throw std::runtime_error("Data services have not been initialized!"); } - - dataServices->RegisterInstRecvCallback(slave_callback); + auto Start = std::chrono::steady_clock::now(); while (1) { - ClientId masterId = 1; - PortId portTargetListId = 66; - std::string portFile = "/code/simulator/targetlist_port.bin"; - std::ifstream ifs(portFile, std::ifstream::binary | std::ios::binary); - std::filebuf *pbuf = ifs.rdbuf(); - int size = pbuf->pubseekoff(0, ifs.end, ifs.in); - pbuf->pubseekpos(0, ifs.in); - char *filebuffer = new (std::nothrow) char[size]; - - if (filebuffer == nullptr) { - std::cout << "error assigning memory!" << std::endl; - } - - pbuf->sgetn(filebuffer, size); - BufferDescriptor bufferdesc((uint8_t *)filebuffer, size); - std::this_thread::sleep_for(std::chrono::seconds(1)); - if (ERROR_CODE_OK != - dataServices->StreamDataPort(masterId, portTargetListId, bufferdesc)) { - return -1; + if(port == "A") { + std::string portFile = "/code/simulator/targetlist_port_v2_1_0.bin"; + stream_port(portFile); } - - std::cout << "sensor is transmitting data! " << std::endl; - ifs.close(); - delete[] filebuffer; - - if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(40)) + else if(port == "B") { + std::string portFile = "/code/simulator/targetlist_port_v3_0_0.bin"; + stream_port(portFile); + } + else { + std::cout << "Invalid input!" << std::endl; + } + if (std::chrono::steady_clock::now() - Start > std::chrono::seconds(30)) break; } return 0; diff --git a/simulator/targetlist_port.bin b/simulator/targetlist_port_v2_1_0.bin similarity index 100% rename from simulator/targetlist_port.bin rename to simulator/targetlist_port_v2_1_0.bin diff --git a/simulator/targetlist_port_v3_0_0.bin b/simulator/targetlist_port_v3_0_0.bin new file mode 100644 index 0000000..413fe5c Binary files /dev/null and b/simulator/targetlist_port_v3_0_0.bin differ diff --git a/smart_extract.sh b/smart_extract.sh index 468b1ef..24fba3e 100755 --- a/smart_extract.sh +++ b/smart_extract.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -smart_pack=SmartAccessAutomotive_1_1_0.tar.gz +smart_pack=SmartAccessAutomotive_2_0_0.tar.gz 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 e2cbbcf..cb47350 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -26,20 +26,20 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -g) endif() find_package(ament_cmake_auto REQUIRED) find_package(Threads REQUIRED) -FetchContent_Declare(json +fetchcontent_declare(json GIT_REPOSITORY https://github.com/nlohmann/json.git - GIT_TAG v3.10.4 + GIT_TAG v3.10.5 ) -FetchContent_GetProperties(json) +fetchcontent_getproperties(json) if(NOT json_POPULATED) - FetchContent_Populate(json) + fetchcontent_populate(json) add_subdirectory(${json_SOURCE_DIR} ${json_BINARY_DIR} EXCLUDE_FROM_ALL) endif() @@ -49,12 +49,13 @@ ament_auto_find_build_dependencies() link_directories(${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9) install(FILES - "${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libsmart_access.so" - "${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libosal.so" - "${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libcom_lib.so" - "${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libbasev1.0.2_user_interface.so" - "${CMAKE_CURRENT_SOURCE_DIR}/smartmicro/lib-linux-x86_64-gcc_9/libumrr11_t132_automotivev1.1.1_user_interface.so" - "${CMAKE_CURRENT_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/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" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -77,16 +78,18 @@ 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/umrr96_t153_automotive_v1_2_1 +smartmicro/include/umrr9f_t169_automotive_v1_1_1) # link smart_access_lib-linux-x86_64-gcc_9 to the node -target_link_libraries(smartmicro_radar_node +target_link_libraries(smartmicro_radar_node smart_access nlohmann_json::nlohmann_json com_lib osal umrr11_t132_automotivev1.1.1_user_interface - umrr96_t153_automotivev1.2.1_user_interface) + umrr96_t153_automotivev1.2.1_user_interface + umrr9f_t169_automotivev1.1.1_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" @@ -94,16 +97,30 @@ 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 "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 + ament_cmake_lint_cmake + ament_cmake_uncrustify + ament_cmake_cpplint + ament_cmake_flake8 + ament_cmake_cppcheck + ament_cmake_pep257 + ) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_copyright("src/smartmicro_radar_node.cpp") + ament_cpplint("src/smartmicro_radar_node.cpp") + ament_cppcheck("src/smartmicro_radar_node.cpp") + endif() ament_auto_package(INSTALL_TO_SHARE config launch param) diff --git a/umrr_ros2_driver/config/sensor_params.json b/umrr_ros2_driver/config/sensor_params.json index c455ef4..ea5c9b8 100644 --- a/umrr_ros2_driver/config/sensor_params.json +++ b/umrr_ros2_driver/config/sensor_params.json @@ -1,53 +1,252 @@ { "parameters": [ { - "model": "umrr11/umrr96", "name": "tx_antenna_idx", - "comment": "Index of Transmit Antenna (UMRR11 : 0 and 1 are ACC, 2 is AEB)", - "min": 0, - "max": 2, - "default": 2 + "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 + } + ] }, { - "model": "umrr11/umrr96", "name": "center_frequency_idx", - "type": "u8", - "comment": "Index of center frequency (UMRR11: 0..3 in ACC selectable. In AEB mode values 2 and 3 are similar to 1.)", - "min": 0, - "max": 3, - "default": 0 - }, - { - "model": "umrr11/umrr96", + "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 + } + ] + }, + { "name": "frequency_sweep_idx", - "comment": "Index of sweep (UMRR11: currently always 0 because only one sweep for every TX-antenna. UMRR96: 0=226MHz, 1=512MHz, 2=1536MHz)", - "min": 0, - "max": 2, - "default": 0 + "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": "umrr11", + { "name": "enable_tx_ant_toggle", "comment": "Automatic toggle of TX-Antenna 0->2->0 every radar cycle", - "min": 0, - "max": 1, - "default": 0 + "sensors": [ + { + "model": "umrr11", + "min": 0, + "max": 1, + "default": 0 + } + ] }, - { - "model": "umrr11", + { "name": "angular_separation", "comment": "0 = Disable Angular Separation, 1 = Enable Angular Separation", - "min": 0, - "max": 1, - "default": 0 + "sensors": [ + { + "model": "umrr11", + "min": 0, + "max": 1, + "default": 0 + } + ] }, - { - "model": "umrr96", + { "name": "range_toggle_mode", - "comment": "Automatic toggle of range:0=off, 1=Short-Med, 2=Short-Long, 3=Med-Long", - "min": 0, - "max": 3, - "default": 0 + "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 + } + ] + }, + { + "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 + } + ] + }, + { + "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 + } + ] + }, + { + "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 + } + ] + }, + { + "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 + } + ] + }, + { + "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 + } + ] + }, + { + "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 + } + ] + }, + { + "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 + } + ] + }, + { + "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 + } + ] }, { "name": "ip_source_address", 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 80bd5cf..51c7e42 100644 --- a/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp +++ b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp @@ -17,54 +17,64 @@ #ifndef UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ #define UMRR_ROS2_DRIVER__SMARTMICRO_RADAR_NODE_HPP_ -#include "umrr_ros2_msgs/srv/set_ip.hpp" -#include "umrr_ros2_msgs/srv/set_mode.hpp" + #include #include #include #include #include -#include -#include #include -#include -#include +#include +#include +#include #include #include #include -namespace smartmicro { -namespace drivers { -namespace radar { - -namespace detail { - -constexpr auto kMaxSensorCount = 10UL; +#include "umrr_ros2_msgs/srv/set_ip.hpp" +#include "umrr_ros2_msgs/srv/set_mode.hpp" -struct SensorConfig { +namespace smartmicro +{ +namespace drivers +{ +namespace radar +{ +namespace detail +{ +constexpr auto kMaxSensorCount = 8UL; + +struct SensorConfig +{ std::uint32_t id{}; std::string ip{}; std::uint32_t port{}; - std::string iface_name{}; std::string frame_id{}; std::uint32_t history_size{}; std::string model{}; }; -} // 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(); } private: /// @@ -73,12 +83,14 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// /// @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 - &target_list_port); + 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); /// /// @brief A callback that is called when a new target list port for @@ -86,12 +98,30 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// /// @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 - &target_list_port); + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr96_t153_automotive_v1_2_1::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr96, + const com::types::ClientId client_id); + + /// + /// @brief A callback that is called when a new target list port for + /// umrr9f 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( + 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); /// /// @brief Read parameters and update the json config files required by @@ -102,51 +132,58 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node { /// /// @brief Callaback for getting the instruction response. /// - void - sensor_response(const com::types::ClientId client_id, - const std::shared_ptr &response); + void sensor_response( + const com::types::ClientId client_id, + const std::shared_ptr & response, + const std::string instruction_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); - - /// - /// @brief A timer to handle the initialization. - /// - void my_timer_callback() { timer->cancel(); } + void ip_address( + const std::shared_ptr request, + std::shared_ptr response); rclcpp::Service::SharedPtr mode_srv_; rclcpp::Service::SharedPtr ip_addr_srv_; - std::shared_ptr m_services{}; + std::array m_sensors{}; - std::array::SharedPtr, - detail::kMaxSensorCount> - m_publishers{}; + + std::array::SharedPtr, detail::kMaxSensorCount> + m_publishers{}; std::size_t m_number_of_sensors{}; rclcpp::TimerBase::SharedPtr timer; com::types::ClientId client_id; std::uint64_t response_type{}; }; -} // namespace radar -} // namespace drivers -} // namespace smartmicro +/// +/// @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_umrr11{}; +std::shared_ptr data_umrr96{}; +std::shared_ptr data_umrr9f{}; + +} // 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 fe98155..39f01ff 100644 --- a/umrr_ros2_driver/launch/radar.launch.py +++ b/umrr_ros2_driver/launch/radar.launch.py @@ -13,6 +13,7 @@ # limitations under the License. import os +import launch_ros from ament_index_python import get_package_share_directory from launch_ros.actions import Node @@ -38,8 +39,8 @@ def generate_launch_description(): radar_node = Node( package=PACKAGE_NAME, executable='smartmicro_radar_node_exe', - name='smartmicro_radar_node', - parameters=[LaunchConfiguration('radar_param_file')] + parameters=[LaunchConfiguration('radar_param_file')], + name='smartmicro_radar_node' ) return LaunchDescription([ diff --git a/umrr_ros2_driver/package.xml b/umrr_ros2_driver/package.xml index 03badca..2695278 100644 --- a/umrr_ros2_driver/package.xml +++ b/umrr_ros2_driver/package.xml @@ -2,7 +2,7 @@ umrr_ros2_driver - 2.1.0 + 3.0.0 A node to publish data from a smartmicro radar. s.m.s, smart microwave sensors GmbH. Apache 2.0 License diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml index 916cda4..6182304 100644 --- a/umrr_ros2_driver/param/radar.template.yaml +++ b/umrr_ros2_driver/param/radar.template.yaml @@ -2,7 +2,7 @@ /**: ros__parameters: # The client_id to be set in smart_access_config.json, must be a unique integer. - master_client_id: 1 + 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 @@ -39,3 +39,16 @@ frame_id: "umrr" # Specify the history size. history_size: 10 + sensor_2: + # The model of the connected sensor. + model: "umrr9f" + # 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 diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 645103f..40ed817 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -14,34 +14,25 @@ // The initial version of the code was developed by Apex.AI and // was thereafter adapted and extended by smartmicro. -#include -#include -#include +#include "umrr_ros2_driver/smartmicro_radar_node.hpp" #include #include -#include -#include -#include -#include - -#include -#include #include #include - -#include -#include #include #include +#include +#include + +#include #include #include #include #include - #include #include #include @@ -50,6 +41,9 @@ #include #include +#include "umrr_ros2_driver/config_path.hpp" +#include "umrr_ros2_driver/sensor_params.hpp" + using com::common::Instruction; using com::master::CmdRequest; using com::master::CommunicationServicesIface; @@ -62,8 +56,8 @@ using com::master::ResponseBatch; using com::master::SetParamRequest; using point_cloud_msg_wrapper::PointCloud2Modifier; -namespace { - +namespace +{ constexpr auto kDefaultClientId = 0; constexpr auto kDefaultInterfaceName = "lo"; constexpr auto kDefaultIp = "127.0.0.1"; @@ -85,12 +79,14 @@ constexpr auto kDevIdJsonTag = "dev_id"; constexpr auto kClientsJsonTag = "clients"; constexpr auto kHwItemsJsonTag = "hwItems"; -constexpr bool float_eq(const float a, const float b) noexcept { +constexpr bool float_eq(const float a, const float b) noexcept +{ const auto maximum = std::max(std::fabs(a), std::fabs(b)); return std::fabs(a - b) <= maximum * std::numeric_limits::epsilon(); } -struct RadarPoint { +struct RadarPoint +{ float x{}; float y{}; float z{}; @@ -98,12 +94,11 @@ struct RadarPoint { float power{}; float RCS{}; float Noise{}; - 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); + 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); } }; @@ -111,22 +106,23 @@ 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); -using Generators = std::tuple; +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>; using RadarCloudModifier = PointCloud2Modifier; -} // namespace - -namespace smartmicro { -namespace drivers { -namespace radar { - -SmartmicroRadarNode::SmartmicroRadarNode( - const rclcpp::NodeOptions &node_options) - : rclcpp::Node{"smartmicro_radar_node", node_options} { +} // namespace + +namespace smartmicro +{ +namespace drivers +{ +namespace radar +{ +SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node{"smartmicro_radar_node", node_options} +{ update_config_files_from_params(); const auto override = false; @@ -134,70 +130,84 @@ SmartmicroRadarNode::SmartmicroRadarNode( // Getting the communication services m_services = CommunicationServicesIface::Get(); - if (!m_services->Init()) { throw std::runtime_error("Initialization failed"); } // Getting the data stream service - std::shared_ptr< - com::master::umrr11_t132_automotive_v1_1_1::DataStreamServiceIface> - data_umrr11 = com::master::umrr11_t132_automotive_v1_1_1:: - DataStreamServiceIface::Get(); - std::shared_ptr< - com::master::umrr96_t153_automotive_v1_2_1::DataStreamServiceIface> - data_umrr96 = com::master::umrr96_t153_automotive_v1_2_1:: - DataStreamServiceIface::Get(); - std::cout << "Data stream services have been received!" << std::endl; + 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 = com::master::umrr9f_t169_automotive_v1_1_1::DataStreamServiceIface::Get(); + + RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); // Wait init time std::this_thread::sleep_for(std::chrono::seconds(2)); for (auto i = 0UL; i < m_number_of_sensors; ++i) { - const auto &sensor = m_sensors[i]; - if (sensor.model == "umrr11" && - com::types::ERROR_CODE_OK != - data_umrr11->RegisterComTargetListPortReceiveCallback( - sensor.id, - std::bind(&SmartmicroRadarNode::targetlist_callback_umrr11, - this, i, std::placeholders::_1))) { - std::cout << "Failed to register targetlist callback for sensor umrr11" - << std::endl; + const auto & sensor = m_sensors[i]; + if ( + sensor.model == "umrr11" && + com::types::ERROR_CODE_OK != + data_umrr11->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr11, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; + } + if ( + sensor.model == "umrr96" && + com::types::ERROR_CODE_OK != + data_umrr96->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr96, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; } - if (sensor.model == "umrr96" && - com::types::ERROR_CODE_OK != - data_umrr96->RegisterComTargetListPortReceiveCallback( - sensor.id, - std::bind(&SmartmicroRadarNode::targetlist_callback_umrr96, - this, i, std::placeholders::_1))) { - std::cout << "Falied to register targetlist callback for sensor umrr96" - << std::endl; + if ( + sensor.model == "umrr9f" && + com::types::ERROR_CODE_OK != + data_umrr9f->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9f, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrr11" << std::endl; } m_publishers[i] = create_publisher( - "umrr/targets_" + std::to_string(i), sensor.history_size); + "umrr/targets_" + std::to_string(i), sensor.history_size); } // create a ros2 service to change the radar parameters mode_srv_ = create_service( - "smartmicro_radar_node/set_radar_mode", - std::bind(&SmartmicroRadarNode::radar_mode, this, std::placeholders::_1, - std::placeholders::_2)); + "smartmicro_radar_node/set_radar_mode", + std::bind( + &SmartmicroRadarNode::radar_mode, this, std::placeholders::_1, std::placeholders::_2)); // create a ros2 service to change the IP address ip_addr_srv_ = create_service( - "smartmicro_radar_node/set_ip_address", - std::bind(&SmartmicroRadarNode::ip_address, this, std::placeholders::_1, - std::placeholders::_2)); + "smartmicro_radar_node/set_ip_address", + std::bind( + &SmartmicroRadarNode::ip_address, 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); }); } -void SmartmicroRadarNode::radar_mode( - const std::shared_ptr request, - std::shared_ptr result) { +void terminate_on_receive(int signal) +{ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "SIGINT has been received"); + check_signal = true; + rclcpp::Rate sleepRate(std::chrono::milliseconds(100)); + sleepRate.sleep(); + rclcpp::shutdown(); +} +void SmartmicroRadarNode::radar_mode( + const std::shared_ptr request, + std::shared_ptr result) +{ std::string instruction_name{}; - std::uint16_t check_value{}; bool check_flag_param = false; bool check_flag_id = false; @@ -205,16 +215,13 @@ void SmartmicroRadarNode::radar_mode( auto param_table = nlohmann::json::parse(instr_file); instruction_name = request->param; - check_value = request->value; client_id = request->sensor_id; - for (const auto &item : param_table.items()) { - for (const auto ¶m : item.value().items()) { + for (const auto & item : param_table.items()) { + for (const auto & param : item.value().items()) { if (instruction_name == param.value()["name"]) { - if (check_value <= param.value()["max"]) { - check_flag_param = true; - break; - } + check_flag_param = true; + break; } } } @@ -224,24 +231,20 @@ void SmartmicroRadarNode::radar_mode( return; } - for (auto &sensor : m_sensors) { + for (auto & sensor : m_sensors) { if (client_id == sensor.id) { check_flag_id = true; break; } } if (!check_flag_id) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "The sensor ID value entered is invalid! "); result->res = "The sensor ID value entered is invalid! "; return; } - std::shared_ptr inst{ - m_services->GetInstructionService()}; + std::shared_ptr inst{m_services->GetInstructionService()}; timer = this->create_wall_timer( - std::chrono::seconds(2), - std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); std::shared_ptr batch; @@ -249,45 +252,48 @@ void SmartmicroRadarNode::radar_mode( result->res = "Failed to allocate instruction batch! "; return; } - std::shared_ptr> radar_mode( - new SetParamRequest("auto_interface_0dim", request->param, - request->value)); - if (!batch->AddRequest(radar_mode)) { - result->res = "Failed to add instruction to batch! "; - return; + std::shared_ptr> radar_mode_01 = + std::make_shared>( + "auto_interface_0dim", request->param, request->value); + + std::shared_ptr> radar_mode_02 = + std::make_shared>("auto_interface_0dim", request->param, request->value); + + if (!batch->AddRequest(radar_mode_01)) { + result->res = "Failed to add instruction to the batch! "; + } + if (!batch->AddRequest(radar_mode_02)) { + result->res = "Failed to add instruction to the batch! "; } - if (com::types::ERROR_CODE_OK != - inst->SendInstructionBatch( - batch, std::bind(&SmartmicroRadarNode::sensor_response, this, - client_id, std::placeholders::_2))) { - result->res = "Service is not conducted"; + if ( + com::types::ERROR_CODE_OK != inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::sensor_response, this, client_id, + std::placeholders::_2, instruction_name))) { + result->res = "Check param is listed for sensor type and the min/max values!"; return; } result->res = "Service conducted successfully"; } void SmartmicroRadarNode::ip_address( - const std::shared_ptr request, - std::shared_ptr result) { - - std::shared_ptr inst{ - m_services->GetInstructionService()}; + const std::shared_ptr request, + std::shared_ptr result) +{ + std::shared_ptr inst{m_services->GetInstructionService()}; timer = this->create_wall_timer( - std::chrono::seconds(2), - std::bind(&SmartmicroRadarNode::my_timer_callback, this)); + std::chrono::seconds(2), std::bind(&SmartmicroRadarNode::my_timer_callback, this)); bool check_flag = false; client_id = request->sensor_id; - for (auto &sensor : m_sensors) { + for (auto & sensor : m_sensors) { if (client_id == sensor.id) { check_flag = true; break; } } if (!check_flag) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Sensor ID entered is not listed in the param file! "); result->res_ip = "Sensor ID entered is not listed in the param file! "; return; } @@ -297,11 +303,14 @@ void SmartmicroRadarNode::ip_address( result->res_ip = "Failed to allocate instruction batch! "; return; } - std::shared_ptr> ip_address( - new SetParamRequest("auto_interface_0dim", "ip_source_address", - request->value_ip)); - std::shared_ptr cmd(new CmdRequest( - "auto_interface_command", "comp_eeprom_ctrl_save_param_sec", 2010)); + + std::shared_ptr> ip_address = + std::make_shared>( + "auto_interface_0dim", "ip_source_address", request->value_ip); + + std::shared_ptr cmd = + std::make_shared("auto_interface_command", "comp_eeprom_ctrl_save_param_sec", 2010); + if (!batch->AddRequest(ip_address)) { result->res_ip = "Failed to add instruction to batch! "; return; @@ -311,154 +320,165 @@ void SmartmicroRadarNode::ip_address( return; } // send instruction batch to the device - if (com::types::ERROR_CODE_OK != - inst->SendInstructionBatch( - batch, std::bind(&SmartmicroRadarNode::sensor_response_ip, this, - client_id, std::placeholders::_2))) { + if ( + com::types::ERROR_CODE_OK != + inst->SendInstructionBatch( + batch, std::bind( + &SmartmicroRadarNode::sensor_response_ip, this, client_id, std::placeholders::_2))) { result->res_ip = "Service not conducted"; return; } else { - RCLCPP_INFO(this->get_logger(), - "Radar must be restarted and the parameters in the param file " - "must be updated !!."); + RCLCPP_INFO( + this->get_logger(), + "Radar must be restarted and the parameters in the param file " + "must be updated !!."); result->res_ip = "Service conducted successfully"; } } void SmartmicroRadarNode::sensor_response( - const com::types::ClientId client_id, - const std::shared_ptr &response) { + const com::types::ClientId client_id, + const std::shared_ptr & response, const std::string instruction_name) +{ std::vector>> myResp_1; + std::vector>> myResp_2; - if (response->GetResponse("auto_interface_0dim", "tx_antenna_idx", - myResp_1)) { - for (auto &resp : myResp_1) { + if (response->GetResponse("auto_interface_0dim", instruction_name.c_str(), myResp_1)) { + for (auto & resp : myResp_1) { response_type = resp->GetResponseType(); - RCLCPP_INFO(this->get_logger(), "Response from tx_antenna mode : %i", - response_type); + RCLCPP_INFO( + this->get_logger(), "Response from '%s' : %i", instruction_name.c_str(), response_type); } } - if (response->GetResponse("auto_interface_0dim", - "frequency_sweep_idx", myResp_1)) { - for (auto &resp : myResp_1) { + if (response->GetResponse("auto_interface_0dim", instruction_name.c_str(), myResp_2)) { + for (auto & resp : myResp_2) { response_type = resp->GetResponseType(); - RCLCPP_INFO(this->get_logger(), - "Response from frequency_sweep service: %i", response_type); - } - } - if (response->GetResponse("auto_interface_0dim", - "center_frequency_idx", myResp_1)) { - for (auto &resp : myResp_1) { - response_type = resp->GetResponseType(); - RCLCPP_INFO(this->get_logger(), - "Response from center frequency service: %i", response_type); - } - } - if (response->GetResponse("auto_interface_0dim", - "enable_tx_ant_toggle", myResp_1)) { - for (auto &resp : myResp_1) { - response_type = resp->GetResponseType(); - RCLCPP_INFO(this->get_logger(), - "Response from enable_tx_ant_toggle service: %i", - response_type); - } - } - if (response->GetResponse("auto_interface_0dim", - "angular_separation", myResp_1)) { - for (auto &resp : myResp_1) { - response_type = resp->GetResponseType(); - RCLCPP_INFO(this->get_logger(), - "Response from angular_separation service: %i", - response_type); - } - } - if (response->GetResponse("auto_interface_0dim", "range_toggle_mode", - myResp_1)) { - for (auto &resp : myResp_1) { - response_type = resp->GetResponseType(); - RCLCPP_INFO(this->get_logger(), - "Response from range_toggle_mode service: %i", response_type); + RCLCPP_INFO( + this->get_logger(), "Response from '%s' : %i", instruction_name.c_str(), response_type); } } } void SmartmicroRadarNode::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) +{ std::vector>> myResp_2; - if (response->GetResponse("auto_interface_0dim", - "ip_source_address", myResp_2)) { - for (auto &resp : myResp_2) { + if (response->GetResponse("auto_interface_0dim", "ip_source_address", myResp_2)) { + for (auto & resp : myResp_2) { response_type = resp->GetResponseType(); - RCLCPP_INFO(this->get_logger(), "Response from sensor for ip change: %i", - response_type); + RCLCPP_INFO(this->get_logger(), "Response from sensor for ip change: %i", response_type); } } } void SmartmicroRadarNode::targetlist_callback_umrr11( - const std::uint32_t sensor_idx, - const std::shared_ptr - &target_list_port) { - std::shared_ptr - port_header = target_list_port->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 : target_list_port->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(); - modifier.push_back( + 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) +{ + std::cout << "Targetlist callback is being called for umrr11" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr11_t132_automotive_v1_1_1::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr11->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_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(); + modifier.push_back( {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), - range * std::sin(elevation_angle), target->GetSpeedRadial(), - target->GetRCS(), target->GetTgtNoise(), target->GetPower()}); - } + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetRCS(), + target->GetTgtNoise(), target->GetPower()}); + } - m_publishers[sensor_idx]->publish(msg); + m_publishers[sensor_idx]->publish(msg); + } } void SmartmicroRadarNode::targetlist_callback_umrr96( - const std::uint32_t sensor_idx, - const std::shared_ptr - &target_list_port) { - std::shared_ptr - port_header = target_list_port->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 : target_list_port->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(); - modifier.push_back( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr96_t153_automotive_v1_2_1::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr96, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrr96" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr96_t153_automotive_v1_2_1::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr96->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_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(); + modifier.push_back( {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), - range * std::sin(elevation_angle), target->GetSpeedRadial(), - target->GetRCS(), target->GetTgtNoise(), target->GetPower()}); + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetRCS(), + target->GetTgtNoise(), target->GetPower()}); + } + + m_publishers[sensor_idx]->publish(msg); } +} - m_publishers[sensor_idx]->publish(msg); +void SmartmicroRadarNode::targetlist_callback_umrr9f( + 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) +{ + std::cout << "Targetlist callback is being called for umrr9f" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr9f_t169_automotive_v1_1_1::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr9f->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->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(); + modifier.push_back( + {range_2d * std::cos(azimuth_angle), range_2d * std::sin(azimuth_angle), + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetRCS(), + target->GetTgtNoise(), target->GetPower()}); + } + + m_publishers[sensor_idx]->publish(msg); + } } -void SmartmicroRadarNode::update_config_files_from_params() { +void SmartmicroRadarNode::update_config_files_from_params() +{ const auto dev_id = declare_parameter(kDevIdTag, 0); if (!dev_id) { throw std::runtime_error("Parameter dev_id not set."); @@ -467,8 +487,7 @@ void SmartmicroRadarNode::update_config_files_from_params() { if (!dev_port) { throw std::runtime_error("Parameter dev_port not set."); } - const auto dev_iface_name = - declare_parameter(kDevIfaceNameTag, std::string{}); + 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."); } @@ -478,26 +497,20 @@ void SmartmicroRadarNode::update_config_files_from_params() { } auto read_sensor_params_if_possible = [&](const std::uint32_t index) { - auto ¤t_sensor = m_sensors[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); + current_sensor.model = this->declare_parameter(prefix + ".model", kDefaultSensorType); + current_sensor.id = this->declare_parameter(prefix + ".id", kDefaultClientId); if (current_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.iface_name = - this->declare_parameter(prefix + ".iface_name", kDefaultInterfaceName); - current_sensor.frame_id = - this->declare_parameter(prefix + ".frame_id", kDefaultFrameId); + 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); + this->declare_parameter(prefix + ".history_size", kDefaultHistorySize); return true; }; @@ -517,30 +530,26 @@ void SmartmicroRadarNode::update_config_files_from_params() { config[kClientIdTag] = master_client_id; std::ofstream{kConfigFilePath, std::ios::trunc} << config; - auto hw_inventory = - nlohmann::json::parse(std::ifstream{kHwInventoryFilePath}); - auto &hw_items = hw_inventory[kHwItemsJsonTag]; + auto hw_inventory = nlohmann::json::parse(std::ifstream{kHwInventoryFilePath}); + auto & hw_items = hw_inventory[kHwItemsJsonTag]; if (hw_items.empty()) { - throw std::runtime_error( - "There are no 'hwItems' defined in the hw_inventory.json file."); + throw std::runtime_error("There are no 'hwItems' defined in the hw_inventory.json file."); } - auto &hw_item = hw_items.front(); + auto & hw_item = hw_items.front(); hw_item[kDevIdJsonTag] = dev_id; hw_item[kPortTag] = dev_port; hw_item[kIfaceNameTag] = dev_iface_name; std::ofstream{kHwInventoryFilePath, std::ios::trunc} << hw_inventory; - auto routing_table = - nlohmann::json::parse(std::ifstream{kRoutingTableFilePath}); - auto &clients = routing_table[kClientsJsonTag]; + auto routing_table = nlohmann::json::parse(std::ifstream{kRoutingTableFilePath}); + auto & clients = routing_table[kClientsJsonTag]; if (clients.empty()) { - throw std::runtime_error( - "There are no 'clients' defined in the routing_table.json file."); + throw std::runtime_error("There are no 'clients' defined in the routing_table.json file."); } - auto client = clients.front(); // Make a copy of the first client. + auto client = clients.front(); // Make a copy of the first client. clients.clear(); for (auto i = 0UL; i < m_number_of_sensors; ++i) { - const auto &sensor = m_sensors[i]; + const auto & sensor = m_sensors[i]; client[kClientIdTag] = sensor.id; client[kPortTag] = sensor.port; client[kIpTag] = sensor.ip; @@ -549,8 +558,8 @@ void SmartmicroRadarNode::update_config_files_from_params() { std::ofstream{kRoutingTableFilePath, std::ios::trunc} << routing_table; } -} // namespace radar -} // namespace drivers -} // namespace smartmicro +} // namespace radar +} // namespace drivers +} // namespace smartmicro RCLCPP_COMPONENTS_REGISTER_NODE(smartmicro::drivers::radar::SmartmicroRadarNode) diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index f35cc5d..4bbc74d 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -44,23 +44,34 @@ def generate_test_description(): ], ) - send_tx_service = ExecuteProcess( + frequency_sweep_service = ExecuteProcess( cmd = [[ 'ros2 service call ', '/smartmicro_radar_node/set_radar_mode ', 'umrr_ros2_msgs/srv/SetMode ', - '"{param: "tx_antenna_idx", value: 2, sensor_id: 100}"' + '"{param: "frequency_sweep_idx", value: 1, sensor_id: 200}"' ]], output='screen', shell = True ) - send_sweep_service = ExecuteProcess( + angular_separation_service = ExecuteProcess( cmd = [[ 'ros2 service call ', '/smartmicro_radar_node/set_radar_mode ', 'umrr_ros2_msgs/srv/SetMode ', - '"{param: "frequency_sweep_idx", value: 1, sensor_id: 200}"' + '"{param: "angular_separation", value: 1, sensor_id: 100}"' + ]], + output='screen', + shell = True + ) + + range_toggle_mode_service = ExecuteProcess( + cmd = [[ + 'ros2 service call ', + '/smartmicro_radar_node/set_radar_mode ', + 'umrr_ros2_msgs/srv/SetMode ', + '"{param: "range_toggle_mode", value: 4, sensor_id: 300}"' ]], output='screen', shell = True @@ -69,8 +80,9 @@ def generate_test_description(): return ( launch.LaunchDescription([ radar_node, - send_tx_service, - send_sweep_service, + frequency_sweep_service, + angular_separation_service, + range_toggle_mode_service, launch_testing.actions.ReadyToTest(), ]), { @@ -105,6 +117,7 @@ def test_smart_node_publishes(self): # Expect the smartnode to publish strings on '/umrr/targets_' data_rx_s1 = [] data_rx_s2 = [] + data_rx_s3 = [] def data_rx_s1_callback(msg): data_rx_s1.append(msg) @@ -112,6 +125,9 @@ def data_rx_s1_callback(msg): def data_rx_s2_callback(msg): data_rx_s2.append(msg) + def data_rx_s3_callback(msg): + data_rx_s3.append(msg) + sub_s1 = self.test_node.create_subscription( sensor_msgs.PointCloud2, '/umrr/targets_0', @@ -124,6 +140,12 @@ def data_rx_s2_callback(msg): data_rx_s2_callback, 10 ) + sub_s3 = self.test_node.create_subscription( + sensor_msgs.PointCloud2, + '/umrr/targets_2', + data_rx_s3_callback, + 10 + ) try: # Wait until the publisher publishes end_time = time.time() + 20 @@ -134,10 +156,13 @@ def data_rx_s2_callback(msg): print(f"Data from S1 received at {time.time()}") if len(data_rx_s2) > 1: print(f"Data from S2 received at {time.time()}") - + if len(data_rx_s3) > 1: + print(f"Data from S3 received at {time.time()}") self.assertGreater(len(data_rx_s1), 1) self.assertGreater(len(data_rx_s2), 1) + self.assertGreater(len(data_rx_s3), 1) finally: self.test_node.destroy_subscription(sub_s1) self.test_node.destroy_subscription(sub_s2) + self.test_node.destroy_subscription(sub_s3) diff --git a/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp b/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp index 8bbf494..7f61f77 100644 --- a/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp @@ -13,14 +13,15 @@ // limitations under the License. #include -#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) { +TEST(SmartmicroRadarNodeTest, Create) +{ rclcpp::init(0, nullptr); ASSERT_TRUE(rclcpp::ok()); rclcpp::NodeOptions node_options{}; @@ -30,9 +31,11 @@ TEST(SmartmicroRadarNodeTest, Create) { 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.101"); - node_options.append_parameter_override("sensors.sensor_1.id", 20); - node_options.append_parameter_override("sensors.sensor_1.ip", "172.22.10.102"); + 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(); } diff --git a/umrr_ros2_msgs/srv/SetIp.srv b/umrr_ros2_msgs/srv/SetIp.srv index 13a88a3..d21c0c2 100644 --- a/umrr_ros2_msgs/srv/SetIp.srv +++ b/umrr_ros2_msgs/srv/SetIp.srv @@ -1,4 +1,4 @@ uint32 value_ip -int32 sensor_id +uint32 sensor_id --- string res_ip diff --git a/umrr_ros2_msgs/srv/SetMode.srv b/umrr_ros2_msgs/srv/SetMode.srv index 031d873..2b33c77 100644 --- a/umrr_ros2_msgs/srv/SetMode.srv +++ b/umrr_ros2_msgs/srv/SetMode.srv @@ -1,5 +1,5 @@ string param -int32 value +float32 value uint32 sensor_id --- string res