Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Publishing Odometry Information over ROS (python)
#!/usr/bin/env python
import math
from math import sin, cos, pi
import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
rospy.init_node('odometry_publisher')
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()
x = 0.0
y = 0.0
th = 0.0
vx = 0.1
vy = -0.1
vth = 0.1
current_time = rospy.Time.now()
last_time = rospy.Time.now()
r = rospy.Rate(1.0)
while not rospy.is_shutdown():
current_time = rospy.Time.now()
# compute odometry in a typical way given the velocities of the robot
dt = (current_time - last_time).to_sec()
delta_x = (vx * cos(th) - vy * sin(th)) * dt
delta_y = (vx * sin(th) + vy * cos(th)) * dt
delta_th = vth * dt
x += delta_x
y += delta_y
th += delta_th
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform(
(x, y, 0.),
odom_quat,
current_time,
"base_link",
"odom"
)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
# publish the message
odom_pub.publish(odom)
last_time = current_time
r.sleep()
@atotto
Copy link
Author

atotto commented Sep 27, 2016

@Redhwanalgabri
Copy link

Redhwanalgabri commented Aug 9, 2018

please check this code again, I need it
Not work with me
when robot is moving or stop the same values
why ???

@droter
Copy link

droter commented Dec 22, 2018

You need to input your wheel encoder data.

vx = speed;
vy = 0;
vth = ((right_speed - left_speed)/lengthWheelBase);

See:
https://answers.ros.org/question/296112/odometry-message-for-ackerman-car/
https://answers.ros.org/question/241602/get-odometry-from-wheels-encoders/

@sajalyadav
Copy link

sajalyadav commented May 30, 2019

will this publish odometry under nav_msgs/Odometry, because I couldn't see nav_msgs anywhere in the code?

@tinhnn
Copy link

tinhnn commented Nov 1, 2019

You need to input your wheel encoder data.

vx = speed;
vy = 0;
vth = ((right_speed - left_speed)/lengthWheelBase);

See:
https://answers.ros.org/question/296112/odometry-message-for-ackerman-car/
https://answers.ros.org/question/241602/get-odometry-from-wheels-encoders/

I tried and OK, thanks

@mekateng
Copy link

mekateng commented Feb 24, 2020

Hi,
I write an Arduino code to calculate the position (x, y and theta) of the differential vehicle. How can I run the code I wrote below integrated with the ros odometry code above. My goal is to obtain the odometry of a real differential vehicle. Could you please help me?

#include<math.h>
uint8_t ticksPerRevolution = 800;
float wheel_radius=12.5; //wheel radius
float distanceWheels = 58.5; // between the distance of the wheels
float x = 0;
float y = 0;
float theta = 0;
float xend = 0;
float yend = 0;
float thetaend = 0;
//void Odometry();
#define encoder1A 18
#define encoder1B 19
#define encoder2A 20
#define encoder2B 21

long encoder_left_ticks = 0;
long encoder_left_ticks_old = 0;
long dleft = 0;

// Right Encoder
long encoder_right_ticks = 0;
long encoder_right_ticks_old = 0;
long dright = 0;

void count1A(){ // left encoder tick count
if(digitalRead(encoder1A)==LOW) {
encoder_right_ticks++;
}else{
encoder_right_ticks--;}
}

void count1B(){ // left encoder tick count
if(digitalRead(encoder1B)==LOW) {
encoder_right_ticks--;
}else{
encoder_right_ticks++;}
}

void count2A(){ // right encoder tick count
if(digitalRead(encoder2A)==LOW) {
encoder_left_ticks--;
}else{
encoder_left_ticks++;}
}

void count2B(){ // right encoder tick count
if(digitalRead(encoder2B)==LOW) {
encoder_left_ticks++;
}else{
encoder_left_ticks--;}
}

void setup()
{
delay(1000);
Serial.begin(9600); //or 115200
pinMode(encoder1A, INPUT);
pinMode(encoder1B, INPUT);
pinMode(encoder2A, INPUT);
pinMode(encoder2B, INPUT);

digitalWrite(encoder1A, HIGH);
digitalWrite(encoder1B, HIGH);
digitalWrite(encoder2A, HIGH);
digitalWrite(encoder2B, HIGH);

attachInterrupt(4,count1A,RISING ); // left encoder new function
attachInterrupt(5,count1B,RISING ); // left encoder new function
attachInterrupt(2,count2A,RISING ); // right encoder new function
attachInterrupt(3,count2B,RISING ); // right encoder new function

}

void loop()
{
//Odometry();

Serial.print(x);
Serial.print('\t');
Serial.print(y);
Serial.print('\t');
Serial.print(theta); //neden 2 ile çarpılıyor ve bu thetaP ve thetaPsent nedir?   

Serial.print('\t');
Serial.print(encoder_left_ticks);
Serial.print('\t');
Serial.println(encoder_right_ticks);
delay(10);
}

void Odometry()
{
float dRight = (encoder_right_ticks - encoder_right_ticks_old) * 2 * PI * wheel_radius /(double) ticksPerRevolution; //d=2pir*(deltaticks)/N
float dLeft = (encoder_left_ticks - encoder_left_ticks_old) * 2 * PI * wheel_radius / (double) ticksPerRevolution;
encoder_left_ticks_old = encoder_left_ticks;
encoder_right_ticks_old = encoder_right_ticks;

float dCenter = (dRight + dLeft) / 2;
float phi = (dRight - dLeft) / distanceWheels;

thetaend = theta + phi;
if (thetaend >= 2.0 * 3.1416) thetaend = thetaend - 2.0 * PI;
if (thetaend < 0.0) thetaend = thetaend + 2.0 * PI;

xend = x + dCenter * cos(theta);
yend = y + dCenter * sin(theta);

theta = thetaend;
x = xend;
y = yend;

}

@JasonRBowling
Copy link

JasonRBowling commented Jan 25, 2021

This is outstanding - a Python conversion of the odom tutorial is exactly what I needed. I wish I had found it two hours ago :-) Thanks!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment