In this section we explore the ways a callback function can be implemented in ROS.
We will be using the code examples installed in Sect.2. We will begin with a simple example including callbacks of a non-threaded nature, then proceed to callbacks in a threaded context.
4.1 Basic Callback Functions
Nodes communicate through messages. These messages are processed in a callback function. In C++, callback functions are implemented through a function pointer.
Consider the following example from ros-thread-tutorial, installed in Sect.2.
#include <ros/ros.h>
#include <ros/network.h>
#include <nav_msgs/Odometry.h>
#include <stdio.h>
std::printf("Pose: (%.4f, %.4f, %.4f)\n", m_xPos, m_yPos, m_aPos);
poseMessage.xPose = m_xPos;
poseMessage.yPose = m_yPos;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "pose_listener");
ros::NodeHandle n;
ros::Rate rate(100);
ros::Subscriber sub = n.subscribe("odom", 10, poseCallback);
ros::Publisher pub = n.advertise<Pose2D>("/pose2d", 100);
while (ros::ok()) {
We will break this example into parts for discussion. The function main begins by initializing the ros node named “pose_listener.” A ros::NodeHandle is then created. This object acts as an interface for subscribing/publishing and other related activity. Such an object is essential in any ros application.
ros::Subscriber sub = n.subscribe("odom", 10, poseCallback);
The above line tells the ros::Subscriber object named “sub” to listen to the topic “odom” with and store the messages with a buffer size of 10. The line also tells the subscriber how the data should be processed, sending each datum to the function poseCallback.
ros::Publisher pub = n.advertise<Pose2D>("/pose2d", 100);
This line tells the nodehandle to advertise a new topic called /pose2d that has a queue of size five; in other words, the topic will store at most five messages should the publisher speed fall behind the requested speed. After the publishers and subscribers
have been declared, we enter a loop that publishes our custom message type. The command ros::spinOnce() tells the topics to refresh themselves, and we pause the loop for the amount of time we specified earlier.
Some sample output is provided below, produced from a simulated Pioneer-3AT robot moving in direction of the positive x-axis inside the Gazebo Simulator.
$ rosrun ros_threading_example pose_echo
4.2 Robots as a Thread
The implementation considered in Sect.4.1is not so useful when working in the context of User Interfaces. Certainly, the ROS functionality should be entirely sepa-rate from any sort of user interface, but must also be connected. Our goal is simple:
we pass the necessary information to and from a continuously running ROS node to the interface. As the reader may verify, this lends very nicely to a threaded imple-mentation, as both the interface and ROS communications are quite hindered by waiting.
To implement a thread in ROS, there are two functions we must ensure exist. These functions include the destructor virtual ˜RobotThread() and the function that calls ros::init(int argc, char ** argv), implemented here as bool init(). bool init(), in the case of Qt, will also be responsible for setting up and starting the thread on which our robot will run.
Here we provide an example of a very minimal implementation of RobotThread that is built upon Qt’s thread library, QThread. In the following example, we will write the same program as before, but in a threaded setting. This form of implemen-tation is not as succinct as that of the prior, but is far more robust. Such a header is also quite reusable, as it can be minimally adapted for a myriad of situations. It is important to note, however, that due to limitations in the roscpp API, multiple instances of this thread cannot be created as it is implemented here. The source for this example was installed with that of the prior example in Sect.2, so we will only examine the file RobotThread.cpp (as the RobotThread.h file holds only the class).
#include "RobotThread.h"
RobotThread::RobotThread(int argc, char** pArgv) : m_Init_argc(argc),
m_pInit_argv(pArgv)
{/** Constructor for the robot thread **/}
RobotThread::˜RobotThread()
return false;//do not if ros is not running
ros::start();
ros::Time::init();
ros::NodeHandle nh;
pose_listener = nh.subscribe("odom", 100, &RobotThread::poseCallback, this);
pose2d_pub = nh.advertise<Pose2D>("/pose2d", 100);
m_pThread->start();
return true;
}//set up ros
void RobotThread::poseCallback(const nav_msgs::Odometry & msg) {
QMutex pMutex = new QMutex();
pMutex->lock();
m_xPos = msg.pose.pose.position.x;
m_yPos = msg.pose.pose.position.y;
m_aPos = msg.pose.pose.orientation.w;
std::printf("Pose: (%.4f, %.4f, %.4f)\n", m_xPos, m_yPos, m_aPos);
pMutex->unlock();
delete pMutex;
}//callback method to echo the robot’s position
void RobotThread::run()
poseMessage.xPose = m_xPos;
}//run while ROS is ok.
}
We begin with the constructor. The function RobotThread::RobotThread (int argc, char ** argv) is responsible for creating the Robot Thread.
It’s function is rather limited: it simply initializes ‘m_Init_argc’ and ‘m_pInit_argv’
which are to be passed to ROS.
The destructor RobotThread::˜RobotThread() serves as a clean exit.
The function first verifies that ROS is running. If so, the function halts its ROS functionality and waits for this action to complete. Lastly, wait() is called to stop the thread from processing.
RobotThread::init()is the function responsible for setting up our thread.
Older Qt implementations tend to subclass QThread. It was later decided that, instead, one should subclass QObject (the most basic of Qt constructions) and move the object onto a thread. This provides both better control and a clearer thread affinity. This process is not too difficult to accomplish, as we simply need to create a thread object, move our implementation onto said object and identify the function that will be running on the thread. We will discuss the meaning of the connect statement later in Sect.5.2.
RobotThread::init()is also responsible for bringing up the ROS services.
As in PoseEcho, ros::init starts the node named pose_listener. To verify that the master node is running appropriately, we make a call to ros::master::
check(); if there is no master node, our function returns false, meaning that initialization of our node failed. ROS is then started, as is the timer for ROS.
The ros::subscriber in this context is stored as a member object called pose_listener. This subscribes to the /odom topic, storing a buffer of ten mes-sages with the callback function RobotThread::poseCallback. Take note of the & in front of the function in this context, in contrast to the prior example. The next line advertises topic /pose2d with a queue of size five.
Lastly, the function RobotThread::run() is the run function for our thread.
We have a member object poseMessage which is a message of type Pose2D.
We also have member variables m_xPos and m_yPos, which are given data by the sensor callback function and are in turn used for the data of Pose2D. Using the same environment as before, we obtain the following output.
$ rosrun ros_threading_example pose_echo_threaded Pose: (0.0664, 0.0010, 1.0000)
Pose: (0.0677, 0.0010, 1.0000) Pose: (0.0690, 0.0010, 1.0000)
Pose: (0.0702, 0.0010, 1.0000) Pose: (0.0715, 0.0010, 1.0000) Pose: (0.0728, 0.0010, 1.0000) Pose: (0.0741, 0.0010, 1.0000)