Select Page

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

#### 13/09/2019

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!

#### 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 ROSDSpick 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
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
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:

# 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.

## Programming a Service client for ROBOTIS Open Manipulator

Hello ROS Developers! In this post, we are going to show a way of creating a custom program to send desired...

## ROBOTIS Open Manipulator on Ubuntu18.04 – ROS Melodic

Hello ROS Developers! In this post we are going to setup the simulation of the robot OpenMANIPULATOR-X from Robotis...

## Creating a Restaurant Robot Simulation, Part 1- #ROS Projects

.                 What we are going to learn How to add modifications to the...