[ROS Q&A] 195 – How to know if robot has moved one meter using Odometry

Learn how to know if the robot has moved one-meter using Odometry.  You’ll learn how to:

• create a Python program for calculating the distance moved by the robot using the Odometry topic and publish it into a topic.
• create a Python program for testing that we can move the robot depending on the Odometry data.

Let’s go!

Related Resources

• ROS Development Studio (ROSDS)
• Robot Ignite Academy
• ROS Basics Course (Python)

Step 1: Get your development environment ready and grab a copy of the code

1. Click here to grab a copy of the ROSject already containing the project. This requires an account on the ROS Development Studio (ROSDS), an online platform for developing for ROS within a PC browser. If you don’t have an account, you will be asked to create one.
2. To open a “terminal” on ROSDS, pick the Shell app from the Tools menu.
3. You can find the IDE app on the Tools menu.

Step 2: Start a Simulation

We are going to use the TurtleBot 2 simulation.

1. Click on Simulations from the main menu.
2. Under Launch a provided simulation, leave “Empty world” selected and click on Choose a robot.
3. Type “turtle” into the search box and select TurtleBot 2.
4. Click Start Simulation.
5. A window showing the TurtleBot 2 in an empty world should show up.

Step 3: Examine the structure of Odometry messages

Let’s see the structure of an Odom message. Open a terminal and run:

user:~$ rostopic echo /odom -n1
header:
  seq: 14929
  stamp:
    secs: 748
    nsecs: 215000000
  frame_id: "odom"
child_frame_id: "base_footprint"
pose:
  pose:
    position:
      x: 0.00668370211388
      y: 0.00010960687178
      z: -0.000246865753431
    orientation:
      x: 0.000389068511716
      y: -0.00720626927928
      z: 0.000354481463316
      w: 0.999973895985
  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: 2.30945682841e-05
      y: 0.000390977104083
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -0.000507764995516
  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 part of the message that stores the current position of the robot is the pose.pose.position:

user:~$ rostopic echo /odom -n1
# ...
pose:
  pose:
    position:
      x: 0.00668370211388
      y: 0.00010960687178
      z: -0.000246865753431
# ...

This part is what is used in the Python script that calculates the distance moved.

Step 4: Understand the Python scripts

1. Open the IDE and browse to the src folder. You will find two packages odom_movement_detector and test_movement. Each of these projects contains a Python script, each explained with comments in the code

odom_movement_detector/src/odom_movement_detector.py

#!/usr/bin/env python

import rospy
import math
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point
from std_msgs.msg import Float64

class MovementDetector(object):
    def __init__(self):
        """Initialize an object of the MovementDetector class."""
        # _mved_distance is for stored distance moved
        # create and initialize it here. Initial value is 0.0
        self._mved_distance = Float64()
        self._mved_distance.data = 0.0

        # Get the inital position. This will be a reference point for calculating
        # the distance moved 
        self.get_init_position()

        # Create a publisher for publishing the distance moved into the topic '/moved_distance'
        self.distance_moved_pub = rospy.Publisher('/moved_distance', Float64, queue_size=1)

        # create a subscriber for getting new Odometry messages
        rospy.Subscriber("/odom", Odometry, self.odom_callback)

    def get_init_position(self):
        """Get the initial position of the robot."""
        """
        Structure of the odom position message:
        user:~$ rostopic echo /odom -n1
        header:
        seq: 14929
        stamp:
            secs: 748
            nsecs: 215000000
        frame_id: "odom"
        child_frame_id: "base_footprint"
        pose:
        pose:
            position:
            x: 0.00668370211388
            y: 0.00010960687178
            z: -0.000246865753431
        """
        data_odom = None
        # wait for a message from the odometry topic and store it in data_odom when available
        while data_odom is None:
            try:
                data_odom = rospy.wait_for_message("/odom", Odometry, timeout=1)
            except:
                rospy.loginfo("Current odom not ready yet, retrying for setting up init pose")
        
        # Store the received odometry "position" variable in a Point instance 
        self._current_position = Point()
        self._current_position.x = data_odom.pose.pose.position.x
        self._current_position.y = data_odom.pose.pose.position.y
        self._current_position.z = data_odom.pose.pose.position.z

    def odom_callback(self, msg):
        """Process odometry data sent by the subscriber."""
        # Get the position information from the odom message
        # See the structure of an /odom message in the `get_init_position` function
        NewPosition = msg.pose.pose.position

        # Calculate the new distance moved, and add it to _mved_distance and 
        self._mved_distance.data += self.calculate_distance(NewPosition, self._current_position)
        
        # Update the current position of the robot so we have a new reference point
        # (The robot has moved and so we need a new reference for calculations)
        self.updatecurrent_positin(NewPosition)
        
        # If distance moved is big enough, publish it to the designated topic
        # Otherwise publish zero
        if self._mved_distance.data < 0.000001:
            aux = Float64()
            aux.data = 0.0
            self.distance_moved_pub.publish(aux)
        else:
            self.distance_moved_pub.publish(self._mved_distance)

    def updatecurrent_positin(self, new_position):
        """Update the current position of the robot."""
        self._current_position.x = new_position.x
        self._current_position.y = new_position.y
        self._current_position.z = new_position.z

    def calculate_distance(self, new_position, old_position):
        """Calculate the distance between two Points (positions)."""
        x2 = new_position.x
        x1 = old_position.x
        y2 = new_position.y
        y1 = old_position.y
        dist = math.hypot(x2 - x1, y2 - y1)
        return dist

    def publish_moved_distance(self):
        # spin() simply keeps python from exiting until this node is stopped
        rospy.spin()

if __name__ == '__main__':
    # create a node for running the program
    rospy.init_node('movement_detector_node', anonymous=True)

    # create an instance of the MovementDetector class and set the code
    # in motion
    movement_obj = MovementDetector()
    movement_obj.publish_moved_distance()

test_movement/src/test_movement.py

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64

def my_callback(msg):
    """Callback function that processes messages from the subscriber."""

    # get the distance moved from the message
    distance_moved = msg.data

    # If distance is less than 2, continue moving the robot
    # Otherwise, stop it (by pubishing `0`)
    if msg.data < 2:
        move.linear.x = 0.1

    if msg.data > 2:
        move.linear.x = 0

    pub.publish(move)

# create a node for running the program
rospy.init_node('test_movement')

# create a subscriber that gets the distance moved
sub = rospy.Subscriber('/moved_distance', Float64, my_callback)

# Create a publisher that moves the robot
pub = rospy.Publisher('/cmd_vel', Twist, queue_size="1")

# Create a global variable for publising a Twist ("cmd_vel") message 
move = Twist()

# Keep the program running
rospy.spin()

Step 5: See the Python scripts in action

Open a terminal and run the movement detector:

user:~$ rosrun odom_movement_detector odom_movement_detector.py

Open another terminal and check the distance being published to the /moved_distance:

user:~$ rostopic echo /moved_distance
data: 0.00280330822873
---
data: 0.00280530307334

You can see from above that the distance is not changing (more or less) because the robot is stationary. Now let’s move the robot:

user:~$ rosrun test_movement test_movement.py

Now you should see the robot moving and moved_distance increasing. It should stop when moved_instance > 2.0.

user:~$ rostopic echo /moved_distance
data: 0.00280330822873
---
data: 0.00280330822873
---
data: 0.00280330822873
---
data: 0.00280330822873
---
data: 0.00280330822873
---
# ...
data: 1.00280330822873
---
# ...
data: 2.00280330822873
---

And that’s it. I hope you had some fun!

Extra: Video of the post

Here below you have a “sights and sounds” version of this post, just in case you prefer it that way. Enjoy!

Feedback

Did you like this post? Do you have any questions about the explanations? 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 or ROS2 topics, please let us know in the comments area and we will do a video or post about it.

Edited by Bayode Aderinola