Kobuki unpredictability

edit

I've been trying to run python scripts on Kobuki Turtlebot in order to make it perform specific patterns on the floor. I've noticed that while executing the same script on the same spot, it provides radically different performances. This is an example of my script:

#!/usr/bin/env python

# for robot A

import rospy
from kobuki_testsuite import TravelForward 
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
import roslib; roslib.load_manifest('kobuki_testsuite')
import sys
import copy
import os
import subprocess

from tf.transformations import euler_from_quaternion
from math import degrees

from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion

global stage
stage = 1
global length
length = 1 # length in metres
global width
width = 1 
#des = 0.5  # desired angle in radians - it's negative if angle is larger than 3.14

def resetter():
    try:
        pub = rospy.Publisher('/mobile_base/commands/reset_odometry', Empty, queue_size=10)
        rospy.init_node('resetter', anonymous=True)
        rate = rospy.Rate(10) # 10hz
        for i in range(0,10):
          pub.publish(Empty())
          rate.sleep()
    except rospy.ROSInterruptException:
        pass

def ImuCallback(data, angle):
  global stage
  quat = data.orientation
  q = [quat.x, quat.y, quat.z, quat.w]
  roll, pitch, yaw = euler_from_quaternion(q)

  sys.stdout.write("\r\033[1mGyro Angle\033[0m: [" + "{0:+.4f}".format(yaw) + " rad]  ["\
                 + "{0: >+7.2f}".format(degrees(yaw)) + " deg]"\
                 + "            \033[1mRate\033[0m: [" + "{0:+.4f}".format(data.angular_velocity.z) + " rad/s]  ["\
                 + "{0: >+7.2f}".format(degrees(data.angular_velocity.z)) + " deg/s] ")
  if angle > 0:
    if yaw > angle or yaw < 0:
      stage += 1
  else:
    if yaw > angle and yaw < 0:
      stage += 1 

def publish(pub, v, sec, step):
    cnt = 0
    m = sec * 2

    while cnt < m and not rospy.is_shutdown() and stage == step: 
        pub.publish(v)
        cnt = cnt + 1
        rospy.sleep(0.5)

def rotation_anticlockwise(step, angle, r):
    rospy.Subscriber("/mobile_base/sensors/imu_data", Imu, ImuCallback, angle)
    cmdvel_topic ='/cmd_vel_mux/input/teleop'

    pub = rospy.Publisher(cmdvel_topic, Twist)
    vel = Twist() 
    print "rotation"
    print stage
    while not rospy.is_shutdown() and stage == step:
      vel.angular.z = 0.5
      vel.linear.x = 0.5 * r
      rospy.loginfo("Anticlockwise")
      publish(pub, vel, 18, step)  
      rospy.loginfo("Done")
      rospy.sleep(2)
  #    if stage == 0:
   #     vel.angular.z = 0
  #  rospy.sleep(2)

if __name__ == '__main__':
    global stage
    resetter()
    rotation_anticlockwise(1, 1.57, 0)
    resetter()
    print stage
    stage = 2
    rotation_anticlockwise(2, -0.02, 0.2 * length)

What I meant for it to do is to rotate to 90 degrees and then make a full circle with a specified radius. Most of the times I run the script I get it to rotate a bit and then stand still or rotate and then make just a part of a circle. What troubles me is I don't understand whether the problem is my script or some mechanics inside the robot. Could it be that its bumper sensors are too sensitive and make it stop unexpectedly? If so, is there a way around?