Skip to content

Instantly share code, notes, and snippets.

@bgromov
Last active May 15, 2023 13:50
Show Gist options
  • Save bgromov/45ebeced9e8067d9f13cceececc00d5b to your computer and use it in GitHub Desktop.
Save bgromov/45ebeced9e8067d9f13cceececc00d5b to your computer and use it in GitHub Desktop.
Demonstrates the use of AsyncSpinner and custom callback queue in ROS
#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();
}
@liruihao
Copy link

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,

@LeDSantos
Copy link

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment