Support for IWRL6432 via launch, cfg, bin, src, and README additions/changes

ros_driver/src/ti_mmwave_rospkg/bin/README.txt

@@ -16,6 +16,9 @@ C:\ti\<mmwave_sdk_version>\packages\ti\demo\<device>\mmw
 
 Correct binary to be flashed for ROS Driver:
 
+xWRL6432:
+xwrl6432_mmw_demo.appimage
+
 xWR6843AOP/ISK + Tracking (Capon Chain):
 3D People Counting binary
 

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
+

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, 

ros_driver/src/ti_mmwave_rospkg/launch/6432_Occupancy.launch

@@ -0,0 +1,34 @@
+<!--
+  This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
+ -->
+
+<launch>
+
+  <!-- Input arguments -->
+  <arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642, 6843]"/>
+  <arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
+  <arg name="max_allowed_elevation_angle_deg" default="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
+  <arg name="max_allowed_azimuth_angle_deg" default="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
+
+  <!-- mmWave_Manager node -->
+  <node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
+    <param name="command_port" value="/dev/ttyACM0"  />
+    <param name="command_rate" value="115200"   />
+    <param name="data_port" value="/dev/ttyACM0"  />
+    <param name="data_rate" value="115200"   />
+    <param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)"   />
+    <param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)"   />
+    <param name="frame_id" value="/ti_mmwave_0"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+    <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
+  </node>
+
+  <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6432_Occupancy.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
+
+  <node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>
+
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_rospkg)/launch/rviz/ti_mmwave_multi.rviz"/>
+</launch>

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 {
+		} else {
-                ROS_ERROR("mmWaveQuickConfig: Command failed (mmWave sensor did not respond with 'Done')");
+	           ROS_INFO("mmWaveQuickConfig: Command failed (mmWave sensor did not respond with 'Done')");
-                ROS_ERROR("mmWaveQuickConfig: Response: '%s'", srv.response.resp.c_str() );
+                   ROS_INFO("mmWaveQuickConfig: Response: '%s'", srv.response.resp.c_str() );
-                return 1;
+		   //return 1;
-            }
+		}
-            } else {
+             } else {
-            ROS_ERROR("mmWaveQuickConfig: Failed to call service mmWaveCLI");
+	        ROS_INFO("mmWaveQuickConfig: Failed to call service mmWaveCLI");
-            ROS_ERROR("%s", srv.request.comm.c_str() );
+                ROS_INFO("%s", srv.request.comm.c_str() );
-            return 1;
+		// 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;
-}
+}