ROS basics in 5 days, Wall following rosject help

Hello all. I’ve trying to complete the wall following behavior assignment, however, I’m finding it very difficult to make the turtlebot3 follow the wall.

This is the code I’ve written.

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist


left, front, right, rear = 0, 0, 0, 0
vel = Twist()



def botCallback(msg):                                    

    global left, right, front, rear 

    front = msg.ranges[359]
    left = msg.ranges[539]
    right = msg.ranges[179]
    rear = msg.ranges[0]

    move()


def move():

    vel.linear.x = 0.08
    vel.angular.z = 0
    pub.publish(vel)
    rate.sleep()

    r_min = 0.15
    r_max = 3.5

    des_dist = 0.40
    err = (max(min((right - des_dist), 3.5), 0.15))
    kp=-5
    #kd = 1.8
    #diff_err = ((des_dist) - err)
    #inp = kp*err #+ kd*diff_err
    #print(inp)

    if (r_max < right or front < 0.3):
        vel.linear.x = -0.05/(front)
        if front > r_min:
            vel.angular.z = 0.3
        vel.angular.z = 0.0
        print("out of range")

    elif r_max > front > 0.3:
        #print(f"is_greater {front}")
        if r_max > right > 0.6:
            vel.linear.x = 0.1 * front
            vel.angular.z = - 0.25
            #vel.angular.z = inp
            print("Turning right")

        elif right < 0.2:
            vel.linear.x = 0.1 * front
            vel.angular.z = 0.25
            #vel.angular.z = inp
            print("turning left")

    elif front < 0.8:
        #print(f"is_lesser {front}")
        vel.linear.x = 0.05
        vel.angular.z = 0.4
        print("bound turn")
   

    pub.publish(vel)
    rate.sleep()

    '''
    while right > 0.2:
        vel.angular.z = -0.5
        if 0.27 > right > 0.2:
            vel.angular.z = 0

        while front < 0.8:
            vel.angular.z = 1

        pub.publish(vel)
        rate.sleep()
'''


while not rospy.is_shutdown():

    rospy.init_node('follow_wall')
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
    sub = rospy.Subscriber('/scan', LaserScan, botCallback)
    rate = rospy.Rate(20)
    rospy.spin()

Please help me in understanding what I’m missing or where my logic is flawed/ can be improved.

Problems I’m facing: -

  1. The robot doesn’t follow the wall in a straight line.
  2. It sometimes runs into the wall, despite having included a check in the code.

Hi @Yogesh_G,

Please edit/post your doubts with the right formatting, it helps you and other read the posts and makes helping easier.

The first thing I notice in the script is that there are while loop in the main() function. If the program is stuck in a while loop it is not certain weather the variable right will be updated or not. So avoid while loops.

Also, you are publishing more than once a command to the /cmd_vel topic. Basically you want to publish once per callback (your function main() is part of the callback). E.g. instead of always first publishing:

regardless of the radar readings; remove the pub.publish(vel) and rate.sleep() lines and leave a single publish line at the very end of main().

Hope it helps!

PD: Using classes in this sort of scenario is great! Once you get a hang on them it makes life easier. You can find examples in the course notebook itself.

1 Like

Thanks for the reply!
I will: -

  1. Avoid using while loops.
  2. Try using classes.

I’d like to get suggestions on the wall following logic if you have any.

It is very time consuming to check the code. I know it is very annoying but if anyone wants to become a software developer step #0 is to code properly. For great improvements:

  1. Write the code for someone else (that someone else includes yourself from the future) to be able to understand it.
  2. Make the variable names more descriptive. Per example: What is r_max? I guess it is the max range (it took me some minutes to find out) but then why not just name it max_range?
  3. Add comments on things like the if statements explaining what it means that said statements is true. E.g.:
    elif r_max > front > 0.3: # The front measurement is in between the boundaries
  4. Please edit the post, select the code and press on the </> symbol in the post editor. If you write python after the initial ‘’’ the formatting will be even better:

Again, I know I am annoying but if anyone wants to be helped in the internet the minimum one can do is to write proper code. Same writing a bureaucratic paper with illegible calligraphy. Unless you are a hospital doctor; there’s exceptions to every rule.

2 Likes

This are some comments I wrote reading the logic part in main(). I used lower case for comments aiding my understanding and capital letters for thoughts:

def move():

    vel.linear.x = 0.08
    vel.angular.z = 0
    pub.publish(vel)
    rate.sleep()

    min_range = 0.15
    max_range = 3.5

    if max_range < right_range or front_range < 0.3: # The followed wall is not seen or too close to front collision
        vel.linear.x = -0.05/front_range # Go backwards in proportion to how close the front wall is -- WHAT IF THE RIGHT WALL IS LOST BUT THE BOT IS FAR FROM FRONT COLLISION?
        if front_range > min_range: # The front wall is further away than the minimum range
            vel.angular.z = 0.3 # Turn left
        vel.angular.z = 0.0 # Don't turn -- IGNORING THE PREVIOUS IF STATEMENT? maybe 'else' forgotten
        print("out of range")
        # It will always go backwards without turning

    elif max_range > front_range > 0.3: # The front measurement is in between acceptable boundaries
        if max_range > right_range > 0.6:  # The right wall is too far away
            vel.linear.x = 0.1 * front_range # Continue forwards (directly proportional to front measurement)
            vel.angular.z = - 0.25 # Turn left
            print("Turning right")

        elif right_range < 0.2: # Too close to the followed wall
            vel.linear.x = 0.1 * front_range # Continue forwards (directly proportional to front measurement)
            vel.angular.z = 0.25 # Turn left
            print("turning left")

        # WHAT IF right_range EQUALS 0.5?
        # Then we continue straight use the first values of x = 0.08 and z = 0 (NOT CLEAR, PROBABLY ALSO PREFER USING `vel.linear.x = 0.1 * front_range`)

    elif front_range < 0.8: # Theres an obstacle too close on front (but over 0.3)
        vel.linear.x = 0.05 # Advance slowly
        vel.angular.z = 0.4 # Turn left rapidly
        print("bound turn")
    
    # WHAT IF front_range IS EQUAL TO 4? NOT COVERED IN ANY IF STATEMENT
   

    pub.publish(vel)
    rate.sleep()

@GasPatxo ,

Dude! This reply is GOLD. I wish I could like a post 1000x !

2 Likes

Thank you SO MUCH for this! I will work on properly writing a legible code, as you’ve suggested.

1 Like

This topic was automatically closed after 3 days. New replies are no longer allowed.