ROS python script for Kangaroo x2 sabertooth 2x32 configuration . Accepts cmd_vel and publishes /odom

edit

Below is the code which will accept (cmd_vel ) and publish (odom). I am using a Sabertooth 2x32 + Kangaroo x2 configuration . My robot is a differential drive robot .

I am communicating to Kangaroo vai an Arduino .

The below command will drive all the wheels in through 1 full rotation (clockwise 360 degrees)

D:100

The below command will turn the robot 90degrees from the stand still position .

T:100

How would i publish the odom for such a robot .

PS :- I have gone through the http://wiki.ros.org/differential_drive . kangaroo works like a servo motor , it accepts the command and sets the wheels to that particular position , i am not currently getting the real time encoder feedback .

I have the basic python skeleton ready ,below is the code .

#!/usr/bin/env python
from time import sleep
import serial
import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
import serial.tools.list_ports as port
from math import sin, cos, pi

class Kangaroo:
def __init__(self, debug=False, ser=None):
    self.debug = debug
    self.source = None
    self.status = True
    self.ser = ser
    self.Drive = "&T:10!"
    self.drive_sum = 0
    self.Turn = "&S:10!"
    self.turn_sum = 0
    self.speed = 200
    self.angle = 100
    self.changed = True
    self.x = 0.0
    self.y = 0.0
    self.th = 0.0
    self.vx = 0.1
    self.vy = -0.1
    self.vth = 0.1
    self.r = 0
    print "\nInit kangaroo x2....\n"
    print "\nDetecting kangaroo x2....\n"
    portlist = list(port.comports())
    #print portlist
    address = ''
    for p in portlist:
        print p
        if ' USB2.0-Serial' in str(p):
            self.address = str(p).split(" ")
            print "\nAddress found @"
            print self.address[0]
    self.ser = serial.Serial(self.address[0], 115200, timeout=2,writeTimeout=5)
    print self.ser.readline()

def callback(self, data ):
   order = True
   drive = data.linear.x * 50
   turn = -data.angular.z * 50
   self.drive_sum =  drive
   self.Drive = "&D:" + str(int(self.drive_sum)) +"!"
   self.turn_sum =  turn
   self.Turn = "&T:" + str(int(self.turn_sum))+ "!"
   if((int(drive) or int(turn) )):
      print data
      self.send_cmd_to_arduino(self.Drive, self.Turn ,self.drive_sum , self.turn_sum)

# translate x, and angular velocity to PWM signal of each wheels, and send to arduino
def send_cmd_to_arduino(self, x, angular , drive ,turn):
    message = "{}  {}".format(x,angular)
    print message
    if (abs(drive) > abs(turn)):
        self.ser.write(x )
    else:
        self.ser.write(angular )
            last_time = rospy.Time.now()
            self.Odom(last_time)

def get_arduino_message(self):
    pub = rospy.Publisher('arduino', String, queue_size=10)
    r = rospy.Rate(10)
    while not rospy.is_shutdown():
        message = self.ser.readline()
        pub.publish(message)
        r.sleep()   



def Odom(self,last_time ):
    x = self.x
    y = self.y
    th = self.th
    vx = self.vx
    vy = self.vy
    vth = self.vth
    current_time = rospy.Time.now()
    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
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
    self.odom_broadcaster.sendTransform(
        (x, y, 0.),
        odom_quat,
        current_time,
        "base_link",
        "odom"
    )
    odom = Odometry ...