Note: This tutorial assumes that you have completed the previous tutorials: Install rosserial_embeddedlinux. |
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 Ros (example publisher)
Description: This tutorial shows step by step how to create an example publisherTutorial Level: BEGINNER
Next Tutorial: Hello Embedded Linux (example subscriber)
Contents
We'll start our exploration into rosserial for embedded linux by creating a "Hello Ros" program for our embedded linux box.
The Code
Use your favorite editor or IDE to open the HelloRos tutorial code which is found in examples/HelloRos/HelloROS.cpp. You should see the following:
1 /*
2 * rosserial Publisher Example
3 * Prints "Hello ROS!"
4 */
5
6 #include <ros.h>
7 #include <std_msgs/String.h>
8 #include <stdio.h>
9
10 ros::NodeHandle nh;
11
12 std_msgs::String str_msg;
13 ros::Publisher chatter("chatter", &str_msg);
14
15 char *rosSrvrIp = "192.168.15.121";
16 char hello[13] = "Hello ROS!";
17
18 int main()
19 {
20 //nh.initNode();
21 nh.initNode(rosSrvrIp);
22 nh.advertise(chatter);
23 while(1) {
24 str_msg.data = hello;
25 chatter.publish( &str_msg );
26 nh.spinOnce();
27 printf("chattered\n");
28 sleep(1);
29 }
30 }
The Code Explained
Now, let's break the code down.
As a part of every ROS embedded linux program, you need to include the ros.h header file and header files for any messages that you will be using.
10 ros::NodeHandle nh;
Next, we need to instantiate the node handle, which allows our program to create publishers and subscribers. The node handle also takes care of communications over serial port or LAN.
We need to instantiate the publishers and subscribers that we will be using. Here we instantiate a Publisher with a topic name of "chatter". The second parameter to Publisher is a reference to the message instance to be used for publishing.
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 published messages to the rest of ROS. Hello is the message to be published - a static string.
21 nh.initNode(rosSrvrIp);
In the main function you need to initialize your ROS node handle. You need to pass in the IP address (and port number if needed) of the system running rosserial_python. If you don't pass in a parameter, the system will use a default serial port.
NodeHandle.initNode() accepts arguments like:
- /dev/ttyUSB1 uses the designated serial port for messages to rosserial_python
- 192.168.1.2 connects to rosserial_python at the specified IP address on the default port 11411
- 192.168.1.2:12345 connects to rosserial_python at the specified IP address on the specified port number
22 nh.advertise(chatter);
Advertise any topics being published, and subscribe to any topics you wish to listen to.
while(1) { str_msg.data = hello;
Finally the program enters a while(1) loop in which it publishes data then sleeps for a second, forever. We use the string name as a pointer and assign it to the message data pointer.
chatter.publish( &str_msg ); nh.spinOnce(); sleep(1); } }
In the loop, the node publishes "Hello ROS" and calls ros::spinOnce() where all of the ROS communication callbacks are handled.
Building and Uploading the Code
Use your system-specific procedures to build the program and load it onto your embedded linux system. This may involve an IDE like TerkIDE, or scp and running from a shell, or other procedures. Specific instructions for your system may be documented in the Installation, Build and Development Overview section of the rosserial_embeddedlinux installation tutorial
Testing the Code
Now, launch roscore on your ros workstation:
$ roscore
Next, in an new window on your ros workstation, run 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
The tcp argument causes serial_node.py to become a server and wait for a connection from a client. serial_node.py prints a message saying it's waiting for a connection (from your embedded linux system).
As noted above, you can alternately specify a serial port on your ros workstation that serial_node.py should use, provided you also change the helloros application to specify a serial port on the embedded linux system to use.
Now start the helloros application on your embedded linux system:
# cd <download_directory> # ./helloros
The helloros application prints "chattered" each time it publishes a message.
Finally, watch the greetings come in from your embedded linux system by launching a new terminal window on your ros workstation and entering :
rostopic echo chatter
Once you've started the helloros program it will attempt to establish communication with the rosserial_python node and publish its message, over and over, ad infinitum, ad nauseum.
Restarting the embedded system or program
The helloros program can be stopped and restarted at will, and it will reconnect to rosserial_python. Indeed, the embedded linux system can be restarted at will, and the next time you run helloros it will reconnect to the rosserial_python node.
Start then terminate the helloros program. Note that rostopic stops printing - messages are no longer being published. Note that rosserial_python prints an error message - it detected the loss of communication with the embedded system.
[INFO] [WallTime: 1348628129.052204] socket.error exception caught
Now re-run helloros. Note that rosserial_python detects a new connection, and rostopic echo starts printing the published messages again.
[INFO] [WallTime: 1348628319.232685] Established a socket connection from 192.168.11.8 on port 40260 [INFO] [WallTime: 1348628319.232919] Forking a socket server process [INFO] [WallTime: 1348628319.235348] launched startSocketServer waiting for socket connection [INFO] [WallTime: 1348628319.237011] starting ROS Serial Python Node serial_node-40260 [INFO] [WallTime: 1348628322.310832] Note: publish buffer size is 512 bytes [INFO] [WallTime: 1348628322.311974] Setup publisher on chatter [std_msgs/String]