Walkthrough: ROS Messages and location_monitor

The ultimate goal of this assignment is to follow all of the steps below and understand what is going on. As you work through the steps, be sure to understand the code and what each step is doing. I highly recommend using the ROS command line tools to introspect your ROS system after each node you create to make sure the topics and nodes are what you expect them to be. The GitHub link that you submit to me should contain a single ROS package that has all of the code discussed below. Here are a few guidelines that you should try and follow:

Git Usage

Your Git repository should only track the relevant files for your package. Don't track autosave or backup files, and don't have an entire workspace in the repo. Normally, with ROS, Git repositories are located in the root directory of a package – so the .git/ directory would be in the same directory as the package.xml and the CMakeLists.txt. Try to commit after every major step to help you get in the habit of using version control correctly. Every repo should have a README file, preferably nicely formatted through a text mark-up language like Markdown (which GitHub automatically parses and formats).

Location Monitor Versions

In the location_monitor sections below (Sections 4.3, 4.4, and 4.5), you will be working with 3 separate versions of the same code. Be sure to illustrate to me that you completed all three steps. You could make three different files and link them in your README, you could make separate commits on each version and mention in your README that I should look at the old commits, you could put them on separate branches, etc. It doesn't matter to me, but it would be a good chance to practice Git usage.

ROS Code

Be sure to follow all of the steps below, and be sure you understand them. I'll be looking that all of the relevant message definitions have been created, and I'll be checking that all of the code for the nodes created is contained within your package. Basically, you are trying to produce a package that proves to me you've followed all steps. Try to follow all ROS naming conventions.

Questions

Please don't hesitate to ask questions in class or during office hours. We really want to make sure these fundamental concepts are solidly understood before moving on.

ASIDE: Package, repo, and directory names

In a ROS package, the package resource name for that package is defined by the <name> tag in the package.xml and the project() function in the CMakeLists.txt (they must match or things won't work correctly). The directory that you have these files stored in has nothing to do with the package name. However, for convenience and readability, it is good practice to have the online repo name, the local directory name, and the package name all match. However, due to the limitations of GitHub classroom, for these types of assignments, we won't be able to make all three of those match — the online repo name will always be something like walkthrough-1-f2018-YOUR-USERNAME, and we can't change it. In this assignment you are going to be creating a ROS package named basics (which obviously doesn't match the GitHub repo name), but there is nothing to stop you from naming the directory containing your local copy of the repo match your package name. In other words, you could have a ~/catkin_ws/src/basics/ directory on your computer that contains a package.xml and CMakeLists.txt declaring that directory to be the basics package, but you would push that repo to a Git remote named walkthrough-1-f2018-YOUR-USERNAME. You can even directly clone the GitHub repo into a particular local directory using the optional second argument to git clone e.g. git clone walkthrough-1-f2018-YOUR-USERNAME basics.

ROS nodes communicate with each other by publishing messages to topics and subscribing to those topics. A message is a simple data structure that is defined in a message file consisting of one field type and one field name per line. These message files are given a .msg extension. Message files support both ROS-specific and standard data types (integers, floating points, Booleans, etc.). ROS has many standard message definitions that are commonly used, but one can also define custom messages. Any ROS message can be used as a field type in another message. In this way, messages can include arbitrarily nested structures and arrays (much like structs in the C programming language).

Nodes can also communicate via a request-and-response paradigm through ROS service calls. These request-and-response communications are defined in files with a .srv extension. The syntax for these files is very similar to that of message files, but we won't go into details for now. We'll learn more about this later on.

Message types use a standard naming convention and file hierarchy. Message types take the form package_name/msg_file_name. For example, the std_msgs package has a file inside called std_msgs/msg/String.msg (see the file on GitHub), and the message type is std_msgs/String.

In addition to the message type, messages are versioned by an MD5 sum of the contents of the .msg file. Nodes can only communicate messages for which both the message type and MD5 sum match. So, for example, let's say two nodes on different computers are trying to communicate with each other over a network using a ROS topic. If the corresponding .msg files on each computer have different contents, then their MD5 sums won't match. This will generate an MD5 mismatch error when one of the computers publishes a message on that topic. This is a complications that can arise when attempting to implement systems with more than one ROS version.

As mentioned above, message files are stored in the msg directory of a package and they are simple text files with a .msg extension. Below is an example of a .msg file

# This is a comment
Header header
Bool ready
Float32 range_max
# The '[3]' below indicates an array of length 3:
Float64[3] point
# The '[]' below indicates an array whose length will be specified in the code
# that is using this message. For example, we could use std::vector::push_back
# in C++ or list.append in Python:
Float64[] ranges
Int8 count
geometry_msgs/PoseWithCovariance pose
# You can also define constants:
Int8 default_state=1

So this message file has 8 field types (Header, Bool, Float32, Float64[3], Float64[], Int8, geometry_msgs/PoseWithCovariance, and Int8) , and 8 corresponding field names (header, ready, range_max, point, ranges, count, pose, and default_state). Each line is a different entry and the format of each line is always <field type> <field name>.

Given these message definitions, ROS can automatically create code that implements a native version of the message data structure for a variety of client libraries when projects are built. So when one runs catkin_make, message definition files are automatically converted into languages-specific implementations that can be used in code. In other words, once you define a message, run catkin_make, and Ta-Da!… it's available for use in all languages that ROS supports (typically this is just C++ and Python, but there is also experimental support for Java, C, Lisp, node.js, and maybe others).

More info on the message format and the conversion from ROS primitives into language-specific native data types is available on the msg ROS wiki page.

Building off the talker example, we want to create a simple ROS node that publishes random complex numbers instead of just a string with hello world in it. The first thing we do in our package directory is to create a message file with the following contents.

float32 real
float32 imaginary

You can do this from the command line using the following:

cd ~/catkin_ws/src/basics
mkdir msg
cd msg
echo -e "float32 real\nfloat32 imaginary" > Complex.msg

We aren't quite ready to build this custom message yet. We have to tell catkin that we are going to be building messages in this package, and let it know about the new message definition so it can generate the language-specific message code. We do that by making sure that both of the following lines to are in our package.xml (they should already be there if you followed the directions up top, but check to make sure)

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

Next we go to our CMakeLists.txt and look for the find_package macro. We verify that message_generation is listed, and that message_runtime is not (you may have to edit your CMakeLists.txt, the template files created from catkin_create_pkg are far from perfect):

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  nav_msgs
  message_generation
  roscpp
  rospy
  std_msgs
)

We then make sure that our catkin_package command lists a catkin dependency on the message_runtime package using the CATKIN_DEPENDS parameter. The standard catkin_package command that was created when we created our package with catkin_create_pkg needs a little editing:

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES basics
CATKIN_DEPENDS geometry_msgs nav_msgs std_msgs message_runtime
#  DEPENDS system_lib
)

Next we tell catkin what message files to compile by adding them to the add_message_files() call:

# Generate messages in the 'msg' folder
add_message_files(
 FILES
 Complex.msg
)

Finally we make sure the generate_messages() call is uncommented and contains the list of catkin packages that define their own messages. If we create a custom message in the basics package that has a field type defined in a message from one of these other packages, this command ensures that the messages from the other packages will be built first so that their language-specific code will be available for use in our custom message.

generate_messages(
  DEPENDENCIES
  geometry_msgs
  nav_msgs
  std_msgs
)

We should now be able to run catkin_make successfully (remember, you must run this in the root of your workspace). Running rosmsg show basics/Complex should then yield

float32 real
float32 imaginary

Our node will listen to a topic that tells us where the simulated robot is in the world. Then the nodes we write will:

• Publish the name and distance to the closest landmark
• Print out a notification when the robot enters a landmark zone

We're going to need at least 3 terminals:

1. In the first terminal run roslaunch turtlebot_gazebo turtlebot_world.launch. This launches a Gazebo simulation with a TurtleBot and a set of obstacles.
2. In the second terminal run roslaunch turtlebot_teleop keyboard_teleop.launch. This allows us to drive the TurtleBot in the world using the keyboard.
3. In the third terminal run rostopic list to see all of the topics being published. We're interested in the /odom topic to figure out where the robot is in the world. run rostopic info /odom to find out more about the topic. You see that it is of type nav_msgs/Odometry where nav_msgs is the package and a file called Odometry.msg would define the message type. This indicates that we'll want nav_msgs as a dependency when we're creating our ROS package.
4. Again in the third terminal run rostopic echo /odom to print out all messages being published on the /odom topic. Now try driving the turtlebot in the second terminal using your keyboard and watch how the pose and orientation change as you drive the simulated TurtleBot.

Change directory to your basics package and create a file called location_monitor_node.py and make it executable with. E.g.

cd ~/catkin_ws/src/basics/src
touch location_monitor_node.py
chmod a+x location_monitor_node.py

Now fill in the following code:

#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry

def odom_cb(msg):
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    rospy.loginfo("x: {}, y: {}".format(x,y))

def main():
    rospy.init_node('location_monitor')
    rospy.Subscriber("odom", Odometry, odom_cb)
    rospy.spin()

if __name__ == '__main__':
    main()

This is version 1 of our location_monitor node. It subscribes to the /odom topic and prints out the \(x\) and \(y\) coordinates of the robot like this

Moving the turtlebot using the teleop terminal while location_monitor node is running will result in the \(x\) and \(y\) coordinates changing. Note that the /odom topic is being published at nearly 100 Hz (as can be seen with rostopic hz /odom). Printing anything out this fast is not generally good practice – it slows your code down, and is difficult to read.

Edit your code from the last section to look like the following.

#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
import math

landmarks = []
landmarks.append(("Cube",0.31,-0.99));
landmarks.append(("Dumpster", 0.11,-2.42));
landmarks.append(("Cylinder", -1.14,-2.88));
landmarks.append(("Barrier", -2.59,-0.83));
landmarks.append(("Bookshelf", -0.09, 0.53));

def distance(x1, y1, x2, y2):
    xd = x1 - x2
    yd = y1 - y2
    return math.sqrt(xd*xd + yd*yd)

def callback(msg):
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    rospy.logdebug("x: {}, y: {}".format(x,y))
    closest_name = None
    closest_distance = None
    for l_name, l_x, l_y in landmarks:
        dist = distance(x, y, l_x, l_y)
        if closest_distance is None or dist < closest_distance:
            closest_name = l_name
            closest_distance = dist
    rospy.loginfo("Landmark: {} || Distance: {}".format(closest_name,closest_distance))

def main():
    rospy.init_node('location_monitor')
    rospy.Subscriber("odom", Odometry, callback)
    rospy.spin()

if __name__ == '__main__':
    main()

In version 2 of our code:

• We hard-code the coordinates of the various landmarks (unfortunately, as a global variable) and append them to a list
• We implement a distance function simply computes the Euclidean distance between two points
• In our callback function we check the distance between where we are and each of the landmarks
• We then print out the closest landmark to the robot and the distance to it

Below is version 3 of the location_monitor node:

#!/usr/bin/env python
import rospy
import math
from nav_msgs.msg import Odometry
from basics.msg import LandmarkDistance #1

def distance(x1, y1, x2, y2):
    xd = x1 - x2
    yd = y1 - y2
    return math.sqrt(xd*xd + yd*yd)

class LandmarkMonitor(object): #2
    def __init__(self, landmark_pub, landmarks):
        self._landmark_pub = landmark_pub
        self._landmarks = landmarks

    def callback(self, msg):
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        closest_name = None
        closest_distance = None
        for l_name,l_x, l_y in self._landmarks:
            dist = distance(x, y, l_x, l_y)
            if closest_distance is None or dist < closest_distance:
                closest_name = l_name
                closest_distance = dist
        ld = LandmarkDistance() #3
        ld.name = closest_name
        ld.distance = closest_distance
        self._landmark_pub.publish(ld) #4
        if closest_distance < 0.5: #5
            rospy.loginfo("I'm near the {}".format(closest_name))


def main():
    rospy.init_node('location_monitor_node')
    landmarks = []
    landmarks.append(("Cube", 0.31, -0.99));
    landmarks.append(("Dumpster", 0.11, -2.42));
    landmarks.append(("Cylinder", -1.14, -2.88));
    landmarks.append(("Barrier", -2.59, -0.83));
    landmarks.append(("Bookshelf", -0.09, 0.53));

    landmark_pub = rospy.Publisher("closest_landmark", LandmarkDistance, queue_size=10)  #6
    monitor = LandmarkMonitor(landmark_pub, landmarks) #7
    rospy.Subscriber("odom", Odometry, monitor.callback) #8

    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")


if __name__ == '__main__':
    main()

Below are some annotations that go along with the code above that describe notable changes that have been implemented in version 3:

1. We write a custom message called basics/LandmarkDistance that contains the name of the landmark and the distance to it in meters. Creating this message and getting it compiled properly is an exercise left to the reader. The contents of LandmarkDistance.msg are as follows:

   string name # Name of the landmark
   float64 distance # distance to the landmark in meters
   

2. We avoid global variables through the use of Python classes.
3. We instantiate an instance of the LandmarkDistance message class that we will fill out and publish each time we receive a new message on the odom topic.
4. Publish the contents of our message each time LandmarkMonitor.callback is run.
5. We print out a message if the robot is within 0.5 meters of the landmark.
   • Since we're publishing a message to a topic, we can view the messages published on that topic with rostopic echo /closest_landmark when the node is running in another terminal.
6. We create a topic called closest_landmark and publish the data which is of message type basics/LandmarkDistance
7. We instantiate an instance of the LandmarkMonitor class.
8. We subscribe to the odom topic and pass the callback method of the monitor instance of the LandmarkMonitor class as the callback argument to the rospy.Subscriber initialization function (whew… that's a mouthful).

1. It is good practice to ensure your Python nodes and packages have different names to help avoid naming conflicts.
2. It is recommended to have the package name (as defined in the package.xml and CMakeLists.txt) to be in a directory of the same name. So in this example, we have a directory ~/catkin_ws/src/basics that contains a package.xml and CMakeLists.txt that define the package name to also be basics. While this isn't strictly required, it does make it easier to recognize packages in a workspace.
3. It is common practice in ROS to create a bunch of messages related to a given topic inside their own package (e.g., std_msgs, geometry_msgs, nav_msgs, sensor_msgs). In other words this package would contain only message, service, and action definitions and no nodes. This results in simpler dependency hierarchies.
4. Here are a few things to think about:
   • In Version 2 could we publish closest_name and closest_distance without using a custom message?
   • If we could, how would you do it?
   • What would be some pros and cons of using custom messages vs fitting your data into already-defined messages?
5. Classes are a good way to help us avoid global variables. This is true in Python and C++. Classes have many other benefits, but in ROS, this is one benefit that comes up often.
6. It's good practice to develop your code incrementally. Get one part working then move on to the next part. This can significantly accelerate the debugging process.