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!

#!/usr/bin/env python3
import rospy
import sys
import math
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Quaternion
from roboclaw_3 import Roboclaw
from time import sleep
from std_msgs.msg import *

address_right = 0x80
address_left = 0x80

loop_rate = 3

class BaseController(object):

def __init__(self):

    self.wheel_odom_pub = rospy.Publisher('wheel_odom', Odometry, queue_size=10)
    rospy.Subscriber("cmd_vel",Twist, self.cmdVelCallback)
    self.odomBroadcaster = tf.TransformBroadcaster()

    self.roboclaw_port   = rospy.get_param('roboclaw_motors_port',"/dev/ttyACM0")
    self._wheel_base     = rospy.get_param('wheel_base', 0.2358)
    self._wheel_radius   = rospy.get_param('wheel_radius', 0.0625)
    self.baud            = rospy.get_param('serial_baudrate', 115200)
    self._ppr            = rospy.get_param('pulse_per_revolution',398*20)
    
    self._p_1_1           = rospy.get_param('P_1_1',4.4714)     #left rear

    self._p_2_1           = rospy.get_param('P_2_1',4.4714)     #right rear


    self._i_1_1           = rospy.get_param('I_1_1',0.5403)    

    self._i_2_1           = rospy.get_param('I_2_1',0.5237)

    
    self._d_1_1           = rospy.get_param('D_1_1',0)

    self._d_2_1           = rospy.get_param('D_2_1',0)


    self._qpps_1_1        = rospy.get_param('QPPS_1_1',4100)

    self._qpps_2_1        = rospy.get_param('QPPS_2_1',4100)


    self.roboclaw = Roboclaw(self.roboclaw_port, self.baud) #Create instance
    self.roboclaw.Open()                  #Open Roboclaws

    #self.roboclaw.SetM1VelocityPID(address_left,4.4714,0.5403,0,4100)
    #self.roboclaw.SetM2VelocityPID(address_right,self._p_1_2,self._i_1_2,self._d_1_2,self._qpps_1_2)
    #self.roboclaw.SetM2VelocityPID(address_left,self._p_2_1,self._i_2_1,self._d_2_1,self._qpps_2_1)
    #self.roboclaw.SetM2VelocityPID(address_right,self._p_2_2,self._i_2_2,self._d_2_2,self._qpps_2_2)

    self.x = 0
    self.y = 0
    self.th = 0

    self._x = 0
    self._y = 0
    self._z = 0

    self.prev_time = rospy.Time.now()
    print ("test ok")
    rate_1 = rospy.Rate(loop_rate)
    

def cmdVelCallback(self, msg):
	linear_x = msg.linear.x
	angular_z = msg.angular.z
	if linear_x>0 or linear_x<0  :
	    print("received linear data")
	if angular_z>0 or angular_z<0:
	    print("received angular data")
	else :
	    print("no data " )
	right_wheel_speed = (linear_x / self._wheel_radius) + (self._wheel_base/self._wheel_radius * angular_z) # rad/s
	left_wheel_speed  = (linear_x / self._wheel_radius) - (self._wheel_base/self._wheel_radius * angular_z)
	self.roboclaw.SpeedM1(address_left,self.QPPS_function(left_wheel_speed))
	self.roboclaw.SpeedM2(address_left,self.QPPS_function(right_wheel_speed))


def QPPS_function(self, rad_per_sec): # rad/sec to qpps conversion
	
    QPPS = (rad_per_sec*self._ppr)/(2*math.pi)
    QPPS = int(QPPS)
    return QPPS

def rad_per_sec_function(self, QPPS):
	
    rad_per_sec = QPPS*(2*math.pi)/self._ppr
    return rad_per_sec    

def publish_odom(self):

    
    rospy.loginfo("Odom !")
    curr_time = rospy.Time.now()
    dt = (curr_time - self.prev_time).to_sec()
    self.prev_time = curr_time

    left_rear   = self.roboclaw.ReadSpeedM1(address_left)
    right_rear  = self.roboclaw.ReadSpeedM1(address_left)

    right_qpps = right_rear[1] #average of right motors 
    left_qpps  = left_rear[1]    #average of left motors

    
    l_speed = -self.rad_per_sec_function(int(left_qpps))
    r_speed = self.rad_per_sec_function(int(right_qpps))
    print ('left-rear= ',l_speed)
    print ('right-rear= ',r_speed)
    vx = (r_speed - l_speed) / 2.0
    vy = 0
    vth = (r_speed + l_speed) / (2 * self._wheel_base)

    delta_x = (vx * math.cos(self.th) - vy * math.sin(self.th)) * dt
    delta_y = (vx * math.sin(self.th) + vy * math.cos(self.th)) * dt
    delta_th = vth * dt
        
    self.x  += delta_x
    self.y  += delta_y
    self.th += delta_th
    odom_quat = Quaternion()
    odom_quat = tf.transformations.quaternion_from_euler(0,0,self.th)
    # Create the odometry transform frame broadcaster (publish tf)
    self.odomBroadcaster.sendTransform(
                        (self.x, self.y, 0),
                        odom_quat,
                        rospy.Time.now(),
                        "base_link",
                        "odom"
                        )

    # Publish Odom

    odom = Odometry()
    odom.header.frame_id = "odom"
    odom.child_frame_id = "base_link"
    odom.header.stamp = rospy.Time.now()
    odom.pose.pose.position.x = self.x
    odom.pose.pose.position.y = self.y
    odom.pose.pose.position.z = self._z
    odom.pose.pose.orientation = Quaternion(*odom_quat)
    odom.twist.twist.linear.x = vx
    odom.twist.twist.linear.y = 0
    odom.twist.twist.angular.z = vth 
    
    self.wheel_odom_pub.publish(odom)

def main():

rospy.init_node("base_controller_node", anonymous=True)

_object = BaseController()

rate = rospy.Rate(loop_rate)


while not rospy.is_shutdown():
    rate.sleep()
    _object.publish_odom()
    rospy.loginfo("Ok !")
    print ("Keyboard Interrupt !")
    _object.roboclaw.SpeedM1(address_left,0)
    _object.roboclaw.SpeedM2(address_left,0)
#_object.roboclaw.SpeedM2(address_right,0)

if name=="main":
main()
this is my code for two motors with roboclaw but i have facing issues that is odometry values not coming
and many errors is there pls help to troubleshooot