Autonomous navigation with obstacle avoidance to goal

edit

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)



rospy.init_node("speed_controller")

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
    else:
        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") 
#else:
    #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


pub.publish(speed)
r.sleep()

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 :(