ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

ROS : Robot moves to certain coordinate point but how to stop and set multiple goals?

asked 2020-09-20 15:18:43 -0500

Varun gravatar image

updated 2020-09-20 15:19:29 -0500

HI

I followed the tutorial Construct ROS tutorial and this is referencing the question of ROS-Answers How to move to a certain point in space using Twist /cmd_vel

I am able to move the robot to a coordinate. But it keeps rotating when it reached the goal

CURRENT STATUS

Currently the robot can go to a single coordinate assigned by goal.x and goal.y when i run the program in the Pi. It keeps rotating once it reaches the goal.

QUESTION:

How do i stop the robot when it reaches my goal coordinate?
How do i add the next goal coordinate in the same code once the first goal is reached?
How do i make the robot face a particular direction (pose) once it reached a goal?

Please do help me anyone....

the code is as follows

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

 x = 0.0 
 y = 0.0 
 theta = 0.0

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

        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y

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

       rospy.init_node ("speed_controller")

sub =rospy.Subscriber("/odometry/filtered",Odometry, newOdom) 
pub =rospy.Publisher("/cmd_vel",Twist,  quene_size=1) 
speed = Twist()

goal = Point ()
goal.x = 5 // x coordinate for goal
goal.y = 5 // y coordinate for goal

while not rospy.is_shutdown():
     inc_x = goal.x - x
     inc_y = goal.y - y 

   angle_to_goal = atan2 (inc_y, inc_x)

   if abs(angle_to_goal - theta) > 0.1:
        speed.linear.x = 0.0
        speed.angular.z = 0.3   
   else:
        speed.linear.x = 0.5
        speed.angular.z = 0.0

   pub.publish(speed)

   r.sleep()

Thanks in advance

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2020-09-20 15:59:04 -0500

miker2808 gravatar image

updated 2020-09-21 02:02:37 -0500

I wrote similar task for a robot, but it was based on long/lat gemoetry, tell me if you'll need the source code and explanation.

But the reason for your robot spinning is because its always trying to reach the goal.x = 0 and goal.y = 0, when it gets close your robot "corrects" itself, but because nothing is 100% precise, it fails to do so and enters an endless loop of trying to reach the point. A solution to this is using a distance and "bearing" (Your theta if I understood right) and setting a distance larger than 0, close enough so the robot can achieve suitable results, (lets say radius of your robots side) so the robot will get somewhere close to your point, but not exactly on it.

For your second problem, I used a list, of co-ordinates as a 'path' I need to reach. lets say I have a cords_list = [[0,1],[3,4],[12,15]] as cords points I need to reach, this will be our path. and I loop the code and set my target as goal.x = cords_list[0][0] and goal.y = cords_list[0][1] and I try to reach it, but set a condition which is related to the distance of you to the point and say

while distance_to_target >= 5:
     # try to reach the point

and when I'm reaching the goal, I delete the first tuple from the list, this way when I iterate again my code, I will enter the loop with the distance to target larget than 5, and will now try to reach the second point. this until the list is empty, and you finish your task.

Now, few key points, you'll need to do 2 nested while loops, one to iterate the calculations, and one inside, to attempt to reach the path, of course, once you enter the path reach loop, you'll still need to do the calculations to update the position and distance of your robot relative to the point you need to rich.

EDIT: Here I used your code and adapted it to do what you've requested. I prefered to use your code, as I know how difficult it can be for beginners to read new code. I rechecked the code I wrote, I was mistaken, you dont really need an additional While loop, so I wrote it as I would recommend it to be (plus its simpler :D) PS: I havn't ran it, tell me how it goes, you might need to tweak some variables/numbers

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

import numpy as np

 x = 0.0 
 y = 0.0 
 theta = 0.0

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

        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y

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

       rospy.init_node ("speed_controller ...
(more)
edit flag offensive delete link more

Comments

Hi milker2808,

Thank you so much for your reply. This is my first time posting a question too.

Is it possible for you to send me your code to svkvarun43@gmail.com. I really understand the solution but I'm new to coding too. Will try to replicate your code for the cords_list into mine and the other loops.

However can you kindly tell me the exact code changes I need to do to make the robot stop at its goals. Should I change this if abs(angle_to_goal - theta) > 0.1 to a higher value that 0.1?

Please let me know. And kindly sent your code Thanks in advance

Varun gravatar image Varun  ( 2020-09-20 16:10:16 -0500 )edit

I've edited my answer, I hope it will help

miker2808 gravatar image miker2808  ( 2020-09-20 16:33:09 -0500 )edit

Hi Thank you so much for your reply, I really like the way you coded there.

Em sure it works. But unfortunately when i run the code the robot (wheels) dosent move. There is no error popping up too. I am not sure what is the issue. Is it some indentation error or some skipped loop. Im not able to figure out :"( BTW i just missed theses two lines in the first code as well. Just spotted it

 from geometry_msgs.msg import Point, Twist 
***from math import atan2***       //to import atan2 

speed = Twist()
**r = rospy.Rate(4)**

If it is not too much, can you try it out the code you gave me on your end please please.

DRIVE link for your code

I just kept the code in home directory /home/pi and did chmod +X filename.py and then ran ./filename.py

Varun gravatar image Varun  ( 2020-09-20 18:55:15 -0500 )edit

Hello, sorry i'm late (I was sleeping ) anyways, I watched your code.. and sadly, I do not have access to your simulator, but the good news, is that I adapted your code to run on the turtle sim (Its working, and is really cute - the code itself is ugly, and bad practice, but'll I'm too tired right now to pretty it - I think i'll later just email you the code) Anyways, few keys I noticed. ONE: Make sure you are really receiving the subscribed data, if you're sure its done, you're good to go ( had a small problem receiving position and theta data, so it failed my code, and my turtle was just spinning around) TWO: I noticed one problem, your robot doesn't move because you dont really update the speed, your "publish(speed)" was inside the else: statement, but your code always ...(more)

miker2808 gravatar image miker2808  ( 2020-09-21 01:56:17 -0500 )edit

additionally, please, get the r.sleep() inside the while loop, as the "while" loop is running without sleeping limitations. the r.sleep() is similar to the basic "sleep()" built in function, which in practice, simply freezes the code for the duration given.

miker2808 gravatar image miker2808  ( 2020-09-21 01:57:28 -0500 )edit

I saw you deleted your comment, Anyways, I sent you an email

miker2808 gravatar image miker2808  ( 2020-09-21 03:32:42 -0500 )edit

Hi milker2808, Is it possible for you to send me your code to tiagocostagameiro@gmail.com Im working on project similar to yours. Thank you

MKMTI gravatar image MKMTI  ( 2022-03-08 04:21:15 -0500 )edit
1

answered 2020-09-21 05:03:08 -0500

Varun gravatar image

Hi Everyone,

The robot can now move to the coordinates you specific and stop at the final way-point successfully. I had to make sure i was subscribing to the topic to get the results.

With the help of miker2808, the final code is

#! /usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Point, Twist
from math import atan2
import numpy as np

x = 0.0
y = 0.0
theta = 0.0

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

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y

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

rospy.init_node ("speed_controller")

sub = rospy.Subscriber("/odometry/filtered", Odometry, newOdom)
pub = rospy.Publisher("/cmd_vel",Twist, queue_size=1)
speed = Twist()

r = rospy.Rate(4)

path_list = [(4,4), (3,1), (0,1), (-8,-8)]
point_index = 0  # instead of deleting stuff from a list (which is anyway bug prone) we'll just iterate through it using index variable.
goal = Point ()

while not rospy.is_shutdown():

     if point_index < len(path_list): # so we won't get an error of trying to reach non-existant index of a list
         goal.x = path_list[point_index][0] # x coordinate for goal
         goal.y = path_list[point_index][1] # y coordinate for goal
     else:
         break # I guess we're done?
     inc_x = goal.x - x
     inc_y = goal.y - y

     angle_to_goal = atan2 (inc_y, inc_x) # this is our "bearing to goal" as I can guess

     # distance_to_goal = np.sqrt(goal.x*goal.x + goal.y*goal.y) 
     2distance_to_goal = np.sqrt(inc_x*inc_x + inc_y*inc_y)

     if 2distance_to_goal >= 0.3: # we'll now head to our target
         if abs(angle_to_goal - theta) > 0.2:
             speed.linear.x = 0.0
             speed.angular.z = 0.9

         else:
             speed.linear.x = 0.8
             speed.angular.z = 0.0
         pub.publish(speed)

     else:
        point_index += 1
     r.sleep()

Hope it is helpful to someone facing problems and is new to python

Next tasks:

> to make the robot repeat the path like for ie. keep making square path around the lawn 
> to make the robot turn quickly to its next way point rather  than making a full 360* and then point to the waypaint

Any assistance in this would be great too.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2020-09-20 15:18:43 -0500

Seen: 2,419 times

Last updated: Sep 21 '20