Python Forum
Turning Robot towards goal, comparison of angles
Thread Rating:
  • 0 Vote(s) - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5
Turning Robot towards goal, comparison of angles
#1
Hey there,

I'm currently working on a project about navigating a robot to a goal point.

I use some basics from ROS but as the code is in python and my question is more about the logic I thought I could try to ask for help here.

I already wrote some code to tackle this task. The code uses the gazebo/modelstates topic to constantly get the position and orientation of the robot. The angle_to_goal is calculated by looking at the currentPosition and the goalPosition.

The robot should turn, until the difference between the yaw of the robot and the angle_to_goal is 0 +/- a threshold of 0.1. If this is the case, the robot should drive towards the goal.

This code is working pretty well in some cases where the robot gets into the acceptable range well and drives straight without any correction needed.

In other cases the robot will turn, then drive a very small distance, then correct its course again a little bit and repeat this which leads to a very slow and jittery path. I can imagine that the problem is that the robot gets into the acceptable range of the angle at the very border of it, then starts driving which makes the difference of the angles grow and leads the robot to turn the robot again but not enough so that it drives relatively fluently afterwards. Another case is that the robot seems to overshoot the desired angle_range or immediately leave it and doesn't stop turning.

In both cases the robot does not really find its way but keeps being stuck, misses the right angle and continues in this sub-optimal behavior. I already tried different angular-velocities and also other thresholds.

As I am a total beginner I don't know if my guess of the problem could be right. I don't know how i could do to solve the problem or at least improve the rate of the robot getting the right direction at the first time. Thank you in advance for any help!

Here is the code:
#! /usr/bin/env python
import rospy
from gazebo_msgs.msg import ModelStates
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Point
from math import atan2

#start is x:0, y:0
x = 0.0
y = 0.0
theta = 0.0     #current angle of robot

#import ipdb; ipdb.set_trace()

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

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


rospy.init_node ('subscriber')
sub = rospy.Subscriber('/gazebo/model_states', ModelStates, callback)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

speed = Twist()

r = rospy.Rate(4)

goal = Point()
goal.x = -2
goal.y = -1

while not rospy.is_shutdown():
    inc_x = goal.x - x                      #distance robot to goal in x
    inc_y = goal.y - y                      #distance robot to goal in y
    angle_to_goal = atan2 (inc_y, inc_x)    #calculate angle through distance from robot to goal in x and y
    print abs(angle_to_goal - theta)
    if abs(angle_to_goal - theta) > 0.1:    #0.1 because it too exact for a robot if both angles should be exactly 0
            speed.linear.x = 0.0
            speed.angular.z = 0.3
    else:
        speed.linear.x = 0.3                #drive towards goal
        speed.angular.z = 0.0

    pub.publish(speed)

 r.sleep()
Reply


Possibly Related Threads…
Thread Author Replies Views Last Post
  I am getting a valueError. And not sure why? My goal is to visualize the correlation ReadytoCode 0 416 Dec-11-2023, 05:33 AM
Last Post: ReadytoCode
  Goal Seek tdutcher05 1 696 Nov-17-2023, 10:33 PM
Last Post: deanhystad
  I get an FileNotFouerror while try to open(file,"rt"). My goal is to replace str decoded 1 1,361 May-06-2022, 01:44 PM
Last Post: Larz60+
  The Wall-E Robot Argentum 1 1,384 Apr-03-2022, 03:01 PM
Last Post: sastonrobert
  DarkPaw Robot code Tyrelex78 3 2,091 Nov-27-2020, 12:06 AM
Last Post: Tyrelex78
  turtle calculate angles thruss 3 2,549 Aug-25-2020, 01:42 PM
Last Post: deanhystad
  I am getting an Error, and not sure why? My goal is to print out rahulne22 7 3,004 Dec-01-2019, 03:53 PM
Last Post: snippsat
  how to program robot to pass wise man puzzle steven12341234 0 1,908 Dec-02-2018, 08:31 AM
Last Post: steven12341234
  Help Threading python robot lucandmendes 0 7,996 May-03-2018, 02:42 PM
Last Post: lucandmendes
  Parallel Processing in Python with Robot crcali 6 5,102 Apr-06-2018, 03:48 AM
Last Post: Larz60+

Forum Jump:

User Panel Messages

Announcements
Announcement #1 8/1/2020
Announcement #2 8/2/2020
Announcement #3 8/6/2020