Implimenting odometry on a real tutlebot

edit

I wrote the following code to navigate my robot in a square using odometry. The objective for the code is use 4 goal positions(the 4 corners of a square) and navigating to it, by first making sure it is lined up(using odometry and goal positions) and then move using a linear twist if it is lined up. The process repeats 4 times to navigate a square.

In simulation(rviz) the code works and i can see the robot trying to implement the algorithm. However, on a real turtlebot, the code is being implimented but the robot barely or hardly moves from its position.

I have checked that the kobuki base works, by implementing a simple code.. Any suggestions or ideas.I really need urgent help and am open to any suggestions you may have.

import roslib
import rospy
import math
import random
import tf
from tf.transformations import euler_from_quaternion
import message_filters #A set of message filters which take in messages and may output those messages at a later time, based on the conditions that filter needs met.
# The odometry message
from nav_msgs.msg import Odometry
# the velocity command message
from geometry_msgs.msg import Twist

# instantiate global variables "globalOdom"
globalOdom = Odometry()
goalX = None
goalY = None
success_percent = 0

arr_goalX = [0, -5, 0]
arr_goalY = [5, 0, -5]

# method to control the robot
def callback(odom):
    # the odometry parameter should be global
    global globalOdom
    globalOdom = odom
    at_goal = False

    global goalX, goalY, success_percent, arr_goalX, arr_goalY

    # make a new twist message
    command = Twist()

    #log the position to a file
    X_file = open('X_data.txt','a')
    Y_file = open('Y_data.txt','a')

    for index in range(1):
        # Fill in the fields.  Field values are unspecified 
        # until they are actually assigned. The Twist message 
        # holds linear and angular velocities.
        print "Success Percent = ", success_percent
        command.linear.x = 0.0
        command.linear.y = 0.0
        command.linear.z = 0.0
        command.angular.x = 0.0
        command.angular.y = 0.0
        command.angular.z = 0.0
        # get goal x and y locations from the launch file
        # get parameter 'lab2/goalX' with a default value of 0
        # find current (x,y) position of robot based on odometry
        currentX = globalOdom.pose.pose.position.x
        currentY = globalOdom.pose.pose.position.y
        print type(currentX)
        if goalX is None:
            goalX = currentX + 5
        if goalY is None:
            goalY = currentY
        print "GoalX = ", goalX, "\tGoalY = ", goalY

        if abs(currentX - goalX) < 0.1 and abs(currentY - goalY) < 0.1:
            print "At goal!!!!"
            if success_percent == 3:
                rospy.signal_shutdown("Goal Reached.")
            else:
                goalX = goalX + arr_goalX[success_percent]
                goalY = goalY + arr_goalY[success_percent]
                success_percent = success_percent + 1

        # find current orientation of robot based on odometry (quaternion coordinates)
        xOr = globalOdom.pose.pose.orientation.x
        yOr = globalOdom.pose.pose.orientation.y
        zOr = globalOdom.pose.pose.orientation.z
        wOr = globalOdom.pose.pose.orientation.w

        # find orientation of robot (Euler coordinates)
        (roll, pitch, yaw) = euler_from_quaternion([xOr, yOr, zOr, wOr])

        # find currentAngle of robot (equivalent to yaw)
        # now that you have yaw, the robot's pose is completely defined by (currentX, currentY, currentAngle)
        currentAngle = yaw

        d_x = goalX - currentX
        d_y = goalY ...