How do I get out of ros::spin() in another thread?
Hi All,
So what I am doing is enumerated in the points below. After that, I explain my questions in detail.
1) I recently got my hands on an ATI force/torque sensor which is pretty cool. I already had code which publishes the (fx,fy,fz,tx,ty,tz) readings from this sensor on the ROS network.
2) My job is to write the subscriber. The subscriber should have the following functionality:
2.a) I should be able to move around with the subscriber code. This means I want it to be preferably in the form of a single header file or something of the likes.
2.b) I want it to have a start_recording() functionality and stop_recording() functionality. These two functions essentially allow me to log the force/torque data to a text file, and stop the logging.
2.c) I want the data logging to be really fast. The more data I collect, the better for my application. The sensor itself publishes at 33 Hz, so anything in that ballpark is good for me.
3) So what I did was wrote a class called ForceTorqueObject which has some private threaded stuff going on, and some interface functions to allow the user to log/stop-log the data. Here is the class declaration.
class ForceTorqueObject
{
private:
volatile bool recording;
pthread_t ft_sensor_thread;
pthread_mutex_t ft_log_mutex;
pthread_attr_t ft_attr;
ros::NodeHandle *nh;
FILE *ft_curr_logfile;
ros::Subscriber ft_listener;
queue<robot_comm::robot_ForceLog> ft_q; //shared object
static void *ft_subscribe_main(void *args)
{
cout<<"Welcome to subscriber thread..."<<endl;
ForceTorqueObject *ft_sensor = (ForceTorqueObject *)args;
ft_sensor->ft_subscribe();
cout<<"End of subscriber thread..."<<endl;
pthread_exit(NULL);
//return((void *)0);
}
void ft_subscribe();
void ft_sensor_callback(const robot_comm::robot_ForceLog &fLog);
void ft_queue_dump();
public:
ForceTorqueObject(ros::NodeHandle *n); //Constructor
~ForceTorqueObject();
//User functions
void ft_start_recording(const char *fLogFile);
void ft_stop_recording();
void ft_force_exit();
};
4) As seen from above, I am first creating a subscription thread, in which I have a callback function which checks for a flag "recording". Accordingly, it pumps the rosmsg carrying the force/torque data into a queue. ft_start_recording() just sets "recording" to 1, while ft_stop_recording() sets "recording" to 0, and dumps all contents of the queue into a text file. Thus I get my desired "log/stop-log" functionality.
5) In the separate thread for subscribing to the topic, I am calling ros::spin()
from the ForceTorqueObject::ft_subscribe()
function.
All this brings me to my set of questions:
1) How do I get out of the ros::spin() in the recording thread!? I wrote a small main() function to test my new class, and found out that once I am done logging and stop-logging, the subscriber thread still continues to live! I then realized that I could just not think of a way in which I can shut down the subscriber thread from the main thread.
2) So I tried changing things a little bit here-and-there. I tried doing
static void *ft_subscribe_main(void *args)
{
cout<<"Welcome to subscriber thread..."<<endl;
ForceTorqueObject *ft_sensor = (ForceTorqueObject *)args;
while(ros::ok())
{
ft_sensor->ft_subscribe();
ros::spinOnce();
}
cout<<"End of ...