DOUBT on Rosject - ROS BASICS PYTHON

[AshishVarma]

import rospy
from lab_project.srv import FindWall, FindWallResponse
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import math

class WallFinder:
    def __init__(self):
        self.count = 0
        self.count2 = 0
        self.min_value = 0
        self.turn_check = 0
        self.move_front_check = 0
        self.service_called = False

        rospy.init_node('service_node')
        self.sub = rospy.Subscriber('/scan', LaserScan, self.laser_callback, queue_size=1)
        self.my_service = rospy.Service('/find_wall', FindWall, self.my_callback)
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.move = Twist()
        self.rate = rospy.Rate(2)
        rospy.spin()

    def value(self, ranges):
        self.min_value = min(ranges)
        return self.min_value

    def laser_callback(self, beam):
        if not self.service_called:
            return

        ranges = beam.ranges
        print("Total values in range array: ", len(ranges))
        print("Front Laser value: ", ranges[360])
        print("Right Laser value: ", ranges[180])

        if self.count == 0:
            self.min_value = self.value(ranges)
            self.count = 1
            self.turn_check = 1
        self.min_value = round(self.min_value, 1)

        front_range = math.floor(ranges[360] * 10) / 10
        print("Front Laser value: ", front_range)

        if front_range > self.min_value and self.turn_check == 1:
            print(self.min_value)
            self.move.angular.z = 0.2
            self.move.linear.x = 0.0
            print("turning")
            self.pub.publish(self.move)

        elif self.turn_check == 1:
            self.move.angular.z = 0.0
            self.move.linear.x = 0.0
            print("stopping")
            self.move_front_check = 1
            self.pub.publish(self.move)

        if self.move_front_check == 1:
            if front_range > 0.2:
                print("moving front")
                self.move.angular.z = 0.0
                self.move.linear.x = 0.1
                self.pub.publish(self.move)
            else:
                self.move.angular.z = 0.0
                self.move.linear.x = 0.0
                self.pub.publish(self.move)
                self.move_front_check = 0
            self.turn_check = 0

        if front_range <= 0.2 or self.count2 == 1:
            if abs(round(ranges[180], 2) - round(min(ranges), 2)) > 0.02:
                print("MIN BEAM VALUE: ", round(min(ranges), 2))
                print("RIGHT BEAM VALUE: ", round(ranges[180], 2))
                self.move.angular.z = 0.2
                self.move.linear.x = 0.0
                self.pub.publish(self.move)
                self.count2 = 1
            else:
                self.move.angular.z = 0.0
                self.move.linear.x = 0.0
                print("DONE")
                self.count2 = 0
                self.pub.publish(self.move)

    def my_callback(self, request):
        self.service_called = True  
        rospy.loginfo("Wall following behavior initiated.")
        my_response = FindWallResponse()
        my_response.wallfound = True
        rospy.loginfo("WALL FOUND.")
        return my_response

    def run(self):
        while not rospy.is_shutdown():
            self.rate.sleep()

if __name__ == '__main__':
    wall_finder = WallFinder()
    wall_finder.run()

This is my service server code of finding wall, my code runs well but the problem is when I launch my server using roslaunch cmd my service getting start, later when I call my service using rosservice call, it being called but the actual thing is the response getting returned as true before the completion of whole execution of my code, I mean before the completion of wall find the response is getting printed as true

Please give some suggestions to optimize my code
Thank you

[girishkumar.kannan]

Hi @AshishVarma ,

The problem is in your program!

@AshishVarma

    def my_callback(self, request):
        self.service_called = True  
        rospy.loginfo("Wall following behavior initiated.")
        my_response = FindWallResponse()
        my_response.wallfound = True
        rospy.loginfo("WALL FOUND.")
        return my_response

Your service callback has no logic to make the robot move, so your callback simply sets the wallfound variable to True and returns immediately!

Fix: Include robot movement inside your service callback.

Also you need to clean your laser callback. You should never overload your subscriber callbacks with robotic movements. That is a very bad practice. Move the entire “robot movement wall finding logic” inside the service callback.

Regards,
Girish

[AshishVarma]

then what should i add in the lasercallback function?

[girishkumar.kannan]

Hi @AshishVarma ,

@AshishVarma

then what should i add in the lasercallback function?

Basically just the data acquisition logic only. Read the laser scan message inside the laser callback and store the values in class variable(s) dedicated to store laser scan readings.

Regards,
Girish

[AshishVarma]

idk whats the problem but my ranges array in servicecallback function is not updating the values so the robot is turning in the loop because it doesn’t reach the minimum value without updating of ranges array in service callback

import rospy
from lab_project.srv import FindWall, FindWallResponse
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import math

class WallFinder:
    def __init__(self):
        self.count = 0
        self.count2 = 0
        self.min_value = 0
        self.turn_check = 0
        self.move_front_check = 0
        self.ranges = None
        self.service_called = False

        rospy.init_node('service_node')
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.move = Twist()
        self.my_service = rospy.Service('/find_wall', FindWall, self.my_callback)
        self.sub = rospy.Subscriber('/scan', LaserScan, self.laser_callback)
        self.rate = rospy.Rate(10)
        rospy.spin()

    def value(self, ranges):
        self.min_value = min(ranges)
        return self.min_value

    def laser_callback(self, beam):
        # if not self.service_called:
        #     return
        self.ranges = beam.ranges
        # global ranges
        # ranges = self.ranges

    def my_callback(self, request):
        ranges = self.ranges
        if ranges is not None: 
            print("Total values in range array: ", len(ranges))
            print("Front Laser value: ", ranges[360])
            print("Right Laser value: ", ranges[180])
            # self.service_called = True
            if self.count == 0:
                self.min_value = self.value(ranges)
                self.count = 1
                self.turn_check = 1
            self.min_value = round(self.min_value, 1)

            front_range = math.floor(ranges[360] * 10) / 10

            while (front_range > self.min_value and self.turn_check == 1):
                print("Front Laser value: ", front_range)
                print(self.min_value)
                self.move.angular.z = 0.2
                self.move.linear.x = 0.0
                print("turning")
                self.pub.publish(self.move)
                rospy.sleep(2)

            if self.turn_check == 1:
                self.move.angular.z = 0.0
                self.move.linear.x = 0.0
                print("stopping")
                self.move_front_check = 1
                self.pub.publish(self.move)

            while (self.move_front_check == 1):
                if front_range > 0.2:
                    print("moving front")
                    self.move.angular.z = 0.0
                    self.move.linear.x = 0.1
                    self.pub.publish(self.move)
                else:
                    self.move.angular.z = 0.0
                    self.move.linear.x = 0.0
                    self.pub.publish(self.move)
                    self.move_front_check = 0
                self.turn_check = 0

            while (front_range <= 0.2 or self.count2 == 1):
                if abs(round(ranges[180], 2) - round(min(ranges), 2)) > 0.02:
                    print("MIN BEAM VALUE: ", round(min(ranges), 2))
                    print("RIGHT BEAM VALUE: ", round(ranges[180], 2))
                    self.move.angular.z = 0.2
                    self.move.linear.x = 0.0
                    self.pub.publish(self.move)
                    self.count2 = 1
                else:
                    self.move.angular.z = 0.0
                    self.move.linear.x = 0.0
                    print("DONE")
                    self.count2 = 0
                    self.pub.publish(self.move)
            rospy.loginfo("Wall following behavior initiated.")
            my_response = FindWallResponse()
            my_response.wallfound = True
            rospy.loginfo("WALL FOUND.")
            return my_response

    def run(self):
        while not rospy.is_shutdown():
            self.rate.sleep()

if __name__ == '__main__':
    wall_finder = WallFinder()
    wall_finder.run()

please solve my issue

[girishkumar.kannan]

Hi @AshishVarma ,

Your programming logic is unnecessarily complicated for an otherwise easy and small task.
Your robot will always be in a “turning” state because of your first while loop inside your my_callback.
Your robot will be stuck there because the first condition cannot be reached: front_range > self.min_value because of rounding off and accuracy issues.

You need to completely change your programming logic.

Let me give you a simple pseudo-code:

[manually position the robot a bit far from a wall first, but not too far from wall]
service_callback:
get the laser scans
find which is the minimum scan value and get the index
round this minimum range to 3 decimal places
now choose a direction and spin
loop over:
   rotate the robot
   wait for few secs for spin to complete (0.5 to 1.0 secs)
   stop the robot
   get laser scans
   find minimum range and index
   check if the new minimum range is same as the first minimum range
   if not the same then continue else break
[at this point the robot should have positioned itself correctly facing the wall]
return with wallfound set to True

This logic should work for you.

Regards,
Girish

[AshishVarma]

Thank you for the response sir but my actual problem is my ranges array in servicecallback function is not updating, so how do i update the ranges array in service callback function? is my code has any wrong in updation of array?

[girishkumar.kannan]

Hi @AshishVarma ,

Your laser scan callback is fine. I do not see any problems there.
However you might want to declare self.ranges as empty list rather than None.
Use self.ranges = [] instead of self.ranges = None.

Regards,
Girish

[AshishVarma]

thank you for the response sir I found the actual error, I should update the ranges array by using

ranges = self.ranges

because my while loop is iterating and checking the conditions I should add ranges = self.ranges inside my while loop so that the array gets updated and the while loop checks the condition every time and the robot will stop after it faces the wall.

It got worked for me and everything is running well.

[girishkumar.kannan]

Hi @AshishVarma ,

Glad to know that you have fixed your problem yourself!

Regards,
Girish