diff --git a/ros_driver/src/ti_mmwave_rospkg/bin/3D_people_count_68xx_demo.bin b/ros_driver/src/ti_mmwave_rospkg/bin/3D_people_count_68xx_demo.bin new file mode 100644 index 0000000..2dc1298 Binary files /dev/null and b/ros_driver/src/ti_mmwave_rospkg/bin/3D_people_count_68xx_demo.bin differ diff --git a/ros_driver/src/ti_mmwave_rospkg/bin/README.txt b/ros_driver/src/ti_mmwave_rospkg/bin/README.txt index 2398120..ad274ca 100644 --- a/ros_driver/src/ti_mmwave_rospkg/bin/README.txt +++ b/ros_driver/src/ti_mmwave_rospkg/bin/README.txt @@ -16,6 +16,9 @@ C:\ti\\packages\ti\demo\\mmw Correct binary to be flashed for ROS Driver: +xWRL6432: +xwrl6432_mmw_demo.appimage + xWR6843AOP/ISK + Tracking (Capon Chain): 3D People Counting binary diff --git a/ros_driver/src/ti_mmwave_rospkg/bin/xwrl6432_mmw_demo.appimage b/ros_driver/src/ti_mmwave_rospkg/bin/xwrl6432_mmw_demo.appimage new file mode 100644 index 0000000..500a128 Binary files /dev/null and b/ros_driver/src/ti_mmwave_rospkg/bin/xwrl6432_mmw_demo.appimage differ diff --git a/ros_driver/src/ti_mmwave_rospkg/cfg/6432_Occupancy.cfg b/ros_driver/src/ti_mmwave_rospkg/cfg/6432_Occupancy.cfg new file mode 100644 index 0000000..b5ace15 --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/cfg/6432_Occupancy.cfg @@ -0,0 +1,32 @@ +% *************************************************************** +% 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 +channelCfg 7 3 0 +chirpComnCfg 18 0 0 128 4 30 0 +chirpTimingCfg 6 40 0 160 59.75 +frameCfg 8 0 150 1 200 0 +guiMonitor 1 0 0 0 0 0 +sigProcChainCfg 64 2 2 0 4 4 +cfarCfg 2 4 3 2 0 13.0 0 0.5 0 1 1 1 +aoaFovCfg -60 60 -40 40 +rangeSelCfg 0.1 4.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 0 +factoryCalibCfg 1 0 40 0 0x1ff000 +% numZones, pointsEntryThreshold, snrEntryThreshold, frameEntryThreshold, pointsMaintainThreshold, snrMaintainThreshold, pointsExitThreshold, frameExitThreshold +occStateMach 1 3 3 2 1 1 0 4 +% ZoneNumber minx maxx miny maxy minz maxz +zoneDef 0 -0.4 0.4 0.05 0.7 -0.5 1.5 +sensorStart 0 0 0 0 + diff --git a/ros_driver/src/ti_mmwave_rospkg/include/mmWave.h b/ros_driver/src/ti_mmwave_rospkg/include/mmWave.h index 3926258..50d9bbc 100644 --- a/ros_driver/src/ti_mmwave_rospkg/include/mmWave.h +++ b/ros_driver/src/ti_mmwave_rospkg/include/mmWave.h @@ -74,11 +74,11 @@ enum MmwDemo_Output_TLV_Types /*! @brief List of detected points side information */ MMWDEMO_OUTPUT_MSG_DETECTED_POINTS_SIDE_INFO = 7, + MMWDEMO_OUTPUT_MSG_MAX, /*! @brief Occupancy State Machine information TLV */ - MMWDEMO_OUTPUT_MSG_OCCUPANCY_STATE_MACHINE = 1030, + MMWDEMO_OUTPUT_MSG_OCCUPANCY_STATE_MACHINE = 1030 - MMWDEMO_OUTPUT_MSG_MAX }; enum SorterState{ READ_HEADER, diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6432_Occupancy.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6432_Occupancy.launch new file mode 100644 index 0000000..a77b7f1 --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/launch/6432_Occupancy.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp b/ros_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp index 217fd17..0f732f5 100644 --- a/ros_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp +++ b/ros_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp @@ -41,16 +41,16 @@ int main(int argc, char **argv) { if (std::regex_search(srv.response.resp, std::regex("Done"))) { // ROS_INFO("mmWaveQuickConfig: Command successful (mmWave sensor responded with 'Done')"); parser.ParamsParser(srv, nh); - } else { - ROS_ERROR("mmWaveQuickConfig: Command failed (mmWave sensor did not respond with 'Done')"); - ROS_ERROR("mmWaveQuickConfig: Response: '%s'", srv.response.resp.c_str() ); - return 1; - } - } else { - ROS_ERROR("mmWaveQuickConfig: Failed to call service mmWaveCLI"); - ROS_ERROR("%s", srv.request.comm.c_str() ); - return 1; - } + } else { + ROS_INFO("mmWaveQuickConfig: Command failed (mmWave sensor did not respond with 'Done')"); + ROS_INFO("mmWaveQuickConfig: Response: '%s'", srv.response.resp.c_str() ); + //return 1; + } + } else { + ROS_INFO("mmWaveQuickConfig: Failed to call service mmWaveCLI"); + ROS_INFO("%s", srv.request.comm.c_str() ); + // return 1; + } } } parser.CalParams(nh); @@ -61,4 +61,4 @@ int main(int argc, char **argv) { } ROS_INFO("mmWaveQuickConfig: mmWaveQuickConfig will now terminate. Done configuring mmWave device using config file: %s", argv[1]); return 0; -} \ No newline at end of file +}