0
$\begingroup$

Rosanswers logo

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()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"
    odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
    odom.child_frame_id = "base_link"
    odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
    self.odom_pub.publish(odom)
    last_time = current_time
    


    
def listener(self):
    rospy.init_node('listener')
    self.r =rospy.Rate(1)
    try:
        rospy.Subscriber("/rpv1_velocity_controller/cmd_vel", Twist, self.callback )
    except:
        print "Twist module failed"
    
    try:
        self.odom_pub =rospy.Publisher("/rpv1_velocity_controller/odom",Odometry,queue_size=1)
        self.odom_broadcaster = tf.TransformBroadcaster()
        
    except:
        print "Odom module failed"
    rospy.spin()


speed = 0
angle = 0
Drive = 0
Turn = 0
if __name__ == '__main__': 
    try:
    kangaroo = Kangaroo(debug=True)
    kangaroo.listener()
   except rospy.ROSInterruptException: 
     pass

The below is the script for publishing the /clock

#!/usr/bin/env python
import rospy
from rosgraph_msgs.msg import Clock
from std_msgs.msg import String
rospy.init_node('tmp', disable_rostime=False)
pub = rospy.Publisher('/clock', Clock, queue_size=10)
print "node initialized"
while not rospy.is_shutdown():
   t = rospy.Time.now()
   msg = Clock()
   msg.clock.secs = t.to_sec()
   msg.clock.nsecs = float(t.to_sec() -int(t.to_sec()))
   print msg

   pub.publish(msg)

[Originally posted](https://answers.ros.org/question/281781/ros-python-script-for-kangaroo-x2-sabertooth-2x32-configuration-. accepts-cmd_vel-and-publishes-/odom/) by chris_sunny on ROS Answers with karma: 47 on 2018-02-05

Post score: 0


Original comments

Comment by lucasw on 2018-02-05:
I see an Odometry publisher in your code, does it not work?

Comment by chris_sunny on 2018-02-05:
Nope its not publishing anything , i tried "rostopic echo /odom" , but its empty .

Comment by chris_sunny on 2018-02-07:
Thanks , clock was not published , that was the issue.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

It's possible your indents didn't cut and paste properly because the following should generate an unexpected indent error:

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)

But if corrected to

    if (abs(drive) > abs(turn)):
        self.ser.write(x )
    else:
        self.ser.write(angular )
    last_time = rospy.Time.now()
    self.Odom(last_time)

then the odom publisher works.

There might be a couple other indent issues also, I corrected them and tried it out with socat:

socat -d -d pty,raw,b115200,link=$HOME/tmp/ttyUSB0,echo=0 pty,raw,b115200,link=$HOME/tmp/ttyUSB1,echo=0

and hardcoded self.address to tmp/ttyUSB0 to make it run, and the odom publisher worked when I published a Twist to cmd_vel.


Originally posted by lucasw with karma: 8729 on 2018-02-06

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.