Fixed terminating with ctrl+c causing escalating to SIGTERM.

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,6 +49,9 @@ 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);
     
@@ -55,6 +59,9 @@ public:
     
     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;
 
@@ -91,6 +98,9 @@ private:
     /*Mutex protected variable which synchronizes threads*/
     int countSync;
 
+    /*Boolean used by signal handling thread to notify other threads to exit*/
+    bool stop_threads;
+    
     /*Read/Write Buffers*/
     std::vector<uint8_t> pingPongBuffers[2];
     

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)
@@ -138,6 +155,13 @@ void *DataUARTHandler::readIncomingData(void)
     
     while(ros::ok())
     {
+        if(stop_threads)
+        {
+            pthread_mutex_unlock(&nextBufp_mutex);
+            pthread_mutex_unlock(&countSync_mutex);
+            pthread_cond_signal(&countSync_max_cv);
+            pthread_exit(NULL);
+        }
         /*Start reading UART data and writing to buffer while also checking for magicWord*/
         last8Bytes[0] = last8Bytes[1];
         last8Bytes[1] = last8Bytes[2];
@@ -228,6 +252,14 @@ 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);
@@ -283,6 +315,14 @@ void *DataUARTHandler::sortIncomingData( void )
     while(ros::ok())
     {
         
+        if(stop_threads)
+        {
+            pthread_mutex_unlock(&currentBufp_mutex);
+            pthread_mutex_unlock(&countSync_mutex);
+            pthread_cond_signal(&countSync_max_cv);
+            pthread_exit(NULL);
+        }
+
         switch(sorterState)
         {
             
@@ -787,6 +827,13 @@ void DataUARTHandler::start(void)
 
     int  iret1, iret2, iret3;
 
+    sigset_t set;
+    int s;
+
+    sigemptyset(&set);
+    sigaddset(&set, SIGINT);
+    s = pthread_sigmask(SIG_BLOCK, &set, NULL);
+    
     pthread_mutex_init(&countSync_mutex, NULL);
     pthread_mutex_init(&nextBufp_mutex, NULL);
     pthread_mutex_init(&currentBufp_mutex, NULL);
@@ -818,14 +865,25 @@ void DataUARTHandler::start(void)
         ros::shutdown();
     }
 
-    ros::spin();
+    s = pthread_sigmask(SIG_UNBLOCK, &set, NULL);
 
-    pthread_join(iret1, NULL);
-    ROS_INFO("DataUARTHandler Read Thread joined");
-    pthread_join(iret2, NULL);
-    ROS_INFO("DataUARTHandler Sort Thread joined");
-    pthread_join(iret3, NULL);
-    ROS_INFO("DataUARTHandler Swap Thread joined");
+    signal(SIGINT, sigHandler);
+    
+    ros::Rate r(10); // 10 hz
+    while(!stop_threads)
+    {
+        ros::spinOnce();
+        r.sleep();
+    }
+
+    pthread_join(uartThread, NULL);
+    ROS_DEBUG("DataUARTHandler Read Thread joined");
+
+    pthread_join(sorterThread, NULL);
+    ROS_DEBUG("DataUARTHandler Sort Thread joined");
+
+    pthread_join(swapThread, NULL);
+    ROS_DEBUG("DataUARTHandler Swap Thread joined");
 
     pthread_mutex_destroy(&countSync_mutex);
     pthread_mutex_destroy(&nextBufp_mutex);
@@ -834,7 +892,7 @@ void DataUARTHandler::start(void)
     pthread_cond_destroy(&read_go_cv);
     pthread_cond_destroy(&sort_go_cv);
 
-    
+    ros::shutdown();
 }
 
 void* DataUARTHandler::readIncomingData_helper(void *context)
@@ -882,3 +940,14 @@ void DataUARTHandler::visualize(const ti_mmwave_rospkg::RadarScan &msg){
 
     marker_pub.publish(marker);
 }
+
+void DataUARTHandler::stop()
+{
+    ROS_DEBUG("Stopping Threads");
+
+    stop_threads = true;
+    
+    pthread_cond_signal(&read_go_cv);
+    pthread_cond_signal(&sort_go_cv);
+    pthread_cond_signal(&countSync_max_cv);
+}