I don't get the response from my Service Server, the send_request function does not finish

[w.ronaldo.cd]

Hi,
I am working on the Services Part of the Wall Follower project. Myproblem is that the send_request() function never finishes. The service client calls the server perfectly, and the robot moves as expected. But, as the send_request() function does not finish, the wall_follower_node does not start.
First, I was trying to launch the Syncronous client from the main function, as the tutorial shows. I got the problem mentioned. Then, I read the following topic:

Problem while launching service server and service client at the same time

Hello, I am working on the Rosject of the ROS2 In 5 Days Python course. I created and tested both the wall_following node and the wall_finding service separately using the terminal to launch the controller and to call the service server and both scripts are working as expected. However, my current task is to launch both the the service server node and the robot controller (the ‘wall_following’) node at the same time. In addition I have to modify the ‘wall_following’ node in order to create a sy…

I tried to implement the service client as in the topic, but I got the same result. I will appreciate your help.

My wall_follower_node code is the following:

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from rclpy.qos import ReliabilityPolicy, QoSProfile
from custom_interfaces.srv import FindWall
from threading import Thread
import numpy as np
import time

class WallFollower(Node):

    def __init__(self):
       
        super().__init__('wall_follower')
        
        # Wall finder client 
        self.wall_finder_client = self.create_client(FindWall, 'find_wall')
        # checks once per second if a Service matching the type and name of the Client is available.
        while not self.wall_finder_client.wait_for_service(timeout_sec=1.0):
            # if it is not available, a message is displayed
            self.get_logger().info('service not available, waiting again...')
        
        self.req = FindWall.Request()
        self.response = FindWall.Response()
        self.request_sent = False

        # Wall follower functionality
        self.cmd_vel_publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        self.laser_subscriber_ = self.create_subscription(
            LaserScan,
            '/scan',
            self.laser_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE))  # is the most used to read LaserScan data and some sensor data.
        
        self.odom_subscriber_ = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE))  # is the most used to read LaserScan data and some sensor data.
        
        self.timer_period = 0.5
        self.publisher_timer = self.create_timer(self.timer_period, self.motion)

        self.distance_to_wall = 0.0
        self.rotation_z = 0.0
        
        self.distance_to_front_wall = 0.0

        self.linear_vel = 0.15
        self.angular_vel = 0.1
        self.shift_angle = 0.0
        self.angle_tol = 0.05 * np.pi / 180

        self.print_timer_period = 2.5
        self.print_timer = self.create_timer(self.print_timer_period, self.print_function)
        self.dist_tol = 0.005
    
    def send_request(self):
        # send the request
        self.get_logger().info('Service has started')
        self.response = self.wall_finder_client.call(self.req)
        # self.get_logger().info('SERVICE HAS FINISHED')
     
    def print_function(self):
        self.get_logger().info('Distance to wall: "%s"' % str(self.distance_to_wall))
        self.get_logger().info('Distance to front wall: "%s"' % str(self.distance_to_front_wall))
        self.get_logger().info('Angle direccion: "%s"' % str(self.rotation_z * 180/ np.pi))
        self.get_logger().info('Shift angle: "%s"' % str(self.shift_angle * 180/ np.pi))

    def motion_to_center(self):
        angular_z = 0.0
        delta_dist_to_wall = self.distance_to_wall - 0.25

        if delta_dist_to_wall < 0:
            turn_sign = +1
        else:
            turn_sign = -1
        
        if abs(delta_dist_to_wall) >= self.dist_tol:
            dist_to_go = abs(delta_dist_to_wall) / np.sin(abs(self.shift_angle))
            if dist_to_go / (self.timer_period * self.linear_vel) >= 1:
                linear_x = self.linear_vel
                angular_z = turn_sign * self.angular_vel
            else:
                linear_x = (dist_to_go / (self.timer_period * self.linear_vel))* self.linear_vel
                angular_z = (-1) * self.shift_angle / self.timer_period
        else:
            linear_x = self.linear_vel
            if self.shift_angle >= self.angle_tol:
                angular_z = (-1) * self.shift_angle / self.timer_period

        return (linear_x, angular_z)
            

    def motion(self):
        # print the data

        if self.request_sent is False:
            # making sure that the request will not be called again in the callback function
            self.request_sent = True
            self.send_request()
            self.get_logger().info('SERVICE HAS FINISHED')

        
        if not self.response.wallfound:
            self.get_logger().info('Service response not received yet')
        else:
            msg = Twist()
            if self.distance_to_wall > 0.3:
                msg.linear.x = self.linear_vel
                msg.angular.z = - self.angular_vel
                msg_to_print = "The robot is approaching the wall"
            elif self.distance_to_wall < 0.2:
                msg.linear.x = self.linear_vel
                msg.angular.z = self.angular_vel
                msg_to_print = "The robot is moving away to the wall"
            else:
                linear_x, angular_z = self.motion_to_center()
                msg.linear.x = linear_x
                msg.angular.z = angular_z 
                msg_to_print = "The robot is advancing forward"
                
            self.get_logger().info(msg_to_print)

            if self.distance_to_front_wall < 0.5:
                msg.linear.x = self.linear_vel
                msg.angular.z = self.angular_vel * 1.5
                self.get_logger().info("The robot is turning left to avoid front wall")

            self.cmd_vel_publisher_.publish(msg)
            

            
        
        # Logic of move
        
    def laser_callback(self, msg):
        # print the log info in the terminal
        shift_angle = self.rotation_z % (np.pi/2)
        # self.get_logger().info('Shift angle 1: "%s"' % str(shift_angle* 180/ np.pi))
        if shift_angle >= np.pi/4 and shift_angle < np.pi/2:
            shift_angle -= (np.pi/2)
        
        self.shift_angle = shift_angle
        
        # self.get_logger().info('Shift angle 2: "%s"' % str(shift_angle* 180/ np.pi))
        
        range_angle = (3 * np.pi / 2) - shift_angle - np.pi
        # self.get_logger().info('Range angle: "%s"' % str(range_angle* 180/ np.pi))
        ranges_length = round((2 * np.pi / msg.angle_increment) + 1)
        # self.get_logger().info('Ranges length: "%s"' % str(ranges_length))
        range_idx = int(range_angle * ranges_length / (2 * np.pi))
        # self.get_logger().info('Ranges index: "%s"' % str(range_idx))
        self.distance_to_wall = msg.ranges[range_idx]
        # self.get_logger().info('Distance to wall: "%s"' % str(self.distance_to_wall))

        self.distance_to_front_wall = msg.ranges[360]

        
    def odom_callback(self, msg):
        # print the log info in the terminal
        x = msg.pose.pose.orientation.x
        y = msg.pose.pose.orientation.x
        z = msg.pose.pose.orientation.z
        w = msg.pose.pose.orientation.w

        _, _, yaw = self.euler_from_quaternion([x, y, z, w])
        # self.get_logger().info('Yaw angle: "%s"' % str(yaw * 180/ np.pi))

        rotation_z = yaw % np.pi
        if yaw < 0:
            rotation_z += np.pi
        
        self.rotation_z = rotation_z
        # self.get_logger().info('Angle direccion: "%s"' % str(rotation_z * 180/ np.pi))
    
    def euler_from_quaternion(self, quaternion):
        """
        Converts quaternion (w in last place) to euler roll, pitch, yaw
        quaternion = [x, y, z, w]
        Below should be replaced when porting for ROS2 Python tf_conversions is done.
        """
        x = quaternion[0]
        y = quaternion[1]
        z = quaternion[2]
        w = quaternion[3]

        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = np.arctan2(sinr_cosp, cosr_cosp)

        sinp = 2 * (w * y - z * x)
        pitch = np.arcsin(sinp)

        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = np.arctan2(siny_cosp, cosy_cosp)

        return roll, pitch, yaw


def main(args=None):
    # rclpy.init(args=args)
    # # declare the node constructor
    # client_node = WallFollower()
    # # run the send_request() method
    # client_node.send_request()

    # spin_thread = Thread(target=rclpy.spin, args=(client_node,))
    # spin_thread.start()

    # response = client_node.send_request()
    # client_node.get_logger().info( 'Wall finder service has finished')
    # client_node.destroy_node()
    # rclpy.shutdown()
    
    rclpy.init(args=args)
    wall_follower_node = WallFollower()
    rclpy.spin(wall_follower_node)
    wall_follower_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Kind regards,
Ronaldo

[w.ronaldo.cd]

Also, when I call the service Server from the command line. The program works as expect, I got the response from the Service Server.

[girishkumar.kannan]

Hi @w.ronaldo.cd ,

Since you have said that the find wall service works as expected, I am going directly to wall follower program. [I am assuming and hoping that your find wall program works as expected.]

In your WallFollower program motion() callback:
You need to make sure that your wall follower logic does not start until service completes. So we know that the service is synchronous, but the callback of wall follower will not wait until service completes. Therefore, when the service actually completes, the service response is not received since the wall follower callback has already started executing. Thus the response from service is ignored.
To solve this you must make your motion() callback wait for service to complete.

Here is a pseudocode (do not copy-paste, it will not work):

motion(self):

if not find_wall_started:
   # immediately set flag to true to exit callback condition
   find_wall_started = True
   send_find_wall_request()

elif not find_wall_complete:
    if not find_wall_response.wallfound:
        time.sleep(timer_period) # timer_period in your case is 0.5
    else:
        # immediately set flag to true to exit callback condition
        find_wall_complete = True

elif wall_follower_started:
    wall_follower_started = True
    start wall_follower_logic()
    # continue code block with required statements

.
.
.

else:
    # statements that need to be executed in else block 
    pass

Implement this logic and your program will (should) work.

Regards,
Girish

[w.ronaldo.cd]

I implemented what you recommended, but I still have the same problem. In the send_request function, you can see that there is a print line after the call function, my code does not reach this line. In the motion function I also put a print line after calling send_request function, likewise this line never gets printed.

When I call the service server from command line with: ros2 service call /find_wall std_srvs/Empty '{}’ it works perfectly. The service call finishes and I get the response from the Service Server.

def send_request(self):
        # send the request
        self.get_logger().info('Send_request has started')
        self.find_wall_response = self.wall_finder_client.call(self.find_wall_req)
        self.get_logger().info('Send_request has finished')

def motion(self):

        if not self.find_wall_started:
            self.find_wall_started = True
            self.send_request()
            self.get_logger().info('SERVICE HAS STARTED')

        elif not self.find_wall_completed:
            if not self.find_wall_response.wallfound:
                time.sleep(self.timer_period)
                self.get_logger().info('SERVICE KEEPS RUNING')
            else:
                self.get_logger().info('SERVICE HAS FINISHED')
                self.find_wall_completed = True
                self.wall_follower_can_start = True
        
        elif self.wall_follower_can_start:
            self.get_logger().info('WALL FOLLOWER HAS STARTED')
            msg = Twist()
            if self.distance_to_wall > 0.3:
                msg.linear.x = self.linear_vel
                msg.angular.z = - self.angular_vel
                msg_to_print = "The robot is approaching the wall"
            elif self.distance_to_wall < 0.2:
                msg.linear.x = self.linear_vel

[girishkumar.kannan]

Hi @w.ronaldo.cd ,

I see what the problem is. You should have your motion() callback running along with service client.
Here you need to make use of callback groups.

1. Define your wall_finder_client with a ReentrantCallbackGroup().
2. Define each subscriber with ReentrantCallbackGroup() - laser and odom.
3. Define your timer with ReentrantCallbackGroup().
4. Finally, define a MultiThreadedExecutor with num_threads=5 (num_threads >= number of reentrant callbacks + 1 and num_threads <= number of cpu cores)

Make these changes and your program should work.

@w.ronaldo.cd

When I call the service server from command line with: ros2 service call /find_wall std_srvs/Empty '{}’ it works perfectly.

Yes, this is supposed to work. The point to note here is, this command is used to check if the server is functioning well, and not to see if the client is working well. This command has a builtin service client that works with all services. It is most often the program, where you have the service client, that does not work well.

Let me know if you still have problems after this.

Regards,
Girish