From 63ce5898d7c098d478a67dce719df92721c411b7 Mon Sep 17 00:00:00 2001 From: Josh Dye Date: Wed, 2 Mar 2022 13:50:24 -0600 Subject: [PATCH] Fixed terminating with ctrl+c causing escalating to SIGTERM. --- .../include/DataHandlerClass.h | 12 ++- .../ti_mmwave_rospkg/src/DataHandlerClass.cpp | 95 ++++++++++++++++--- 2 files changed, 93 insertions(+), 14 deletions(-) diff --git a/ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h b/ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h index f2206f3..82b68d2 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 #include #include +#include #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 by signal handling thread to notify other threads to exit*/ + bool stop_threads; /*Read/Write Buffers*/ std::vector pingPongBuffers[2]; diff --git a/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp b/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp index a9dd8a8..9e2df2d 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 #include +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,8 +252,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(¤tBufp_mutex); pthread_mutex_lock(&nextBufp_mutex); @@ -283,6 +315,14 @@ void *DataUARTHandler::sortIncomingData( void ) while(ros::ok()) { + if(stop_threads) + { + pthread_mutex_unlock(¤tBufp_mutex); + pthread_mutex_unlock(&countSync_mutex); + pthread_cond_signal(&countSync_max_cv); + pthread_exit(NULL); + } + switch(sorterState) { @@ -782,10 +822,17 @@ void *DataUARTHandler::sortIncomingData( void ) void DataUARTHandler::start(void) { - + pthread_t uartThread, sorterThread, swapThread; - + 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); @@ -817,24 +864,35 @@ void DataUARTHandler::start(void) ROS_INFO("Error - pthread_create() return code: %d\n",iret1); ros::shutdown(); } - - ros::spin(); - 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"); + s = pthread_sigmask(SIG_UNBLOCK, &set, NULL); + + 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); pthread_mutex_destroy(¤tBufp_mutex); pthread_cond_destroy(&countSync_max_cv); pthread_cond_destroy(&read_go_cv); pthread_cond_destroy(&sort_go_cv); - - + + ros::shutdown(); } void* DataUARTHandler::readIncomingData_helper(void *context) @@ -881,4 +939,15 @@ 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; + + pthread_cond_signal(&read_go_cv); + pthread_cond_signal(&sort_go_cv); + pthread_cond_signal(&countSync_max_cv); } \ No newline at end of file