diff --git a/ros2_driver/README.md b/ros2_driver/README.md index 30d9625..f97c0f2 100644 --- a/ros2_driver/README.md +++ b/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 diff --git a/ros2_driver/src/ti_mmwave_rospkg/bin/3D_people_count_68xx_demo.bin b/ros2_driver/src/ti_mmwave_rospkg/bin/3D_people_track_6843_and_6843AOP.bin similarity index 73% rename from ros2_driver/src/ti_mmwave_rospkg/bin/3D_people_count_68xx_demo.bin rename to ros2_driver/src/ti_mmwave_rospkg/bin/3D_people_track_6843_and_6843AOP.bin index 2dc1298..03dc312 100644 Binary files a/ros2_driver/src/ti_mmwave_rospkg/bin/3D_people_count_68xx_demo.bin and b/ros2_driver/src/ti_mmwave_rospkg/bin/3D_people_track_6843_and_6843AOP.bin differ diff --git a/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_FineMotion.cfg b/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_FineMotion.cfg new file mode 100644 index 0000000..e0f57f3 --- /dev/null +++ b/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 +% "\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 diff --git a/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_StaticTracking.cfg b/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_StaticTracking.cfg new file mode 100644 index 0000000..97c5438 --- /dev/null +++ b/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 +% "\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 diff --git a/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_Tracking.cfg b/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_Tracking.cfg index 8f04020..bb77be1 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_Tracking.cfg +++ b/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 +% "\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 -sensorStart \ No newline at end of file +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 diff --git a/ros2_driver/src/ti_mmwave_rospkg/include/mmWave.h b/ros2_driver/src/ti_mmwave_rospkg/include/mmWave.h index 24fdaea..2a9248a 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/include/mmWave.h +++ b/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; diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_FineMotion.py b/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_FineMotion.py new file mode 100644 index 0000000..b950c63 --- /dev/null +++ b/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 diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_StaticTracking.py b/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_StaticTracking.py new file mode 100644 index 0000000..cb4dc5d --- /dev/null +++ b/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 diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Occupancy.cpython-310.pyc b/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Occupancy.cpython-310.pyc deleted file mode 100644 index fc50352..0000000 Binary files a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Occupancy.cpython-310.pyc and /dev/null differ 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 deleted file mode 100644 index 9495c4e..0000000 Binary files a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard.cpython-310.pyc and /dev/null differ diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard_Compressed.cpython-310.pyc b/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard_Compressed.cpython-310.pyc deleted file mode 100644 index e740ed1..0000000 Binary files a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard_Compressed.cpython-310.pyc and /dev/null differ diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard_Uncompressed.cpython-310.pyc b/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard_Uncompressed.cpython-310.pyc deleted file mode 100644 index 48375b1..0000000 Binary files a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6432_Standard_Uncompressed.cpython-310.pyc and /dev/null differ diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Occupancy.cpython-310.pyc b/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Occupancy.cpython-310.pyc deleted file mode 100644 index f136315..0000000 Binary files a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Occupancy.cpython-310.pyc and /dev/null differ diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Occupany.cpython-310.pyc b/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Occupany.cpython-310.pyc deleted file mode 100644 index 0f66776..0000000 Binary files a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Occupany.cpython-310.pyc and /dev/null differ diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Standard.cpython-310.pyc b/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Standard.cpython-310.pyc deleted file mode 100644 index c7303c9..0000000 Binary files a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Standard.cpython-310.pyc and /dev/null differ diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Tracking.cpython-310.pyc b/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Tracking.cpython-310.pyc deleted file mode 100644 index ce4377c..0000000 Binary files a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/6843ISK_Tracking.cpython-310.pyc and /dev/null differ diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/test_launch.cpython-310.pyc b/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/test_launch.cpython-310.pyc deleted file mode 100644 index b2541d8..0000000 Binary files a/ros2_driver/src/ti_mmwave_rospkg/launch/__pycache__/test_launch.cpython-310.pyc and /dev/null differ diff --git a/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp b/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp index ae974d2..43db460 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp +++ b/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, ¤tBufp->at(currentDatap), sizeof(mmwData.newHeightOut.targetID)); + currentDatap += ( sizeof(mmwData.newHeightOut.targetID) ); + + //get track's associated height max (float) + memcpy( &mmwData.newHeightOut.maxZ, ¤tBufp->at(currentDatap), sizeof(mmwData.newHeightOut.maxZ)); + currentDatap += ( sizeof(mmwData.newHeightOut.maxZ) ); + + //get track's associated height min (float) + memcpy( &mmwData.newHeightOut.minZ, ¤tBufp->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;