yemai/mmwave_ti_ros
1
0
Fork 0
mirror of https://git.ti.com/git/mmwave_radar/mmwave_ti_ros.git synced 2026-05-21 01:12:08 +00:00
Code Issues Packages Projects Releases Wiki Activity

Tracker visualization in RVIZ and support for human/non-human classification TLVs including microdoppler TLVs

Browse Source
This commit is contained in:
Pedrhom Nafisi
2024-10-01 16:28:17 -05:00
parent a244861eec
commit e0da5749a4
15 changed files with 636 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Classification.cfg Normal file
View File

@@ -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

36
ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Standard_Uncompressed.cfg
View File

@@ -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

11
ros2_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h
View File

@@ -15,14 +15,21 @@
#include <boost/shared_ptr.hpp>
#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<sensor_msgs::msg::PointCloud2>::SharedPtr radar_scan_pcl_pub;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr radar_track_marker_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarScan>::SharedPtr radar_scan_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarClassifier>::SharedPtr radar_classifier_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarOccupancy>::SharedPtr radar_occupancy_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarPointTrackID>::SharedPtr radar_trackid_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarTrackArray>::SharedPtr radar_trackarray_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarMicroDopplerDataArray>::SharedPtr radar_mdoppler_data_pub;
rclcpp::Publisher<ti_mmwave_rospkg_msgs::msg::RadarMicroDopplerFeatureArray>::SharedPtr radar_mdoppler_feature_pub;
private:

71
ros2_driver/src/ti_mmwave_rospkg/include/mmWave.h
View File

@@ -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};

114
ros2_driver/src/ti_mmwave_rospkg/launch/6432_Classification.py Normal file
View File

@@ -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

BIN
ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard.cpython-310.pyc Normal file
View File

Binary file not shown.

24
ros2_driver/src/ti_mmwave_rospkg/launch/rviz.rviz
View File

@@ -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: <Fixed 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

176
ros2_driver/src/ti_mmwave_rospkg/launch/rviz_old.rviz Normal file
View File

@@ -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: <Fixed 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: <Fixed 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

197
ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp
View File

@@ -63,10 +63,17 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (mmWaveCloudType,
DataUARTHandler::DataUARTHandler(std::shared_ptr<rclcpp::Node> nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1])
{
radar_scan_pcl_pub = nh->create_publisher<sensor_msgs::msg::PointCloud2>("/ti_mmwave/radar_scan_pcl", 100);
radar_track_marker_pub = nh->create_publisher<visualization_msgs::msg::Marker>("/ti_mmwave/radar_track_marker", 100);
radar_scan_pub = nh->create_publisher<ti_mmwave_rospkg_msgs::msg::RadarScan>("/ti_mmwave/radar_scan", 100);
radar_occupancy_pub = nh->create_publisher<ti_mmwave_rospkg_msgs::msg::RadarOccupancy>("/ti_mmwave/radar_occupancy", 100);
radar_trackid_pub = nh->create_publisher<ti_mmwave_rospkg_msgs::msg::RadarPointTrackID>("/ti_mmwave/radar_point_track_id", 100);
radar_trackarray_pub = nh->create_publisher<ti_mmwave_rospkg_msgs::msg::RadarTrackArray>("/ti_mmwave/radar_track_array", 100);
radar_mdoppler_data_pub = nh->create_publisher<ti_mmwave_rospkg_msgs::msg::RadarMicroDopplerDataArray>("/ti_mmwave/radar_micro_doppler_data_array", 100);
radar_mdoppler_feature_pub = nh->create_publisher<ti_mmwave_rospkg_msgs::msg::RadarMicroDopplerFeatureArray>("/ti_mmwave/radar_micro_doppler_feature_array", 100);
radar_classifier_pub = nh->create_publisher<ti_mmwave_rospkg_msgs::msg::RadarClassifier>("/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, &currentBufp->at(currentDatap), sizeof(mmwData.newPointCloudCompOut.noiseUnit));
currentDatap += ( sizeof(mmwData.newPointCloudCompOut.noiseUnit) );
//get number of detected objects
memcpy( &mmwData.newPointCloudCompOut.numDetectedPoints, &currentBufp->at(currentDatap), sizeof(mmwData.newPointCloudCompOut.numDetectedPoints));
currentDatap += ( sizeof(mmwData.newPointCloudCompOut.numDetectedPoints) );
//get number of detected objects major
memcpy( &mmwData.newPointCloudCompOut.numDetectedPointsMajor, &currentBufp->at(currentDatap), sizeof(mmwData.newPointCloudCompOut.numDetectedPointsMajor));
currentDatap += ( sizeof(mmwData.newPointCloudCompOut.numDetectedPointsMajor) );
//get number of detected objects minor
memcpy( &mmwData.newPointCloudCompOut.numDetectedPointsMinor, &currentBufp->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, &currentBufp->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, &currentBufp->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, &currentBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.fLow));
currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.fLow) );
// get upper frequency border of the occupied bandwidth
memcpy( &mmwData.newMicroDopplerFeature.fUp, &currentBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.fUp));
currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.fUp) );
// get power within the occupied bandwidth
memcpy( &mmwData.newMicroDopplerFeature.bwPwr, &currentBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.bwPwr));
currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.bwPwr) );
// get mean frequency of the power spectral density estimate
memcpy( &mmwData.newMicroDopplerFeature.meanFreq, &currentBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.meanFreq));
currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.meanFreq) );
// get median frequency of the power spectral density estimate
memcpy( &mmwData.newMicroDopplerFeature.medFreq, &currentBufp->at(currentDatap), sizeof(mmwData.newMicroDopplerFeature.medFreq));
currentDatap += ( sizeof(mmwData.newMicroDopplerFeature.medFreq) );
// get spectral entropy of the power spectral density estimate
memcpy( &mmwData.newMicroDopplerFeature.sEntropy, &currentBufp->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, &currentBufp->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, &currentBufp->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, &currentBufp->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;
}

7
ros2_driver/src/ti_mmwave_rospkg_msgs/CMakeLists.txt
View File

@@ -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)

2
ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarClassifier.msg Normal file
View File

@@ -0,0 +1,2 @@
uint16 track_id
float32 value

3
ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerDataArray.msg Normal file
View File

@@ -0,0 +1,3 @@
std_msgs/Header header
uint32 num_tracks
RadarMicroDopplerDataContents[] track

1
ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerDataContents.msg Normal file
View File

@@ -0,0 +1 @@
float32 value

3
ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerFeatureArray.msg Normal file
View File

@@ -0,0 +1,3 @@
std_msgs/Header header
uint32 num_tracks
RadarMicroDopplerFeatureContents[] track

7
ros2_driver/src/ti_mmwave_rospkg_msgs/msg/RadarMicroDopplerFeatureContents.msg Normal file
View File

@@ -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