Logic for wall finding not working

For the first step, I am trying to stop the robot when the front laser range becomes the minimum value in the ranges list but the robot keeps rotating I don’t know why.
Here is the code:

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from rosject.srv import findwall, findwallResponse

def callback(msg):
    r = list(msg.ranges)
    minimum_value = min(r)
    print(minimum_value)
    var.angular.z = 0.2
    pub.publish(var)
    var.angular.z = 0
    if msg.ranges[360] == min(r):
        pub.publish(var)

rospy.init_node('findwall_node')
sub = rospy.Subscriber('/scan', LaserScan, callback)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)    
var = Twist()
rospy.spin()

Hi @mamojiz97 ,

Firstly, you must name your custom message files in CamelCaseStyle.
So, please change findwall to FindWall.

Please STOP using names like var. Use descriptive names like twist_msg or twist_cmd.

Let us go though your callback, shall we?

  1. You initialized a variable r to hold your ranges.
  2. You initialized a variable minimum_value to hold minimum of ranges.
  3. You set angular velocity to 0.2 and publish it.
  4. Then you immediately set angular velocity to 0.
  5. You publish this to make the robot stop only when ray 360 is equal to minimum of ranges.

Your logic will never work. Here is the reason why:
Your flow of thoughts is partly correct but you have not accounted for the drift in laser scan ranges due to the angular velocity with which you turn/rotate your robot.

  1. Minimum of ranges will never be a constant value.
  2. You are not storing the minimum value to compare previous minimum to current minimum.
  3. You will overshoot / slip the actual minimum position when you are spinning at the rate of 0.2.
  4. You are checking a value that has 15 decimal places. These values will never be the same, by which I mean that your last scan minimum will not be the same as current scan minimum.

So think about these issues, to give you some hints:

  1. Implement a float value decimator that would round off range values to, say, 3-4 decimals.
  2. Spin slower or faster depending upon the location / direction of the minimum ray.
  3. Stop > Scan > Check > Turn instead of Turn > Check > Stop > Scan.

In summary, you need to rework on your logic to stop the robot at the minimum range in front.

Regards,
Girish

2 Likes

@girishkumar.kannan
Thank You for the suggestion. I did find a way around with that but I am stuck at a rather odd problem.

def callback(msg):
    r = list(msg.ranges)
    minimum_value = min(r)
    print(minimum_value)
    index_minimum = r.index(minimum_value)
    ray_angle = msg.angle_min + (index_minimum * msg.angle_increment)
    countdown_sec1 = 0

    if ray_angle < 0:
        twist_instance.angular.z = 0.2
    else:
        twist_instance.angular.z = -0.2
    
    rotation_duration = (abs(ray_angle)/0.2) - 1
    
    while countdown_sec1 <= rotation_duration:
        pub.publish(twist_instance)
        rate.sleep()
        countdown_sec1 = countdown_sec1 + 1
    
    twist_instance.angular.z = 0
    twist_instance.linear.x = 0
    pub.publish(twist_instance)
    rate.sleep()
    print('front distance',msg.ranges[360])

    close_loop = True
    if close_loop is True:
        print('final distance',msg.ranges[360])
        rospy.signal_shutdown("Exit loop")

Is this particular code I exit the loop once the function is complete for my reference only.
My question here is that I try to print the final front distance after the completion of function and it gives an incorrect value. It seems that the front distance and final distance that I print in the code are the same and don’t change whatsoever.
I verified this by writing another piece of program where I just print the front distance msg.ranges[360] and I find the output value different and to my understanding the correct one. Can you help me find where I am heading wrong at?
Thank You

Hi @mamojiz97 ,

There seems to be a flaw in your algorithm.

Here, angle_min is a positive non-zero value. Ray indexes are between 0 and 719, so those values are also positive. angle_increment is another positive non-zero number.
Therefore, you will never get a negative value for ray_angle.
So the following logic is wrong:

Therefore, I believe your robot is spinning, and must be towards right (if I am correct?).

This, I believe, is mostly because you are not updating the range value within your callback. Also, I am not sure If you are moving the robot during the measurement. The best way to avoid this, in my experience, is to not overload callbacks with robot procedures. Just have the callback update the necessary class variables that store information like ranges. Make use of the values in the main program and call the main function under if __name__ == "__main__" or in the __init__ function of the class.

You seem to be struggling with the logic. Let me give you a rough steps of the idea:

  1. When robot is stopped, perform a full-round scan.
  2. Get the ray number (between 0 and 719) of the ray with minimum range.
  3. Round of the minimum range value to 3 decimal places, so that a number like 3.141592653589793 just reads 3.142. [Remember that range measurements are in meters, so more than 3 decimal place precision is just waste of computation].
  4. Now find the difference between 360 and the minimum ray number.
  5. If the number is more than 360 (front ray id) spin left/counter-clockwise, otherwise spin right/clockwise.
  6. While spinning, spin slowly. Spin slow → check if ray 360 measures 3.142 → spin again and check…
  7. Continue spinning and checking for the range until ray 360 actually measures 3.142 after rounding each measurement during the spin.
  8. Break the loop and complete the process.

You seem to be complicating the logic by adding rotation duration and things you don’t actually need.

My advice again: Redo your logic. Understand what is going on first. I am not sure how else I can help you with this.

Regards,
Girish

1 Like

@girishkumar.kannan
Thank you once again for taking time to help me.

ray_angle = msg.angle_min + (index_minimum * mgs.angle_increment) 

Here the angle_min is a negative non-zero value. The laser scan works in a way that the front laser is taken as the 0 angle. Hence for index values between 0-360 the ray_angle will be negative and the rest will be positive.

No I stopped the robot once after rotation and then printed out msg.ranges[360] which printed out an incorrect value. ( I say this because I tested it in the same position with another program and it gave me a value that was different and in my opinion correct).

I performed the logic as suggested before I cooked up the above program logic.
Using the logic flow like you suggested, the program I wrote is:

def callback(msg):
    r = list(msg.ranges)
    minimum_value = min(r)
    print(minimum_value)
    index_minimum = r.index(minimum_value)
    rounded_min = round(minimum_value,3)
    print('min index',index_minimum)
    front_range = round(msg.ranges[360],3)

    if index_minimum > 360:
        twist_instance.angular.z = 0.05
    else:
        twist_instance.angular.z = -0.05
    while front_range >= rounded_min:
        print('rounded min', rounded_min)
        print('front ',round(msg.ranges[360],3))
        pub.publish(twist_instance)
        rate.sleep()
    print('new front', front_range)
    twist_instance.linear.x = 0.1
    twist_instance.angular.z = 0
    while round(msg.ranges[360],1) >= 0.3:
        pub.publish(twist_instance)
        rate.sleep()
    twist_instance.linear.x = 0
    pub.publish(twist_instance)

Earlier I believed that the values rounded_min & msg.ranges[360] change continuously as the robot moves or rotates in this case but here I see that when I print the values in the while loop the values don’t change.
I think it is because I am using a loop inside the callback, in the line while round(msg.ranges[360],3) != rounded_min . Then, the callback gets “stuck”, and it is called only once, causing msg.ranges to remain constant. However, I maybe wrong and if I am right I don’t know how to go about it?

Thank You

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