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()

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