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: -
- The robot doesn’t follow the wall in a straight line.
- 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: -
- Avoid using while loops.
- 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:
- Write the code for someone else (that someone else includes yourself from the future) to be able to understand it.
- 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?
- 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
- 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.