Robot keep moving a tiny distance and stop

edit

I wrote a small program in python for moving the robot to any point .first the robot will check the heading angle and then keep moving to the given goal. however, the robot keeps moving for a small distance and stop. how can I fix the problem?

Robot is Eva_Robot : https://github.com/inomuh/uplat

I created a video for the problem on Youtube: https://www.youtube.com/watch?v=NnkLj...

I am using Kinect and working with differential drive robot. I will appreciate any support on how to solve this problem. same code from this video: https://www.youtube.com/watch?v=eJ4QP...

code available:-

import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import  Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Point
from math import atan2

x=0
y=0
theta=0

msg=Odometry()


def position(msg):
    global x
    global y
    global theta

    x=msg.pose.pose.position.x
    y=msg.pose.pose.position.y
    ora_q=msg.pose.pose.orientation
    (roll,pitch,theta)=euler_from_quaternion([ora_q.x,ora_q.y,ora_q.z,ora_q.w])


rospy.init_node('Speed_Controller')
sub=rospy.Subscriber('/odom',Odometry,callback=position)
pub=rospy.Publisher('/cmd_vel',Twist,queue_size=10)
rospy.sleep(1)
r=rospy.Rate(4)

#to set a goal
goal=Point()

goal.x=4
goal.y=4
speed=Twist()

while not rospy.is_shutdown():
    x_diff=goal.x-x
    y_diff=goal.y-y
    t1=time.time()
    angle_to_go=atan2(y_diff,x_diff)
    if abs(angle_to_go-theta)>0.1: 
        speed.linear.x =0
        speed.angular.z=0.3
    else:
        speed.linear.x =0.3
        speed.angular.z=0.0

    pub.publish(speed)
    r.sleep()
    t2=time.time()

    print("x : {:0.2f}, Y : {:0.2f} , theta : {},").format(x, y,theta)
    print("                     angle_to_go : {},").format(angle_to_go)
    print("                    Time Elabled : {:0.2f},").format(t2-t1)

console reading:-

$ rosrun pubSub heading_angle.py 
x : 0.04, Y : 0.00 , theta : 2.0205650193,
                     angle_to_go : 0.789670659524,
                    Time Elabled : 0.25,
x : 0.04, Y : 0.00 , theta : 2.11090182303,
                     angle_to_go : 0.789634745613,
                    Time Elabled : 0.25,
x : 0.04, Y : 0.00 , theta : 2.17089921691,
                     angle_to_go : 0.789578678222,
                    Time Elabled : 0.25,
x : 0.04, Y : 0.00 , theta : 2.2611964571,
                     angle_to_go : 0.789533706252,
                    Time Elabled : 0.25,
x : 0.04, Y : 0.00 , theta : 2.32164457709,
                     angle_to_go : 0.789463802215,
                    Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : 2.41271103439,
                     angle_to_go : 0.789420819597,
                    Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : 2.47366439793,
                     angle_to_go : 0.78935622329,
                    Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : 2.56427253319,
                     angle_to_go : 0.789311608704,
                    Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : 2.62417187987,
                     angle_to_go : 0.789249596674,
                    Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : 2.71319027876,
                     angle_to_go : 0.78921956178,
                    Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : 2.77313767638,
                     angle_to_go : 0.7891678104,
                    Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : 2.86355262368,
                     angle_to_go : 0.789118611592,
                    Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : 2.9235892517,
                     angle_to_go : 0.789047890566,
                    Time ...