ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

How do I get out of ros::spin() in another thread?

asked 2013-03-13 15:20:44 -0600

Nishant gravatar image

updated 2014-01-28 17:15:40 -0600

ngrennan gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2013-03-13 16:03:50 -0600

jbohren gravatar image

Ok, so there are a few issues here.

  1. If you're already publishing the force/torque data over ROS, is there a reason why you're writing your own data logger instead of using rosbag?

  2. Assuming ft_sensor->ft_subscribe() is creating a subscriber, when you replace ros::spin() with ros::spinOnce() in a loop, you're creating new subscriptions as fast as possible, over and over. Each time you create a subscriber, you are necessitating communication with the ROS master.

  3. If you're already using ROS, then it's not a lot to use boost threads instead of pthreads. On POSIX systems, boost threads are pthreads, but with a more modern c++-like interface which you will find less error-prone.

  4. I suspect you're doing something wonky with threading to keep your program from exiting. Once a SIGINT (ctrl-c) is received by the program, ros::shutdown() should be called. If you're doing something to keep that signal from being handled, then when you want ros::ok() to return false or ros::spin() to return, then simply call ros::shutdown() manually as shown here.

edit flag offensive delete link more

Comments

@jbohren : Thanks for the quick answers! Here are my thoughts on your points: 1) I want to write the data that I log to text files. Can I do that with rosbag? (I am fairly new to it) 2) This is a perfect answer to my corresponding question. I totally agree 3) I'll look into this.

Nishant gravatar image Nishant  ( 2013-03-13 17:31:09 -0600 )edit

@Nishant Yeah, converting ROS bagfiles to text files (usually comma-seprated files) or matlab files is pretty common: 123

jbohren gravatar image jbohren  ( 2013-03-13 17:43:54 -0600 )edit

@jbohren : This is very interesting. I shall try to migrate to rosbag over time. As per your answer (4) and provided links, I have a question which is provided in the comment below (ran out of space on this one)

Nishant gravatar image Nishant  ( 2013-03-14 11:36:45 -0600 )edit

Suppose I am calling ros::spinOnce() in a while(ros::ok()) loop. What is the difference between this setup, and calling ros::spinOnce() from within a while(stayActive) (assuming stayActive is a bool which tells the loop to stay active) ? Can't I just do option (2) and turn stayActive=0 to exit loop?

Nishant gravatar image Nishant  ( 2013-03-14 11:41:03 -0600 )edit

@jbohren : I cannot thank you enough jbohren. I have found out so much in this single answer of yours itself! How callbacks work, about rosbag and boost threads. For now, what I am doing is executing all that is in GlobalCallbackQueue in a while(stayActive) loop in the subscriber thread.

Nishant gravatar image Nishant  ( 2013-03-14 11:55:04 -0600 )edit

When I want the thread to shut down, I do stayActive=0 (using a mutex). I observed over multiple runs, that I get a slightly lesser recording frequency (~32.7 Hz) compared to original 33 Hz. This might be due to the fact that ros::spin() uses some inbuilt optimization which I am not using...correct?

Nishant gravatar image Nishant  ( 2013-03-14 11:58:31 -0600 )edit

There should be no performance difference between spin() and spinOnce() in a loop. What's probably happening is your mutex-protected stayActive is blocking your spin loop sometimes. You don't really need to protect a bool with a mutex when you're using it like that. (http://goo.gl/cK8Qf)

jbohren gravatar image jbohren  ( 2013-03-14 13:05:10 -0600 )edit

Also, if you weren't sure about it, all spinOnce() does is call all of the waiting callbacks (subscriber callbacks, service callbacks, etc). They are all called from that function call and all need to return before that call returns.

jbohren gravatar image jbohren  ( 2013-03-14 13:08:53 -0600 )edit
0

answered 2016-02-13 15:25:51 -0600

user23fj239 gravatar image

@add I was spinning within boost::thread which subscribed to a few topics. What I simply forgot was to add the ros::ok() check:

        //subscribe to topics ...

        //pump callbacks but check if SIGINT is not set
        while(ros::ok()) ros::spin();
edit flag offensive delete link more

Question Tools

Stats

Asked: 2013-03-13 15:20:44 -0600

Seen: 8,730 times

Last updated: Feb 13 '16