Merge pull request #4 in MITL/mmwave_ti_ros from JD-J7-robo-sdk to master

* commit '5f4426fe55bc746a09f584fcb92a99b1a6e9a76c':
  Parametrized CLI service name
  updated pcl message header to include timestamp
  Added launch files for visualization on remote PC
  Renamed launch file. Fixed default values
  Improved escalating to sigterm fix
  Fixed terminating with ctrl+c causing escalating to SIGTERM.
  Modified launch file for radar driver on J7. Added launch file for multiple radar case
  Added support for running node on ROS Noetic on TDA4VM

ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt

@@ -1,6 +1,9 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(ti_mmwave_rospkg)
 
+set (CMAKE_CXX_STANDARD 14)
+set (CMAKE_CXX_STANDARD_REQUIRED ON)
+
 ## Add support for C++11, supported in ROS Kinetic and newer
  add_definitions(-std=c++11)
 
@@ -207,7 +210,6 @@ add_dependencies(mmwave ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TA
 #   # myfile2
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
-set (CMAKE_CXX_STANDARD 11)
 
 add_executable(${PROJECT_NAME} src/mmWaveLoader.cpp)
 target_link_libraries(mmwave ${serial_LIBRARIES} ${PCL_LIBRARIES})

ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h

@@ -19,6 +19,7 @@
 #include <pcl/point_types.h>
 #include <visualization_msgs/Marker.h>
 #include <cmath>
+#include <signal.h>
 #define COUNT_SYNC_MAX 2
 
 class DataUARTHandler{
@@ -48,13 +49,19 @@ public:
     /*User callable function to start the handler's internal threads*/
     void start(void);
     
+    /*User callable function to stop the handler's internal threads*/
+    void stop();
+
     /*Helper functions to allow pthread compatability*/
     static void* readIncomingData_helper(void *context);
     
     static void* sortIncomingData_helper(void *context);
     
     static void* syncedBufferSwap_helper(void *context);
-    
+
+    /* Function to handle signals such as SIGINT */
+    static void sigHandler(int32_t sig);
+
     /*Sorted mmwDemo Data structure*/
     mmwDataPacket mmwData;
 
@@ -90,6 +97,9 @@ private:
     
     /*Mutex protected variable which synchronizes threads*/
     int countSync;
+
+    /*Boolean used to notify threads to exit*/
+    bool stop_threads;
     
     /*Read/Write Buffers*/
     std::vector<uint8_t> pingPongBuffers[2];

ros_driver/src/ti_mmwave_rospkg/include/mmWaveCommSrv.hpp

@@ -76,6 +76,8 @@ class mmWaveCommSrv : public nodelet::Nodelet
    std::string mySerialPort;
    
    int myBaudRate;
+
+   std::string mmWaveCLIName;
 }; //Class mmWaveCommSrv 
 
 } //namespace ti_mmwave_rospkg 

ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_2d_0.launch

@@ -15,11 +15,14 @@
     <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/1443_2d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.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"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_2d_1.launch

@@ -15,11 +15,14 @@
     <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_1"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
     <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
   </node>
 
   <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
-  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
 
   <node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
   

ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_3d_0.launch

@@ -15,11 +15,14 @@
     <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/1443_3d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.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"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_3d_1.launch

@@ -15,11 +15,14 @@
     <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_1"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
     <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
   </node>
 
   <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
-  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
 
   <node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
   

ros_driver/src/ti_mmwave_rospkg/launch/1642_multi_2d_0.launch

@@ -19,12 +19,15 @@
     <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/1642_2d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.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"/>

ros_driver/src/ti_mmwave_rospkg/launch/1642_multi_2d_1.launch

@@ -19,11 +19,14 @@
     <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_1"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
     <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
   </node>
 
   <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
-  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
 
   <node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 -1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_2d_0.launch

@@ -15,11 +15,14 @@
     <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/1843_2d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.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"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_2d_1.launch

@@ -15,11 +15,14 @@
     <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_1"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
     <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
   </node>
 
   <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
-  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
 
   <node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
   

ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_3d_0.launch

@@ -15,11 +15,14 @@
     <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/1843_3d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.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"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_3d_1.launch

@@ -15,11 +15,14 @@
     <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_1"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
     <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
   </node>
 
   <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
-  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
 
   <node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
   

ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_2d_0.launch

@@ -19,11 +19,14 @@
     <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/6843AOP_2d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.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"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_2d_1.launch

@@ -19,11 +19,14 @@
     <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_1"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
     <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
   </node>
 
   <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
-  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
 
   <node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_3d_0.launch

@@ -19,11 +19,14 @@
     <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/6843AOP_3d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.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"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_3d_1.launch

@@ -19,11 +19,14 @@
     <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_1"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
     <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
   </node>
 
   <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
-  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
 
   <node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/6843_3d_TDA4VM.launch

@@ -0,0 +1,40 @@
+<!--
+  This file will launch the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config. This launch file is intended
+  to be used to launch the mmWave sensor node on TI's TDA4VM Jacinto Processors, as such a separate command must be used to launch rViz 
+  for visualization on a PC.
+ -->
+
+<launch>
+
+  <!-- Input arguments -->
+  <arg name="device_type" 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]}"/>
+  <arg name="device_num" default="0"/>
+  <arg name="com_user" default="/dev/ttyUSB0"/>
+  <arg name="com_data" default="/dev/ttyUSB1"/>
+  <arg name="node_start_delay" default="0"/>
+  <arg name="tf_args" default="0 0 0 0 0 0" doc="Values to be used for tf transformation"/>
+
+  <!-- mmWave_Manager node -->
+  <node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_$(arg device_num)" output="screen" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@'">
+    <param name="command_port" value="$(arg com_user)"  />
+    <param name="command_rate" value="115200"   />
+    <param name="data_port" value="$(arg com_data)"  />
+    <param name="data_rate" value="921600"   />
+    <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_$(arg device_num)"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI_$(arg device_num)" />
+    <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_$(arg device_num)"/>
+  </node>
+
+  <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_$(arg device_num)" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@'">
+    <param name="mmWaveCLI_name" value="/mmWaveCLI_$(arg device_num)" />
+  </node>
+
+  <node pkg="tf" type="static_transform_publisher" name="radar_baselink_$(arg device_num)" args="$(arg tf_args) ti_mmwave_pcl ti_mmwave_$(arg device_num) 100" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@'"/>
+
+</launch>

ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_2d_0.launch

@@ -19,11 +19,14 @@
     <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/6843_2d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.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"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_2d_1.launch

@@ -19,11 +19,14 @@
     <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_1"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
     <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
   </node>
 
   <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
-  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
 
   <node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_3d_0.launch

@@ -19,11 +19,14 @@
     <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/6843_3d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.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"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_3d_1.launch

@@ -19,11 +19,14 @@
     <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_1"/>
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
     <remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
   </node>
 
   <!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
-  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
 
   <node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="5 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/6843_quad_3d_TDA4VM.launch

@@ -0,0 +1,40 @@
+<!--
+  This file will launch four instances of the mmWave sensor node and configure each TI mmWave 6843 sensor using a 3D config. This launch file is intended
+  to be used to launch the mmWave sensor node on TI's TDA4VM Jacinto Processors, as such a separate command must be used to launch rViz 
+  for visualization on a PC.
+ -->
+
+<launch>
+
+  <include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
+    <arg name="device_num" value="0" />
+    <arg name="tf_args" value="0 0 0 0 0 0" />
+    <arg name="com_user" value="/dev/ttyUSB0"/>
+    <arg name="com_data" value="/dev/ttyUSB1"/>
+  </include>
+
+  <include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
+    <arg name="device_num" value="1" />
+    <arg name="tf_args" value="0 0 0 0 0 0" />
+    <arg name="com_user" value="/dev/ttyUSB2"/>
+    <arg name="com_data" value="/dev/ttyUSB3"/>
+    <arg name="node_start_delay" value="2"/>
+  </include>
+
+  <include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
+    <arg name="device_num" value="2" />
+    <arg name="tf_args" value="0 0 0 0 0 0" />
+    <arg name="com_user" value="/dev/ttyUSB4"/>
+    <arg name="com_data" value="/dev/ttyUSB5"/>
+    <arg name="node_start_delay" value="4"/>
+  </include>
+
+  <include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
+    <arg name="device_num" value="3" />
+    <arg name="tf_args" value="0 0 0 0 0 0" />
+    <arg name="com_user" value="/dev/ttyUSB6"/>
+    <arg name="com_data" value="/dev/ttyUSB7"/>
+    <arg name="node_start_delay" value="6"/>
+  </include>
+  
+</launch>

ros_driver/src/ti_mmwave_rospkg/launch/camera_overlay.launch

@@ -25,10 +25,13 @@
     <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>
 
-  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="mmWaveQuickConfig" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642es2_short_range.cfg" output="screen" />
+  <node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="mmWaveQuickConfig" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642es2_short_range.cfg" output="screen" >
+    <param name="mmWaveCLI_name" value="/mmWaveCLI" />
+  </node>
 
   <node pkg="tf" type="static_transform_publisher" name="radar_baselink" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>
 

ros_driver/src/ti_mmwave_rospkg/launch/rviz/ti_mmwave_quad.rviz

@@ -0,0 +1,244 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1/Offset1
+        - /PointCloud21/Autocompute Value Bounds1
+      Splitter Ratio: 0.5
+    Tree Height: 573
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+      - /Current View1/Focal Point1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 5
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 0.5
+        Min Value: -0.5
+        Value: false
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 0; 0
+      Color Transformer: Intensity
+      Decay Time: 0.4000000059604645
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 102; 0; 0
+      Min Color: 255; 102; 102
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.10000000149011612
+      Style: Spheres
+      Topic: /ti_mmwave/radar_scan_pcl_0
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 0.5
+        Min Value: -0.5
+        Value: false
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 204; 204; 0
+      Color Transformer: Intensity
+      Decay Time: 0.4000000059604645
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 102; 102; 0
+      Min Color: 255; 255; 51
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.10000000149011612
+      Style: Spheres
+      Topic: /ti_mmwave/radar_scan_pcl_1
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0.4000000059604645
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 74; 112; 0
+      Min Color: 170; 255; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.10000000149011612
+      Style: Spheres
+      Topic: /ti_mmwave/radar_scan_pcl_2
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0.4000000059604645
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 0; 118; 118
+      Min Color: 0; 255; 255
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.10000000149011612
+      Style: Spheres
+      Topic: /ti_mmwave/radar_scan_pcl_3
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: <Fixed Frame>
+      Show Trail: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: ti_mmwave_pcl
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 14.119680404663086
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 5
+        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.5597963333129883
+      Target Frame: <Fixed Frame>
+      Yaw: 3.144761800765991
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 876
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001ad000002c6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000004afc0100000002fb0000000800540069006d00650100000000000005ff0000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000044c000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1535
+  X: 331
+  Y: 27

ros_driver/src/ti_mmwave_rospkg/launch/viz_remote_pc.launch

@@ -0,0 +1,10 @@
+<!--
+  This file will launch rViz for visualization of point cloud data collected by mmWave radar. 
+  This file is intended to be used for visualization on a remote PC while radar driver runs on TDA4 device.
+ -->
+
+<launch>
+
+  <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/launch/viz_remote_pc_quad.launch

@@ -0,0 +1,10 @@
+<!--
+  This file will launch rViz for visualization of point cloud data collected by four mmWave radar sensors. 
+  This file is intended to be used for visualization on a remote PC while the radar driver runs on TDA4 device.
+ -->
+
+<launch>
+
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_rospkg)/launch/rviz/ti_mmwave_quad.rviz"/>
+
+</launch>

ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp

@@ -7,6 +7,19 @@
 #include <pcl/point_cloud.h>
 #include <pcl/io/pcd_io.h>
 
+DataUARTHandler* gDataHandlerPtr;
+
+void DataUARTHandler::sigHandler(int32_t sig)
+{
+    switch(sig)
+    {
+    case SIGINT:
+        gDataHandlerPtr->stop();
+
+    }
+    
+}
+
 struct mmWaveCloudType
 {
     PCL_ADD_POINT4D;
@@ -54,6 +67,10 @@ DataUARTHandler::DataUARTHandler(ros::NodeHandle* nh) : currentBufp(&pingPongBuf
 
     ROS_INFO("\n\n==============================\nList of parameters\n==============================\nNumber of range samples: %d\nNumber of chirps: %d\nf_s: %.3f MHz\nf_c: %.3f GHz\nBandwidth: %.3f MHz\nPRI: %.3f us\nFrame time: %.3f ms\nMax range: %.3f m\nRange resolution: %.3f m\nMax Doppler: +-%.3f m/s\nDoppler resolution: %.3f m/s\n==============================\n", \
         nr, nd, fs/1e6, fc/1e9, BW/1e6, PRI*1e6, tfr*1e3, max_range, vrange, max_vel/2, vvel);
+
+    gDataHandlerPtr = this;
+
+    stop_threads = false;
 }
 
 void DataUARTHandler::setFrameID(char* myFrameID)
@@ -228,8 +245,16 @@ void *DataUARTHandler::syncedBufferSwap(void)
     
         while(countSync < COUNT_SYNC_MAX)
         {
+            if(stop_threads)
+            {
+                pthread_mutex_unlock(&countSync_mutex);
+                pthread_cond_signal(&sort_go_cv);
+                pthread_cond_signal(&read_go_cv);
+                pthread_exit(NULL);
+            }
+
             pthread_cond_wait(&countSync_max_cv, &countSync_mutex);
-            
+
             pthread_mutex_lock(&currentBufp_mutex);
             pthread_mutex_lock(&nextBufp_mutex);
             
@@ -282,7 +307,7 @@ void *DataUARTHandler::sortIncomingData( void )
     
     while(ros::ok())
     {
-        
+
         switch(sorterState)
         {
             
@@ -381,6 +406,7 @@ void *DataUARTHandler::sortIncomingData( void )
             // RScan->header.seq = 0;
             // RScan->header.stamp = (uint64_t)(ros::Time::now());
             // RScan->header.stamp = (uint32_t) mmwData.header.timeCpuCycles;
+            pcl_conversions::toPCL(ros::Time::now(), RScan->header.stamp);
             RScan->header.frame_id = frameID;
             RScan->height = 1;
             RScan->width = mmwData.numObjOut;
@@ -671,9 +697,8 @@ void *DataUARTHandler::sortIncomingData( void )
                     
                     //ROS_INFO("mmwData.numObjOut after = %d", mmwData.numObjOut);
                     //ROS_INFO("DataUARTHandler Sort Thread: number of obj = %d", mmwData.numObjOut );
-                    
-                    DataUARTHandler_pub.publish(RScan);
                 }
+                DataUARTHandler_pub.publish(RScan);
 
                 //ROS_INFO("DataUARTHandler Sort Thread : CHECK_TLV_TYPE state says tlvCount max was reached, going to switch buffer state");
                 sorterState = SWAP_BUFFERS;
@@ -782,10 +807,18 @@ void *DataUARTHandler::sortIncomingData( void )
 
 void DataUARTHandler::start(void)
 {
-    
+
     pthread_t uartThread, sorterThread, swapThread;
-    
+
     int  iret1, iret2, iret3;
+
+    sigset_t set;
+    int s;
+
+    /* Block SIGINT on main thread and subsequently created threads */
+    sigemptyset(&set);
+    sigaddset(&set, SIGINT);
+    s = pthread_sigmask(SIG_BLOCK, &set, NULL);
     
     pthread_mutex_init(&countSync_mutex, NULL);
     pthread_mutex_init(&nextBufp_mutex, NULL);
@@ -817,24 +850,29 @@ void DataUARTHandler::start(void)
         ROS_INFO("Error - pthread_create() return code: %d\n",iret1);
         ros::shutdown();
     }
+
+    /* Unlock SIGINT on main thread */
+    s = pthread_sigmask(SIG_UNBLOCK, &set, NULL);
+
+    signal(SIGINT, sigHandler);
     
     ros::spin();
 
-    pthread_join(iret1, NULL);
+    pthread_join(uartThread, NULL);
     ROS_INFO("DataUARTHandler Read Thread joined");
-    pthread_join(iret2, NULL);
+
+    pthread_join(sorterThread, NULL);
     ROS_INFO("DataUARTHandler Sort Thread joined");
-    pthread_join(iret3, NULL);
+
+    pthread_join(swapThread, NULL);
     ROS_INFO("DataUARTHandler Swap Thread joined");
-    
+
     pthread_mutex_destroy(&countSync_mutex);
     pthread_mutex_destroy(&nextBufp_mutex);
     pthread_mutex_destroy(&currentBufp_mutex);
     pthread_cond_destroy(&countSync_max_cv);
     pthread_cond_destroy(&read_go_cv);
     pthread_cond_destroy(&sort_go_cv);
-    
-    
 }
 
 void* DataUARTHandler::readIncomingData_helper(void *context)
@@ -881,4 +919,16 @@ void DataUARTHandler::visualize(const ti_mmwave_rospkg::RadarScan &msg){
     marker.color.b = 1;
 
     marker_pub.publish(marker);
+}
+
+void DataUARTHandler::stop()
+{
+    ROS_DEBUG("Stopping Threads");
+
+    stop_threads = true;
+    ros::shutdown();
+    
+    pthread_cond_signal(&read_go_cv);
+    pthread_cond_signal(&sort_go_cv);
+    pthread_cond_signal(&countSync_max_cv);
 }

ros_driver/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp

@@ -15,11 +15,12 @@ void mmWaveCommSrv::onInit()
     private_nh2.getParam("command_port", mySerialPort);
     
     private_nh2.getParam("command_rate", myBaudRate);
+    private_nh2.getParam("mmWaveCLI_name", mmWaveCLIName);
     
     ROS_INFO("mmWaveCommSrv: command_port = %s", mySerialPort.c_str());
     ROS_INFO("mmWaveCommSrv: command_rate = %d", myBaudRate);
     
-    commSrv = private_nh.advertiseService("/mmWaveCLI", &mmWaveCommSrv::commSrv_cb, this);
+    commSrv = private_nh.advertiseService(mmWaveCLIName, &mmWaveCommSrv::commSrv_cb, this);
     
     NODELET_DEBUG("mmWaveCommsrv: Finished onInit function");
 }

ros_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp

@@ -9,6 +9,7 @@
 int main(int argc, char **argv) {
     ros::init(argc, argv, "mmWaveQuickConfig");
     ros::NodeHandle nh;
+    ros::NodeHandle private_nh("~");
     ti_mmwave_rospkg::mmWaveCLI srv;
     if (argc != 2) {
         ROS_INFO("mmWaveQuickConfig: usage: mmWaveQuickConfig /file_directory/params.cfg");
@@ -16,11 +17,13 @@ int main(int argc, char **argv) {
     } else 
         ROS_INFO("mmWaveQuickConfig: Configuring mmWave device using config file: %s", argv[1]);
     
-    ros::ServiceClient client = nh.serviceClient<ti_mmwave_rospkg::mmWaveCLI>("/mmWaveCLI");
+    std::string mmWaveCLIName;
+    private_nh.getParam("mmWaveCLI_name", mmWaveCLIName);
+    ros::ServiceClient client = nh.serviceClient<ti_mmwave_rospkg::mmWaveCLI>(mmWaveCLIName);
     std::ifstream myParams;
     ti_mmwave_rospkg::ParameterParser parser;
     //wait for service to become available
-    ros::service::waitForService("/mmWaveCLI", 10000); 
+    ros::service::waitForService(mmWaveCLIName, 10000); 
     
     //wait 0.5 secs to avoid multi-sensor conflicts
     ros::Duration(0.5).sleep();