Publishing Odometry Information over ROS (python)

ros_odometry_publisher_example.py

#!/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()

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;

}

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!