yemai/mmwave_ti_ros
1
0
Fork 0
mirror of https://git.ti.com/git/mmwave_radar/mmwave_ti_ros.git synced 2026-05-21 09:22:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

Improved escalating to sigterm fix

Browse Source
This commit is contained in:
Josh Dye
2022-03-04 10:33:44 -06:00
parent 63ce5898d7
commit 00949b72da
2 changed files with 8 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h
View File

@@ -98,7 +98,7 @@ private:
/*Mutex protected variable which synchronizes threads*/ /*Mutex protected variable which synchronizes threads*/
int countSync; int countSync;
/*Boolean used by signal handling thread to notify other threads to exit*/ /*Boolean used to notify threads to exit*/
bool stop_threads; bool stop_threads;
/*Read/Write Buffers*/ /*Read/Write Buffers*/

33
ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp
View File

@@ -155,13 +155,6 @@ void *DataUARTHandler::readIncomingData(void)
while(ros::ok()) 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*/ /*Start reading UART data and writing to buffer while also checking for magicWord*/
last8Bytes[0] = last8Bytes[1]; last8Bytes[0] = last8Bytes[1];
last8Bytes[1] = last8Bytes[2]; last8Bytes[1] = last8Bytes[2];
@@ -315,14 +308,6 @@ void *DataUARTHandler::sortIncomingData( void )
while(ros::ok()) 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) switch(sorterState)
{ {
@@ -830,6 +815,7 @@ void DataUARTHandler::start(void)
sigset_t set; sigset_t set;
int s; int s;
/* Block SIGINT on main thread and subsequently created threads */
sigemptyset(&set); sigemptyset(&set);
sigaddset(&set, SIGINT); sigaddset(&set, SIGINT);
s = pthread_sigmask(SIG_BLOCK, &set, NULL); s = pthread_sigmask(SIG_BLOCK, &set, NULL);
@@ -865,25 +851,21 @@ void DataUARTHandler::start(void)
ros::shutdown(); ros::shutdown();
} }
/* Unlock SIGINT on main thread */
s = pthread_sigmask(SIG_UNBLOCK, &set, NULL); s = pthread_sigmask(SIG_UNBLOCK, &set, NULL);
signal(SIGINT, sigHandler); signal(SIGINT, sigHandler);
ros::Rate r(10); // 10 hz ros::spin();
while(!stop_threads)
{
ros::spinOnce();
r.sleep();
}
pthread_join(uartThread, NULL); pthread_join(uartThread, NULL);
ROS_DEBUG("DataUARTHandler Read Thread joined"); ROS_INFO("DataUARTHandler Read Thread joined");
pthread_join(sorterThread, NULL); pthread_join(sorterThread, NULL);
ROS_DEBUG("DataUARTHandler Sort Thread joined"); ROS_INFO("DataUARTHandler Sort Thread joined");
pthread_join(swapThread, NULL); pthread_join(swapThread, NULL);
ROS_DEBUG("DataUARTHandler Swap Thread joined"); ROS_INFO("DataUARTHandler Swap Thread joined");
pthread_mutex_destroy(&countSync_mutex); pthread_mutex_destroy(&countSync_mutex);
pthread_mutex_destroy(&nextBufp_mutex); pthread_mutex_destroy(&nextBufp_mutex);
@@ -891,8 +873,6 @@ void DataUARTHandler::start(void)
pthread_cond_destroy(&countSync_max_cv); pthread_cond_destroy(&countSync_max_cv);
pthread_cond_destroy(&read_go_cv); pthread_cond_destroy(&read_go_cv);
pthread_cond_destroy(&sort_go_cv); pthread_cond_destroy(&sort_go_cv);
ros::shutdown();
} }
void* DataUARTHandler::readIncomingData_helper(void *context) void* DataUARTHandler::readIncomingData_helper(void *context)
@@ -946,6 +926,7 @@ void DataUARTHandler::stop()
ROS_DEBUG("Stopping Threads"); ROS_DEBUG("Stopping Threads");
stop_threads = true; stop_threads = true;
ros::shutdown();
pthread_cond_signal(&read_go_cv); pthread_cond_signal(&read_go_cv);
pthread_cond_signal(&sort_go_cv); pthread_cond_signal(&sort_go_cv);