diff --git a/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt b/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt
index e9bdea3..0c14c80 100644
--- a/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt
+++ b/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})
diff --git a/ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h b/ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h
index f2206f3..ee83232 100644
--- a/ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h
+++ b/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];
diff --git a/ros_driver/src/ti_mmwave_rospkg/include/mmWaveCommSrv.hpp b/ros_driver/src/ti_mmwave_rospkg/include/mmWaveCommSrv.hpp
index 6b21f10..f73bdde 100644
--- a/ros_driver/src/ti_mmwave_rospkg/include/mmWaveCommSrv.hpp
+++ b/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 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_2d_0.launch b/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_2d_0.launch
index 6fe54ac..20b5e60 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_2d_0.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_2d_1.launch b/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_2d_1.launch
index e80e9d5..56c0386 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_2d_1.launch
+++ b/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"/>
   
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_3d_0.launch b/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_3d_0.launch
index 54199fe..08cb1cb 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_3d_0.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_3d_1.launch b/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_3d_1.launch
index 96544a2..bd6b743 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_3d_1.launch
+++ b/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"/>
   
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/1642_multi_2d_0.launch b/ros_driver/src/ti_mmwave_rospkg/launch/1642_multi_2d_0.launch
index 45c2b70..91301a1 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/1642_multi_2d_0.launch
+++ b/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"/>
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/1642_multi_2d_1.launch b/ros_driver/src/ti_mmwave_rospkg/launch/1642_multi_2d_1.launch
index 26d7291..1296f2c 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/1642_multi_2d_1.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_2d_0.launch b/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_2d_0.launch
index 724f46e..4b81e58 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_2d_0.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_2d_1.launch b/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_2d_1.launch
index 7068db7..bcc9a8d 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_2d_1.launch
+++ b/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"/>
   
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_3d_0.launch b/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_3d_0.launch
index 0ca4288..b97263a 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_3d_0.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_3d_1.launch b/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_3d_1.launch
index 6c5c6c4..53dbd55 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_3d_1.launch
+++ b/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"/>
   
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_2d_0.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_2d_0.launch
index 5213a6a..e2ab7e5 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_2d_0.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_2d_1.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_2d_1.launch
index 2118238..a364804 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_2d_1.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_3d_0.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_3d_0.launch
index 25d745d..c42b379 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_3d_0.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_3d_1.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_3d_1.launch
index 741a764..f919569 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_3d_1.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843_3d_TDA4VM.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843_3d_TDA4VM.launch
new file mode 100644
index 0000000..ce6f20d
--- /dev/null
+++ b/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>
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_2d_0.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_2d_0.launch
index 4eaaac1..00a1e20 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_2d_0.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_2d_1.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_2d_1.launch
index e850eaa..2babc10 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_2d_1.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_3d_0.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_3d_0.launch
index 4e014c3..86596de 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_3d_0.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_3d_1.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_3d_1.launch
index 959e174..7e12a6f 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_3d_1.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843_quad_3d_TDA4VM.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843_quad_3d_TDA4VM.launch
new file mode 100644
index 0000000..758f5c1
--- /dev/null
+++ b/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>
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/camera_overlay.launch b/ros_driver/src/ti_mmwave_rospkg/launch/camera_overlay.launch
index 0bd6f95..6eb6b21 100644
--- a/ros_driver/src/ti_mmwave_rospkg/launch/camera_overlay.launch
+++ b/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"/>
 
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/rviz/ti_mmwave_quad.rviz b/ros_driver/src/ti_mmwave_rospkg/launch/rviz/ti_mmwave_quad.rviz
new file mode 100644
index 0000000..2206c0b
--- /dev/null
+++ b/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
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/viz_remote_pc.launch b/ros_driver/src/ti_mmwave_rospkg/launch/viz_remote_pc.launch
new file mode 100644
index 0000000..277d8ee
--- /dev/null
+++ b/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>
diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/viz_remote_pc_quad.launch b/ros_driver/src/ti_mmwave_rospkg/launch/viz_remote_pc_quad.launch
new file mode 100644
index 0000000..ac009ee
--- /dev/null
+++ b/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>
diff --git a/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp b/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp
index a9dd8a8..f89fac4 100644
--- a/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp
+++ b/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);
 }
\ No newline at end of file
diff --git a/ros_driver/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp b/ros_driver/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp
index 9933326..9a25067 100644
--- a/ros_driver/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp
+++ b/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");
 }
diff --git a/ros_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp b/ros_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp
index 1c322a6..217fd17 100644
--- a/ros_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp
+++ b/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();