Autonomous navigation with obstacle avoidance to goal

asked 2021-04-25 09:52:48 -0500

anish27 gravatar image

updated 2021-04-25 22:03:48 -0500

jayess gravatar image

Hi everyone, I've been trying to make the turtlebot3 (waffle) move across a custom world autonomously and avoiding obstacles to reach a goal. While I have been able to put up something together for the autonomous navigation, it wasn't the most efficient code, therefore I decided to put bits and pieces together. I am now able to navigate autonomously but I am having issues to reach my goal. I am unsure of the logic to be used to combine all of these three tasks.

-Autonomous navigation -Obstacle Avoidance -Go to Goal

Basically I don't know how to plug in the value below to reach the goal without spinning in a continuous circle.

 abs(angle_to_goal - theta)

Please find my code below and links to codes I have used.

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

x = 0.0
y = 0.0 
theta = 0.0
laser_distance1 = 0
laser_distance2 = 0
laser_distance3 = 0
turtlebot = []

def newOdom(msg):
    global x
    global y
    global theta
    global laser_distance1
    global laser_distance2
    global laser_distance3
    input_message_type = str(msg._type)

if input_message_type == "sensor_msgs/LaserScan":
    laser_distance1 = msg.ranges[0]
    laser_distance2 = msg.ranges[15]
    laser_distance3 = msg.ranges[345]

if input_message_type == "nav_msgs/Odometry":
    turtlebot = msg.pose.pose    
    x = turtlebot.position.x
    y = turtlebot.position.y
    rot_q = turtlebot.orientation
    (roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])

#print("X position:", x)    
#print("Angle:", theta) 
#print("Laser Distance", laser_distance)


sub = rospy.Subscriber("/odom", Odometry, newOdom)
pub = rospy.Publisher("/cmd_vel", Twist, queue_size = 1)
sub = rospy.Subscriber('scan', LaserScan, newOdom)

speed = Twist()

r = rospy.Rate(10)

goal = Point()

goal.x = -1.8
goal.y = -0.3

thr1 = 0.8 
thr2 = 0.8

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

angle_to_goal = atan2(inc_y, inc_x)

#print("Angle:", theta) 
#print("Laser Distance1", laser_distance1)
#print("Laser Distance2", laser_distance2)
#print("Laser Distance3", laser_distance3)
#print("Angle to goal - theta", angle_to_goal - theta)

if abs(angle_to_goal - theta) > 0.1:
    speed.linear.x = 0.0
    speed.angular.z = 0.3
    if laser_distance1>thr1 and laser_distance2>thr2 and laser_distance3>thr2:
        speed.linear.x = 0.5
        speed.angular.z = 0.0
        speed.linear.x = 0.0
        speed.angular.z = 0.5
        if laser_distance1>thr1 and laser_distance2>thr2 and laser_distance3>thr2:
            speed.linear.x = 0.5
            speed.angular.z = 0.0

#if laser_distance1>thr1 and laser_distance2>thr2 and laser_distance3>thr2:
    #speed.linear.x = 0.5
    #speed.angular.z = 0.0
    #print("Moving Forward") 
    #print("Obstacle rotating!")
    #speed.linear.x = 0.0
    #speed.angular.z = 0.5
    #if laser_distance1>thr1 and laser_distance2>thr2 and laser_distance3>thr2:
        #print("obstacle avoided, moving forward!")
        #speed.linear.x = 0.5
        #speed.angular.z = 0.0


These are the links I have used to put up what I have currently.

Obstacle avoidance and navigation -> link text Move to Goal -> link text

Any help would be appreciated, I have been struggling for days with that :(

edit retag flag offensive close merge delete


Could you please update the code with the same formatting that you use to run it? In its current state, it won't run.

jayess gravatar image jayess  ( 2021-04-25 22:06:53 -0500 )edit