From dc85dab614d3dab63f2bfeaeacd08739cfe67f11 Mon Sep 17 00:00:00 2001 From: Shahrukh <82996646+smartSRA@users.noreply.github.com> Date: Fri, 11 Nov 2022 14:18:12 +0100 Subject: [PATCH] Introduce signal-to-noise ratio (#24) Fix for max offered sensors and modify print statements --- CHANGELOG.md | 6 +- Readme.md | 2 +- .../smartmicro_radar_node.hpp | 2 +- umrr_ros2_driver/package.xml | 2 +- .../src/smartmicro_radar_node.cpp | 64 ++++++++----------- 5 files changed, 36 insertions(+), 40 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c88caee..a9e0af3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,4 +22,8 @@ Major release includes the new smartmicro sensor DRVEGRD 169. The driver offers ## v3.1.0 - 2022-10-19 -This release includes the new smartmicro sensor DRVEGRD 152. The driver offers mode changes and configuration of the DRVEGRD 152 along with publishing the radar targets as pointcloud data. The callbacks for datastream now require a clientID. \ No newline at end of file +This release includes the new smartmicro sensor DRVEGRD 152. The driver offers mode changes and configuration of the DRVEGRD 152 along with publishing the radar targets as pointcloud data. The callbacks for datastream now require a clientID. + +## v3.2.0 - 2022-11-11 + +This minor release introduces signal-to-noise field in the pointclouds and also fixes the max number of sensors that could be connected at once. \ No newline at end of file diff --git a/Readme.md b/Readme.md index e17d4ae..26a1b96 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: `October 19, 2022` +- Date of release: `November 11, 2022` - Smart Access Automotive version: `v2.1.0` - User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.1` - User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.1` 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 242a15a..e462f66 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 @@ -45,7 +45,7 @@ namespace radar { namespace detail { -constexpr auto kMaxSensorCount = 8UL; +constexpr auto kMaxSensorCount = 10UL; struct SensorConfig { diff --git a/umrr_ros2_driver/package.xml b/umrr_ros2_driver/package.xml index 2695278..4b02a9b 100644 --- a/umrr_ros2_driver/package.xml +++ b/umrr_ros2_driver/package.xml @@ -2,7 +2,7 @@ umrr_ros2_driver - 3.0.0 + 3.2.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/src/smartmicro_radar_node.cpp b/umrr_ros2_driver/src/smartmicro_radar_node.cpp index b92b490..cd8943d 100644 --- a/umrr_ros2_driver/src/smartmicro_radar_node.cpp +++ b/umrr_ros2_driver/src/smartmicro_radar_node.cpp @@ -23,10 +23,10 @@ #include #include #include -#include -#include #include #include +#include +#include #include @@ -37,6 +37,7 @@ #include #include #include + #include #include #include @@ -96,11 +97,12 @@ struct RadarPoint float power{}; float RCS{}; float Noise{}; + float snr{}; constexpr friend bool operator==(const RadarPoint & p1, const RadarPoint & p2) noexcept { return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && float_eq(p1.z, p2.z) && float_eq(p1.radial_speed, p2.radial_speed) && float_eq(p1.power, p2.power) && - float_eq(p1.RCS, p2.RCS) && float_eq(p1.Noise, p2.Noise); + float_eq(p1.RCS, p2.RCS) && float_eq(p1.Noise, p2.Noise) && float_eq(p1.snr, p2.snr); } }; @@ -108,10 +110,11 @@ LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(radial_speed); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(power); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(RCS); LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(Noise); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(snr); using Generators = std::tuple< point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, point_cloud_msg_wrapper::field_z_generator, field_radial_speed_generator, field_RCS_generator, - field_Noise_generator, field_power_generator>; + field_Noise_generator, field_power_generator, field_snr_generator>; using RadarCloudModifier = PointCloud2Modifier; } // namespace @@ -164,7 +167,7 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option 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; + std::cout << "Failed to register targetlist callback for sensor umrr96" << std::endl; } if ( sensor.model == "umrr9f" && @@ -173,17 +176,17 @@ SmartmicroRadarNode::SmartmicroRadarNode(const rclcpp::NodeOptions & node_option 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; + std::cout << "Failed to register targetlist callback for sensor umrr9f" << std::endl; } if ( sensor.model == "umrr9d" && com::types::ERROR_CODE_OK != data_umrr9d->RegisterComTargetListPortReceiveCallback( sensor.id, std::bind( - &SmartmicroRadarNode::targetlist_callback_umrr9d, this, i, - std::placeholders::_1, std::placeholders::_2))) { - std::cout <<"Failed to register targetlist callback for sensor umrr9d"<< std::endl; - } + &SmartmicroRadarNode::targetlist_callback_umrr9d, 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( "umrr/targets_" + std::to_string(i), sensor.history_size); @@ -409,10 +412,11 @@ void SmartmicroRadarNode::targetlist_callback_umrr11( const auto elevation_angle = target->GetElevationAngle(); const auto range_2d = range * std::cos(elevation_angle); const auto azimuth_angle = target->GetAzimuthAngle(); + const auto snr = target->GetPower() - target->GetTgtNoise(); 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->GetPower(), + target->GetRCS(), target->GetTgtNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); @@ -444,10 +448,11 @@ void SmartmicroRadarNode::targetlist_callback_umrr96( 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->GetRCS(), - target->GetTgtNoise(), target->GetPower()}); + range * std::sin(elevation_angle), target->GetSpeedRadial(), target->GetPower(), + target->GetRCS(), target->GetTgtNoise(), snr}); } m_publishers[sensor_idx]->publish(msg); @@ -479,17 +484,11 @@ void SmartmicroRadarNode::targetlist_callback_umrr9f( 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->GetRCS(), - target->GetTgtNoise(), - target->GetPower() - } - ); + {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); @@ -504,7 +503,7 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d( const com::types::ClientId client_id) { std::cout << "Targetlist callback is being called for umrr9d" << std::endl; - if(!check_signal) { + if (!check_signal) { std::shared_ptr< com::master::umrr9d_t152_automotive_v1_0_1::comtargetlistport::GenericPortHeader> port_header; @@ -521,24 +520,17 @@ void SmartmicroRadarNode::targetlist_callback_umrr9d( 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->GetRCS(), - target->GetTgtNoise(), - target->GetPower() - } - ); + {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::update_config_files_from_params() { const auto dev_id = declare_parameter(kDevIdTag, 0);