Note: This tutorial assumes that you have completed the previous tutorials: Hello Ros (example publisher).
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Hello Embedded Linux (example subscriber)

Description: This tutorial shows how to make an embedded linux application subscribe to a topic published in ROS.

Tutorial Level: BEGINNER

Next Tutorial: Example service

We'll continue our exploration into rosserial for embedded linux by creating a subscriber program for our embedded linux box. It will subscribe to the same topic as in the previous tutorial (chatter), and print strings that are published to the chatter topic.

The Code

Use your favorite editor or IDE to open the ExampleSubscriber tutorial code which is found in examples/ExampleSubscriber/ExampleSubscriber.cpp. You should see the following:

   1 /*
   2  * rosserial_embeddedlinux subscriber example
   3  *
   4  * Prints a string sent on a subscribed ros topic.
   5  * The string can be sent with e.g.
   6  * $ rostopic pub chatter std_msgs/String -- "Hello Embedded Linux"
   7  */
   8 
   9 #include <ros.h>
  10 #include <std_msgs/String.h>
  11 #include <stdio.h>
  12 
  13 ros::NodeHandle  nh;
  14 char *rosSrvrIp = "192.168.15.149";
  15 
  16 void messageCb(const std_msgs::String& received_msg){
  17         printf("Received subscribed chatter message: %s\n", received_msg.data);
  18 }
  19 ros::Subscriber<std_msgs::String> sub("chatter", messageCb );
  20 
  21 int main()
  22 {
  23         //nh.initNode();
  24         nh.initNode(rosSrvrIp);
  25         nh.subscribe(sub);
  26 
  27         while(1) {
  28                   sleep(1);
  29                   nh.spinOnce();
  30         }
  31 }

The Code Explained

Now, let's break the code down.

   9 #include <ros.h>
  10 #include <std_msgs/String.h>
  11 #include <stdio.h>
  12 
  13 ros::NodeHandle  nh;
  14 char *rosSrvrIp = "192.168.15.149";

The beginning of the program is very similar to that of the Hello Ros example publisher in the previous tutorial. As before, you need to include the ros.h header file and header files for any messages that you will be using; in this case, the String message.

You need to declare the node handle which will be used to create the subscriber and handle communications to rosserial_python. RosSrvrIp is the IP address of the ros workstation where rosserial_python runs; edit this value and set it to the IP address of your ros workstation. Rosserial_python will proxy the subscribed messages from the rest of ROS, and even from your embedded linux system!

  16 void messageCb(const std_msgs::String& received_msg){
  17         printf("Received subscribed chatter message: %s\n", received_msg.data);
  18 }

You then create the callback function for our subscriber. The call back function will be called whenever a message on topic "chatter" is received. It takes a constant reference of a message as its argument. In our callback messageCb, the type of message is std_msgs::String and the message name will be received_msg.

  19 ros::Subscriber<std_msgs::String> sub("chatter", messageCb );

We need to instantiate the publishers and subscribers that we will be using. Here we instantiate a subscriber with a topic name of "chatter" and type std_msgs::String. Instantiating the subscriber connects the topic to the callback function.

With subscribers, you must remember to template the subscriber upon the message. Its two arguments are the topic it will be subscribing to and the callback function it will be using.

  24         nh.initNode(rosSrvrIp);

As before, you need to initialize your ROS node handle, and pass in the IP address (and port number if needed) of the system running rosserial_python.

  25         nh.subscribe(sub);

Advertise any topics being published, and subscribe to any topics you wish to listen to.

  27         while(1) {
  28                   sleep(1);
  29                   nh.spinOnce();
  30         }

Finally the program enters a while(1) loop in which it sleeps for a second then handles any communication callbacks. This will result in the subscriber callback function being called when a message on the subscribed topic is received.

Building and Uploading the Code

As before, use your system-specific procedures to build the program and load it onto your embedded linux system.

Testing the Code

If you haven't already done so, launch roscore on your ros workstation and start the rosserial_python.py proxy application that passes your embedded linux ROS messages to and from the rest of ROS. Make sure to specify tcp as the communication channel:

rosrun rosserial_python serial_node.py tcp

If rosserial_python.py is still running from the previous tutorial, you can leave it running. It handles rosserial tcp connections and associated subscriptions and publications as they come and go.

Now start the ExampleSubscriber application on your embedded linux system:

# cd <download_directory>
# ./ExampleSubscriber

The subscriber is now waiting for messages on the chatter topic, and will print them as they arrive.

Open a new window on your ros workstation and run the command below, which publishes the string "Hello Embedded Linux" to the chatter topic:

$ rostopic pub chatter std_msgs/String -1 -- "Hello Embedded Linux"

You should see "Hello Embedded Linux" printed to stdout from the ExampleSubscriber application. The -1 parameter tells rostopic to publish just once then exit. The -- causes whatever follows to be sent to rostopic on stdin, with the result that it gets used as data for the String message that's published to chatter.

If you know a better way to publish a string, please edit this wiki page and add it.

Finally, let's run the publisher from the last tutorial as a separate application. Leave the ExampleSubscriber application running, and open a new window on your embedded linux system. Start the helloRos application from the previous tutorial. Each time helloRos prints "chattered" you should see ExampleSubscriber print "Received subscribed chatter message: Hello Ros!".

You can monitor what's going on by running a couple of programs on your ros workstation:

$ rxgraph &
$ rostopic echo chatter

This starts the ros node display program in the background, and runs rostopic in the foreground.

You will see two serial_node_xxxx nodes in the rxgraph window. One, the helloRos publisher proxy, is publishing /chatter to rostopic, and also to the second serial_node_xxxx, which is the ExampleSubscriber proxy. xxxx is the process ID - each new rosserial_embeddedlinux connection gets a new process to proxy for it.

Warning: The rosserial_embeddedlinux package is not designed to handle continuous disconnects and reconnects - you'll run out of nodes. Instead, it's designed to gracefully recover from the occasional exception condition.

Wiki: rosserial_embeddedlinux/Tutorials/Example Subscriber (last edited 2012-09-23 08:59:30 by PaulBouchier)