From e0da5749a430c67285e7355d15665dfca3a6a335 Mon Sep 17 00:00:00 2001 From: Pedrhom Nafisi Date: Tue, 1 Oct 2024 16:28:17 -0500 Subject: [PATCH] Tracker visualization in RVIZ and support for human/non-human classification TLVs including microdoppler TLVs --- .../cfg/6432_Classification.cfg | 39 ++++ .../cfg/6432_Standard_Uncompressed.cfg | 36 ++-- .../include/DataHandlerClass.h | 11 + .../src/ti_mmwave_rospkg/include/mmWave.h | 71 ++++++- .../launch/6432_Classification.py | 114 ++++++++++ .../__pycache__/6432_Standard.cpython-310.pyc | Bin 0 -> 1974 bytes .../src/ti_mmwave_rospkg/launch/rviz.rviz | 24 ++- .../src/ti_mmwave_rospkg/launch/rviz_old.rviz | 176 ++++++++++++++++ .../ti_mmwave_rospkg/src/DataHandlerClass.cpp | 197 +++++++++++++++--- .../src/ti_mmwave_rospkg_msgs/CMakeLists.txt | 7 + .../msg/RadarClassifier.msg | 2 + .../msg/RadarMicroDopplerDataArray.msg | 3 + .../msg/RadarMicroDopplerDataContents.msg | 1 + .../msg/RadarMicroDopplerFeatureArray.msg | 3 + .../msg/RadarMicroDopplerFeatureContents.msg | 7 + 15 files changed, 636 insertions(+), 55 deletions(-) create mode 100644 ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Classification.cfg create mode 100644 ros2_driver/src/ti_mmwave_rospkg/launch/6432_Classification.py create mode 100644 ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard.cpython-310.pyc create mode 100644 ros2_driver/src/ti_mmwave_rospkg/launch/rviz_old.rviz create mode 100644 ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarClassifier.msg create mode 100644 ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerDataArray.msg create mode 100644 ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerDataContents.msg create mode 100644 ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerFeatureArray.msg create mode 100644 ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerFeatureContents.msg diff --git a/ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Classification.cfg b/ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Classification.cfg new file mode 100644 index 0000000..7ddd280 --- /dev/null +++ b/ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Classification.cfg @@ -0,0 +1,39 @@ +% *************************************************************** +% PresenceDetect: Chirp configuration and Processing chain are +% optimized to detect any kind of motion, including fine movements +% (even small movements that are present while sitting still, +% such as, movement caused by typing, breathing, etc.). However, +% there is no velocity measurement reported in this case. +% It is typically useful for applications such as identifying +% presence or absence of occupants in an indoor setting. +% Localization (Angle estimation) of the object is possible. +% *************************************************************** +sensorStop 0 +channelCfg 5 3 0 +chirpComnCfg 8 0 0 128 4 28 0 +chirpTimingCfg 6 63 0 75 60 +frameCfg 2 0 350 32 200 0 +antGeometryCfg 0 0 1 1 0 2 0 1 1 2 0 3 2.418 2.418 +guiMonitor 2 0 0 0 0 0 0 0 1 1 1 +sigProcChainCfg 32 4 3 2 8 8 1 0.3 +cfarCfg 2 8 4 3 0 11.0 0 0.5 0 1 1 1 +aoaFovCfg -70 70 -40 40 +rangeSelCfg 0.1 10.0 +clutterRemoval 1 +compRangeBiasAndRxChanPhase 0.0 1.00000 0.00000 -1.00000 0.00000 1.00000 0.00000 -1.00000 0.00000 1.00000 0.00000 -1.00000 0.00000 +adcDataSource 0 adc_data_0001_CtestAdc6Ant.bin +adcLogging 0 +lowPowerCfg 1 +factoryCalibCfg 1 0 40 0 0x1ff000 +boundaryBox -3.5 3.5 0 6 -0.5 3 +sensorPosition 0 0 1.9 0 0 +staticBoundaryBox -3 3 0.5 6 0 3 +gatingParam 3 2 2 2 4 +stateParam 3 3 12 50 5 200 +allocationParam 6 10 0.1 4 0.5 20 +maxAcceleration 0.4 0.4 0.1 +trackingCfg 1 2 100 5 61.3 191.7 200 +presenceBoundaryBox -3 3 0.5 6 0 3 +microDopplerCfg 1 0 0.5 0 1 1 12.5 87.5 1 +classifierCfg 1 3 4 +sensorStart 0 0 0 0 diff --git a/ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Standard_Uncompressed.cfg b/ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Standard_Uncompressed.cfg index b883965..ee50241 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Standard_Uncompressed.cfg +++ b/ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Standard_Uncompressed.cfg @@ -10,26 +10,28 @@ % *************************************************************** sensorStop 0 channelCfg 7 3 0 -chirpComnCfg 8 0 0 256 4 28 0 -chirpTimingCfg 6 63 0 75 60 -frameCfg 2 0 250 32 150 0 -guiMonitor 1 0 0 0 0 0 0 0 0 0 0 -sigProcChainCfg 16 16 1 1 4 4 0 15 -cfarCfg 2 8 4 3 0 10.0 0 0.5 0 1 1 1 +chirpComnCfg 16 0 0 128 4 28 3 +chirpTimingCfg 6 32 0 40 60 +frameCfg 2 0 250 64 200 0 +antGeometryCfg 0 0 1 1 0 2 0 1 1 2 0 3 2.418 2.418 +guiMonitor 1 0 0 0 0 0 0 0 1 0 0 +sigProcChainCfg 16 8 3 2 8 8 1 0.3 +cfarCfg 2 8 4 3 0 8.0 0 0.5 0 1 0 0 aoaFovCfg -60 60 -40 40 -rangeSelCfg 0.1 12.0 +rangeSelCfg 0.1 7.5 clutterRemoval 1 compRangeBiasAndRxChanPhase 0.0 1.00000 0.00000 -1.00000 0.00000 1.00000 0.00000 -1.00000 0.00000 1.00000 0.00000 -1.00000 0.00000 -adcDataSource 0 C:/ti/mmwave_lp_sdk/examples/datapath/common/testBench/major_motion/adc_data_0001_CtestAdc6Ant.bin +adcDataSource 0 adc_data_0001_CtestAdc6Ant.bin adcLogging 0 -lowPowerCfg 1 +lowPowerCfg 0 factoryCalibCfg 1 0 40 0 0x1ff000 -mpdBoundaryBox 1 0 1.48 0 1.95 0 3 -mpdBoundaryBox 2 0 1.48 1.95 3.9 0 3 -mpdBoundaryBox 3 -1.48 0 0 1.95 0 3 -mpdBoundaryBox 4 -1.48 0 1.95 3.9 0 3 -sensorPosition 0 0 1.44 0 0 -minorStateCfg 5 4 40 8 4 30 8 8 -majorStateCfg 4 2 30 10 8 80 4 4 -clusterCfg 1 0.5 2 +boundaryBox -3.5 3.5 0 5.5 -0.25 3 +sensorPosition 0 0 1.2 0 0 +staticBoundaryBox -3.5 3.5 0 5.5 0 3 +gatingParam 3 2.5 2.5 2 8 +stateParam 3 50 100 100 5 300 +allocationParam 1 3 0.1 1 1 100 +maxAcceleration 2 2 0.4 +trackingCfg 1 2 100 6 61.4 191.8 200 +presenceBoundaryBox -3 3 0.5 7.5 0 3 sensorStart 0 0 0 0 diff --git a/ros2_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h b/ros2_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h index a3cfdc8..92d2214 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h +++ b/ros2_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h @@ -15,14 +15,21 @@ #include #include "mmWave.h" #include "std_msgs/msg/header.hpp" +#include "std_msgs/msg/char.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/msg/point_field.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" +#include "visualization_msgs/msg/marker.hpp" #include "ti_mmwave_rospkg_msgs/msg/radar_scan.hpp" #include "ti_mmwave_rospkg_msgs/msg/radar_occupancy.hpp" +#include "ti_mmwave_rospkg_msgs/msg/radar_classifier.hpp" #include "ti_mmwave_rospkg_msgs/msg/radar_point_track_id.hpp" #include "ti_mmwave_rospkg_msgs/msg/radar_track_array.hpp" +#include "ti_mmwave_rospkg_msgs/msg/radar_micro_doppler_data_array.hpp" +#include "ti_mmwave_rospkg_msgs/msg/radar_micro_doppler_data_contents.hpp" +#include "ti_mmwave_rospkg_msgs/msg/radar_micro_doppler_feature_array.hpp" +#include "ti_mmwave_rospkg_msgs/msg/radar_micro_doppler_feature_contents.hpp" #define COUNT_SYNC_MAX 2 @@ -82,10 +89,14 @@ public: mmwDataPacket mmwData; rclcpp::Publisher::SharedPtr radar_scan_pcl_pub; + rclcpp::Publisher::SharedPtr radar_track_marker_pub; rclcpp::Publisher::SharedPtr radar_scan_pub; + rclcpp::Publisher::SharedPtr radar_classifier_pub; rclcpp::Publisher::SharedPtr radar_occupancy_pub; rclcpp::Publisher::SharedPtr radar_trackid_pub; rclcpp::Publisher::SharedPtr radar_trackarray_pub; + rclcpp::Publisher::SharedPtr radar_mdoppler_data_pub; + rclcpp::Publisher::SharedPtr radar_mdoppler_feature_pub; private: diff --git a/ros2_driver/src/ti_mmwave_rospkg/include/mmWave.h b/ros2_driver/src/ti_mmwave_rospkg/include/mmWave.h index d04f88b..24fdaea 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/include/mmWave.h +++ b/ros2_driver/src/ti_mmwave_rospkg/include/mmWave.h @@ -79,6 +79,22 @@ enum MmwDemo_Output_TLV_Types /*! @brief List of compressed detection points for MMWAVE-L SDK 5.x */ MMWDEMO_OUTPUT_EXT_MSG_DETECTED_POINTS = 301, + /*! @brief List of targets/tracks contents for MMWAVE-L SDK 5.x */ + MMWDEMO_OUTPUT_EXT_MSG_TARGET_LIST = 308, + + /*! @brief Each point associated with a track is given the track ID it is associated with MMWAVE-L SDK 5.x */ + MMWDEMO_OUTPUT_EXT_MSG_TARGET_INDEX = 309, + + /*! @brief Unprocess micro doppler spectrum for MMWAVE-L SDK 5.x + Stored in a 2D array size of dopplerData[numberOfTracks][numberOfDopplerBins]*/ + MMWDEMO_OUTPUT_EXT_MSG_MICRO_DOPPLER_RAW_DATA = 310, + + /*! @brief List of features extracted from the micro doppler spectrum, listed per track MMWAVE-L SDK 5.x */ + MMWDEMO_OUTPUT_EXT_MSG_MICRO_DOPPLER_FEATURES = 311, + + /*! @brief Classifier output listed per track MMWAVE-L SDK 5.x */ + MMWDEMO_OUTPUT_EXT_MSG_CLASSIFIER_INFO = 317, + /*! @brief Tracker TLV's */ MMWDEMO_OUTPUT_MSG_TRACKERPROC_3D_TARGET_LIST = 1010, @@ -105,6 +121,9 @@ SWAP_BUFFERS, READ_SPHERE_POINT_CLOUD, READ_3D_TARGET_LIST, READ_TARGET_INDEX, +READ_MICRO_DOPPLER_DATA, +READ_MICRO_DOPPLER_FEATURES, +READ_CLASSIFIER, READ_COMPRESSED_POINT_CLOUD, READ_SIDE_INFO, READ_OCCUPANCY}; @@ -235,6 +254,28 @@ typedef struct DPIF_TargetIndex_t }DPIF_TargetIndex_t; +typedef struct DPIF_MicroDopplerRawData_t +{ + float value; + +}DPIF_MicroDopplerData_t; + +typedef struct DPIF_MicroDopplerFeature_t +{ + float fLow; + + float fUp; + + float bwPwr; + + float meanFreq; + + float medFreq; + + float sEntropy; + +}DPIF_MicroDopplerFeature_t; + typedef struct DPIF_SphericalPointCloud_t { @@ -294,15 +335,17 @@ typedef struct DPIF_PointCloudCompressed_t float noiseUnit; - uint16_t numDetectedPoints[2]; + uint16_t numDetectedPointsMajor; - uint16_t x; + uint16_t numDetectedPointsMinor; - uint16_t y; + int16_t x; - uint16_t z; + int16_t y; - uint16_t doppler; + int16_t z; + + int16_t doppler; uint8_t snr; @@ -316,10 +359,18 @@ typedef struct DPIF_PointCloudCompressed_t typedef struct DPIF_PointCloudOccupancy_t { -/*! @brief state - CFAR cell to side noise ratio in dB expressed in 0.1 steps of dB */ uint32_t state; -}DPIF_PointCloudOccupancy; +}DPIF_PointCloudOccupancy_t; + +typedef struct DPIF_ClassifierOutput_t +{ + + uint16_t track_id; + char value; + +}DPIF_ClassifierOutput; + struct mmwDataPacket{ MmwDemo_output_message_header_t header; @@ -329,11 +380,15 @@ MmwDemo_DetectedObj objOut; // only used for SDK 1.x and 2.x DPIF_PointCloudCartesian_t newObjOut; // used for SDK 3.x DPIF_PointCloudSideInfo_t sideInfo; // used for SDK 3.x -DPIF_PointCloudOccupancy occupancy; // added for Occupancy Zones +DPIF_PointCloudOccupancy_t occupancy; // added for Occupancy Zones DPIF_TargetList3D_t newListOut; // added for Tracker DPIF_TargetIndex_t newIndexOut; // added for Tracker DPIF_SphericalPointCloud_t newSphereCloudOut; // added for Capon/Tracker DPIF_PointCloudCompressed_t newPointCloudCompOut; // added for MMWAVE-L SDK 5.x +DPIF_MicroDopplerData_t newMicroDopplerValue; +DPIF_MicroDopplerFeature_t newMicroDopplerFeature; +DPIF_ClassifierOutput_t newClassifier; + }; const uint8_t magicWord[8] = {2, 1, 4, 3, 6, 5, 8, 7}; diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/6432_Classification.py b/ros2_driver/src/ti_mmwave_rospkg/launch/6432_Classification.py new file mode 100644 index 0000000..2c3f572 --- /dev/null +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6432_Classification.py @@ -0,0 +1,114 @@ +import os +import launch +import launch_ros.actions +import pytest +from launch import LaunchDescription +from launch.actions import TimerAction +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + path = os.path.join(my_package_dir,'cfg','6432_Classification.cfg') + device = "6432" + name = "/mmWaveCLI" + command_port = "/dev/ttyACM0" + command_rate = "115200" + data_port = "/dev/ttyACM0" + data_rate = "115200" + + ld = LaunchDescription() + ConfigParameters = os.path.join( + my_package_dir, + 'config', + 'global_params.yaml', + 'launch/*.rviz' + ) + global_param_node = Node( + package='ti_mmwave_rospkg', + executable='ConfigParameterServer', + name='ConfigParameterServer', + parameters=[ConfigParameters] + ) + + mmWaveCommSrv = Node( + package="ti_mmwave_rospkg", + executable="mmWaveCommSrv", + name="mmWaveCommSrv", + output="screen", + emulate_tty=True, + parameters=[ + {"command_port": command_port}, + {"command_rate": command_rate}, + {"data_port": data_port}, + {"data_rate": data_rate}, + {"max_allowed_elevation_angle_deg": "90"}, + {"max_allowed_azimuth_angle_deg": "90"}, + {"frame_id": "/ti_mmwave_0"}, + {"mmwavecli_name": name}, + {"mmwavecli_cfg": path} + ] + ) + + mmWaveQuickConfig = Node( + package="ti_mmwave_rospkg", + executable="mmWaveQuickConfig", + name="mmWaveQuickConfig", + output="screen", + emulate_tty=True, + parameters=[ + {"mmwavecli_name": name}, + {"mmwavecli_cfg": path} + ] + ) + + ParameterParser = Node( + package="ti_mmwave_rospkg", + executable="ParameterParser", + name="ParameterParser", + output="screen", + emulate_tty=True, + parameters=[ + {"device_name": device}, + {"mmwavecli_name": name}, + {"mmwavecli_cfg": path} + ] + ) + + delay = TimerAction( + period=5.0, + actions=[Node( + package="ti_mmwave_rospkg", + executable="DataHandlerClass", + name="DataHandlerClass", + output="screen", + emulate_tty=True, + parameters=[ + {"mmwavecli_name": name}, + {"mmwavecli_cfg": path}, + {"data_port": data_port}, + {"data_rate": data_rate}, + {"frame_id": "ti_mmwave_0"}, + {"max_allowed_elevation_angle_deg": 90}, + {"max_allowed_azimuth_angle_deg": 90} + ] + ), + Node( + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] + + )] + ) + + ld.add_action(global_param_node) + ld.add_action(mmWaveCommSrv) + ld.add_action(mmWaveQuickConfig) + ld.add_action(ParameterParser) + ld.add_action(delay) + + return ld + diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard.cpython-310.pyc b/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9495c4ec18ec833befb2a839a80c46dce0f9fa6d GIT binary patch literal 1974 zcmZ7%%WfMta7nFprPaf-Bim^lCr)3f`j8#BNl~CE5;zFjA~jqnMS;C6=q0`0cpn0n zQY=9_C0GALeC)NK!)wt)ZoL&XoS;LltVc-fa5ys@&Wp2Yt>zdo{$`g)q-q#{sm=Vr zuz3hyCIC=_qR4Rb6}d?7v0Koq>6)bc87T4W6;3ZmqWw=Web&Qtt25vU?TSzWHCB4Kye#|nThg}h-2@osK!kF<# z{$$%cAmaGvG)y|Z!%T#tH=&#RDPTG6!t3TVFH=O1hMf5# zLT1QhQv4)q>R) z=1XH-no5J4)HyfDWl^MvR%m5Z4iK%LnPW#&YP3e{bcr@bW{BwWs0eUnWC6T3vH>>f zD!?@j*Xeb@KNuAN-WZ_}1!cN%W{oRa(@lC)Ynta{&97?yCf(HhTO<6QmV%b535(v= zl3R4^eN78Ctd^AC+F)Bj+3; zLt~XGR-IzCDOS%hD3I2^Y>3c{;}@jQJf3FVlRy^yqhNUJ!8c#;dL2O$N;vg?OZ+tM za+YO`wt+8A#UDD`ar`?R-Fy1eu)0lIe_M#bqrG2tq;>D!{oNf{{WLiWgW*aLrH3Sf zTHz$l+5-|t!&;<^a{Kc(?}tOVx~D}ClmruucNp(8-ZG^C0Xvxik$PAQ|Jb0yznN zrmqU%5RB$S*?0_I{|F(87~hLXmdVPTiEmtxl0}u1-68&pN-NLPu&hWrSFOU#Oj#y8 z=*27%Sqn?6%XpYlX%pR6vQ~-PN*zu!X?CH(Nb@@ zU%eSAuyOn-gL-_Xo+isculYme*(6+KV!l zVsF5UNrVT#8FuQyWyfjKp1l1`myxzv=V=Z+P)AGO$0_Ya><9R@7?~=Hl2JlxR_7=F EACUwjtpET3 literal 0 HcmV?d00001 diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/rviz.rviz b/ros2_driver/src/ti_mmwave_rospkg/launch/rviz.rviz index 6849f29..0ef10f6 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/rviz.rviz +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/rviz.rviz @@ -8,6 +8,7 @@ Panels: - /Status1 - /PointCloud21 - /Axes1 + - /Marker1 Splitter Ratio: 0.5 Tree Height: 555 - Class: rviz_common/Selection @@ -90,6 +91,19 @@ Visualization Manager: Radius: 0.05000000074505806 Reference Frame: ti_mmwave_0 Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ti_mmwave/radar_track_marker + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -136,7 +150,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 18.371187210083008 + Distance: 18.564899444580078 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -154,7 +168,7 @@ Visualization Manager: Pitch: 1.5697963237762451 Target Frame: Value: Orbit (rviz) - Yaw: 3.135396957397461 + Yaw: 3.140397787094116 Saved: ~ Window Geometry: Displays: @@ -162,7 +176,7 @@ Window Geometry: Height: 846 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000003dd000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -172,5 +186,5 @@ Window Geometry: Views: collapsed: true Width: 1200 - X: 60 - Y: 60 + X: 1186 + Y: -26 diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/rviz_old.rviz b/ros2_driver/src/ti_mmwave_rospkg/launch/rviz_old.rviz new file mode 100644 index 0000000..6849f29 --- /dev/null +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/rviz_old.rviz @@ -0,0 +1,176 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /Axes1 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0.25 + Enabled: true + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 20 + Min Color: 255; 0; 0 + Min Intensity: 15 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ti_mmwave/radar_scan_pcl + Use Fixed Frame: true + Use rainbow: false + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: ti_mmwave_0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: ti_mmwave_0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 18.371187210083008 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.135396957397461 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 60 + Y: 60 diff --git a/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp b/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp index ae157a1..ae974d2 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp +++ b/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp @@ -63,10 +63,17 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (mmWaveCloudType, DataUARTHandler::DataUARTHandler(std::shared_ptr nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1]) { radar_scan_pcl_pub = nh->create_publisher("/ti_mmwave/radar_scan_pcl", 100); + radar_track_marker_pub = nh->create_publisher("/ti_mmwave/radar_track_marker", 100); + radar_scan_pub = nh->create_publisher("/ti_mmwave/radar_scan", 100); radar_occupancy_pub = nh->create_publisher("/ti_mmwave/radar_occupancy", 100); radar_trackid_pub = nh->create_publisher("/ti_mmwave/radar_point_track_id", 100); radar_trackarray_pub = nh->create_publisher("/ti_mmwave/radar_track_array", 100); + + radar_mdoppler_data_pub = nh->create_publisher("/ti_mmwave/radar_micro_doppler_data_array", 100); + radar_mdoppler_feature_pub = nh->create_publisher("/ti_mmwave/radar_micro_doppler_feature_array", 100); + radar_classifier_pub = nh->create_publisher("/ti_mmwave/radar_classifier", 100); + maxAllowedElevationAngleDeg = 90; // Use max angle if none specified maxAllowedAzimuthAngleDeg = 90; // Use max angle if none specified gDataHandlerPtr = this; @@ -288,10 +295,10 @@ void *DataUARTHandler::sortIncomingData(void) uint32_t tlvLen = 0; uint32_t headerSize; uint32_t tlvSize = 0; - unsigned int currentDatap = 0; + uint32_t currentDatap = 0; + uint32_t addressHolder = 0; SorterState sorterState = READ_HEADER; - int i = 0, tlvCount = 0, offset = 0; - int j = 0; + int i = 0, j = 0, tlvCount = 0, offset = 0, numDopplerBins, numClassifications, temp; float maxElevationAngleRatioSquared; float maxAzimuthAngleRatio; float realElevation; @@ -307,8 +314,25 @@ void *DataUARTHandler::sortIncomingData(void) ti_mmwave_rospkg_msgs::msg::RadarPointTrackID radartrackid; ti_mmwave_rospkg_msgs::msg::RadarTrackArray radartrackarray; ti_mmwave_rospkg_msgs::msg::RadarTrackContents radartrackcontents; + ti_mmwave_rospkg_msgs::msg::RadarMicroDopplerDataArray radarmicrodopplerdataarray; + ti_mmwave_rospkg_msgs::msg::RadarMicroDopplerDataContents radarmicrodopplerdatacontents; + ti_mmwave_rospkg_msgs::msg::RadarMicroDopplerFeatureArray radarmicrodopplerfeaturearray; + ti_mmwave_rospkg_msgs::msg::RadarMicroDopplerFeatureContents radarmicrodopplerfeaturecontents; ti_mmwave_rospkg_msgs::msg::RadarOccupancy radaroccupancy; ti_mmwave_rospkg_msgs::msg::RadarScan radarscan; + ti_mmwave_rospkg_msgs::msg::RadarClassifier radarclassifier; + + visualization_msgs::msg::Marker tracklist; + geometry_msgs::msg::Point trackcenter; + tracklist.id = 0; + tracklist.type = visualization_msgs::msg::Marker::CUBE_LIST; + tracklist.scale.x = 0.8; + tracklist.scale.y = 0.8; + tracklist.scale.z = 0.8; + tracklist.color.r = 1; + tracklist.color.a = 0.5; + tracklist.pose.orientation.w = 1.0; + tracklist.frame_locked = true; //wait for first packet to arrive pthread_mutex_lock(&countSync_mutex); @@ -807,13 +831,23 @@ void *DataUARTHandler::sortIncomingData(void) radartrackcontents.accy = -mmwData.newListOut.accX; radartrackcontents.accz = mmwData.newListOut.accZ; radartrackarray.track.push_back(radartrackcontents); + + trackcenter.x = radartrackcontents.posx; + trackcenter.y = radartrackcontents.posy; + trackcenter.z = radartrackcontents.posz; + tracklist.points.push_back(trackcenter); + i++; } radartrackarray.header.stamp = nodeHandle->now(); radartrackarray.header.frame_id = frameID; + tracklist.header.stamp = nodeHandle->now(); + tracklist.header.frame_id = frameID; radar_trackarray_pub->publish(radartrackarray); + radar_track_marker_pub->publish(tracklist); radartrackarray.track.clear(); + tracklist.points.clear(); sorterState = CHECK_TLV_TYPE; break; } @@ -839,17 +873,21 @@ void *DataUARTHandler::sortIncomingData(void) memcpy( &mmwData.newPointCloudCompOut.noiseUnit, ¤tBufp->at(currentDatap), sizeof(mmwData.newPointCloudCompOut.noiseUnit)); currentDatap += ( sizeof(mmwData.newPointCloudCompOut.noiseUnit) ); - //get number of detected objects - memcpy( &mmwData.newPointCloudCompOut.numDetectedPoints, ¤tBufp->at(currentDatap), sizeof(mmwData.newPointCloudCompOut.numDetectedPoints)); - currentDatap += ( sizeof(mmwData.newPointCloudCompOut.numDetectedPoints) ); + //get number of detected objects major + memcpy( &mmwData.newPointCloudCompOut.numDetectedPointsMajor, ¤tBufp->at(currentDatap), sizeof(mmwData.newPointCloudCompOut.numDetectedPointsMajor)); + currentDatap += ( sizeof(mmwData.newPointCloudCompOut.numDetectedPointsMajor) ); + + //get number of detected objects minor + memcpy( &mmwData.newPointCloudCompOut.numDetectedPointsMinor, ¤tBufp->at(currentDatap), sizeof(mmwData.newPointCloudCompOut.numDetectedPointsMinor)); + currentDatap += ( sizeof(mmwData.newPointCloudCompOut.numDetectedPointsMinor) ); RScan->header.frame_id = frameID; RScan->height = 1; - RScan->width = (mmwData.newPointCloudCompOut.numDetectedPoints[0] + mmwData.newPointCloudCompOut.numDetectedPoints[1]); + RScan->width = (mmwData.newPointCloudCompOut.numDetectedPointsMajor + mmwData.newPointCloudCompOut.numDetectedPointsMinor); RScan->is_dense = 1; RScan->points.resize(RScan->width * RScan->height); - while(i < (mmwData.newPointCloudCompOut.numDetectedPoints[0] + mmwData.newPointCloudCompOut.numDetectedPoints[1])) { + while(i < (RScan->width)) { //get x value memcpy( &mmwData.newPointCloudCompOut.x, ¤tBufp->at(currentDatap), sizeof(mmwData.newPointCloudCompOut.x)); @@ -896,24 +934,11 @@ void *DataUARTHandler::sortIncomingData(void) radarscan.x = realY; radarscan.y = -(realX); radarscan.z = realZ; - radarscan.range = realRange; radarscan.velocity = realDoppler; radarscan.intensity = realSNR; - - if (((maxElevationAngleRatioSquared == -1) || - (((RScan->points[i].z * RScan->points[i].z) / (RScan->points[i].x * RScan->points[i].x + - RScan->points[i].y * RScan->points[i].y) - ) < maxElevationAngleRatioSquared) - ) && - ((maxAzimuthAngleRatio == -1) || (fabs(RScan->points[i].y / RScan->points[i].x) < maxAzimuthAngleRatio)) && - (RScan->points[i].x != 0) - ) - { - radar_scan_pub->publish(radarscan); - } - radar_scan_pub->publish(radarscan); i++; + } tlvSize = 0; sorterState = CHECK_TLV_TYPE; @@ -956,6 +981,107 @@ void *DataUARTHandler::sortIncomingData(void) sorterState = CHECK_TLV_TYPE; break; + case READ_MICRO_DOPPLER_DATA: + + i = 0; + j = 0; + radarmicrodopplerdataarray.num_tracks = (int) radartrackarray.num_tracks; + numDopplerBins = (int) tlvLen / (radarmicrodopplerdataarray.num_tracks * 4); + // mdopplerdata[numTrack][numDopplerBins] + + while( i < radarmicrodopplerdataarray.num_tracks ) { + while ( j < numDopplerBins ) { + // get value + memcpy( &mmwData.newMicroDopplerValue.value, ¤tBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerValue.value)); + currentDatap += ( sizeof(mmwData.newMicroDopplerValue.value) ); + radarmicrodopplerdatacontents.value = mmwData.newMicroDopplerValue.value; + radarmicrodopplerdataarray.track.push_back(radarmicrodopplerdatacontents); + j++; + + } + j = 0; + i++; + } + radarmicrodopplerdataarray.header.stamp = nodeHandle->now(); + radarmicrodopplerdataarray.header.frame_id = frameID; + radar_mdoppler_data_pub->publish(radarmicrodopplerdataarray); + radarmicrodopplerdataarray.track.clear(); + sorterState = CHECK_TLV_TYPE; + break; + + case READ_MICRO_DOPPLER_FEATURES: + + i = 0; + //6 floats expected per track, 6 * 4 = 192 + radarmicrodopplerfeaturearray.num_tracks = (int)radarmicrodopplerdataarray.num_tracks; + + while( i < radarmicrodopplerfeaturearray.num_tracks ) { + + // get lower frequency border of the occupied bandwidth + memcpy( &mmwData.newMicroDopplerFeature.fLow, ¤tBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.fLow)); + currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.fLow) ); + + // get upper frequency border of the occupied bandwidth + memcpy( &mmwData.newMicroDopplerFeature.fUp, ¤tBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.fUp)); + currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.fUp) ); + + // get power within the occupied bandwidth + memcpy( &mmwData.newMicroDopplerFeature.bwPwr, ¤tBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.bwPwr)); + currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.bwPwr) ); + + // get mean frequency of the power spectral density estimate + memcpy( &mmwData.newMicroDopplerFeature.meanFreq, ¤tBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.meanFreq)); + currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.meanFreq) ); + + // get median frequency of the power spectral density estimate + memcpy( &mmwData.newMicroDopplerFeature.medFreq, ¤tBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.medFreq)); + currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.medFreq) ); + + // get spectral entropy of the power spectral density estimate + memcpy( &mmwData.newMicroDopplerFeature.sEntropy, ¤tBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.sEntropy)); + currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.sEntropy) ); + + radarmicrodopplerfeaturecontents.header.stamp = nodeHandle->now(); + radarmicrodopplerfeaturecontents.header.frame_id = frameID; + radarmicrodopplerfeaturecontents.f_low = mmwData.newMicroDopplerFeature.fLow; + radarmicrodopplerfeaturecontents.f_up = mmwData.newMicroDopplerFeature.fUp; + radarmicrodopplerfeaturecontents.bw_pwr = mmwData.newMicroDopplerFeature.bwPwr; + radarmicrodopplerfeaturecontents.mean_freq = mmwData.newMicroDopplerFeature.meanFreq; + radarmicrodopplerfeaturecontents.med_freq = mmwData.newMicroDopplerFeature.medFreq; + radarmicrodopplerfeaturecontents.s_entropy = mmwData.newMicroDopplerFeature.sEntropy; + + radarmicrodopplerfeaturearray.track.push_back(radarmicrodopplerfeaturecontents); + i++; + } + radarmicrodopplerfeaturearray.header.stamp = nodeHandle->now(); + radarmicrodopplerfeaturearray.header.frame_id = frameID; + radar_mdoppler_feature_pub->publish(radarmicrodopplerfeaturearray); + radarmicrodopplerfeaturearray.track.clear(); + sorterState = CHECK_TLV_TYPE; + break; + + case READ_CLASSIFIER: + + i = 0; + j = 0; + numClassifications = 2; + + while( i < (radarmicrodopplerfeaturearray.num_tracks * numClassifications) ) { + + // get classifier value per track + memcpy( &mmwData.newClassifier.value, ¤tBufp->at(currentDatap), sizeof(mmwData.newClassifier.value)); + currentDatap += ( sizeof(mmwData.newClassifier.value) ); + radarclassifier.track_id = i / numClassifications; + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Value before is : %c", mmwData.newClassifier.value); + temp = (int) mmwData.newClassifier.value; + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Value now is : %c", mmwData.newClassifier.value); + radarclassifier.value = (float) temp / 128; + radar_classifier_pub->publish(radarclassifier); + i++; + } + sorterState = CHECK_TLV_TYPE; + break; + case READ_LOG_MAG_RANGE: { sorterState = CHECK_TLV_TYPE; @@ -1060,13 +1186,10 @@ void *DataUARTHandler::sortIncomingData(void) memcpy( &tlvType, ¤tBufp->at(currentDatap), sizeof(tlvType)); currentDatap += ( sizeof(tlvType) ); - //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : sizeof(tlvType) = %d", sizeof(tlvType)); //get tlvLen (32 bits) & remove from queue memcpy( &tlvLen, ¤tBufp->at(currentDatap), sizeof(tlvLen)); currentDatap += ( sizeof(tlvLen) ); - //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : sizeof(tlvLen) = %d", sizeof(tlvLen)); - //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : tlvType = %d, tlvLen = %d", (int) tlvType, tlvLen); switch(tlvType) @@ -1134,11 +1257,35 @@ void *DataUARTHandler::sortIncomingData(void) sorterState = READ_TARGET_INDEX; break; + case MMWDEMO_OUTPUT_EXT_MSG_TARGET_LIST: + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : 3D Target List TLV"); + sorterState = READ_3D_TARGET_LIST; + break; + + case MMWDEMO_OUTPUT_EXT_MSG_TARGET_INDEX: + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : Target Index TLV"); + sorterState = READ_TARGET_INDEX; + break; + case MMWDEMO_OUTPUT_EXT_MSG_DETECTED_POINTS: //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : Compressed Points TLV MMWAVE-L SDK 5.x"); sorterState = READ_COMPRESSED_POINT_CLOUD; break; + case MMWDEMO_OUTPUT_EXT_MSG_MICRO_DOPPLER_RAW_DATA: + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : Micro Doppler Raw Data"); + sorterState = READ_MICRO_DOPPLER_DATA; + break; + + case MMWDEMO_OUTPUT_EXT_MSG_MICRO_DOPPLER_FEATURES: + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : Micro Doppler Feature List"); + sorterState = READ_MICRO_DOPPLER_FEATURES; + break; + + case MMWDEMO_OUTPUT_EXT_MSG_CLASSIFIER_INFO: + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : Classifier"); + sorterState = READ_CLASSIFIER; + break; default: break; } diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/CMakeLists.txt b/ros2_driver/src/ti_mmwave_rospkg_msgs/CMakeLists.txt index 02baa45..2bbd07b 100644 --- a/ros2_driver/src/ti_mmwave_rospkg_msgs/CMakeLists.txt +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/CMakeLists.txt @@ -12,14 +12,20 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) ## Generate messages in the 'msg' folder set(msg_files "msg/RadarScan.msg" "msg/RadarOccupancy.msg" + "msg/RadarClassifier.msg" "msg/RadarTrackArray.msg" "msg/RadarTrackContents.msg" "msg/RadarPointTrackID.msg" + "msg/RadarMicroDopplerDataArray.msg" + "msg/RadarMicroDopplerDataContents.msg" + "msg/RadarMicroDopplerFeatureArray.msg" + "msg/RadarMicroDopplerFeatureContents.msg" ) set(srv_files @@ -30,6 +36,7 @@ rosidl_generate_interfaces(ti_mmwave_rospkg_msgs ${msg_files} ${srv_files} DEPENDENCIES std_msgs + visualization_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarClassifier.msg b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarClassifier.msg new file mode 100644 index 0000000..fd8536d --- /dev/null +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarClassifier.msg @@ -0,0 +1,2 @@ +uint16 track_id +float32 value \ No newline at end of file diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerDataArray.msg b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerDataArray.msg new file mode 100644 index 0000000..beb0f0f --- /dev/null +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerDataArray.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +uint32 num_tracks +RadarMicroDopplerDataContents[] track diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerDataContents.msg b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerDataContents.msg new file mode 100644 index 0000000..de75b5e --- /dev/null +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerDataContents.msg @@ -0,0 +1 @@ +float32 value diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerFeatureArray.msg b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerFeatureArray.msg new file mode 100644 index 0000000..631a116 --- /dev/null +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerFeatureArray.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +uint32 num_tracks +RadarMicroDopplerFeatureContents[] track diff --git a/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerFeatureContents.msg b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerFeatureContents.msg new file mode 100644 index 0000000..a6623ec --- /dev/null +++ b/ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerFeatureContents.msg @@ -0,0 +1,7 @@ +std_msgs/Header header +float32 f_low +float32 f_up +float32 bw_pwr +float32 mean_freq +float32 med_freq +float32 s_entropy