Demonstrates the use of AsyncSpinner and custom callback queue in ROS

test_spinner.cpp

#include <ros/ros.h>
#include <ros/spinner.h>
#include <ros/callback_queue.h>

#include <std_msgs/Empty.h>
#include <std_msgs/Bool.h>

boost::shared_ptr<ros::AsyncSpinner> g_spinner;

bool g_enable = false;
bool g_trigger = false;

// called from global callback queue
void enableCb(const std_msgs::BoolConstPtr& msg)
{
  if (g_enable != msg->data)
  {
    g_enable = msg->data;
    g_trigger = true;
  }
}

// called from custom callback queue
void heartbeatCb(const std_msgs::EmptyConstPtr& msg)
{
  ROS_INFO("Heartbeat");
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "test_node");

  // custom callback queue
  ros::CallbackQueue queue;

  // This node handle uses global callback queue
  ros::NodeHandle n;
  // and this one uses custom queue
  ros::NodeHandle hb_n;
  // Set custom callback queue
  hb_n.setCallbackQueue(&queue);

  ros::Subscriber enable = n.subscribe<std_msgs::Bool>("enable", 10, enableCb);
  // Note that we use different NodeHandle here
  ros::Subscriber hb = hb_n.subscribe<std_msgs::Empty>("heartbeat", 100, heartbeatCb);

  // Create AsyncSpinner, run it on all available cores and make it process custom callback queue
  g_spinner.reset(new ros::AsyncSpinner(0, &queue));

  g_enable = true;
  g_trigger = true;

  // Loop with 100 Hz rate
  ros::Rate loop_rate(100);
  while (ros::ok())
  {
    // Enable state changed
    if (g_trigger)
    {
      if (g_enable)
      {
        // Clear old callback from the queue
        queue.clear();
        // Start the spinner
        g_spinner->start();
        ROS_INFO("Spinner enabled");
      }
      else
      {
        // Stop the spinner
        g_spinner->stop();
        ROS_INFO("Spinner disabled");
      }
      // Reset trigger
      g_trigger = false;
    }

    // Process messages on global callback queue
    ros::spinOnce();
    loop_rate.sleep();
  }
  // Release AsyncSpinner object
  g_spinner.reset();

  // Wait for ROS threads to terminate
  ros::waitForShutdown();
}

Hello，

Thank you for your code. I have one problem with callbackqueue::clear() function, when I call clear() function and start the spinner again, I found that the clear function is useless, it can not clear the message queue and the callback function will deal with previous stored message.
Do you have any idea about this?

Kind Regards,

Great example, the AsyncSpinner is exactingly what it need for my project. Thanks!!!