From 39e04fdecb4c32af2909ac3c7f5688bd90a43db6 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 19 Jun 2023 11:23:19 +0200 Subject: [PATCH 01/16] Added new interface for DRVGRD 152 --- umrr_ros2_driver/CMakeLists.txt | 7 +- .../smartmicro_radar_node.hpp | 24 ++++++- .../src/smartmicro_radar_node.cpp | 64 ++++++++++++++++--- 3 files changed, 82 insertions(+), 13 deletions(-) diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index 6820363..2079c83 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -58,6 +58,7 @@ install(FILES "${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/lib-linux-x86_64-gcc_9/libumrr9d_t152_automotivev1.2.0_user_interface.so" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -87,7 +88,8 @@ 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/umrr9d_t152_automotive_v1_0_2 +smartmicro/include/umrr9d_t152_automotive_v1_2_0) # link smart_access_lib-linux-x86_64-gcc_9 to the node target_link_libraries(smartmicro_radar_node @@ -99,7 +101,8 @@ 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) + umrr9d_t152_automotivev1.0.2_user_interface + umrr9d_t152_automotivev1.2.0_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp index dd5a7b2..16849a5 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 @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -150,11 +151,27 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// @param[in] client_id The client_id of the sensor /// - void targetlist_callback_umrr9d( + 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_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_v1_2_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_2_0::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9d_v1_2_0, const com::types::ClientId client_id); /// @@ -232,7 +249,8 @@ 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_umrr9d_v1_0_2{}; +std::shared_ptr data_umrr9d_v1_2_0{}; } // namespace radar } // namespace drivers diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index ddc6fc6..b5917f7 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include #include #include @@ -147,7 +149,8 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option 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_umrr9d_v1_0_2 = com::master::umrr9d_t152_automotive_v1_0_2::DataStreamServiceIface::Get(); + data_umrr9d_v1_2_0 = com::master::umrr9d_t152_automotive_v1_2_0::DataStreamServiceIface::Get(); RCLCPP_INFO(this->get_logger(), "Data stream services have been received!"); // Wait init time @@ -192,11 +195,20 @@ 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 == "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_0" && + com::types::ERROR_CODE_OK != + data_umrr9d_v1_2_0->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_0, this, i, std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr9d" << std::endl; } @@ -636,11 +648,11 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_0_0( } } -void SmartmicroRadarNode::targetlist_callback_umrr9d( +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; @@ -648,7 +660,43 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d( 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_0( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_2_0::comtargetlistport::ComTargetListPort> & + targetlist_port_umrr9d_v1_2_0, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrr9d" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrr9d_t152_automotive_v1_2_0::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrr9d_v1_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()}; @@ -656,7 +704,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_0->GetTargetList()) { const auto range = target->GetRange(); const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); From b945579cd9bd82af0f8f290674e1ea2e92a68e71 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 19 Jun 2023 12:39:53 +0200 Subject: [PATCH 02/16] Set library value --- umrr_ros2_driver/CMakeLists.txt | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index 2079c83..c0dc734 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -45,20 +45,22 @@ 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/lib-linux-x86_64-gcc_9/libumrr9d_t152_automotivev1.2.0_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}/libumrr9d_t152_automotivev1.0.2_user_interface.so" + "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.2.0_user_interface.so" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") From 06321596e72850f250dc1e800de1a7222ab75c82 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Wed, 21 Jun 2023 11:28:33 +0200 Subject: [PATCH 03/16] Added A4 and T152 new interfaces --- umrr_ros2_driver/CMakeLists.txt | 9 +- .../smartmicro_radar_node.hpp | 28 +++++-- umrr_ros2_driver/param/radar.template.yaml | 82 +++++++++---------- .../src/smartmicro_radar_node.cpp | 80 ++++++++++++++---- 4 files changed, 135 insertions(+), 64 deletions(-) diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index c0dc734..d72f219 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -60,7 +60,8 @@ install(FILES "${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}/libumrr9d_t152_automotivev1.0.2_user_interface.so" - "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.2.0_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_automotivev0.5.0_user_interface.so" DESTINATION lib) set(LIB_PATH "${CMAKE_INSTALL_PREFIX}/lib") @@ -91,7 +92,8 @@ 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/umrr9d_t152_automotive_v1_2_0) +smartmicro/include/umrr9d_t152_automotive_v1_2_1 +smartmicro/include/umrra4_automotive_v0_5_0) # link smart_access_lib-linux-x86_64-gcc_9 to the node target_link_libraries(smartmicro_radar_node @@ -104,7 +106,8 @@ target_link_libraries(smartmicro_radar_node umrr9f_t169_automotivev1.1.1_user_interface umrr9f_t169_automotivev2.0.0_user_interface umrr9d_t152_automotivev1.0.2_user_interface - umrr9d_t152_automotivev1.2.0_user_interface) + umrr9d_t152_automotivev1.2.1_user_interface + umrra4_automotivev0.5.0_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp index 16849a5..e9adddd 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,12 +25,13 @@ #include #include +#include #include #include #include #include #include -#include +#include #include #include @@ -167,11 +168,27 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// @param[in] client_id The client_id of the sensor /// - void targetlist_callback_umrr9d_v1_2_0( + void targetlist_callback_umrr9d_v1_2_1( const std::uint32_t sensor_idx, const std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_2_0::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9d_v1_2_0, + 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 + /// umrr9d_v1_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_umrra4( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra4automotive_v0_5_0::comtargetlistport::ComTargetListPort> & + targetlist_port_umrra4, const com::types::ClientId client_id); /// @@ -245,12 +262,13 @@ void terminate_on_receive(int signal); bool check_signal = false; std::shared_ptr m_services{}; +std::shared_ptr data_umrra4{}; 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_v1_0_2{}; -std::shared_ptr data_umrr9d_v1_2_0{}; +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..7965e3a 100644 --- a/umrr_ros2_driver/param/radar.template.yaml +++ b/umrr_ros2_driver/param/radar.template.yaml @@ -16,53 +16,53 @@ sensor_0: # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, # umrr9f_v1_1_1, umrr9f_v2_0_0 - model: "umrr11" + model: "umrr9d_v1_2_1" # The client_id of the sensor/source, must be a unique integer. id: 100 # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.101" - # The port to be used. - port: 55555 - # The frame_id to be set to the published messages. - frame_id: "umrr" - # Specify the history size. - history_size: 10 - sensor_1: - # The model of the connected sensor. - model: "umrr96" - # The client_id of the sensor/source, must be a unique integer. - id: 200 - # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.102" - # The port to be used. - port: 55555 - # The frame_id to be set to the published messages. - frame_id: "umrr" - # Specify the history size. - history_size: 10 - sensor_2: - # The model of the connected sensor. - model: "umrr9f_v1_1_1" - # The client_id of the sensor/source, must be a unique integer. - id: 300 - # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.103" - # The port to be used. - port: 55555 - # The frame_id to be set to the published messages. - frame_id: "umrr" - # Specify the history size. - history_size: 10 - sensor_3: - # The model of the connected sensor. - model: "umrr9d" - # The client_id of the sensor/source, must be a unique integer. - id: 400 - # The ip address of the sensor or of the source acting as a sensor. - ip: "172.22.10.104" + ip: "192.168.11.11" # The port to be used. port: 55555 # The frame_id to be set to the published messages. frame_id: "umrr" # Specify the history size. history_size: 10 + #sensor_1: + # # The model of the connected sensor. + # model: "umrr96" + # # The client_id of the sensor/source, must be a unique integer. + # id: 200 + # # The ip address of the sensor or of the source acting as a sensor. + # ip: "172.22.10.102" + # # The port to be used. + # port: 55555 + # # The frame_id to be set to the published messages. + # frame_id: "umrr" + # # Specify the history size. + # history_size: 10 + #sensor_2: + # # The model of the connected sensor. + # model: "umrr9f_v1_1_1" + # # The client_id of the sensor/source, must be a unique integer. + # id: 300 + # # The ip address of the sensor or of the source acting as a sensor. + # ip: "172.22.10.103" + # # The port to be used. + # port: 55555 + # # The frame_id to be set to the published messages. + # frame_id: "umrr" + # # Specify the history size. + # history_size: 10 + #sensor_3: + # # The model of the connected sensor. + # model: "umrr9d" + # # The client_id of the sensor/source, must be a unique integer. + # id: 400 + # # The ip address of the sensor or of the source acting as a sensor. + # ip: "172.22.10.104" + # # The port to be used. + # port: 55555 + # # The frame_id to be set to the published messages. + # frame_id: "umrr" + # # Specify the history size. + # history_size: 10 diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index b5917f7..197d899 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -19,14 +19,16 @@ #include #include +#include +#include #include #include #include #include #include #include -#include -#include +#include +#include #include #include #include @@ -145,12 +147,13 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option } // Getting the data stream service + data_umrra4 = com::master::umrra4_automotive_v0_5_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_v1_0_2 = com::master::umrr9d_t152_automotive_v1_0_2::DataStreamServiceIface::Get(); - data_umrr9d_v1_2_0 = com::master::umrr9d_t152_automotive_v1_2_0::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 @@ -158,6 +161,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" && + com::types::ERROR_CODE_OK != + data_umrra4->RegisterComTargetListPortReceiveCallback( + sensor.id, std::bind( + &SmartmicroRadarNode::targetlist_callback_umrra4, this, i, + std::placeholders::_1, std::placeholders::_2))) { + std::cout << "Failed to register targetlist callback for sensor umrra4" << std::endl; + } if ( sensor.model == "umrr11" && com::types::ERROR_CODE_OK != @@ -204,17 +220,15 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option std::cout << "Failed to register targetlist callback for sensor umrr9d" << std::endl; } if ( - sensor.model == "umrr9d_v1_2_0" && + sensor.model == "umrr9d_v1_2_1" && com::types::ERROR_CODE_OK != - data_umrr9d_v1_2_0->RegisterComTargetListPortReceiveCallback( + data_umrr9d_v1_2_1->RegisterComTargetListPortReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_0, this, i, + &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 @@ -504,6 +518,42 @@ void SmartmicroRadarNode::command_response( } } +void SmartmicroRadarNode::targetlist_callback_umrra4( + const std::uint32_t sensor_idx, + const std::shared_ptr< + com::master::umrra4_automotive_v0_5_0::comtargetlistport::ComTargetListPort> & + targetlist_port_umrra4, + const com::types::ClientId client_id) +{ + std::cout << "Targetlist callback is being called for umrra4" << std::endl; + if (!check_signal) { + std::shared_ptr< + com::master::umrra4_automotive_v0_5_0::comtargetlistport::GenericPortHeader> + port_header; + port_header = targetlist_port_umrra4->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->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< @@ -684,19 +734,19 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_2( } } -void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_0( +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_0::comtargetlistport::ComTargetListPort> & - targetlist_port_umrr9d_v1_2_0, + 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" << std::endl; if (!check_signal) { std::shared_ptr< - com::master::umrr9d_t152_automotive_v1_2_0::comtargetlistport::GenericPortHeader> + com::master::umrr9d_t152_automotive_v1_2_1::comtargetlistport::GenericPortHeader> port_header; - port_header = targetlist_port_umrr9d_v1_2_0->GetGenericPortHeader(); + 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()}; @@ -704,7 +754,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_0( const auto nanosec = std::chrono::duration_cast(timestamp - sec); msg.header.stamp.sec = sec.count(); msg.header.stamp.nanosec = nanosec.count(); - for (const auto & target : targetlist_port_umrr9d_v1_2_0->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); From 3bb2e708fa2035118ffe99db1a09a4353de43c6c Mon Sep 17 00:00:00 2001 From: smartSRA Date: Fri, 23 Jun 2023 14:49:39 +0200 Subject: [PATCH 04/16] Adjust sesnor naming --- .../include/umrr_ros2_driver/smartmicro_radar_node.hpp | 2 +- umrr_ros2_driver/param/radar.template.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) 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 e9adddd..e700c95 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 @@ -187,7 +187,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node void targetlist_callback_umrra4( const std::uint32_t sensor_idx, const std::shared_ptr< - com::master::umrra4automotive_v0_5_0::comtargetlistport::ComTargetListPort> & + com::master::umrra4_automotive_v0_5_0::comtargetlistport::ComTargetListPort> & targetlist_port_umrra4, const com::types::ClientId client_id); diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml index 7965e3a..2706e36 100644 --- a/umrr_ros2_driver/param/radar.template.yaml +++ b/umrr_ros2_driver/param/radar.template.yaml @@ -8,7 +8,7 @@ # This sets the port in the hw_inventory.json hw_port: 55555 # This sets the iface_name in the hw_inventory.json - hw_iface_name: "eth0" + hw_iface_name: "enx002432163c8d" # An array of sensors to subscribe to. sensors: # As many as 10 sensors all named as "sensor_" in increasing order of numbers, @@ -16,7 +16,7 @@ sensor_0: # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, # umrr9f_v1_1_1, umrr9f_v2_0_0 - model: "umrr9d_v1_2_1" + model: "umrra4" # The client_id of the sensor/source, must be a unique integer. id: 100 # The ip address of the sensor or of the source acting as a sensor. From 71b7c2884e9d8a051f19d57dc10caab12cd283c6 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Tue, 27 Jun 2023 14:34:18 +0200 Subject: [PATCH 05/16] Included extra commands and params --- umrr_ros2_driver/config/sensor_commands.json | 29 ++++++- umrr_ros2_driver/config/sensor_params.json | 30 +++++++ umrr_ros2_driver/param/radar.template.yaml | 84 ++++++++++---------- 3 files changed, 98 insertions(+), 45 deletions(-) diff --git a/umrr_ros2_driver/config/sensor_commands.json b/umrr_ros2_driver/config/sensor_commands.json index f8a9919..2561e88 100644 --- a/umrr_ros2_driver/config/sensor_commands.json +++ b/umrr_ros2_driver/config/sensor_commands.json @@ -3,15 +3,38 @@ [ { "name": "comp_eeprom_ctrl_save_param_sec", - "comment": "Save the parameter inside the EEPROM. (3102.3)" + "comment": "Save the parameter inside the EEPROM." }, { "name": "comp_eeprom_ctrl_reset_param_sec", - "comment": "Restore default values in RAM. EEPROM content is not changed. (3102.2)" + "comment": "Restore default values in RAM. EEPROM content is not changed." }, { "name": "comp_eeprom_ctrl_default_param_sec", - "comment": "Restore default values in RAM and EEPROM. (3102.1)" + "comment": "Restore default values in RAM and EEPROM." + }, + { "name": "comp_fsm_core0_opmode", + "comment": "Select top level FSM operation mode." + }, + { + "name": "comp_sensor_reset_delayed_app_start", + "comment": "Perform a device reset and stay the given value [seconds] in the bootloader at next startup." + }, + { + "name": "comp_fsm_core0_opmode", + "comment": "Select top level FSM operation mode." + }, + { + "name": "comp_eeprom_ctrl_factory_reset", + "comment": "Performs factory reset." + }, + { + "name": "comp_sensor_reset", + "comment": "Reset command which starts from BIOS (if available) or bootloader." + }, + { + "name": "comp_pdi_requestor_can", + "comment": "Send PDI data to client." } ] } diff --git a/umrr_ros2_driver/config/sensor_params.json b/umrr_ros2_driver/config/sensor_params.json index c930190..97ed66d 100644 --- a/umrr_ros2_driver/config/sensor_params.json +++ b/umrr_ros2_driver/config/sensor_params.json @@ -334,6 +334,36 @@ } ] }, + { + "name": "output_control_target_list_can", + "comment": "send raw targets via CAN, 0 = disabled, 1 = enabled", + "sensors": [ + { + "model": "umrr9f", + "min": 0, + "max": 1, + "default": 1 + }, + { + "model": "umrr9d", + "min": 0, + "max": 1, + "default": 0 + }, + { + "model": "umrr96", + "min": 0, + "max": 1, + "default": 0 + }, + { + "model": "umrr11", + "min": 0, + "max": 1, + "default": 0 + } + ] + }, { "name": "ip_source_address", "comment": "IP source address (32bit)", diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml index 2706e36..8678d03 100644 --- a/umrr_ros2_driver/param/radar.template.yaml +++ b/umrr_ros2_driver/param/radar.template.yaml @@ -8,7 +8,7 @@ # This sets the port in the hw_inventory.json hw_port: 55555 # This sets the iface_name in the hw_inventory.json - hw_iface_name: "enx002432163c8d" + hw_iface_name: "eth0" # An array of sensors to subscribe to. sensors: # As many as 10 sensors all named as "sensor_" in increasing order of numbers, @@ -16,53 +16,53 @@ sensor_0: # The model of the connected sensor. Available models umrr11, umrr9d, umrr96, # umrr9f_v1_1_1, umrr9f_v2_0_0 - model: "umrra4" + model: "umrr11" # The client_id of the sensor/source, must be a unique integer. id: 100 # The ip address of the sensor or of the source acting as a sensor. - ip: "192.168.11.11" + ip: "172.22.10.101" # The port to be used. port: 55555 # The frame_id to be set to the published messages. frame_id: "umrr" # Specify the history size. history_size: 10 - #sensor_1: - # # The model of the connected sensor. - # model: "umrr96" - # # The client_id of the sensor/source, must be a unique integer. - # id: 200 - # # The ip address of the sensor or of the source acting as a sensor. - # ip: "172.22.10.102" - # # The port to be used. - # port: 55555 - # # The frame_id to be set to the published messages. - # frame_id: "umrr" - # # Specify the history size. - # history_size: 10 - #sensor_2: - # # The model of the connected sensor. - # model: "umrr9f_v1_1_1" - # # The client_id of the sensor/source, must be a unique integer. - # id: 300 - # # The ip address of the sensor or of the source acting as a sensor. - # ip: "172.22.10.103" - # # The port to be used. - # port: 55555 - # # The frame_id to be set to the published messages. - # frame_id: "umrr" - # # Specify the history size. - # history_size: 10 - #sensor_3: - # # The model of the connected sensor. - # model: "umrr9d" - # # The client_id of the sensor/source, must be a unique integer. - # id: 400 - # # The ip address of the sensor or of the source acting as a sensor. - # ip: "172.22.10.104" - # # The port to be used. - # port: 55555 - # # The frame_id to be set to the published messages. - # frame_id: "umrr" - # # Specify the history size. - # history_size: 10 + sensor_1: + # The model of the connected sensor. + model: "umrr96" + # The client_id of the sensor/source, must be a unique integer. + id: 200 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.102" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + sensor_2: + # The model of the connected sensor. + model: "umrr9f_v1_1_1" + # The client_id of the sensor/source, must be a unique integer. + id: 300 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.103" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 + sensor_3: + # The model of the connected sensor. + model: "umrr9d" + # The client_id of the sensor/source, must be a unique integer. + id: 400 + # The ip address of the sensor or of the source acting as a sensor. + ip: "172.22.10.104" + # The port to be used. + port: 55555 + # The frame_id to be set to the published messages. + frame_id: "umrr" + # Specify the history size. + history_size: 10 \ No newline at end of file From 3494a8046df4ac532a84bc9d17c20577e0c52232 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Tue, 11 Jul 2023 10:24:51 +0200 Subject: [PATCH 06/16] Added modes for a4 --- umrr_ros2_driver/config/sensor_params.json | 105 +++++++++++++++++++++ 1 file changed, 105 insertions(+) diff --git a/umrr_ros2_driver/config/sensor_params.json b/umrr_ros2_driver/config/sensor_params.json index 97ed66d..c5d2627 100644 --- a/umrr_ros2_driver/config/sensor_params.json +++ b/umrr_ros2_driver/config/sensor_params.json @@ -51,6 +51,12 @@ "min": 0, "max": 3, "default": 1 + }, + { + "model": "umrra4", + "min": 0, + "max": 3, + "default": 1 } ] }, @@ -84,6 +90,13 @@ "min": 0, "max": 1, "default": 0 + }, + { + "model": "umrra4", + "comment": "0=medium-range, 1=long-range, 2=X long range, 3=X medium range, 4=short range", + "min": 0, + "max": 4, + "default": 0 } ] }, @@ -135,6 +148,13 @@ "min": 0, "max": 1, "default": 0 + }, + { + "model": "umrra4", + "comment": " 0=off, 1=X long-X medium, 2=X long-short, 3=X medium-short", + "min": 0, + "max": 3, + "default": 0 } ] }, @@ -153,6 +173,12 @@ "min": 0, "max": 2, "default": 1 + }, + { + "model": "umrra4", + "min": 0, + "max": 2, + "default": 1 } ] }, @@ -177,6 +203,12 @@ "min": -112.0, "max": 56.0, "default": -112.0 + }, + { + "model": "umrra4", + "min": -112.0, + "max": 56.0, + "default": -112.0 } ] }, @@ -201,6 +233,12 @@ "min": -112.0, "max": 56.0, "default": -112.0 + }, + { + "model": "umrra4", + "min": -112.0, + "max": 56.0, + "default": -112.0 } ] }, @@ -225,6 +263,12 @@ "min": -112.0, "max": 56.0, "default": -112.0 + }, + { + "model": "umrra4", + "min": -112.0, + "max": 56.0, + "default": -112.0 } ] }, @@ -242,6 +286,24 @@ "min": -112.0, "max": 56.0, "default": -112.0 + }, + { + "model": "umrra4", + "min": -112.0, + "max": 56.0, + "default": -112.0 + } + ] + }, + { + "name": "tv_min_speed_sweep_idx_4", + "comment": "Target Validation: minimum speed of target at fourth sweep", + "sensors": [ + { + "model": "umrra4", + "min": -112.0, + "max": 56.0, + "default": -112.0 } ] }, @@ -266,6 +328,12 @@ "min": -112.0, "max": 56.0, "default": 56.0 + }, + { + "model": "umrra4", + "min": -112.0, + "max": 56.0, + "default": 56.0 } ] }, @@ -290,6 +358,12 @@ "min": -112.0, "max": 56.0, "default": 56.0 + }, + { + "model": "umrra4", + "min": -112.0, + "max": 56.0, + "default": 56.0 } ] }, @@ -314,6 +388,12 @@ "min": -112.0, "max": 56.0, "default": 56.0 + }, + { + "model": "umrra4", + "min": -112.0, + "max": 56.0, + "default": 56.0 } ] }, @@ -331,6 +411,24 @@ "min": -112.0, "max": 56.0, "default": 56.0 + }, + { + "model": "umrra4", + "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": "umrra4", + "min": -112.0, + "max": 56.0, + "default": 56.0 } ] }, @@ -362,6 +460,13 @@ "max": 1, "default": 0 } + , + { + "model": "umrra4", + "min": 0, + "max": 1, + "default": 1 + } ] }, { From 7d66b131ab89dccdda06d806f13fb7325fd092ee Mon Sep 17 00:00:00 2001 From: smartSRA Date: Wed, 19 Jul 2023 15:55:34 +0200 Subject: [PATCH 07/16] updated user-interface version for A4 --- Readme.md | 3 +++ umrr_ros2_driver/CMakeLists.txt | 6 +++--- .../include/umrr_ros2_driver/smartmicro_radar_node.hpp | 6 +++--- umrr_ros2_driver/param/radar.template.yaml | 4 ++-- umrr_ros2_driver/src/smartmicro_radar_node.cpp | 10 +++++----- 5 files changed, 16 insertions(+), 13 deletions(-) diff --git a/Readme.md b/Readme.md index 8976d81..0f60922 100644 --- a/Readme.md +++ b/Readme.md @@ -33,6 +33,8 @@ sure the version used to publish the data is compatible with this version: - User interface version: `UMRR9F Type 169 AUTOMOTIVE v1.1.1` - User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.0.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 +43,7 @@ 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 +- 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 diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index d72f219..4b5b3c3 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -61,7 +61,7 @@ install(FILES "${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.0.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_automotivev0.5.0_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") @@ -93,7 +93,7 @@ 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/umrr9d_t152_automotive_v1_2_1 -smartmicro/include/umrra4_automotive_v0_5_0) +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 @@ -107,7 +107,7 @@ target_link_libraries(smartmicro_radar_node umrr9f_t169_automotivev2.0.0_user_interface umrr9d_t152_automotivev1.0.2_user_interface umrr9d_t152_automotivev1.2.1_user_interface - umrra4_automotivev0.5.0_user_interface) + umrra4_automotivev1.0.0_user_interface) rclcpp_components_register_node(smartmicro_radar_node PLUGIN "smartmicro::drivers::radar::SmartmicroRadarNode" diff --git a/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp index e700c95..8df3a2f 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,7 +25,7 @@ #include #include -#include +#include #include #include #include @@ -187,7 +187,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node void targetlist_callback_umrra4( const std::uint32_t sensor_idx, const std::shared_ptr< - com::master::umrra4_automotive_v0_5_0::comtargetlistport::ComTargetListPort> & + com::master::umrra4_automotive_v1_0_0::comtargetlistport::ComTargetListPort> & targetlist_port_umrra4, const com::types::ClientId client_id); @@ -262,7 +262,7 @@ void terminate_on_receive(int signal); bool check_signal = false; std::shared_ptr m_services{}; -std::shared_ptr data_umrra4{}; +std::shared_ptr data_umrra4{}; std::shared_ptr data_umrr11{}; std::shared_ptr data_umrr96{}; std::shared_ptr data_umrr9f_v1_1_1{}; diff --git a/umrr_ros2_driver/param/radar.template.yaml b/umrr_ros2_driver/param/radar.template.yaml index 8678d03..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. @@ -65,4 +65,4 @@ # The frame_id to be set to the published messages. frame_id: "umrr" # Specify the history size. - history_size: 10 \ No newline at end of file + history_size: 10 diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 197d899..d24edb7 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -147,7 +147,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option } // Getting the data stream service - data_umrra4 = com::master::umrra4_automotive_v0_5_0::DataStreamServiceIface::Get(); + data_umrra4 = 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(); @@ -521,14 +521,14 @@ void SmartmicroRadarNode::command_response( void SmartmicroRadarNode::targetlist_callback_umrra4( const std::uint32_t sensor_idx, const std::shared_ptr< - com::master::umrra4_automotive_v0_5_0::comtargetlistport::ComTargetListPort> & + com::master::umrra4_automotive_v1_0_0::comtargetlistport::ComTargetListPort> & targetlist_port_umrra4, const com::types::ClientId client_id) { std::cout << "Targetlist callback is being called for umrra4" << std::endl; if (!check_signal) { std::shared_ptr< - com::master::umrra4_automotive_v0_5_0::comtargetlistport::GenericPortHeader> + com::master::umrra4_automotive_v1_0_0::comtargetlistport::GenericPortHeader> port_header; port_header = targetlist_port_umrra4->GetGenericPortHeader(); sensor_msgs::msg::PointCloud2 msg; From 504ec0e4e20266ad77bf798fa208b88f2b576c69 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Thu, 27 Jul 2023 08:59:47 +0200 Subject: [PATCH 08/16] Support new interface for T169 --- umrr_ros2_driver/CMakeLists.txt | 7 ++- .../smartmicro_radar_node.hpp | 27 ++++++++-- .../src/smartmicro_radar_node.cpp | 51 +++++++++++++++++-- .../test/radar_node_test.launch.py | 1 + .../test/test_smartmicro_radar_node.cpp | 27 ++++++++-- 5 files changed, 96 insertions(+), 17 deletions(-) diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index 4b5b3c3..841ea44 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -59,6 +59,7 @@ install(FILES "${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" @@ -91,6 +92,7 @@ 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/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) @@ -105,6 +107,7 @@ 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 + 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) @@ -118,10 +121,6 @@ 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/include/umrr_ros2_driver/smartmicro_radar_node.hpp b/umrr_ros2_driver/include/umrr_ros2_driver/smartmicro_radar_node.hpp index 8df3a2f..5701c4b 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 @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -114,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 @@ -129,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 @@ -145,7 +146,23 @@ 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_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 @@ -161,7 +178,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// /// @brief A callback that is called when a new target list port for - /// umrr9d_v1_2_0 arrives. + /// 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 @@ -177,7 +194,7 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// /// @brief A callback that is called when a new target list port for - /// umrr9d_v1_2_0 arrives. + /// umrra4 arrives. /// /// @param[in] sensor_idx The sensor id for respective published topic. /// @param[in] target_list_port The target list port diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index d24edb7..3421a4e 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -152,6 +152,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option 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_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(); @@ -210,6 +211,15 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr9f_v2_0_0" << std::endl; } + if ( + sensor.model == "umrr9f_v2_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 != @@ -228,7 +238,6 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option std::placeholders::_1, std::placeholders::_2))) { std::cout << "Failed to register targetlist callback for sensor umrr9d" << std::endl; } - } // create a ros2 service to change the radar parameters @@ -698,6 +707,42 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f_v2_0_0( } } +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< @@ -705,7 +750,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_0_2( 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> @@ -741,7 +786,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d_v1_2_1( targetlist_port_umrr9d_v1_2_1, 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_2_1" << std::endl; if (!check_signal) { std::shared_ptr< com::master::umrr9d_t152_automotive_v1_2_1::comtargetlistport::GenericPortHeader> diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index 230a3b2..5a1fddb 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -114,6 +114,7 @@ def setUpClass(cls): @classmethod def tearDownClass(cls): # Shutdown the ROS context + print("Shutdown node") rclpy.shutdown() def setUp(self): diff --git a/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp b/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp index 7f61f77..7bc6c82 100644 --- a/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp @@ -19,23 +19,40 @@ using smartmicro::drivers::radar::SmartmicroRadarNode; +class SmartmicroRadarNodeTest : public ::testing::Test { +protected: + void TearDown() override { + smartmicro::drivers::radar::check_signal = true; + rclcpp::Rate sleepRate(std::chrono::seconds(5)); + sleepRate.sleep(); + std::cout << "SHUTDOWN PROCEDURE" << std::endl; + rclcpp::shutdown(); + } +}; + /// @test Test that if we publish one message, it generates a state estimate which is sent out. -TEST(SmartmicroRadarNodeTest, Create) +TEST_F(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("master_client_id", 11); 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.id", 22); 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)); + EXPECT_TRUE(node != nullptr); + smartmicro::drivers::radar::check_signal = true; + rclcpp::Rate sleepRate(std::chrono::seconds(10)); sleepRate.sleep(); rclcpp::shutdown(); } + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 0d364e47ffc27a788155e7d242d4e4fadc817ac9 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Fri, 28 Jul 2023 10:26:41 +0200 Subject: [PATCH 09/16] Remove gtest We already have an integration test based on ros launch testing. Both tests are similar and hence we could just stick to one. --- umrr_ros2_driver/CMakeLists.txt | 2 +- .../smartmicro_radar_node.hpp | 1 + .../test/test_smartmicro_radar_node.cpp | 58 ------------------- 3 files changed, 2 insertions(+), 59 deletions(-) delete mode 100644 umrr_ros2_driver/test/test_smartmicro_radar_node.cpp diff --git a/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index 841ea44..30e1b03 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -120,7 +120,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") - + # Not applying ros2 linters on external sources list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright 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 5701c4b..a185295 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 @@ -284,6 +284,7 @@ std::shared_ptr data_umrr96{}; std::shared_ptr data_umrr9f_v1_1_1{}; std::shared_ptr data_umrr9f_v2_0_0{}; +std::shared_ptr data_umrr9f_v2_2_0{}; std::shared_ptr data_umrr9d_v1_0_2{}; std::shared_ptr data_umrr9d_v1_2_1{}; 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 7bc6c82..0000000 --- a/umrr_ros2_driver/test/test_smartmicro_radar_node.cpp +++ /dev/null @@ -1,58 +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; - -class SmartmicroRadarNodeTest : public ::testing::Test { -protected: - void TearDown() override { - smartmicro::drivers::radar::check_signal = true; - rclcpp::Rate sleepRate(std::chrono::seconds(5)); - sleepRate.sleep(); - std::cout << "SHUTDOWN PROCEDURE" << std::endl; - rclcpp::shutdown(); - } -}; - -/// @test Test that if we publish one message, it generates a state estimate which is sent out. -TEST_F(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", 11); - 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", 22); - 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); - EXPECT_TRUE(node != nullptr); - smartmicro::drivers::radar::check_signal = true; - rclcpp::Rate sleepRate(std::chrono::seconds(10)); - sleepRate.sleep(); - rclcpp::shutdown(); -} - -int main(int argc, char** argv) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From 18f54bdc9d065c9a0fb21d8f53d56e22cbae1931 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Fri, 28 Jul 2023 11:40:21 +0200 Subject: [PATCH 10/16] Remove mode and command files The binaries include a user_interface directory which should be now directly used. Updated Readme to the new changes. --- Readme.md | 17 +- umrr_ros2_driver/CMakeLists.txt | 8 - umrr_ros2_driver/config/sensor_commands.json | 40 -- umrr_ros2_driver/config/sensor_params.json | 478 ------------------ .../src/smartmicro_radar_node.cpp | 42 -- 5 files changed, 8 insertions(+), 577 deletions(-) delete mode 100644 umrr_ros2_driver/config/sensor_commands.json delete mode 100644 umrr_ros2_driver/config/sensor_params.json diff --git a/Readme.md b/Readme.md index 0f60922..7f72bde 100644 --- a/Readme.md +++ b/Readme.md @@ -81,7 +81,7 @@ For more details, see the [`radar.template.yaml`](umrr_ros2_driver/param/radar.t ## 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) @@ -91,6 +91,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: @@ -111,15 +118,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/umrr_ros2_driver/CMakeLists.txt b/umrr_ros2_driver/CMakeLists.txt index 30e1b03..d6680e8 100644 --- a/umrr_ros2_driver/CMakeLists.txt +++ b/umrr_ros2_driver/CMakeLists.txt @@ -70,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") diff --git a/umrr_ros2_driver/config/sensor_commands.json b/umrr_ros2_driver/config/sensor_commands.json deleted file mode 100644 index 2561e88..0000000 --- a/umrr_ros2_driver/config/sensor_commands.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "commands": - [ - { - "name": "comp_eeprom_ctrl_save_param_sec", - "comment": "Save the parameter inside the EEPROM." - }, - { - "name": "comp_eeprom_ctrl_reset_param_sec", - "comment": "Restore default values in RAM. EEPROM content is not changed." - }, - { - "name": "comp_eeprom_ctrl_default_param_sec", - "comment": "Restore default values in RAM and EEPROM." - }, - { "name": "comp_fsm_core0_opmode", - "comment": "Select top level FSM operation mode." - }, - { - "name": "comp_sensor_reset_delayed_app_start", - "comment": "Perform a device reset and stay the given value [seconds] in the bootloader at next startup." - }, - { - "name": "comp_fsm_core0_opmode", - "comment": "Select top level FSM operation mode." - }, - { - "name": "comp_eeprom_ctrl_factory_reset", - "comment": "Performs factory reset." - }, - { - "name": "comp_sensor_reset", - "comment": "Reset command which starts from BIOS (if available) or bootloader." - }, - { - "name": "comp_pdi_requestor_can", - "comment": "Send PDI data to client." - } - ] -} diff --git a/umrr_ros2_driver/config/sensor_params.json b/umrr_ros2_driver/config/sensor_params.json deleted file mode 100644 index c5d2627..0000000 --- a/umrr_ros2_driver/config/sensor_params.json +++ /dev/null @@ -1,478 +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 - }, - { - "model": "umrra4", - "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 - }, - { - "model": "umrra4", - "comment": "0=medium-range, 1=long-range, 2=X long range, 3=X medium range, 4=short range", - "min": 0, - "max": 4, - "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 - }, - { - "model": "umrra4", - "comment": " 0=off, 1=X long-X medium, 2=X long-short, 3=X medium-short", - "min": 0, - "max": 3, - "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 - }, - { - "model": "umrra4", - "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 - }, - { - "model": "umrra4", - "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 - }, - { - "model": "umrra4", - "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 - }, - { - "model": "umrra4", - "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 - }, - { - "model": "umrra4", - "min": -112.0, - "max": 56.0, - "default": -112.0 - } - ] - }, - { - "name": "tv_min_speed_sweep_idx_4", - "comment": "Target Validation: minimum speed of target at fourth sweep", - "sensors": [ - { - "model": "umrra4", - "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 - }, - { - "model": "umrra4", - "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 - }, - { - "model": "umrra4", - "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 - }, - { - "model": "umrra4", - "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 - }, - { - "model": "umrra4", - "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": "umrra4", - "min": -112.0, - "max": 56.0, - "default": 56.0 - } - ] - }, - { - "name": "output_control_target_list_can", - "comment": "send raw targets via CAN, 0 = disabled, 1 = enabled", - "sensors": [ - { - "model": "umrr9f", - "min": 0, - "max": 1, - "default": 1 - }, - { - "model": "umrr9d", - "min": 0, - "max": 1, - "default": 0 - }, - { - "model": "umrr96", - "min": 0, - "max": 1, - "default": 0 - }, - { - "model": "umrr11", - "min": 0, - "max": 1, - "default": 0 - } - , - { - "model": "umrra4", - "min": 0, - "max": 1, - "default": 1 - } - ] - }, - { - "name": "ip_source_address", - "comment": "IP source address (32bit)", - "default": 3232238347 - } - ] -} diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index 3421a4e..d152e04 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -51,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; @@ -276,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; @@ -409,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; From a069b29c49086d955bd8714495a1741788c82800 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Tue, 1 Aug 2023 14:46:33 +0200 Subject: [PATCH 11/16] Update sensor types in Readme Also update the smart access binary release to be used by the driver --- Readme.md | 8 +++++--- smart_extract.sh | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Readme.md b/Readme.md index 7f72bde..220bfc1 100644 --- a/Readme.md +++ b/Readme.md @@ -22,16 +22,17 @@ 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, UMRRA4, 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` +- Smart Access Automotive version: `v3.1.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` @@ -42,7 +43,8 @@ This ROS2 driver release is compatible with the following sensor firmwares: - UMRR96 Type 153: V5.2.4 - UMRR9D Type 152: V2.1.0 - UMRR9F Type 169: V1.3.0 -- UMRR9F Type 169: V2.0.2 +- UMRR9F Type 169: V2.0.1 +- UMRR9F Type 169: V2.2.0 - UMRRA4 Type 171: V1.0.0 ### Point cloud message wrapper library 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 From d65fca13d77fc86f93cec2cdb4317ca97b0f32d9 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Fri, 18 Aug 2023 10:00:56 +0200 Subject: [PATCH 12/16] Update smart release version in Readme --- Readme.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Readme.md b/Readme.md index 220bfc1..3425885 100644 --- a/Readme.md +++ b/Readme.md @@ -27,7 +27,7 @@ required to run this node. This code is bundled with a version of Smart Access A sure the version used to publish the data is compatible with this version: - Date of release: `February 06, 2023` -- Smart Access Automotive version: `v3.1.0` +- 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` From ff0ed590b2f95b42653f91555f6c4a078c12cf16 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Fri, 18 Aug 2023 10:33:47 +0200 Subject: [PATCH 13/16] Use marketing name and add versioning in callbacks --- Readme.md | 8 ++++---- .../smartmicro_radar_node.hpp | 6 +++--- .../src/smartmicro_radar_node.cpp | 20 +++++++++---------- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/Readme.md b/Readme.md index 3425885..711e315 100644 --- a/Readme.md +++ b/Readme.md @@ -22,11 +22,11 @@ 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, UMRRA4, 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` +- Date of release: `August 18, 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` @@ -43,7 +43,7 @@ This ROS2 driver release is compatible with the following sensor firmwares: - UMRR96 Type 153: V5.2.4 - UMRR9D Type 152: V2.1.0 - UMRR9F Type 169: V1.3.0 -- UMRR9F Type 169: V2.0.1 +- UMRR9F Type 169: V2.0.2 - UMRR9F Type 169: V2.2.0 - UMRRA4 Type 171: V1.0.0 @@ -79,7 +79,7 @@ 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 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 a185295..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 @@ -201,11 +201,11 @@ class UMRR_ROS2_DRIVER_PUBLIC SmartmicroRadarNode : public ::rclcpp::Node /// @param[in] client_id The client_id of the sensor /// - void targetlist_callback_umrra4( + 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, + targetlist_port_umrra4_v1_0_0, const com::types::ClientId client_id); /// @@ -279,7 +279,7 @@ void terminate_on_receive(int signal); bool check_signal = false; std::shared_ptr m_services{}; -std::shared_ptr data_umrra4{}; +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{}; diff --git a/umrr_ros2_driver/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index d152e04..42aed5f 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -145,7 +145,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option } // Getting the data stream service - data_umrra4 = com::master::umrra4_automotive_v1_0_0::DataStreamServiceIface::Get(); + 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(); @@ -165,13 +165,13 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option "smart_radar/targets_" + std::to_string(i), sensor.history_size); if ( - sensor.model == "umrra4" && + sensor.model == "umrra4_v1_0_0" && com::types::ERROR_CODE_OK != - data_umrra4->RegisterComTargetListPortReceiveCallback( + data_umrra4_v1_0_0->RegisterComTargetListPortReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrra4, this, i, + &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" << std::endl; + std::cout << "Failed to register targetlist callback for sensor umrra4_v1_0_0" << std::endl; } if ( sensor.model == "umrr11" && @@ -485,19 +485,19 @@ void SmartmicroRadarNode::command_response( } } -void SmartmicroRadarNode::targetlist_callback_umrra4( +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, + targetlist_port_umrra4_v1_0_0, const com::types::ClientId client_id) { - std::cout << "Targetlist callback is being called for umrra4" << std::endl; + 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->GetGenericPortHeader(); + 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()}; @@ -505,7 +505,7 @@ void SmartmicroRadarNode::targetlist_callback_umrra4( 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->GetTargetList()) { + 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); From df120c874fb96789ce7f9b6c71a7df3edad980e6 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Fri, 18 Aug 2023 10:53:58 +0200 Subject: [PATCH 14/16] Remove debug statements --- umrr_ros2_driver/test/radar_node_test.launch.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/umrr_ros2_driver/test/radar_node_test.launch.py b/umrr_ros2_driver/test/radar_node_test.launch.py index 5a1fddb..6fe683f 100644 --- a/umrr_ros2_driver/test/radar_node_test.launch.py +++ b/umrr_ros2_driver/test/radar_node_test.launch.py @@ -108,22 +108,18 @@ class TestSmartNode(unittest.TestCase): @classmethod def setUpClass(cls): # Initialize the ROS context for the test node - print("Initialize") rclpy.init() @classmethod def tearDownClass(cls): # Shutdown the ROS context - print("Shutdown node") rclpy.shutdown() 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): From 94959fa0e20772bd8e256aeaa04bbe61dbecf137 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Fri, 18 Aug 2023 12:17:38 +0200 Subject: [PATCH 15/16] Include log for the upcoming release --- CHANGELOG.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 36c78a0..5b27958 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-18 + +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 whcih are accessed using the ros2 services. \ No newline at end of file From e41ebb9dd0720b21e935bae3224f80c5d70bdd79 Mon Sep 17 00:00:00 2001 From: smartSRA Date: Mon, 21 Aug 2023 11:44:31 +0200 Subject: [PATCH 16/16] Update release date --- CHANGELOG.md | 4 ++-- Readme.md | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5b27958..036247c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -36,6 +36,6 @@ This release fixes the offset which causes anomaly in pointclouds from DRVEGRD 1 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-18 +## 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 whcih are accessed using the ros2 services. \ No newline at end of file +This release includes the new DRVEGRD 171 and DRVEGRD 152 user-interface. The release includes a complete list of all params and commands for all sensors which are accessed using the ros2 services. \ No newline at end of file diff --git a/Readme.md b/Readme.md index 711e315..322f3c9 100644 --- a/Readme.md +++ b/Readme.md @@ -26,7 +26,7 @@ A [smartmicro](https://www.smartmicro.com/automotive-radar) UMRR96, UMRR11, DRVE required to run this node. This code is bundled with a version of Smart Access API. Please make sure the version used to publish the data is compatible with this version: -- Date of release: `August 18, 2023` +- 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`