Robot keep moving a tiny distance and stop
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=NnkLj4rWy0k&feature=youtu.be
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=eJ4QPrYqMlw
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 Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : 3.01334984668,
angle_to_go : 0.789014904185,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : 3.07326335534,
angle_to_go : 0.788988194342,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -3.11980884792,
angle_to_go : 0.788966957031,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -3.05963893045,
angle_to_go : 0.788925282425,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.96922594761,
angle_to_go : 0.788899159961,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.90880947592,
angle_to_go : 0.788868761193,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.81874967014,
angle_to_go : 0.788851904993,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.7588494621,
angle_to_go : 0.788825694135,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.66881342806,
angle_to_go : 0.788808315562,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.60885720629,
angle_to_go : 0.788790470099,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.51874319285,
angle_to_go : 0.788781080676,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.45844013339,
angle_to_go : 0.78877039415,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.36758503908,
angle_to_go : 0.788767005064,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.30675825173,
angle_to_go : 0.788767109453,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.21527810224,
angle_to_go : 0.788771018391,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.1548394055,
angle_to_go : 0.78878258473,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.06499630332,
angle_to_go : 0.788793025429,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -2.00564509343,
angle_to_go : 0.788807940778,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -1.91706967384,
angle_to_go : 0.78881656518,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -1.85724279455,
angle_to_go : 0.788838357862,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -1.76716917481,
angle_to_go : 0.788859336566,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -1.70731918881,
angle_to_go : 0.788897035601,
Time Elabled : 0.25,
x : 0.03, Y : 0.00 , theta : -1.61734897081,
angle_to_go : 0.788923560413,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -1.55719797252,
angle_to_go : 0.788963753938,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -1.46698691022,
angle_to_go : 0.788993457634,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -1.4067548076,
angle_to_go : 0.789037564254,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -1.31611555962,
angle_to_go : 0.789069169928,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -1.25560229846,
angle_to_go : 0.789120784804,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -1.16496947322,
angle_to_go : 0.789151528019,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -1.10438146321,
angle_to_go : 0.789195200782,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -1.01346561873,
angle_to_go : 0.789225054906,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -0.953434606771,
angle_to_go : 0.789284694925,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -0.863047948656,
angle_to_go : 0.789330893024,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -0.80252959072,
angle_to_go : 0.789393388426,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -0.711359401825,
angle_to_go : 0.789438151491,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -0.65041079749,
angle_to_go : 0.789507389091,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -0.560293234331,
angle_to_go : 0.789552430522,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -0.500501013223,
angle_to_go : 0.789603966555,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -0.411580317941,
angle_to_go : 0.789633461288,
Time Elabled : 0.26,
x : 0.03, Y : -0.00 , theta : -0.352686623712,
angle_to_go : 0.789675843727,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -0.26392044628,
angle_to_go : 0.789702097217,
Time Elabled : 0.26,
x : 0.03, Y : -0.00 , theta : -0.204361287279,
angle_to_go : 0.789716963703,
Time Elabled : 0.24,
x : 0.03, Y : -0.00 , theta : -0.114894552568,
angle_to_go : 0.78972909856,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : -0.0550457987202,
angle_to_go : 0.789755001274,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : 0.0350859639657,
angle_to_go : 0.789776717833,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : 0.0952704322735,
angle_to_go : 0.789814634209,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : 0.18537026545,
angle_to_go : 0.789838373256,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : 0.24528191515,
angle_to_go : 0.789869942794,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : 0.335113409298,
angle_to_go : 0.789892481305,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : 0.395080588759,
angle_to_go : 0.789920525649,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : 0.485238560783,
angle_to_go : 0.789936226369,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : 0.545420946348,
angle_to_go : 0.789952323549,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : 0.635590843444,
angle_to_go : 0.789959999689,
Time Elabled : 0.25,
x : 0.03, Y : -0.00 , theta : 0.695938029304,
angle_to_go : 0.789968452532,
Time Elabled : 0.25,
x : 0.05, Y : 0.02 , theta : 0.79750366867,
angle_to_go : 0.789971434953,
Time Elabled : 0.25,
x : 0.09, Y : 0.06 , theta : 0.817487708423,
angle_to_go : 0.790077461218,
Time Elabled : 0.25,
x : 0.16, Y : 0.13 , theta : 0.817484898359,
angle_to_go : 0.789848517472,
Time Elabled : 0.25,
x : 0.20, Y : 0.17 , theta : 0.817483061933,
angle_to_go : 0.789389138631,
Time Elabled : 0.25,
x : 0.26, Y : 0.23 , theta : 0.81747770542,
angle_to_go : 0.789076047341,
Time Elabled : 0.25,
x : 0.30, Y : 0.28 , theta : 0.817474697601,
angle_to_go : 0.788595242083,
Time Elabled : 0.25,
x : 0.36, Y : 0.34 , theta : 0.817470711671,
angle_to_go : 0.788265208111,
Time Elabled : 0.25,
x : 0.40, Y : 0.39 , theta : 0.817466984719,
angle_to_go : 0.787755404835,
Time Elabled : 0.25,
x : 0.46, Y : 0.45 , theta : 0.817464882939,
angle_to_go : 0.787405790945,
Time Elabled : 0.25,
x : 0.51, Y : 0.50 , theta : 0.817464660074,
angle_to_go : 0.786865611428,
Time Elabled : 0.25,
x : 0.57, Y : 0.56 , theta : 0.817459604085,
angle_to_go : 0.786493866632,
Time Elabled : 0.25,
x : 0.61, Y : 0.61 , theta : 0.817456194316,
angle_to_go : 0.785921295885,
Time Elabled : 0.25,
x : 0.67, Y : 0.67 , theta : 0.817451642332,
angle_to_go : 0.785527084744,
Time Elabled : 0.25,
x : 0.71, Y : 0.72 , theta : 0.817447595969,
angle_to_go : 0.784916589411,
Time Elabled : 0.25,
x : 0.77, Y : 0.78 , theta : 0.817446910092,
angle_to_go : 0.784496879718,
Time Elabled : 0.25,
x : 0.81, Y : 0.83 , theta : 0.817445735153,
angle_to_go : 0.783846145229,
Time Elabled : 0.25,
x : 0.87, Y : 0.89 , theta : 0.817441688341,
angle_to_go : 0.783397663912,
Time Elabled : 0.25,
x : 0.92, Y : 0.94 , theta : 0.817439817719,
angle_to_go : 0.782703977349,
Time Elabled : 0.25,
x : 0.98, Y : 1.00 , theta : 0.817433541127,
angle_to_go : 0.782224171259,
Time Elabled : 0.25,
x : 1.02, Y : 1.04 , theta : 0.817431475269,
angle_to_go : 0.781480383663,
Time Elabled : 0.25,
x : 1.08, Y : 1.11 , theta : 0.817429447053,
angle_to_go : 0.780966259328,
Time Elabled : 0.25,
x : 1.12, Y : 1.15 , theta : 0.817427560612,
angle_to_go : 0.780167214292,
Time Elabled : 0.25,
x : 1.18, Y : 1.22 , theta : 0.817426357632,
angle_to_go : 0.779615876713,
Time Elabled : 0.25,
x : 1.22, Y : 1.26 , theta : 0.817422207928,
angle_to_go : 0.778756497249,
Time Elabled : 0.25,
x : 1.28, Y : 1.33 , theta : 0.817418412342,
angle_to_go : 0.778162421559,
Time Elabled : 0.25,
x : 1.33, Y : 1.37 , theta : 0.817415116154,
angle_to_go : 0.777234636121,
Time Elabled : 0.25,
x : 1.39, Y : 1.44 , theta : 0.817412450599,
angle_to_go : 0.776591198294,
Time Elabled : 0.25,
x : 1.43, Y : 1.48 , theta : 0.817411722248,
angle_to_go : 0.775587661254,
Time Elabled : 0.25,
x : 1.49, Y : 1.55 , theta : 0.817408322715,
angle_to_go : 0.774889867823,
Time Elabled : 0.25,
x : 1.53, Y : 1.59 , theta : 0.817407380092,
angle_to_go : 0.773801298634,
Time Elabled : 0.25,
x : 1.59, Y : 1.66 , theta : 0.817402606554,
angle_to_go : 0.773041888015,
Time Elabled : 0.25,
x : 1.63, Y : 1.70 , theta : 0.81740170388,
angle_to_go : 0.77185473443,
Time Elabled : 0.25,
x : 1.70, Y : 1.77 , theta : 0.817398034445,
angle_to_go : 0.771025016467,
Time Elabled : 0.25,
x : 1.74, Y : 1.81 , theta : 0.817397138597,
angle_to_go : 0.76972579191,
Time Elabled : 0.25,
x : 1.80, Y : 1.88 , theta : 0.81739331905,
angle_to_go : 0.768817371582,
Time Elabled : 0.25,
x : 1.84, Y : 1.92 , theta : 0.81739153845,
angle_to_go : 0.767390510728,
Time Elabled : 0.25,
x : 1.90, Y : 1.99 , theta : 0.817388504976,
angle_to_go : 0.766390599435,
Time Elabled : 0.25,
x : 1.94, Y : 2.03 , theta : 0.817387470432,
angle_to_go : 0.764813316889,
Time Elabled : 0.25,
x : 2.00, Y : 2.09 , theta : 0.81738546036,
angle_to_go : 0.763706698996,
Time Elabled : 0.25,
x : 2.04, Y : 2.14 , theta : 0.817383422404,
angle_to_go : 0.761957126457,
Time Elabled : 0.25,
x : 2.11, Y : 2.20 , theta : 0.817379545107,
angle_to_go : 0.760726610418,
Time Elabled : 0.25,
x : 2.15, Y : 2.25 , theta : 0.817376825667,
angle_to_go : 0.758774680768,
Time Elabled : 0.25,
x : 2.21, Y : 2.31 , theta : 0.81737412264,
angle_to_go : 0.757397668283,
Time Elabled : 0.25,
x : 2.25, Y : 2.36 , theta : 0.817373521528,
angle_to_go : 0.755203911939,
Time Elabled : 0.25,
x : 2.31, Y : 2.42 , theta : 0.81737251928,
angle_to_go : 0.753650736946,
Time Elabled : 0.25,
x : 2.35, Y : 2.47 , theta : 0.81737111824,
angle_to_go : 0.751170921603,
Time Elabled : 0.25,
x : 2.41, Y : 2.53 , theta : 0.817366599782,
angle_to_go : 0.74940871683,
Time Elabled : 0.25,
x : 2.45, Y : 2.58 , theta : 0.817364001428,
angle_to_go : 0.746581851732,
Time Elabled : 0.25,
x : 2.52, Y : 2.64 , theta : 0.817360414449,
angle_to_go : 0.744562063053,
Time Elabled : 0.25,
x : 2.56, Y : 2.69 , theta : 0.817360624954,
angle_to_go : 0.741307315397,
Time Elabled : 0.25,
x : 2.62, Y : 2.75 , theta : 0.817358926887,
angle_to_go : 0.738970007926,
Time Elabled : 0.25,
x : 2.66, Y : 2.79 , theta : 0.81735886876,
angle_to_go : 0.735188644043,
Time Elabled : 0.25,
x : 2.72, Y : 2.86 , theta : 0.817354077949,
angle_to_go : 0.732454647586,
Time Elabled : 0.25,
x : 2.76, Y : 2.90 , theta : 0.817352457432,
angle_to_go : 0.728002496118,
Time Elabled : 0.25,
x : 2.82, Y : 2.97 , theta : 0.817348441317,
angle_to_go : 0.724760078467,
Time Elabled : 0.25,
x : 2.87, Y : 3.01 , theta : 0.817348308994,
angle_to_go : 0.719440026071,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.852177880459,
angle_to_go : 0.715535782482,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.912695718323,
angle_to_go : 0.711509494111,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 1.00400903835,
angle_to_go : 0.711478855181,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 1.06500091827,
angle_to_go : 0.711396262668,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 1.15656697265,
angle_to_go : 0.711332062118,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 1.21765897755,
angle_to_go : 0.71122711231,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 1.30931664255,
angle_to_go : 0.711152001817,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 1.37037354029,
angle_to_go : 0.71100544861,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 1.46147187797,
angle_to_go : 0.710889802912,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 1.5219442283,
angle_to_go : 0.710716363013,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 1.61228586459,
angle_to_go : 0.710609340518,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 1.67237559756,
angle_to_go : 0.710440336066,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 1.76246972141,
angle_to_go : 0.710297341906,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 1.82235299642,
angle_to_go : 0.710061950082,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 1.91211902157,
angle_to_go : 0.709904250615,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 1.97203076695,
angle_to_go : 0.709681036693,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.06209991185,
angle_to_go : 0.70953088736,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.12230012965,
angle_to_go : 0.709280763048,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.21245021062,
angle_to_go : 0.709115839907,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.27275138376,
angle_to_go : 0.708869257153,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.36359474235,
angle_to_go : 0.708716926531,
Time Elabled : 0.26,
x : 2.90, Y : 3.06 , theta : 2.42441181569,
angle_to_go : 0.708450286067,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.51564116403,
angle_to_go : 0.708254550512,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.57595069016,
angle_to_go : 0.708013624538,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.6656673782,
angle_to_go : 0.707899935382,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.72497585636,
angle_to_go : 0.707760617321,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.81461219246,
angle_to_go : 0.707642174609,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.8749617521,
angle_to_go : 0.707367045255,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 2.96529740592,
angle_to_go : 0.707210697087,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 3.02531539543,
angle_to_go : 0.707016669977,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : 3.11533946708,
angle_to_go : 0.706895070985,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -3.10776688787,
angle_to_go : 0.706729986211,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -3.01740408367,
angle_to_go : 0.706629260782,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.95697719101,
angle_to_go : 0.706519344409,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.86661449601,
angle_to_go : 0.706462951732,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.80652293856,
angle_to_go : 0.706372099078,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.71616817949,
angle_to_go : 0.706313430639,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.65572382336,
angle_to_go : 0.706262458256,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.56511340266,
angle_to_go : 0.706244473669,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.50498263331,
angle_to_go : 0.706235773952,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.41442588471,
angle_to_go : 0.706229157516,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.35378688846,
angle_to_go : 0.706238267842,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.26242310098,
angle_to_go : 0.706255472057,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.20164305148,
angle_to_go : 0.706305671894,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.11205712265,
angle_to_go : 0.706354908205,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -2.05256962201,
angle_to_go : 0.706437403616,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -1.96398164234,
angle_to_go : 0.706481424645,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -1.90457091009,
angle_to_go : 0.706564375894,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -1.81478127626,
angle_to_go : 0.706651053466,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -1.75500515073,
angle_to_go : 0.706818773311,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -1.66538498737,
angle_to_go : 0.706917354461,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -1.60544744794,
angle_to_go : 0.707008089142,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -1.51525087413,
angle_to_go : 0.707112730764,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -1.45507572941,
angle_to_go : 0.70730062729,
Time Elabled : 0.25,
x : 2.90, Y : 3.06 , theta : -1.36459477017,
angle_to_go : 0.707422190733,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -1.30410909211,
angle_to_go : 0.707599513923,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -1.21351498815,
angle_to_go : 0.707753011571,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -1.15293858068,
angle_to_go : 0.707998416258,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -1.0617122786,
angle_to_go : 0.708139024787,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -1.00159794831,
angle_to_go : 0.708337381643,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.911413073582,
angle_to_go : 0.708504436355,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.851043327186,
angle_to_go : 0.7087815779,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.760091425355,
angle_to_go : 0.708955612087,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.699203585072,
angle_to_go : 0.709198878139,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.608187048893,
angle_to_go : 0.709364162731,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.548070198314,
angle_to_go : 0.709606269092,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.458614629702,
angle_to_go : 0.709738898675,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.399506091534,
angle_to_go : 0.709863176106,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.310725305611,
angle_to_go : 0.709894641006,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.251291600577,
angle_to_go : 0.709961212876,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.16192645073,
angle_to_go : 0.710019206215,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.102209195956,
angle_to_go : 0.710092722669,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : -0.0123049800447,
angle_to_go : 0.710146713129,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.0478438850276,
angle_to_go : 0.71026037713,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.137960529451,
angle_to_go : 0.710355138311,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.197934060105,
angle_to_go : 0.710481911321,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.287799220551,
angle_to_go : 0.710553432545,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.347749095524,
angle_to_go : 0.710649859465,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.437867225893,
angle_to_go : 0.710714206769,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.498118944168,
angle_to_go : 0.710770599201,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.588346882574,
angle_to_go : 0.710791052332,
Time Elabled : 0.25,
x : 2.90, Y : 3.05 , theta : 0.648527554479,
angle_to_go : 0.710805307683,
Time Elabled : 0.25,
x : 2.92, Y : 3.07 , theta : 0.739593264024,
angle_to_go : 0.710810583833,
Time Elabled : 0.25,
x : 2.97, Y : 3.11 , theta : 0.75475999784,
angle_to_go : 0.710713765373,
Time Elabled : 0.25,
x : 3.03, Y : 3.17 , theta : 0.754764118551,
angle_to_go : 0.708992109585,
Time Elabled : 0.26,
x : 3.07, Y : 3.22 , theta : 0.754765726503,
angle_to_go : 0.705737938499,
Time Elabled : 0.25,
x : 3.14, Y : 3.28 , theta : 0.754766402992,
angle_to_go : 0.703314477415,
Time Elabled : 0.25,
x : 3.18, Y : 3.32 , theta : 0.754768161501,
angle_to_go : 0.699197557499,
Time Elabled : 0.25,
x : 3.25, Y : 3.38 , theta : 0.754771030975,
angle_to_go : 0.696058972482,
Time Elabled : 0.25,
x : 3.29, Y : 3.42 , theta : 0.754773148578,
angle_to_go : 0.690634147591,
Time Elabled : 0.25,
x : 3.36, Y : 3.48 , theta : 0.754778073789,
angle_to_go : 0.686427355854,
Time Elabled : 0.25,
x : 3.40, Y : 3.52 , theta : 0.754779922409,
angle_to_go : 0.678961141664,
Time Elabled : 0.25,
x : 3.47, Y : 3.59 , theta : 0.754780627208,
angle_to_go : 0.673013007858,
Time Elabled : 0.25,
x : 3.51, Y : 3.63 , theta : 0.754782297572,
angle_to_go : 0.66211728985,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : 0.781204006302,
angle_to_go : 0.653074558447,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : 0.841312540507,
angle_to_go : 0.642626777246,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 0.932548908933,
angle_to_go : 0.642537559594,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 0.993383204812,
angle_to_go : 0.642339699808,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.08483341677,
angle_to_go : 0.642163292568,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.14588402278,
angle_to_go : 0.641875505211,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.23752050674,
angle_to_go : 0.64165394884,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.29862165836,
angle_to_go : 0.641315986049,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.39013989072,
angle_to_go : 0.641076706846,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.45082425554,
angle_to_go : 0.640627322687,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.54153106951,
angle_to_go : 0.640263830528,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.60174986158,
angle_to_go : 0.639746320392,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.69187986356,
angle_to_go : 0.639407878706,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.75192939442,
angle_to_go : 0.638917038206,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.84206609069,
angle_to_go : 0.638614811595,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.90228657679,
angle_to_go : 0.638063607954,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 1.9927420871,
angle_to_go : 0.637690216131,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.05250999151,
angle_to_go : 0.637059382751,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.14235396002,
angle_to_go : 0.636632348785,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.2024402318,
angle_to_go : 0.63605354509,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.29291748166,
angle_to_go : 0.635599764244,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.35347221162,
angle_to_go : 0.634843452541,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.44468315169,
angle_to_go : 0.63436735157,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.50542596378,
angle_to_go : 0.633739551776,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.59586042,
angle_to_go : 0.633318812508,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.65603040327,
angle_to_go : 0.632603132692,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.74541187878,
angle_to_go : 0.632079817864,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.80472270265,
angle_to_go : 0.63156401143,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.89386095752,
angle_to_go : 0.631458647865,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 2.95343193158,
angle_to_go : 0.631322625139,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 3.04296752859,
angle_to_go : 0.631238353208,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 3.10287027544,
angle_to_go : 0.631000668125,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -3.09013793192,
angle_to_go : 0.630797323612,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -3.03000560218,
angle_to_go : 0.630486976475,
Time Elabled : 0.26,
x : 3.55, Y : 3.67 , theta : -2.93974608574,
angle_to_go : 0.630278014795,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.87946574601,
angle_to_go : 0.630067608286,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.78875242264,
angle_to_go : 0.629969387824,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.72880727266,
angle_to_go : 0.629855525827,
Time Elabled : 0.26,
x : 3.55, Y : 3.67 , theta : -2.63894531769,
angle_to_go : 0.6297964459,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.57890636729,
angle_to_go : 0.629724641002,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.48863501794,
angle_to_go : 0.629715797645,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.42823594371,
angle_to_go : 0.6297501253,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.33724713315,
angle_to_go : 0.629806437992,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.27631093138,
angle_to_go : 0.629935909045,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.18555215279,
angle_to_go : 0.630050741948,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.12541627092,
angle_to_go : 0.630254268166,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -2.03611016424,
angle_to_go : 0.630469037528,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.97672542414,
angle_to_go : 0.630768166956,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.88778474508,
angle_to_go : 0.630869140476,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.82835592896,
angle_to_go : 0.631003406606,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.73896565049,
angle_to_go : 0.631104533227,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.67925732813,
angle_to_go : 0.631341811909,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.5893696081,
angle_to_go : 0.63156684812,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.52922526647,
angle_to_go : 0.631964641022,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.43900774101,
angle_to_go : 0.632259191747,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.37880222218,
angle_to_go : 0.632723450318,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.28894600541,
angle_to_go : 0.633132645237,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.22915145524,
angle_to_go : 0.633834459991,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.13936713598,
angle_to_go : 0.634271496129,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -1.079447345,
angle_to_go : 0.634866408173,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -0.989526365703,
angle_to_go : 0.635291425986,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -0.929421859628,
angle_to_go : 0.636027066717,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : -0.838907240343,
angle_to_go : 0.636482033744,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.778301578342,
angle_to_go : 0.637123565409,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.68702479528,
angle_to_go : 0.637516109033,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.626083979405,
angle_to_go : 0.638173333346,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.535760172519,
angle_to_go : 0.638649740275,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.476029695501,
angle_to_go : 0.639135403655,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.387196615171,
angle_to_go : 0.639344522886,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.327956975383,
angle_to_go : 0.639454069102,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.238817969575,
angle_to_go : 0.63957887746,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.17924623412,
angle_to_go : 0.639792421121,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.0896832142681,
angle_to_go : 0.639899067285,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : -0.0297520008631,
angle_to_go : 0.640084619561,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : 0.0604303571265,
angle_to_go : 0.640250971189,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : 0.120512953728,
angle_to_go : 0.640586863307,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : 0.210511825969,
angle_to_go : 0.640790159876,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : 0.270442006048,
angle_to_go : 0.641032058346,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : 0.360419172031,
angle_to_go : 0.641163723508,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : 0.420525644396,
angle_to_go : 0.641359480572,
Time Elabled : 0.25,
x : 3.55, Y : 3.66 , theta : 0.51096691582,
angle_to_go : 0.641438358787,
Time Elabled : 0.25,
x : 3.55, Y : 3.67 , theta : 0.571427593847,
angle_to_go : 0.641487451822,
Time Elabled : 0.25,
x : 3.57, Y : 3.68 , theta : 0.662393219449,
angle_to_go : 0.64150039934,
Time Elabled : 0.25,
x : 3.62, Y : 3.72 , theta : 0.677403907474,
angle_to_go : 0.641630529359,
Time Elabled : 0.25,
x : 3.69, Y : 3.78 , theta : 0.677403912886,
angle_to_go : 0.637676256947,
Time Elabled : 0.25,
x : 3.74, Y : 3.81 , theta : 0.677405642091,
angle_to_go : 0.62823929088,
Time Elabled : 0.25,
x : 3.81, Y : 3.87 , theta : 0.677404805563,
angle_to_go : 0.619093362028,
Time Elabled : 0.25,
x : 3.85, Y : 3.91 , theta : 0.677405902182,
angle_to_go : 0.596507356513,
Time Elabled : 0.25,
x : 3.90, Y : 3.94 , theta : 0.70359042825,
angle_to_go : 0.568463667964,
Time Elabled : 0.25,
x : 3.90, Y : 3.94 , theta : 0.763689193649,
angle_to_go : 0.515906636459,
Time Elabled : 0.25,
x : 3.90, Y : 3.94 , theta : 0.854917469877,
angle_to_go : 0.515534663188,
Time Elabled : 0.25,
x : 3.90, Y : 3.94 , theta : 0.915739861444,
angle_to_go : 0.514453531752,
Time Elabled : 0.25,
x : 3.90, Y : 3.94 , theta : 1.00698664483,
angle_to_go : 0.51337322044,
Time Elabled : 0.25,
x : 3.90, Y : 3.94 , theta : 1.06790772162,
angle_to_go : 0.512093057824,
Time Elabled : 0.25,
x : 3.90, Y : 3.94 , theta : 1.15946520354,
angle_to_go : 0.51078159702,
Time Elabled : 0.25,
x : 3.90, Y : 3.94 , theta : 1.22054843603,
angle_to_go : 0.50896617729,
Time Elabled : 0.25,
x : 3.90, Y : 3.94 , theta : 1.31218232188,
angle_to_go : 0.507633545963,
Time Elabled : 0.25,
x : 3.90, Y : 3.94 , theta : 1.37322877316,
angle_to_go : 0.505203663925,
Time Elabled : 0.25,
Asked by q8wwe on 2019-08-09 16:06:37 UTC
Answers
You would expect it to only go straight, right?
This is not happening, because the robot rotates, until it points at the goal. Then it drives. But while getting closer to the goal, the heading angle difference keeps increasing until it is out of the .1 rad margin.
Maybe you would like the behaviour better if it could also turn in the other direction ..
if angle_to_go-theta > 0:
speed.angular.z = 0.3
elif angle_to_go-theta < 0:
speed.angular.z = -0.3
Asked by ct2034 on 2019-08-10 09:37:53 UTC
Comments
First robot should rotate until facing the desired heading angle after that drive towards.in your answer, you might refer to overshoot so the robot rotates again .in such case do we need a feedback loop if so how to cooperate it inside the code. I am a beginner in Ros and will appreciate any help. one more thing I noticed if the angular speed (z) or linear speed(x) increased from 0.3 action performance become even worse. i tried the suggested method but didn't work, robot didn't move and kept rotating
Asked by q8wwe on 2019-08-10 11:33:18 UTC
okay. But all I am trying to say is that what you are seeing is absolutely the expected behaviour. It is based on trigonometry
Asked by ct2034 on 2019-08-10 11:52:20 UTC
how can I fix it so it can go directly to the goal with no stop .
Asked by q8wwe on 2019-08-10 12:00:50 UTC
You can have a look at the carrot planner in ros: https://wiki.ros.org/carrot_planner
Asked by ct2034 on 2019-08-10 12:45:19 UTC
is there is a solution to solve this problem? I am still unable to solve it
Asked by q8wwe on 2019-08-19 14:21:10 UTC
Comments
Asked by Humpelstilzchen on 2019-08-10 02:49:41 UTC
I created a small video and uploaded to youtube, you can find the link on the question I can't figure out the problem behind this.
Asked by q8wwe on 2019-08-10 07:25:10 UTC
The robot doesn't look like it is stopping in the video. Also please provide the console output as text.
Asked by Humpelstilzchen on 2019-08-10 10:15:41 UTC
in the video, the robot keeps stopping for checking the orientation. what I am expecting is the robot keep going until reach the goal, but as you see the robot keep stopping in the way. also, note if we change angular or linear velocity the performance will be double worse .
I added console output to the main question.
Asked by q8wwe on 2019-08-10 11:58:23 UTC