Skip to content

Instantly share code, notes, and snippets.

@atotto
Last active November 8, 2023 10:15
Show Gist options
  • Star 31 You must be signed in to star a gist
  • Fork 14 You must be signed in to fork a gist
  • Save atotto/f2754f75bedb6ea56e3e0264ec405dcf to your computer and use it in GitHub Desktop.
Save atotto/f2754f75bedb6ea56e3e0264ec405dcf to your computer and use it in GitHub Desktop.
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()
@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

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

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

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!

@rocky-sigetech
Copy link

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

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