[SOLVED] Thread does not terminate
Hello, I've a problem with terminating a node and I hope someone can help. Consider some code like below:
unsigned char interestingNumber;
std::mutex mu;
std_msgs::UInt16 msgUInt;
// Function prototypes
void threadCallback(const ros::Publisher* const);
void subscriberCallback(const std_msgs::Float32MultiArray::ConstPtr&);
int main(int argc, char **argv)
{
ros::init(argc, argv, "someNode");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<std_msgs::UInt16>("topic", 1);
ros::Subscriber sub = n.subscribe("interestingtopic", 10, subscriberCallback);
msgUInt.data = 1;
std::thread someThread(threadCallback, &pub);
ros::spin();
someThread.join();
return 0;
}
void subscriberCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
{
mu.lock();
interestingNumber = msg->layout.dim[2].size;
mu.unlock();
}
void threadCallback(const ros::Publisher* const p)
{
ros::Rate loopRateInner(1000);
while(ros::ok())
{
if(interestingNumber == 0)
msgUint.data = 1;
while(interestingNumber == 0)
{
if(msgUInt.data == 2000)
msgUInt.data = 1;
msgUInt.data++;
p->publish(msgUInt);
loopRateInner.sleep();
}
}
}
So what this basically does is, it subscribes to a topic and assigns a variable with a value of the messages on that subscribed topic. Asynchronously to that another thread has to publish to another topic depending on that received value. This also works fine. My problem is, that the thread someThread
does not terminate as soon as I shut down all other nodes and the roscore. Also CTRL+C doesn't help. I always have to kill this node via the system manager. And I wonder why this happens, since the outer loop should stop executing as soon as ros::ok() returns false.
How can I fix this?
Thanks in advance for any help!