Writing a Simple Publisher and Subscriber (C++)
Writing a Simple Publisher and Subscriber (C++)
Writing the Publisher Node
"Node" is the ROS term for an executable that is connected to the ROS network. Here we'll create the publisher ("talker") node which will continually broadcast a message.
Change directory into the beginner_tutorials package, you created in the earlier tutorial,creating a package::
roscd beginner_tutorials
The Code
Create the src/talker.cpp file within the beginner_tutorials package and paste the following inside it:
https://code.ros.org/svn/ros/stacks/ros_tutorials/trunk/roscpp_tutorials/talker/talker.cpp
27 #include "ros/ros.h"
28 #include "std_msgs/String.h"
29
30 #include <sstream>
31
32 /**
33 * This tutorial demonstrates simple sending of messages over the ROS system.
34 */
35 int main(int argc, char **argv)
36 {
37 /**
38 * The ros::init() function needs to see argc and argv so that it can perform
39 * any ROS arguments and name remapping that were provided at the command line. For programmatic
40 * remappings you can use a different version of init() which takes remappings
41 * directly, but for most command-line programs, passing argc and argv is the easiest
42 * way to do it. The third argument to init() is the name of the node.
43 *
44 * You must call one of the versions of ros::init() before using any other
45 * part of the ROS system.
46 */
47 ros::init(argc, argv, "talker");
48
49 /**
50 * NodeHandle is the main access point to communications with the ROS system.
51 * The first NodeHandle constructed will fully initialize this node, and the last
52 * NodeHandle destructed will close down the node.
53 */
54 ros::NodeHandle n;
55
56 /**
57 * The advertise() function is how you tell ROS that you want to
58 * publish on a given topic name. This invokes a call to the ROS
59 * master node, which keeps a registry of who is publishing and who
60 * is subscribing. After this advertise() call is made, the master
61 * node will notify anyone who is trying to subscribe to this topic name,
62 * and they will in turn negotiate a peer-to-peer connection with this
63 * node. advertise() returns a Publisher object which allows you to
64 * publish messages on that topic through a call to publish(). Once
65 * all copies of the returned Publisher object are destroyed, the topic
66 * will be automatically unadvertised.
67 *
68 * The second parameter to advertise() is the size of the message queue
69 * used for publishing messages. If messages are published more quickly
70 * than we can send them, the number here specifies how many messages to
71 * buffer up before throwing some away.
72 */
73 ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
74
75 ros::Rate loop_rate(10);
76
77 /**
78 * A count of how many messages we have sent. This is used to create
79 * a unique string for each message.
80 */
81 int count = 0;
82 while (ros::ok())
83 {
84 /**
85 * This is a message object. You stuff it with data, and then publish it.
86 */
87 std_msgs::String msg;
88
89 std::stringstream ss;
90 ss << "hello world " << count;
91 msg.data = ss.str();
92
93 ROS_INFO("%s", msg.data.c_str());
94
95 /**
96 * The publish() function is how you send messages. The parameter
97 * is the message object. The type of this object must agree with the type
98 * given as a template parameter to the advertise<>() call, as was done
99 * in the constructor above.
100 */
101 chatter_pub.publish(msg);
102
103 ros::spinOnce();
104
105 loop_rate.sleep();
106 ++count;
107 }
108
109
110 return 0;
111 }
The Code Explained
Now, let's break the code down.
27 #include "ros/ros.h"
28
ros/ros.h is a convenience include that includes all the headers necessary to use the most common public pieces of the ROS system.
28 #include "std_msgs/String.h"
29
This includes thestd_msgs/String message, which resides in thestd_msgs package. This is a header generated automatically from theString.msg file in that package. For more information on message definitions, see themsg page.
47 ros::init(argc, argv, "talker");
Initialize ROS. This allows ROS to do name remapping through the command line -- not important for now. This is also where we specify the name of our node. Node names must be unique in a running system.
The name used here must be a base name, ie. it cannot have a / in it.
54 ros::NodeHandle n;
Create a handle to this process' node. The firstNodeHandle created will actually do the initialization of the node, and the last one destructed will cleanup any resources the node was using.
73 ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
Tell the master that we are going to be publishing a message of typestd_msgs/String on the topicchatter. This lets the master tell any nodes listening onchatter that we are going to publish data on that topic. The second argument is the size of our publishing queue. In this case if we are publishing too quickly it will buffer up a maximum of 1000 messages before beginning to throw away old ones.
NodeHandle::advertise() returns aros::Publisher object, which serves two purposes: 1) it contains apublish() method that lets you publish messages onto the topic it was created with, and 2) when it goes out of scope, it will automatically unadvertise.
75 ros::Rate loop_rate(10);
Aros::Rate object allows you to specify a frequency that you would like to loop at. It will keep track of how long it has been since the last call toRate::sleep(), and sleep for the correct amount of time.
In this case we tell it we want to run at 10hz.
81 int count = 0;
82 while (ros::ok())
83 {
By default roscpp will install a SIGINT handler which provides Ctrl-C handling which will causeros::ok() to return false if that happens.
ros::ok() will return false if:
- a SIGINT is received (Ctrl-C)
- we have been kicked off the network by another node with the same name
-
ros::shutdown() has been called by another part of the application.
Once ros::ok() returns false, all ROS calls will fail.
87 std_msgs::String msg;
88
89 std::stringstream ss;
90 ss << "hello world " << count;
91 msg.data = ss.str();
We broadcast a message on ROS using a message-adapted class, generally generated from amsg file. More complicated datatypes are possible, but for now we're going to use the standardString message, which has one member: "data".
101 chatter_pub.publish(msg);
Now we actually broadcast the message to anyone who is connected.
93 ROS_INFO("%s", msg.data.c_str());
ROS_INFO and friends are our replacement forprintf/cout. See the rosconsole documentation for more information.
103 ros::spinOnce();
Callingros::spinOnce() here is not necessary for this simple program, because we are not receiving any callbacks. However, if you were to add a subscription into this application, and did not haveros::spinOnce() here, your callbacks would never get called. So, add it for good measure.
105 loop_rate.sleep();
Now we use theros::Rate object to sleep for the time remaining to let us hit our 10hz publish rate.
Here's the condensed version of what's going on:
- Initialize the ROS system
-
Advertise that we are going to be publishingstd_msgs/String messages on thechatter topic to the master
-
Loop while publishing messages tochatter 10 times a second
Now we need to write a node to receive the messsages.
Writing the Subscriber Node
The Code
Create the src/listener.cpp file within thebeginner_tutorials package and paste the following inside it:
https://code.ros.org/svn/ros/stacks/ros_tutorials/trunk/roscpp_tutorials/listener/listener.cpp
28 #include "ros/ros.h"
29 #include "std_msgs/String.h"
30
31 /**
32 * This tutorial demonstrates simple receipt of messages over the ROS system.
33 */
34 void chatterCallback(const std_msgs::String::ConstPtr& msg)
35 {
36 ROS_INFO("I heard: [%s]", msg->data.c_str());
37 }
38
39 int main(int argc, char **argv)
40 {
41 /**
42 * The ros::init() function needs to see argc and argv so that it can perform
43 * any ROS arguments and name remapping that were provided at the command line. For programmatic
44 * remappings you can use a different version of init() which takes remappings
45 * directly, but for most command-line programs, passing argc and argv is the easiest
46 * way to do it. The third argument to init() is the name of the node.
47 *
48 * You must call one of the versions of ros::init() before using any other
49 * part of the ROS system.
50 */
51 ros::init(argc, argv, "listener");
52
53 /**
54 * NodeHandle is the main access point to communications with the ROS system.
55 * The first NodeHandle constructed will fully initialize this node, and the last
56 * NodeHandle destructed will close down the node.
57 */
58 ros::NodeHandle n;
59
60 /**
61 * The subscribe() call is how you tell ROS that you want to receive messages
62 * on a given topic. This invokes a call to the ROS
63 * master node, which keeps a registry of who is publishing and who
64 * is subscribing. Messages are passed to a callback function, here
65 * called chatterCallback. subscribe() returns a Subscriber object that you
66 * must hold on to until you want to unsubscribe. When all copies of the Subscriber
67 * object go out of scope, this callback will automatically be unsubscribed from
68 * this topic.
69 *
70 * The second parameter to the subscribe() function is the size of the message
71 * queue. If messages are arriving faster than they are being processed, this
72 * is the number of messages that will be buffered up before beginning to throw
73 * away the oldest ones.
74 */
75 ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
76
77 /**
78 * ros::spin() will enter a loop, pumping callbacks. With this version, all
79 * callbacks will be called from within this thread (the main one). ros::spin()
80 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
81 */
82 ros::spin();
83
84 return 0;
85 }
The Code Explained
Now, let's break it down piece by piece, ignoring some pieces that have already been explained above.
34 void chatterCallback(const std_msgs::String::ConstPtr& msg)
35 {
36 ROS_INFO("I heard: [%s]", msg->data.c_str());
37 }
This is the callback function that will get called when a new message has arrived on thechatter topic. The message is passed in a boost shared_ptr, which means you can store it off if you want, without worrying about it getting deleted underneath you, and without copying the underlying data.
75 ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
Subscribe to thechatter topic with the master. ROS will call thechatterCallback() function whenever a new message arrives. The 2nd argument is the queue size, in case we are not able to process messages fast enough. In this case, if the queue reaches 1000 messages, we will start throwing away old messages as new ones arrive.
NodeHandle::subscribe() returns aros::Subscriber object, that you must hold on to until you want to unsubscribe. When the Subscriber object is destructed, it will automatically unsubscribe from thechatter topic.
There are versions of the NodeHandle::subscribe() function which allow you to specify a class member function, or even anything callable by a Boost.Function object. Theroscpp overview contains more information.
82 ros::spin();
ros::spin() enters a loop, calling message callbacks as fast as possible. Don't worry though, if there's nothing for it to do it won't use much CPU. ros::spin() will exit once ros::ok() returns false, which meansros::shutdown() has been called, either by the default Ctrl-C handler, the master telling us to shutdown, or it being called manually.
There are other ways of pumping callbacks, but we won't worry about those here. Theroscpp_tutorials package has some demo applications which demonstrate this. Theroscpp overview also contains more information.
Again, here's a condensed version of what's going on:
- Initialize the ROS system
-
Subscribe to the chatter topic
- Spin, waiting for messages to arrive
-
When a message arrives, the chatterCallback() function is called
Building your nodes
roscreate-pkg will create a default Makefile and CMakeLists.txt for your package.
$ rosed beginner_tutorials CMakeLists.txt
It should look something like this:
-
cmake_minimum_required(VERSION 2.4.6) include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) # Set the build type. Options are: # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage # Debug : w/ debug symbols, w/o optimization # Release : w/o debug symbols, w/ optimization # RelWithDebInfo : w/ debug symbols, w/ optimization # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries #set(ROS_BUILD_TYPE RelWithDebInfo) rosbuild_init() #set the default path for built executables to the "bin" directory set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) #set the default path for built libraries to the "lib" directory set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) #uncomment if you have defined messages #rosbuild_genmsg() #uncomment if you have defined services #rosbuild_gensrv() #common commands for building c++ executables and libraries #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) #target_link_libraries(${PROJECT_NAME} another_library) #rosbuild_add_boost_directories() #rosbuild_link_boost(${PROJECT_NAME} thread) #rosbuild_add_executable(example examples/example.cpp) #target_link_libraries(example ${PROJECT_NAME})
Adding the following at the bottom:
rosbuild_add_executable(talker src/talker.cpp)
rosbuild_add_executable(listener src/listener.cpp)
This will create two executables,talker andlistener, which by default will go into the "bin" directory.
For more information on using CMake with ROS, seeCMakeListsNow run make:
$ make