In this post, you will learn how to access and output odometry data programmatically. More specifically, you will learn how to create a C++ program for subscribing to and printing different parts of the odometry message. This is useful for cases where you need to make a decision based on the robot’s current position.

This post is an answer to the following question found on ROS Answers: https://answers.ros.org/question/333391/way-to-output-odometry-info/.

Let’s get started!

Step 1: Grab the ROS Project (ROSject) on ROSDS

Click here to get a copy of the ROS project used in this post. Next, click open to load the project. Please be patient while the project loads.

Step 2: Launch the necessary tools

1. Launch a simulation from the Simulations menu.
   • Click on the Simulations menu. A dropdown menu opens.
   • In the section “Launch a provided simulation”, leave the empty world selected and choose the Turtlebot 2 robot.
   • Click “Start simulation”.
2. Pick a Shell tool from the Tools menu. We’re be firing some commands soon.
3. Pick the IDE tool from the Tools menu. We’ll use this to examine the code used for solving this problem.

Step 3: Examine the structure of the Odometry message using the command line

Run the following command on the Shell tool to print the current odometry of the robot:

# The -n1 switch to the command to ensure it prints one command and exits. 
# Otherwise, it would print continuously until you press Ctrl + C.
user:~$ rostopic echo /odom -n1
header:
  seq: 4732
  stamp:
    secs: 236
    nsecs: 935000000
  frame_id: "odom"
child_frame_id: "base_footprint"
pose:
  pose:
    position:
      x: 0.00211759816401
      y: 4.81456536954e-05
      z: -0.000247189070632
    orientation:
      x: 0.000385225477218
      y: -0.00721104301471
      z: 0.000107238788561
      w: 0.99997392014
  covariance: [1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0,0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
twist:
  twist:
    linear:
      x: -0.000279472699265
      y: -4.17203978095e-05
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -7.05905617719e-06
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

The command line output above shows the parts the make up an odometry message. The hottest part is pose, which tells us the current position of the robot. twist is also important, because it tells us the current message being published to move the robot.

Now let’s see how to access and output odometry data, considering the hottest part.

Step 4: See how to access and output odometry data in code

Switch to the IDE tool and locate the catkin_ws directory. You should find a C++ file in a package odom_subscriber. The full path to the file is catkin_ws/src/odom_subscriber/src/odom_sub.cpp.

Let’s see the content of this file:

#include <nav_msgs/Odometry.h> // Needed for accessing Odometry data
#include <ros/ros.h>           // Needed for creating the node, etc

// This functions get called when a new odometry message gets to the subscriber
// It automatically gets the odometry message as a parameter
// It prints out various parts of the message
void counterCallback(const nav_msgs::Odometry::ConstPtr &msg) {
  // ROS_INFO("%s", msg->header.frame_id.c_str());
  // ROS_INFO("%f", msg->twist.twist.linear.x);
  ROS_INFO("%f", msg->pose.pose.position.x);
}

int main(int argc, char **argv) {
  // Create a node to run the code
  ros::init(argc, argv, "odom_sub_node");
  ros::NodeHandle nh;

  // create a subscriber to the "/odom" topic so we can get the odometry message
  ros::Subscriber sub = nh.subscribe("odom", 1000, counterCallback);
  
  // Run program until manually stopped
  ros::spin();

  return 0;
}

I have added comments to the code to explain what each line or code block does, for better clarity.

The `counterCallback` function prints out the current x position of the robot. Observe how parts of the message are accessed in hierarchical format; using the structure in Step 3 as a guide, you can access and output any part of the Odometry message. 

You can uncomment the commented parts to print out parts of the header and twist messages or even write other statements that print out other parts of the message!

Step 5: Move the robot and see the odometry data printed in real-time!

Run the C++ program in the shell, to print out the current x position of the robot. We can see the robot is basically at the “origin” and is stationary. Leave this program running.

user:~$ rosrun odom_subscriber odom_sub_node
[ INFO] [1575339924.601377783, 49.183000000]: 0.000240
[ INFO] [1575339924.651682994, 49.233000000]: 0.000240
...

Pick another Shell tool from the tools menu and run a command to move the robot:

user:~$ rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.2
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0"
publishing and latching message. Press ctrl-C to terminate

Now you should see the robot moving. You should also see that the Odometry data being output in the other shell is changing.

In the second Shell, press Ctrl + C to stop the current program. Then publish the following message to stop the robot. After this, you should see the odometry position data become static again.

user:~$ rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0"
publishing and latching message. Press ctrl-C to terminate

Ant that was it!

Extra: Video

Prefer to watch a video demonstrating the steps above? We have one for you below!

Related Resources and Further Learning

If you are a ROS beginner and want to learn ROS basics fast, we recommend you take any of the following courses on Robot Ignite Academy:

• ROS Basics In 5 Days (Python)
• ROS Basics In 5 Days (C++)

Feedback

Did you like this post? Do you have questions about what is explained? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know in the comments area and we will do a video or post about it.

—

Edited by Bayode Aderinola

—
#ROStutorials #Odometry #ROSsubscriber #ROS #Robot #C++