Homework Exercise: Sending and Receiving our First ROS Messages

Objectives

This exercise is designed to give you an introduction to sending and receiving ROS messages. If you run into any challenges, please reach out to the teaching team via Slack for help.

Additional Resources

We highly recommend that you check out the ROS resources page to learn more about ROS and tools that you may find useful while ROS programming.

Let's Send and Receive Some ROS Messages!

Create a new ROS package

Let's navigate to our directory within our catkin_ws for all of our in-class exercises and create a ROS package for today's coding exercises (to learn more about ROS package creation visit this ROS tutorial page). To do so, run the following commands in a terminal:

$ cd ~/catkin_ws/src/intro_robo
$ catkin_create_pkg first_ros_nodes rospy std_msgs geometry_msgs sensor_msgs

Then be sure to run the catkin_make command within your catkin workspace and source the devel/setup.bash file.

$ cd ~/catkin_ws && catkin_make
$ source devel/setup.bash

Note: Every time you create a new package, change the package dependencies or anything in the package.xml or CMakeLists.txt files, or create a new message type within a package, you'll need to re-build the catkin workspace and source the setup file. You do not need to re-build when you make new python scripts or launch files.

Create a ROS node that publishes ROS messages

We're going to write Python code to construct a ROS node that publishes the location of a point in space (with x, y, and z coordinates) twice per second. Create a new python file named point_location_sender.py and ensure that it's full path is ~/catkin_ws/src/intro_robo/first_ros_nodes/scripts/point_location_sender.py (to do this you'll have to create a scripts directory within the first_ros_nodes directory). To get started put the following lines to your newly created point_location_sender.py:

#!/usr/bin/env python3
""" This script publishes ROS messages containing the 3D coordinates of a single point """
import rospy
from geometry_msgs.msg import PointStamped

The first line (#!/usr/bin/env python3) ensures that the file is executed as a Python script. We import rospy to ensure we have access to ROS's python libraries. We also import the standard ROS message type geometry_msgs/PointStamped, which is the ROS message type we will use to send the 3D coordinates of our point. In order to determine the components of the PointStamped message, run:

$ rosmsg show geometry_msgs/PointStamped

You should see the following output:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z

We see that the PointStamped message is composed of 1) a header attribute of type std_msgs/Header and 2) a point attribute of type geometry_msgs/Point. We can also see the attributes of the std_msgs/Header (seq, stamp, and frame_id) and geometry_msgs/Point (x, y, and z). If you want to print out the attributes of each type individually, you can run rosmsg show std_msgs/Header and rosmsg show geometry_msgs/Point.

In order to instantiate a PointStamped message, we'll also need to import the message types of PointStamped's attributes. Thus, we'll add the following lines:

from std_msgs.msg import Header
from geometry_msgs.msg import Point

Next, we'll need to initialize our Python ROS node with a name, this is essential for your node to start communicating with ROS Master. Each ROS node must have a unique name. If you try to run two nodes with the same name, ROS will throw an error.

rospy.init_node('send_point_location')    # initialize our ROS node

Now, we can define the components (header and point) of the PointStamped message that we want to send.

my_header = Header(stamp=rospy.Time.now(), frame_id="odom")
my_point = Point(1.0, 2.0, 0.0)

We set the header stamp to the current ROS time rospy.Time.now(), seq is automatically set by ROS, and frame_id specifies the name of the coordinate frame ("odom"). We initialize the point to the [x, y, z] coordinates of [1.0, 2.0, 0.0]. Now that we've defined my_header and my_point, we can define our PointStamped message.

my_point_stamped = PointStamped(header=my_header, point=my_point)

If you were to print out my_point_stamped at this point in your code, print(my_point_stamped), you would see an output similar to:

header:
  seq: 0
  stamp:
    secs: 1441500244
    nsecs: 244467020
  frame_id: odom
point:
  x: 1.0
  y: 2.0
  z: 0.0

Note: It's also possible to define a PointStamped message in one line, e.g.,

my_point_stamped = PointStamped(header=Header(stamp=rospy.Time.now(),
                                              frame_id="odom"),
                                point=Point(1.0, 2.0, 0.0))

Now let's publish our PointStamped message at a rate of 2 times per second (2 Hz). We'll published our PointStamped messages a ROS topic named /my_point.

publisher = rospy.Publisher('/my_point', PointStamped, queue_size=10)
# rospy.Rate specifies the rate of the loop (in this case 2 Hz)
r = rospy.Rate(2)
while not rospy.is_shutdown():
    my_point_stamped.header.stamp = rospy.Time.now()    # update timestamp
    publisher.publish(my_point_stamped)
    r.sleep()

Now, let's run our code! Before running your code, you'll need to make sure that your script is executable:

$ chmod u+x ~/catkin_ws/src/intro_robo/first_ros_nodes/scripts/point_location_sender.py

In a first terminal run roscore. In a second terminal, we can run our code by executing the following:

$ rosrun first_ros_nodes point_location_sender.py

To check that our ROS node is really sending our PointStamped messages, open a third terminal and run the following command:

$ rostopic echo /my_point

You should see your PointStamped messages printed to the console at a rate of 2 Hz.

Create a ROS node that subscribes to ROS messages using callback functions

In addition to publishing ROS messages, it's equally important to understand how ROS nodes can "listen to" or subscribe to ROS messages that are published by other nodes. This is accomplished with ROS callback functions. We'll now write a second Python script to construct a ROS node that subscribes to our PointStamped messages on the /my_point ROS topic and prints them out in the terminal.

Create a new Python script named point_location_receiver.py in the directory ~/catkin_ws/src/intro_robo/first_ros_nodes/scripts. Let's start out with the following first lines in point_location_receiver.py:

#!/usr/bin/env python3
""" This script receives ROS messages containing the 3D coordinates of a single point and prints them out """
import rospy
from geometry_msgs.msg import PointStamped

rospy.init_node('receive_point_location')

In these lines, we've ensured that this script will be executed as a Python script (line 1), provided a descriptive comment of the ROS node (line 2), imported the necessary libraries (lines 3-4), and initialized our ROS node with the name receive_point_location (final line).

Next, we'll set up our callback function. This function will be called every time a message is published to a specified ROS topic (in our case, the /my_point ROS topic). ROS callback functions have a single input, which will be a Python object of the types of the message of the ROS topic (in our case, type geometry_msgs/PointStamped). Within our callback function, we'll print out the 3D location of the point in a readable format.

def process_point(data):
    print("[{:.2f}, {:.2f}, {:.2f}]".format(data.point.x, data.point.y, data.point.z))

Remember that point is an attribute of the geometry_msgs/PointStamped object and x, y, and z are attributes of the geometry_msgs/point object.

Now, we'll set up our node to subscribe to the /my_point ROS topic, calling back to our process_point callback function every time a message is published to /my_point.

rospy.Subscriber("/my_point", PointStamped, process_point)

As you can see above, when we set up our subscriber, we must give it the ROS topic to subscribe to (argument 1), the type of message published to that topic (argument 2), and the callback function name (argument 3).

We'll need one final line that will keep our ROS node running until we're ready to shut it down:

rospy.spin()

rospy.spin() will keep a ROS node running (listening for ROS messages) until you shut it down (e.g., hitting Ctrl+C in the terminal window).

That's it! Test it out, and you should see that your receive_point_location ROS node prints out the 3D point locations sent by the send_point_location ROS node:

$ rosrun first_ros_nodes point_location_receiver.py

Making your code object-oriented

The code we produced above is a great introduced to programming in ROS, however, as our programs become more complex we'll want to use object-oriented techniques to structure our code.

Note: If you need a refresher on object-oriented programming (OOP) in Python, feel free to check out the Real Python OOP in Python3 tutorial and/or the DataCamp Python OOP tutorial.

Our next task is going to be editing the two ROS nodes we just wrote (point_location_sender.py and point_location_receiver.py) so that they're object oriented. Here are some guidelines and suggestions for how to make your code object-oriented:

Create a class, that will represent the ROS node.
Define attributes of that class that pertain to any important states of the node that you want to keep track of.
Use the __init__ method to do some setup for the node (e.g., calling rospy.init_node, creating publishers/subscribers, initializing any attributes).
Integrate callback functions as methods of your class. You can call them using self.my_callback and you'll have to add the self input to your callback function.
For any run loops (e.g., rospy.spin()), encapsulate that functionality in a run() method of your class.
Within if __name__ == '__main__': instantiate your ROS node and run it.

Here's an example:

class Counter(object):
"""This node publishes an increasing counter"""

    def __init__(self):
        rospy.init_node('counter')
        self.count_pub = rospy.Publisher('my_count', int8)

    def run(self):
        r = rospy.Rate(1)
        counter = 0
        while not rospy.is_shutdown():
            self.count_pub.publish(counter)
            r.sleep()
            counter += 1

if __name__ == '__main__':
    node = Counter()
    node.run()

Sample Solutions

At the following link, you can find sample solutions for all of the scripts written in this homework exercise.

Acknowledgments

This coding exercise was based on the exercise presented in A Computational Introduction to Robotics course's Day 02 page.