{{ message }}

Instantly share code, notes, and snippets.

atotto/ros_odometry_publisher_example.py

Last active May 17, 2022
Publishing Odometry Information over ROS (python)
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode characters
 #!/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()

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 commented Dec 22, 2018

You need to input your wheel encoder data.

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

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

tinhnn commented Nov 1, 2019

You need to input your wheel encoder data.

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

I tried and OK, thanks

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 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
encoder_right_ticks++;
}else{
encoder_right_ticks--;}
}

void count1B(){ // left encoder tick count
encoder_right_ticks--;
}else{
encoder_right_ticks++;}
}

void count2A(){ // right encoder tick count
encoder_left_ticks--;
}else{
encoder_left_ticks++;}
}

void count2B(){ // right encoder tick count
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 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!