Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

New interfaces for DRVEGRD 152 and 169 #46

Merged
merged 3 commits into from
Jan 26, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -46,4 +46,8 @@ This release includes CAN communication for all the provided sensor types and se

## v6.0.0 - 2023-12-20

This release includes features for downloading sensor firmware on to the sensors. It also provides custom RVIZ plugins to log the target list data, record and save it. It has a plugin to send instructions to the sensors and also it is possible to dowload the firmware too. This release also includes a python GUI to send custom CAN messages. The radar param templates have now been merged into one param file. The pointcloud has been extended to also include the polar coordinates. Additionally, the release also includes new user interface for sensor A4 T171.
This release includes features for downloading sensor firmware on to the sensors. It also provides custom RVIZ plugins to log the target list data, record and save it. It has a plugin to send instructions to the sensors and also it is possible to dowload the firmware too. This release also includes a python GUI to send custom CAN messages. The radar param templates have now been merged into one param file. The pointcloud has been extended to also include the polar coordinates. Additionally, the release also includes new user interface for sensor A4 T171.

## v6.1.0 - 2024-01-26

This release includes the new user interfaces for DRVEGRD 169 and DRVEGRD 152. This releaase also fixes some issues with the smart record plugin and offers now azimuth and elevation angles in degrees.
9 changes: 7 additions & 2 deletions Readme.md
Original file line number Diff line number Diff line change
Expand Up @@ -42,16 +42,18 @@ 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: `December 20, 2023`
- Smart Access Automotive version: `v3.5.0`
- Date of release: `January 26, 2023`
- Smart Access Automotive version: `v3.7.0`
- User interface version: `UMRR96 Type 153 AUTOMOTIVE v1.2.2`
- User interface version: `UMRR11 Type 132 AUTOMOTIVE v1.1.2`
- User interface version: `UMRR9F Type 169 AUTOMOTIVE v1.1.1`
- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.0.0`
- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.1.1`
- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.2.1`
- User interface version: `UMRR9F Type 169 AUTOMOTIVE v2.5.0`
- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.0.3`
- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.2.2`
- User interface version: `UMRR9D Type 152 AUTOMOTIVE v1.5.0`
- User interface version: `UMRRA4 Type 171 AUTOMOTIVE v1.0.1`
- User interface version: `UMRRA4 Type 171 AUTOMOTIVE v1.3.0`

Expand All @@ -61,11 +63,14 @@ This ROS2 driver release is compatible with the following sensor firmwares:
- UMRR96 Type 153: V5.2.4
- UMRR9D Type 152: V2.1.0
- UMRR9D Type 152: V2.5.0
- UMRR9D Type 152: V2.7.0
- UMRR9F Type 169: V1.3.0
- UMRR9F Type 169: V2.0.2
- UMRR9F Type 169: V2.2.0
- UMRR9F Type 169: V2.4.0
- UMRRA4 Type 171: V1.0.0
- UMRRA4 Type 171: V1.2.1
- UMRRA4 Type 171: V1.3.0

### Point cloud message wrapper library
To add targets to the point cloud in a safe and quick fashion a
Expand Down
2 changes: 1 addition & 1 deletion smart_extract.sh
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#!/bin/bash
set -e

smart_pack=SmartAccessAutomotive_3_5_0.tgz
smart_pack=SmartAccessAutomotive_3_7_0.tgz
URL_smartbinaries=https://www.smartmicro.com/fileadmin/media/Downloads/Automotive_Radar/Software/${smart_pack}

cat << EOF
Expand Down
23 changes: 22 additions & 1 deletion smart_rviz_plugin/include/smart_rviz_plugin/smart_recorder.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include <QTextStream>
#include <QTimer>
#include <QVBoxLayout>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <rviz_common/panel.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
Expand All @@ -20,6 +21,22 @@

namespace smart_rviz_plugin
{

struct RecordedData {
float range;
float power;
float azimuth_deg;
float elevation_deg;
float rcs;
float noise;
float snr;
float radial_speed;
float azimuth_angle;
float elevation_angle;
uint32_t timestamp_sec;
uint32_t timestamp_nanosec;
};

class SmartRadarRecorder : public rviz_common::Panel
{
Q_OBJECT
Expand All @@ -37,6 +54,10 @@ private slots:
private:
void initializeRecorder();
void callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name);
void updateRecordedData(
float range, float power, float azimuth_deg, float elevation_deg, float rcs,
float noise, float snr, float radial_speed, float azimuth_angle,
float elevation_angle, uint32_t timestamp_sec, uint32_t timestamp_nanosec);

private:
QTableWidget * table_data_;
Expand All @@ -52,7 +73,7 @@ private slots:
rclcpp::Node::SharedPtr node_;
std::unordered_map<std::string, rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
subscribers_{};
QVector<QVector<QString>> recorded_data;
std::vector<RecordedData> recorded_data;
std::string selected_topic_;
bool recording_active_{false};
};
Expand Down
125 changes: 75 additions & 50 deletions smart_rviz_plugin/src/smart_recorder.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include "smart_rviz_plugin/smart_recorder.hpp"

#include <cmath>
#include <sensor_msgs/point_cloud2_iterator.hpp>

namespace smart_rviz_plugin
Expand Down Expand Up @@ -35,12 +36,11 @@ void SmartRadarRecorder::initializeRecorder()
connect(topic_dropdown_, SIGNAL(currentIndexChanged(int)), this, SLOT(updateTable()));

table_data_ = new QTableWidget();
table_data_->setColumnCount(8);
table_data_->setColumnCount(10);
table_data_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
table_data_->setHorizontalHeaderLabels(
{"RadialSpeed [m/s]", "Power [dB]", "RCS [dB]", "Noise [dB]", "SNR", "AzimuthAngle [rad]",
"ElevationAngle [rad]", "Range [m]"});

{"Range [m]", "Power [dB]", "AzimuthAngle [Deg]", "ElevationAngle [Deg]", "RCS [dB]",
"Noise [dB]", "SNR [dB]", "RadialSpeed [m/s]", "AzimuthAngle [rad]", "ElevationAngle [rad]"});
table_timestamps_ = new QTableWidget();
table_timestamps_->setColumnCount(2);
table_timestamps_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
Expand Down Expand Up @@ -77,6 +77,28 @@ void SmartRadarRecorder::initializeRecorder()
setLayout(gui_layout_);
}

void SmartRadarRecorder::updateRecordedData(
float range, float power, float azimuth_deg, float elevation_deg, float rcs, float noise,
float snr, float radial_speed, float azimuth_angle, float elevation_angle,
uint32_t timestamp_sec, uint32_t timestamp_nanosec)
{
RecordedData data;
data.range = range;
data.power = power;
data.azimuth_deg = azimuth_deg;
data.elevation_deg = elevation_deg;
data.rcs = rcs;
data.noise = noise;
data.snr = snr;
data.radial_speed = radial_speed;
data.azimuth_angle = azimuth_angle;
data.elevation_angle = elevation_angle;
data.timestamp_sec = timestamp_sec;
data.timestamp_nanosec = timestamp_nanosec;

recorded_data.push_back(data);
}

void SmartRadarRecorder::callback(
const sensor_msgs::msg::PointCloud2::SharedPtr msg, const std::string topic_name)
{
Expand All @@ -85,60 +107,48 @@ void SmartRadarRecorder::callback(
auto timestamp_nanosec = msg->header.stamp.nanosec;

// Create iterators for the pc2 fields
sensor_msgs::PointCloud2ConstIterator<float> iter_radial_speed(*msg, "radial_speed");
sensor_msgs::PointCloud2ConstIterator<float> iter_range(*msg, "range");
sensor_msgs::PointCloud2ConstIterator<float> iter_power(*msg, "power");
sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth_angle(*msg, "azimuth_angle");
sensor_msgs::PointCloud2ConstIterator<float> iter_elevation_angle(*msg, "elevation_angle");
sensor_msgs::PointCloud2ConstIterator<float> iter_rcs(*msg, "rcs");
sensor_msgs::PointCloud2ConstIterator<float> iter_noise(*msg, "noise");
sensor_msgs::PointCloud2ConstIterator<float> iter_snr(*msg, "snr");
sensor_msgs::PointCloud2ConstIterator<float> iter_azimuth_angle(*msg, "azimuth_angle");
sensor_msgs::PointCloud2ConstIterator<float> iter_elevation_angle(*msg, "elevation_angle");
sensor_msgs::PointCloud2ConstIterator<float> iter_range(*msg, "range");

// Initialize variables for recorded_data
QString radial_speed_value_str;
QString power_value_str;
QString rcs_value_str;
QString noise_value_str;
QString snr_value_str;
QString azimuth_value_str;
QString elevation_value_str;
QString range_value_str;
sensor_msgs::PointCloud2ConstIterator<float> iter_radial_speed(*msg, "radial_speed");

table_data_->setRowCount(0);

for (size_t i = 0; i < msg->height * msg->width; ++i, ++iter_radial_speed, ++iter_power,
++iter_rcs, ++iter_noise, ++iter_snr, ++iter_azimuth_angle, ++iter_elevation_angle,
++iter_range) {
// Conversion from radians to degrees
const double radToDeg = 180.0 / M_PI;
double azimuth_deg, elevation_deg;

for (size_t i = 0; i < msg->height * msg->width; ++i, ++iter_range, ++iter_power,
++iter_azimuth_angle, ++iter_elevation_angle, ++iter_rcs, ++iter_noise,
++iter_snr) {
azimuth_deg = *iter_azimuth_angle * radToDeg;
elevation_deg = *iter_elevation_angle * radToDeg;

// Update the recorded data
if (recording_active_) {
updateRecordedData(
*iter_range, *iter_power, azimuth_deg, elevation_deg, *iter_rcs, *iter_noise, *iter_snr,
*iter_radial_speed, *iter_azimuth_angle, *iter_elevation_angle, timestamp_sec,
timestamp_nanosec);
}
// Add items to the table
table_data_->insertRow(0);
table_data_->setItem(0, 0, new QTableWidgetItem(QString::number(*iter_radial_speed, 'f', 2)));
table_data_->setItem(0, 0, new QTableWidgetItem(QString::number(*iter_range, 'f', 2)));
table_data_->setItem(0, 1, new QTableWidgetItem(QString::number(*iter_power, 'f', 2)));
table_data_->setItem(0, 2, new QTableWidgetItem(QString::number(*iter_rcs, 'f', 2)));
table_data_->setItem(0, 3, new QTableWidgetItem(QString::number(*iter_noise, 'f', 2)));
table_data_->setItem(0, 4, new QTableWidgetItem(QString::number(*iter_snr, 'f', 2)));
table_data_->setItem(0, 2, new QTableWidgetItem(QString::number(azimuth_deg, 'f', 2)));
table_data_->setItem(0, 3, new QTableWidgetItem(QString::number(elevation_deg, 'f', 2)));
table_data_->setItem(0, 4, new QTableWidgetItem(QString::number(*iter_rcs, 'f', 2)));
table_data_->setItem(0, 5, new QTableWidgetItem(QString::number(*iter_noise, 'f', 2)));
table_data_->setItem(0, 6, new QTableWidgetItem(QString::number(*iter_snr, 'f', 2)));
table_data_->setItem(0, 7, new QTableWidgetItem(QString::number(*iter_radial_speed, 'f', 2)));
table_data_->setItem(
0, 5, new QTableWidgetItem(QString::number(*iter_azimuth_angle, 'f', 2)));
0, 8, new QTableWidgetItem(QString::number(*iter_azimuth_angle, 'f', 2)));
table_data_->setItem(
0, 6, new QTableWidgetItem(QString::number(*iter_elevation_angle, 'f', 2)));
table_data_->setItem(0, 7, new QTableWidgetItem(QString::number(*iter_range, 'f', 2)));
// Update variables for recorded_data
radial_speed_value_str = QString::number(*iter_radial_speed, 'f', 2);
power_value_str = QString::number(*iter_power, 'f', 2);
rcs_value_str = QString::number(*iter_rcs, 'f', 2);
noise_value_str = QString::number(*iter_noise, 'f', 2);
snr_value_str = QString::number(*iter_snr, 'f', 2);
azimuth_value_str = QString::number(*iter_azimuth_angle, 'f', 2);
elevation_value_str = QString::number(*iter_elevation_angle, 'f', 2);
range_value_str = QString::number(*iter_range, 'f', 2);
}

// Append data to recorded_data
if (recording_active_) {
recorded_data.append(
{radial_speed_value_str, power_value_str, rcs_value_str, noise_value_str, snr_value_str,
azimuth_value_str, elevation_value_str, range_value_str});
recorded_data.back().append(QString::number(timestamp_sec));
recorded_data.back().append(QString::number(timestamp_nanosec));
0, 9, new QTableWidgetItem(QString::number(*iter_elevation_angle, 'f', 2)));
}

// Update the timestamp table
Expand Down Expand Up @@ -173,7 +183,7 @@ void SmartRadarRecorder::stopRecording()
void SmartRadarRecorder::saveDataToCSV()
{
qDebug() << "Saving data to CSV!";
if (!recorded_data.isEmpty()) {
if (!recorded_data.empty()) {
QFileDialog file_dialog;
QString file_path =
file_dialog.getSaveFileName(this, "Save Data", "", "CSV Files (*.csv);;All Files (*)");
Expand All @@ -182,11 +192,26 @@ void SmartRadarRecorder::saveDataToCSV()
QFile csvfile(file_path);
if (csvfile.open(QIODevice::WriteOnly | QIODevice::Text)) {
QTextStream csv_writer(&csvfile);
csv_writer << "RadialSpeed [m/s], Power [dB], RCS [dB], Noise [dB], SNR, AzimuthAngle "
"[rad], ElevationAngle [rad], Range [m], TimestampSec, TimestampNanoSec \n";
csv_writer << "Range [m], Power [dB], AzimuthAngle [Deg], ElevationAngle [Deg], RCS [dB], "
"Noise [dB], SNR [dB], "
"RadialSpeed [m/s], ElevationAngle [rad], AzimuthAngle [rad], "
"TimestampSec, TimestampNanoSec\n";

for (const auto & data_row : recorded_data) {
QStringList data_str_list = QStringList::fromVector(data_row);
QStringList data_str_list;
data_str_list << QString::number(data_row.range, 'f', 2);
data_str_list << QString::number(data_row.power, 'f', 2);
data_str_list << QString::number(data_row.azimuth_angle * 180.0 / M_PI, 'f', 2);
data_str_list << QString::number(data_row.elevation_angle * 180.0 / M_PI, 'f', 2);
data_str_list << QString::number(data_row.rcs, 'f', 2);
data_str_list << QString::number(data_row.noise, 'f', 2);
data_str_list << QString::number(data_row.snr, 'f', 2);
data_str_list << QString::number(data_row.radial_speed, 'f', 2);
data_str_list << QString::number(data_row.azimuth_angle, 'f', 2);
data_str_list << QString::number(data_row.elevation_angle, 'f', 2);
data_str_list << QString::number(data_row.timestamp_sec);
data_str_list << QString::number(data_row.timestamp_nanosec);

csv_writer << data_str_list.join(", ") << "\n";
}

Expand Down
6 changes: 6 additions & 0 deletions umrr_ros2_driver/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -61,8 +61,10 @@ install(FILES
"${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.1.1_user_interface.so"
"${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.2.1_user_interface.so"
"${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9f_t169_automotivev2.5.0_user_interface.so"
"${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.0.3_user_interface.so"
"${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.2.2_user_interface.so"
"${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrr9d_t152_automotivev1.5.0_user_interface.so"
"${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_automotivev1.0.1_user_interface.so"
"${PROJECT_SOURCE_DIR}/smartmicro/${SMARTMICRO_LIB_DIR}/libumrra4_automotivev1.3.0_user_interface.so"
DESTINATION lib)
Expand Down Expand Up @@ -91,8 +93,10 @@ smartmicro/include/umrr9f_t169_automotive_v1_1_1
smartmicro/include/umrr9f_t169_automotive_v2_0_0
smartmicro/include/umrr9f_t169_automotive_v2_1_1
smartmicro/include/umrr9f_t169_automotive_v2_2_1
smartmicro/include/umrr9f_t169_automotive_v2_5_0
smartmicro/include/umrr9d_t152_automotive_v1_0_3
smartmicro/include/umrr9d_t152_automotive_v1_2_2
smartmicro/include/umrr9d_t152_automotive_v1_5_0
smartmicro/include/umrra4_automotive_v1_0_1
smartmicro/include/umrra4_automotive_v1_3_0)

Expand All @@ -108,8 +112,10 @@ target_link_libraries(smartmicro_radar_node
umrr9f_t169_automotivev2.0.0_user_interface
umrr9f_t169_automotivev2.1.1_user_interface
umrr9f_t169_automotivev2.2.1_user_interface
umrr9f_t169_automotivev2.5.0_user_interface
umrr9d_t152_automotivev1.0.3_user_interface
umrr9d_t152_automotivev1.2.2_user_interface
umrr9d_t152_automotivev1.5.0_user_interface
umrra4_automotivev1.0.1_user_interface
umrra4_automotivev1.3.0_user_interface)

Expand Down
Loading
Loading