Rosject ROS Basics in 5 Days section 2.2 : wall following starts even before getting the result of the service (wall_found)

Dear community,

The task requires that first the robot looks for the wall, then it starts to follow it, but when I launch the main launch file, wall following starts even before getting the result of the service.
Could you help me check why this happens? I use self.wall_found flag but it does not work…

Here is my launch file main.launch:

<launch>
  <include file="$(find wall_following)/launch/start_find_wall_service_server.launch"/>
  <node pkg="wall_following" type="wall_follower.py" name="wall_follower_node" output="screen"/>
</launch>

Here is wall_follower.py:

#!/usr/bin/env python

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

from find_wall_service_client import FindWallServiceClient  # Import the service client

class WallFollow():
    def __init__(self):
    
        self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
        self.sub = rospy.Subscriber("/scan", LaserScan, self.laser_callback)
    
        self.move_cmd = Twist()

        self.num_ray = None
        self.front_index = None
        self.right_index = None
        self.angle_increment = None
        self.laser_front_list = []
        self.laser_right_list = []
        self.front_distance = 0
        self.right_distance = 0

        self.find_wall_client = FindWallServiceClient()  # Initialize the service client
        rospy.loginfo("Initialized the service client...")

        self.wall_found = False  # Initialize the wall_found flag
    
        # Register the shutdown hook
        rospy.on_shutdown(self.stop)

        # Call the service to find the wall
        rospy.loginfo("Call find wall service before starting wall-following behavior")
        self.call_find_wall_service()

    def call_find_wall_service(self):
        # Call the service before starting wall-following behavior
        rospy.loginfo("Send request ...")
        self.wall_found = self.find_wall_client.send_request()
        if self.wall_found:
            rospy.loginfo("Wall found, starting wall-following behavior")
        else:
            rospy.logerr("Failed to find the wall. Shutting down.")
            rospy.signal_shutdown("Failed to find wall")
            return

    def calculate_laser_indices(self, msg):
        # get info about laser data
        #rospy.loginfo(f"laser angle_min: {msg.angle_min}")
        #rospy.loginfo(f"laser angle_max: {msg.angle_max}")
        #rospy.loginfo(f"laser beam number: {len(msg.ranges)}")
        #rospy.loginfo(f"laser angle increment: {msg.angle_increment}")

        # Calculate indices 
        self.num_ray =len(msg.ranges)
        self.front_index = int((0 - msg.angle_min) / msg.angle_increment)
        self.right_index = int(self.front_index / 2)
        #self.left_index = self.front_index + int((self.num_ray - self.front_index) / 2)



    def laser_callback(self, msg):

        if self.front_index is None:
            self.calculate_laser_indices(msg)

        # Use multiple laser readings to make decisions more reliable
        self.laser_front_list = msg.ranges[(self.front_index-3):(self.front_index+3)]  # Average the readings in front
        self.laser_right_list = msg.ranges[(self.right_index-3):(self.right_index+3)]   # Average the readings on the right

        # Calculate the average distances
        self.laser_front = sum(self.laser_front_list) / len(self.laser_front_list)
        self.laser_right = sum(self.laser_right_list) / len(self.laser_right_list)

        # Log distances for debugging
        rospy.loginfo(f"Front: {self.front_distance:.2f}, Right: {self.right_distance:.2f}")

        # # Control logic based on laser readings: use the values of the laser to decide the com_vel
        
        # reset the location to desire path 
        if self.laser_front <= 0.3:
            rospy.loginfo("avoid to hit the wall -> back")
            self.move_cmd.linear.x = -0.3
            self.move_cmd.angular.z = 0
        # desire path
        elif self.laser_front <= 0.5 and self.laser_right > 0.2:
            rospy.loginfo("Closed to the next wall -> turn fast to the left (moving forward at the same time).")
            self.move_cmd.linear.x = 0.1
            self.move_cmd.angular.z = 1.2
        elif self.laser_right > 0.3:
            rospy.loginfo("Away from the wall -> turn right")
            self.move_cmd.linear.x = 0.1
            self.move_cmd.angular.z = -0.2
        elif self.laser_right < 0.2:
            rospy.loginfo("Too close to the wall -> turn left")
            self.move_cmd.linear.x = 0.1
            self.move_cmd.angular.z = 0.2
        else:
            rospy.loginfo("within desired distance -> move forward")
            self.move_cmd.linear.x = 0.1
            self.move_cmd.angular.z = 0



    def move(self):
        rate = rospy.Rate(15)
        while not rospy.is_shutdown():
            if self.wall_found:  # Only publish if the wall is found
                self.pub.publish(self.move_cmd)
            rate.sleep()

    def stop(self):
        rospy.loginfo("Stopping the robot...")
        self.move_cmd.linear.x = 0
        self.move_cmd.angular.z = 0
        self.pub.publish(self.move_cmd)  # Publish the stop command

        


if __name__ =='__main__':
    rospy.init_node("wall_follow_node", anonymous=True)
    try:
        wall_follow = WallFollow()
        wall_follow.move()
    except rospy.ROSInterruptException:
        wall_follow.stop()

Here is find_wall_service_client.py:

#!/usr/bin/env python

import rospy
from wall_following.srv import FindWall, FindWallRequest
import time

class FindWallServiceClient:
    def __init__(self):
        self.service_name = '/find_wall'

        # Wait for the service client to be running
        rospy.wait_for_service(self.service_name)

        # Create the connection object to the service
        self.service = rospy.ServiceProxy(self.service_name, FindWall)

        # Create a MsgRequest object
        self.request = FindWallRequest()

    def send_request(self):
        time.sleep(2)  # Add a delay to ensure the service server is ready

        # Send the MsgRequest object through the connection object to be executed by the robot
        result = self.service(self.request)

        # Print the result given by the service called
        rospy.loginfo(result)

        try:
            result = self.service(self.request)
            # Print the result given by the service called
            rospy.loginfo(f"service call result: wallfound {result.wallfound}")
            return result.wallfound
        except rospy.ServiceException as e:
            rospy.logerr(f"Service call failed: {e}")
            return False


if __name__ == '__main__':
    rospy.init_node('find_wall_service_client', anonymous=True)
    service_client = FindWallServiceClient()
    try:
        service_client.send_request()
    except rospy.ROSInterruptException:
        pass

Here is find_wall_service_server.py:

#!/usr/bin/env python

import rospy
from wall_following.srv import FindWall, FindWallResponse
from find_wall_movement import FindWallMove 


class FindWallServiceServer:
    def __init__(self):
        self.service_name = "/find_wall"
        
        self.find_wall = FindWallMove()
        self.service = rospy.Service(self.service_name, FindWall, self.callback)
        
        rospy.on_shutdown(self.find_wall.stop_robot)
        
        rospy.loginfo(f"Service {self.service_name} is ready")

    def callback(self, request):
        rospy.loginfo(f"Service {self.service_name} has been called")
        
        wall_found = self.find_wall.find_wall()

        rospy.loginfo(f"Finished ervice {self.service_name}.")
        
        return FindWallResponse(wallfound=wall_found)

    def spin(self):
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('find_wall_service_server')
    service_server = FindWallServiceServer()
    try:
        service_server.spin()
    except rospy.ROSInterruptException:
        pass  # Normal shutdown, no additional action needed because on_shutdown will handle stop_robot
    except Exception as e:
        rospy.logerr(f"An unexpected error occurred: {e}")
        service_server.find_wall.stop_robot()

Here is find_wall_movement.py:

#!/usr/bin/env python

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

class FindWallMove:
    def __init__(self):
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.sub = rospy.Subscriber('/scan', LaserScan, self.laser_callback)

        self.rate = rospy.Rate(10)
        self.move_cmd = Twist()  

        self.laser_data = []
        self.num_ray = None
        self.front_index = None
        self.angle_increment = None


    def laser_callback(self, msg):
        self.laser_data = msg.ranges
        if self.front_index is None:
            self.calculate_laser_indices(msg)

    def calculate_laser_indices(self, msg):
         # Determine the indices for the key directions
        self.angle_increment = msg.angle_increment

        # Calculate indices 
        self.num_ray =len(msg.ranges)
        self.front_index = int((0 - msg.angle_min) / msg.angle_increment)
        #self.right_index = int(self.front_index / 2)
        #self.left_index = self.front_index + int((self.num_ray - self.front_index) / 2)

    def find_wall(self):
        rospy.loginfo("Finding the nearest wall...")

        # Step 1: Identify the shortest laser ray . Assume the closest obstacle is the wall (only in the simulation/real enviroment in this course).
        while not self.laser_data:
            self.rate.sleep()

        min_distance = min(self.laser_data)
        min_index = self.laser_data.index(min_distance)

        # Step 2: Rotate the robot until the front of it is facing the wall (front laser ray is the smallest one). 
        angle_to_rotate = (self.front_index - min_index) * self.angle_increment
        
        rospy.loginfo(f"Rotating by {angle_to_rotate} radians to face the nearest wall")
        self.rotate(angle_to_rotate)

        # Step 3: Move forward until the front ray is smaller than 30 cm
        while self.laser_data[self.front_index] > 0.3:
            self.move_cmd.angular.z = 0
            self.move_cmd.linear.x = 0.2
            self.pub.publish(self.move_cmd)
            self.rate.sleep()

        # Step 4: Rotate left by 90 degrees to align with the wall
        rospy.loginfo("Rotating left by 90 degrees to align with the wall")
        self.rotate(1.5708)  # Rotate 90 degrees left (π/2 radians)


        # Stop the robot after positioning
        self.stop_robot()
        return True

    def rotate(self, angle):
        # Rotate by a specific angle
        angular_speed = 0.2  # radians per second
        duration = abs(angle) / angular_speed
        self.move_cmd.linear.x = 0
        self.move_cmd.angular.z = (angular_speed if angle > 0 else -angular_speed)

        end_time = rospy.Time.now() + rospy.Duration(duration)
        while rospy.Time.now() < end_time and not rospy.is_shutdown():
            self.pub.publish(self.move_cmd)
            self.rate.sleep()

        # Stop rotation
        self.move_cmd.angular.z = 0
        self.pub.publish(self.move_cmd)

    def stop_robot(self):
        rospy.loginfo("Stopping the robot")
        self.move_cmd.linear.x = 0
        self.move_cmd.angular.z = 0
        self.pub.publish(self.move_cmd)

Hi @min.wang.1997.01.13, from what I can see, your code is well structured. I recommend actually printing out the wall_found flag for when you receive the server response.

1 Like

Thanks :blush:!

I think it is because of the laser_callback method. As I defined subscriber in init(), as long as the robot get laser reading, it will call self.laser_callback however, we want to call the service and find the wall at first(the movement logic is defined in find_wall_movement.py) before doing anything in the wall_follower.py.

I move the client call service to the main function to solve this issue.

#!/usr/bin/env python

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

from find_wall_service_client import FindWallServiceClient  # Import the service client

class WallFollow():
    def __init__(self):
    
        self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
    
        self.move_cmd = Twist()

        self.num_ray = None
        self.front_index = None
        self.right_index = None
        self.angle_increment = None
        self.laser_front_list = []
        self.laser_right_list = []
        self.front_distance = 0
        self.right_distance = 0

        self.find_wall_client = FindWallServiceClient()  # Initialize the service client
        rospy.loginfo("Initialized the service client...")
    
        # Register the shutdown hook
        rospy.on_shutdown(self.stop)

    
    def start(self):
        rospy.loginfo("Wall following behavior started.")
        self.sub = rospy.Subscriber("/scan", LaserScan, self.laser_callback)

    def calculate_laser_indices(self, msg):
        # get info about laser data
        #rospy.loginfo(f"laser angle_min: {msg.angle_min}")
        #rospy.loginfo(f"laser angle_max: {msg.angle_max}")
        #rospy.loginfo(f"laser beam number: {len(msg.ranges)}")
        #rospy.loginfo(f"laser angle increment: {msg.angle_increment}")

        # Calculate indices 
        self.num_ray =len(msg.ranges)
        self.front_index = int((0 - msg.angle_min) / msg.angle_increment)
        self.right_index = int(self.front_index / 2)
        #self.left_index = self.front_index + int((self.num_ray - self.front_index) / 2)



    def laser_callback(self, msg):

        if self.front_index is None:
            self.calculate_laser_indices(msg)

        # Use multiple laser readings to make decisions more reliable
        self.laser_front_list = msg.ranges[(self.front_index-3):(self.front_index+3)]  # Average the readings in front
        self.laser_right_list = msg.ranges[(self.right_index-3):(self.right_index+3)]   # Average the readings on the right

        # Calculate the average distances
        self.laser_front = sum(self.laser_front_list) / len(self.laser_front_list)
        self.laser_right = sum(self.laser_right_list) / len(self.laser_right_list)

        # Log distances for debugging
        rospy.loginfo(f"Front: {self.front_distance:.2f}, Right: {self.right_distance:.2f}")

        # # Control logic based on laser readings: use the values of the laser to decide the com_vel
        
        # reset the location to desire path 
        if self.laser_front <= 0.3:
            rospy.loginfo("avoid to hit the wall -> back")
            self.move_cmd.linear.x = -0.3
            self.move_cmd.angular.z = 0
        # desire path
        elif self.laser_front <= 0.5 and self.laser_right > 0.2:
            rospy.loginfo("Closed to the next wall -> turn fast to the left (moving forward at the same time).")
            self.move_cmd.linear.x = 0.1
            self.move_cmd.angular.z = 1.2
        elif self.laser_right > 0.3:
            rospy.loginfo("Away from the wall -> turn right")
            self.move_cmd.linear.x = 0.1
            self.move_cmd.angular.z = -0.2
        elif self.laser_right < 0.2:
            rospy.loginfo("Too close to the wall -> turn left")
            self.move_cmd.linear.x = 0.1
            self.move_cmd.angular.z = 0.2
        else:
            rospy.loginfo("within desired distance -> move forward")
            self.move_cmd.linear.x = 0.1
            self.move_cmd.angular.z = 0



    def move(self):
        rate = rospy.Rate(15)
        while not rospy.is_shutdown():
            self.pub.publish(self.move_cmd)
            rate.sleep()

    def stop(self):
        rospy.loginfo("Stopping the robot...")
        self.move_cmd.linear.x = 0
        self.move_cmd.angular.z = 0
        self.pub.publish(self.move_cmd)  # Publish the stop command

        


if __name__ =='__main__':
    rospy.init_node("wall_follow_node", anonymous=True)
    wall_follow = WallFollow()
    try:
        # Call the service before starting wall-following behavior
        find_wall_client = FindWallServiceClient()
        wall_found = find_wall_client.send_request()
        rospy.loginfo(f"Wall found: {wall_found} ...")

        if wall_found:
            rospy.loginfo("Wall found, starting wall-following behavior")
            wall_follow.start()
            wall_follow.move() 
        else:
            rospy.logerr("Failed to find the wall. Shutting down.")
            rospy.signal_shutdown("Failed to find wall")

    except rospy.ROSInterruptException:
        wall_follow.stop()
    except Exception as e:
        rospy.logerr(f"An unexpected error occurred: {e}")
        wall_follow.stop()

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.