ROS2 Driver v1.3, check README.md in ros2_driver directory for details

ros2_driver/README.md

@@ -4,6 +4,17 @@
 ---
 ### Release Notes
 
+#### ROS2 Driver v1.3.0 20 Aug 2024
+
+* Added tracker visualization in RVIZ
+* Added support for the following 6432 TLVs
+  * 6432 Compressed Point Cloud TLV (TLV Type 301)
+  *	6432 Tracker (TLV Type 308 and 309)
+  *	Microdoppler Raw Data   (TLV Type 310)
+  *	Microdoppler Feature Extraction (TLV Type 311)
+  *	Classification Output Information (TLV Type 317)
+* Added new launch files and configurations for 6843AOP for static object detection and stationary target detection (fineMotion)
+
 #### ROS2 Driver v1.2.2 20 Aug 2024
 
 * Fixed launch file issues with legacy devices

ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_FineMotion.cfg

@@ -0,0 +1,50 @@
+% SDK Parameters
+% See the SDK user's guide for more information
+% "C:\ti\mmwave_sdk_[VER]\docs\mmwave_sdk_user_guide.pdf"
+sensorStop
+flushCfg
+dfeDataOutputMode 1
+channelCfg 15 7 0
+adcCfg 2 1
+adcbufCfg -1 0 1 1 1
+lowPower 0 0
+bpmCfg -1 1 1 2
+
+% Detection Layer Parameters
+% See the Detection Layer Tuning Guide for more information
+% "<RADAR_TOOLBOX_INSTALL_DIR>\source\ti\examples\People_Tracking\docs\3D_people_tracking_detection_layer_tuning_guide.pdf"
+
+profileCfg 0 60 10.00 10.00 59.10 0 0 54.71 1 94 3000.00 1 0 36
+
+chirpCfg 0 0 0 0 0 0 0 1
+chirpCfg 1 1 0 0 0 0 0 6
+chirpCfg 2 2 0 0 0 0 0 6
+frameCfg 0 2 32 0 100 1 0
+
+dynamicRACfarCfg -1 4 1 2 2 8 12 4 8 7.00 9.00 0.40 1 1
+staticRACfarCfg -1 15 4 2 2 8 8 6 4 8.00 10.00 0.70 1 0
+dynamicRangeAngleCfg -1 0.75 0.0010 1 0
+dynamic2DAngleCfg -1 1.5 0.0300 1 0 1 0.30 0.85 8.00
+staticRangeAngleCfg -1 0 10 10
+
+antGeometry0 -1 -1 0 0 -3 -3 -2 -2 -1 -1 0 0
+antGeometry1 -1 0 -1 0 -3 -2 -3 -2 -3 -2 -3 -2
+antPhaseRot 1 -1 1 -1 1 -1 1 -1 1 -1 1 -1
+fovCfg -1 50.0 50.0
+compRangeBiasAndRxChanPhase 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0
+
+fineMotionCfg -1 1 1 1 10
+
+% Tracker Layer Parameters
+% See the Tracking Layer Tuning Guide for more information
+% "C:\ti\mmwave_industrial_toolbox_[VER]\labs\people_counting\docs\3D_people_counting_tracker_layer_tuning_guide.pdf"
+staticBoundaryBox -1.8 1.8 0.2 6.5 0.0 2.0
+boundaryBox -1.8 1.8 0.2 6.5 0.0 2.0
+sensorPosition 0.5 0 0
+gatingParam 4 2 2 2 10
+stateParam 5 12 6 13 5 100
+allocationParam 20 20 0.05 15 1 5
+maxAcceleration 3 3 0.5
+trackingCfg 1 2 400 10 20 260 100
+presenceBoundaryBox -1.8 1.8 0.2 6.5 0.0 2.0
+sensorStart

ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_StaticTracking.cfg

@@ -0,0 +1,49 @@
+% SDK Parameters
+% See the SDK user's guide for more information
+% "C:\ti\mmwave_sdk_[VER]\docs\mmwave_sdk_user_guide.pdf"
+sensorStop
+flushCfg
+dfeDataOutputMode 1
+channelCfg 15 7 0
+adcCfg 2 1
+adcbufCfg -1 0 1 1 1
+lowPower 0 0
+
+% Detection Layer Parameters
+% See the Detection Layer Tuning Guide for more information
+% "<RADAR_TOOLBOX_INSTALL_DIR>\source\ti\examples\People_Tracking\docs\3D_people_tracking_detection_layer_tuning_guide.pdf"
+
+profileCfg 0 60 10.00 10.00 59.10 0 0 54.71 1 94 3000.00 1 0 36
+
+chirpCfg 0 0 0 0 0 0 0 1
+chirpCfg 1 1 0 0 0 0 0 2
+chirpCfg 2 2 0 0 0 0 0 4
+frameCfg 0 2 16 0 150 1 0
+
+dynamicRACfarCfg -1 4 1 2 2 8 12 4 8 9.00 10.00 0.40 1 1
+staticRACfarCfg -1 15 4 2 2 8 8 6 4 5.00 10.00 0.70 0 0
+dynamicRangeAngleCfg -1 0.75 0.0010 1 0
+dynamic2DAngleCfg -1 1.5 0.0300 1 0 1 0.30 0.85 8.00
+staticRangeAngleCfg -1 1 8 10
+
+antGeometry0 -1 -1 0 0 -3 -3 -2 -2 -1 -1 0 0
+antGeometry1 -1 0 -1 0 -3 -2 -3 -2 -3 -2 -3 -2
+antPhaseRot 1 -1 1 -1 1 -1 1 -1 1 -1 1 -1
+fovCfg -1 60.0 20.0
+compRangeBiasAndRxChanPhase 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0
+
+fineMotionCfg -1 0 1 1 10
+
+% Tracker Layer Parameters
+% See the Tracking Layer Tuning Guide for more information
+% "C:\ti\mmwave_industrial_toolbox_[VER]\labs\people_counting\docs\3D_people_counting_tracker_layer_tuning_guide.pdf"
+staticBoundaryBox -1.8 1.8 0.2 6.5 0.0 2.0
+boundaryBox -1.8 1.8 0.2 6.5 0.0 2.0
+sensorPosition 0.5 0 0
+gatingParam 4 2 2 2 10
+stateParam 5 12 6 13 5 100
+allocationParam 20 20 0.05 15 1 5
+maxAcceleration 3 3 0.5
+trackingCfg 1 2 400 10 20 260 100
+presenceBoundaryBox -1.8 1.8 0.2 6.5 0.0 2.0
+sensorStart

ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_Tracking.cfg

@@ -1,7 +1,6 @@
 % SDK Parameters
 % See the SDK user's guide for more information
 % "C:\ti\mmwave_sdk_[VER]\docs\mmwave_sdk_user_guide.pdf"
-resetDevice
 sensorStop
 flushCfg
 dfeDataOutputMode 1
@@ -9,36 +8,43 @@ channelCfg 15 7 0
 adcCfg 2 1
 adcbufCfg -1 0 1 1 1
 lowPower 0 0
+bpmCfg -1 1 1 2
 
-% Radar and Point Cloud Detection Layer Parameters
+% Detection Layer Parameters
 % See the Detection Layer Tuning Guide for more information
-% "C:\ti\mmwave_industrial_toolbox_[VER]\labs\people_counting\docs\3D_people_counting_detection_layer_tuning_guide.pdf"
-profileCfg 0 60.75 10.00 10.00 59.10 657930 0 54.71 1 94 3000.00 2 1 36
+% "<RADAR_TOOLBOX_INSTALL_DIR>\source\ti\examples\People_Tracking\docs\3D_people_tracking_detection_layer_tuning_guide.pdf"
+
+profileCfg 0 60 10.00 10.00 59.10 0 0 54.71 1 94 3000.00 1 0 36
+
 chirpCfg 0 0 0 0 0 0 0 1
-chirpCfg 1 1 0 0 0 0 0 2
-chirpCfg 2 2 0 0 0 0 0 4
-frameCfg 0 2 64 0 50 1 0
-dynamicRACfarCfg -1 4 1 2 2 8 12 4 8 15.00 18.00 0.40 1 1
-staticRACfarCfg -1 4 1 2 2 8 8 6 4 12.00 19.00 0.30 0 0
+chirpCfg 1 1 0 0 0 0 0 6
+chirpCfg 2 2 0 0 0 0 0 6
+frameCfg 0 2 32 0 100 1 0
+
+dynamicRACfarCfg -1 4 1 2 2 8 12 4 8 8.00 10.00 0.40 1 1
+staticRACfarCfg -1 15 4 2 2 8 8 6 4 8.00 10.00 0.70 1 0
 dynamicRangeAngleCfg -1 0.75 0.0010 1 0
 dynamic2DAngleCfg -1 1.5 0.0300 1 0 1 0.30 0.85 8.00
-staticRangeAngleCfg -1 0 8 8
+staticRangeAngleCfg -1 0 10 10
+
 antGeometry0 -1 -1 0 0 -3 -3 -2 -2 -1 -1 0 0
 antGeometry1 -1 0 -1 0 -3 -2 -3 -2 -3 -2 -3 -2
 antPhaseRot 1 -1 1 -1 1 -1 1 -1 1 -1 1 -1
-fovCfg -1 70.0 30.0
+fovCfg -1 50.0 50.0
 compRangeBiasAndRxChanPhase 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0
 
+fineMotionCfg -1 0 1 1 10
+
 % Tracker Layer Parameters
 % See the Tracking Layer Tuning Guide for more information
 % "C:\ti\mmwave_industrial_toolbox_[VER]\labs\people_counting\docs\3D_people_counting_tracker_layer_tuning_guide.pdf"
-staticBoundaryBox -1.8 1.8 0.4 7.8 -0.2 1.8
-boundaryBox -2 2 0.5 8 -0.3 2
+staticBoundaryBox -1.8 1.8 0.2 6.5 0.0 2.0
+boundaryBox -1.8 1.8 0.2 6.5 0.0 2.0
 sensorPosition 0.5 0 0
 gatingParam 4 2 2 2 10
-stateParam 10 5 5 50 1 600
-allocationParam 20 20 0.05 4 2 2
-maxAcceleration 0.1 0.1 0.1
-trackingCfg 1 2 250 10 20 260 94
-presenceBoundaryBox -3 3 2 6 0.5 2.5
+stateParam 5 12 6 13 5 100
+allocationParam 20 20 0.05 15 1 5
+maxAcceleration 3 3 0.5
+trackingCfg 1 2 400 10 20 260 100
+presenceBoundaryBox -1.8 1.8 0.2 6.5 0.0 2.0
 sensorStart

ros2_driver/src/ti_mmwave_rospkg/include/mmWave.h

@@ -101,6 +101,9 @@ enum MmwDemo_Output_TLV_Types
     /*! @brief   Tracker TLV's*/
     MMWDEMO_OUTPUT_MSG_TRACKERPROC_TARGET_INDEX = 1011,
 
+    /*! @brief   Tracker TLV's*/
+    MMWDEMO_OUTPUT_MSG_TRACKERPROC_TARGET_HEIGHT = 1012,
+
     /*! @brief   3D Spherical Compressed Point Cloud */
     MMWDEMO_OUTPUT_MSG_COMPRESSED_POINTS = 1020,
 
@@ -121,6 +124,7 @@ SWAP_BUFFERS,
 READ_SPHERE_POINT_CLOUD, 
 READ_3D_TARGET_LIST, 
 READ_TARGET_INDEX,
+READ_TARGET_HEIGHT,
 READ_MICRO_DOPPLER_DATA,
 READ_MICRO_DOPPLER_FEATURES,
 READ_CLASSIFIER,
@@ -254,6 +258,15 @@ typedef struct DPIF_TargetIndex_t
 
 }DPIF_TargetIndex_t;
 
+typedef struct DPIF_TargetHeight_t
+{
+
+  uint32_t targetID;
+  float maxZ;
+  float minZ;
+
+}DPIF_TargetHeight_t;
+
 typedef struct DPIF_MicroDopplerRawData_t
 {
   float value;
@@ -383,6 +396,7 @@ DPIF_PointCloudSideInfo_t sideInfo; // used for SDK 3.x
 DPIF_PointCloudOccupancy_t occupancy; // added for Occupancy Zones
 DPIF_TargetList3D_t newListOut; // added for Tracker
 DPIF_TargetIndex_t newIndexOut; // added for Tracker
+DPIF_TargetHeight_t newHeightOut; // 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;

ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_FineMotion.py

@@ -0,0 +1,29 @@
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    rviz_arg = DeclareLaunchArgument('rviz', default_value='true', description='Enable RViz')
+
+    # include IWR6843.py
+    package_dir = get_package_share_directory('ti_mmwave_rospkg')
+    iwr6843_include = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(package_dir,'launch','IWR6843.py')),
+        launch_arguments={
+            "cfg_file": '6843AOP_FineMotion.cfg',
+            "command_port": "/dev/ttyUSB0",
+            "data_port": "/dev/ttyUSB1",
+            "rviz": LaunchConfiguration('rviz'),
+        }.items()
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(rviz_arg)
+    ld.add_action(iwr6843_include)
+
+    return ld

ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_StaticTracking.py

@@ -0,0 +1,29 @@
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    rviz_arg = DeclareLaunchArgument('rviz', default_value='true', description='Enable RViz')
+
+    # include IWR6843.py
+    package_dir = get_package_share_directory('ti_mmwave_rospkg')
+    iwr6843_include = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(package_dir,'launch','IWR6843.py')),
+        launch_arguments={
+            "cfg_file": '6843AOP_StaticTracking.cfg',
+            "command_port": "/dev/ttyUSB0",
+            "data_port": "/dev/ttyUSB1",
+            "rviz": LaunchConfiguration('rviz'),
+        }.items()
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(rviz_arg)
+    ld.add_action(iwr6843_include)
+
+    return ld

ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp

@@ -981,6 +981,29 @@ void *DataUARTHandler::sortIncomingData(void)
             sorterState = CHECK_TLV_TYPE;
             break;
 
+        case READ_TARGET_HEIGHT:
+
+            i = 0;
+
+            while (i < radartrackarray.num_tracks){
+
+                //get track's associated id (int)
+                memcpy( &mmwData.newHeightOut.targetID, &currentBufp->at(currentDatap), sizeof(mmwData.newHeightOut.targetID));
+                currentDatap += ( sizeof(mmwData.newHeightOut.targetID) );
+
+                //get track's associated height max (float)
+                memcpy( &mmwData.newHeightOut.maxZ, &currentBufp->at(currentDatap), sizeof(mmwData.newHeightOut.maxZ));
+                currentDatap += ( sizeof(mmwData.newHeightOut.maxZ) );
+
+                //get track's associated height min (float)
+                memcpy( &mmwData.newHeightOut.minZ, &currentBufp->at(currentDatap), sizeof(mmwData.newHeightOut.minZ));
+                currentDatap += ( sizeof(mmwData.newHeightOut.minZ) );
+                i++;
+                
+            }
+            sorterState = CHECK_TLV_TYPE;
+            break;
+
         case READ_MICRO_DOPPLER_DATA:
 
             i = 0;
@@ -1257,6 +1280,11 @@ void *DataUARTHandler::sortIncomingData(void)
                         sorterState = READ_TARGET_INDEX;
                         break;
 
+                    case MMWDEMO_OUTPUT_MSG_TRACKERPROC_TARGET_HEIGHT:
+                            //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : Target Height TLV");
+                        sorterState = READ_TARGET_HEIGHT;
+                        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;