Ask Your Question
0

Robot keep moving a tiny distance and stop

asked 2019-08-09 16:06:37 -0500

q8wwe gravatar image

updated 2019-08-10 11:57:13 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

  1. I wonder how this can work without "import time" 2. Please provide the console output.
Humpelstilzchen gravatar imageHumpelstilzchen ( 2019-08-10 02:49:41 -0500 )edit

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.

q8wwe gravatar imageq8wwe ( 2019-08-10 07:25:10 -0500 )edit

The robot doesn't look like it is stopping in the video. Also please provide the console output as text.

Humpelstilzchen gravatar imageHumpelstilzchen ( 2019-08-10 10:15:41 -0500 )edit

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.

q8wwe gravatar imageq8wwe ( 2019-08-10 11:58:23 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-08-10 09:37:53 -0500

ct2034 gravatar image

updated 2019-08-10 12:44:21 -0500

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
edit flag offensive delete link more

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

q8wwe gravatar imageq8wwe ( 2019-08-10 11:33:18 -0500 )edit

okay. But all I am trying to say is that what you are seeing is absolutely the expected behaviour. It is based on trigonometry

ct2034 gravatar imagect2034 ( 2019-08-10 11:52:20 -0500 )edit

how can I fix it so it can go directly to the goal with no stop .

q8wwe gravatar imageq8wwe ( 2019-08-10 12:00:50 -0500 )edit

You can have a look at the carrot planner in ros: https://wiki.ros.org/carrot_planner

ct2034 gravatar imagect2034 ( 2019-08-10 12:45:19 -0500 )edit

is there is a solution to solve this problem? I am still unable to solve it

q8wwe gravatar imageq8wwe ( 2019-08-19 14:21:10 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-08-09 16:06:37 -0500

Seen: 47 times

Last updated: Aug 10