(ROS2 in 5 days) Service won't end after using Client to call Server

ROSDS Support
1

I have been completing the ROS2 in 5 days (Python) course. I am working on Part II of the Rosject, which involves making a Service Server and Client for the Find Wall behaviour.

I have implemented client functionality into the node and it successfully calls the Find Wall Server at startup. However, the Server never ends. I’m not sure if client.future.done() is needed here, but after completing it’s behaviour, the program never goes back to the wall-following behaviour. If i remove the Service related code, then it executes the wall-following behaviour correctly. Here are the two scripts

wall_following.py:

import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
# import the LaserScan module from sensor_msgs interface
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from std_srvs.srv import Empty
# import Quality of Service library, to set the correct profile and reliability to read sensor data.
from rclpy.qos import ReliabilityPolicy, QoSProfile


class WallFollower(Node):
    def __init__(self):

        super().__init__('wall_follower')

        self.dist_right = 0.0
        self.dist_fwd = 0.0

        self.laser_subscriber = self.create_subscription( 
            LaserScan, '/scan', 
            self.wall_dist, 
            QoSProfile(depth=10, 
            reliability=ReliabilityPolicy.RELIABLE))

        self.move_pub_ = self.create_publisher(
            Twist, 'cmd_vel', 10)

        # Find Wall Behaviour
        self.client = self.create_client(Empty, 'find_wall')
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = Empty.Request()

        timer_period = 0.5

        self.timer = self.create_timer(timer_period, self.move)


    def send_request(self):
        self.client.future = self.client.call(self.req)
        
        
        
    def wall_dist(self, msg):
        # print the log info in the terminal
        self.dist_right = msg.ranges[520]
        self.dist_left = msg.ranges[180]
        self.dist_fwd = msg.ranges[0]
        self.all_dist = msg
        # self.get_logger().info('Dist fwd: "%s"' % str(self.dist_fwd))
        # self.get_logger().info('Dist to wall: "%s"' % str(self.dist_right))
        # self.get_logger().info('Dist to left: "%s"' % str(self.dist_left))


    def move(self):

        cmd = Twist()
        self.get_logger().info('Dist fwd YA MADFE IT HERE: "%s"' % str(self.dist_fwd))

        if self.dist_fwd < 0.5:
            cmd.angular.z = 0.35
            cmd.linear.x = 0.01
        elif self.dist_right > 0.3:
            cmd.angular.z = -0.2
            cmd.linear.x = 0.03
        elif self.dist_right < 0.3 and self.dist_right > 0.2:
            cmd.angular.z = 0.0
            cmd.linear.x = 0.05
        elif self.dist_right < 0.2:
            cmd.angular.z = 0.2
            cmd.linear.x = 0.03


        self.move_pub_.publish(cmd)


def main(args=None):
    # initialize the ROS communication
    rclpy.init(args=args)
    # declare the node constructor
    wall_follower = WallFollower()
    # pause the program execution, waits for a request to kill the node (ctrl+c)
    wall_follower.send_request()
    rclpy.spin(wall_follower)
    # Explicity destroy the node
    wall_follower.destroy_node()
    # shutdown the ROS communication
    rclpy.shutdown()


if __name__ == '__main__':
    main()

And wall_finder.py:

import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
# import the LaserScan module from sensor_msgs interface
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from std_srvs.srv import Empty
# import Quality of Service library, to set the correct profile and reliability to read sensor data.
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor



RIGHT = 520
LEFT = 180
FWD = 0


class WallFinder(Node):
    def __init__(self):

        super().__init__('wall_finder')

        self.reentrant_group_1 = ReentrantCallbackGroup()

        self.srv = self.create_service(Empty, 'find_wall', self.find_wall_callback, callback_group=self.reentrant_group_1)

        self.dist_right = 0.0
        self.dist_fwd = 0.0

        self.laser_subscriber = self.create_subscription(
            LaserScan, '/scan', 
            self.wall_dist, 
            QoSProfile(depth=10, 
            reliability=ReliabilityPolicy.RELIABLE), callback_group=self.reentrant_group_1)

        self.move_pub_ = self.create_publisher(
            Twist, 'cmd_vel', 10)

        self.move_order = "TURN 1"

        
        
        
    def wall_dist(self, msg):
        # print the log info in the terminal
        self.dist_left = msg.ranges[LEFT]
        self.dist_right = msg.ranges[RIGHT]

        self.dist_fwd = msg.ranges[FWD]
        self.all_dist = msg
        self.get_logger().info('Dist fwd: "%s"' % str(self.dist_fwd))
        self.get_logger().info('Dist to wall: "%s"' % str(self.dist_right))
        # self.get_logger().info('Dist to left: "%s"' % str(self.dist_left))


    def find_wall_callback(self, request, response):

        cmd = Twist()


        # response.success = True 
        
        while self.move_order != "FINISHED":
            self.get_logger().info('MOVEMENT: "%s"' % str(self.move_order))

            if self.move_order == "TURN 1":

                index = self.all_dist.ranges.index(min(self.all_dist.ranges))
                if index < 360:
                    cmd.angular.z = 0.2
                elif index >= 360:
                    # turn right
                    cmd.angular.z = -0.2
                
                if index == FWD:
                    cmd.angular.z = 0.0
                    self.move_order = "MOVE 1"

            elif self.move_order == "MOVE 1":
                if min(self.all_dist.ranges) > 0.305:
                    cmd.linear.x = 0.05
                elif min(self.all_dist.ranges) <= 0.295:
                    cmd.linear.x = -0.05
                else:
                    cmd.linear.x = 0.0
                    self.move_order = "TURN 2"


            elif self.move_order == "TURN 2":

                index = self.all_dist.ranges.index(min(self.all_dist.ranges))
                if index >= LEFT and index <= (LEFT+360):
                    cmd.angular.z = -0.2
                else:
                    cmd.angular.z = 0.2

                if index == RIGHT:
                    cmd.angular.z = 0.0
                    self.move_order = "FINISHED"
                    # response.success = False
                    # response.message = "Finished"

            self.move_pub_.publish(cmd)

        return response


def main(args=None):
    # initialize the ROS communication
    rclpy.init(args=args)
    # declare the node constructor
    wall_finder = WallFinder()
    # pause the program execution, waits for a request to kill the node (ctrl+c)

    executor = MultiThreadedExecutor(num_threads=2)
    executor.add_node(wall_finder)

    try:
        executor.spin()
    finally:
    # shutdown the ROS communication
        wall_finder.destroy_node()
        executor.shutdown()


if __name__ == '__main__':
    main()

Thank you for your time, let me know if you have any questions or need any more information!

Cheers

2

Without looking at your code, I can see what seems to be the problem here, based on my understanding of a service:

  • A service server never ends indeed, it should keep running in the background once started, so a client can call it at any time. Therefore, service server code should never block any other code; it should run in the background.
  • A service client, however, needs to end at some point - after the server has responded to the request. So you need a way to detect that in your code. A service client may block the code, depending on the application, but it should end at some point.

Your call to the service server (the service client call) seems to be blocking the rest of the wall follower code, because it’s not returning properly from that call.

4

The comment by bayodesegun is correct. The final line of the comment says

Your call to the service server (the service client call) seems to be blocking the rest of the wall follower code, because it’s not returning properly from that call.

This is precisely my question. I am unable to find out which part of the code is stopping the service client call from returning properly. Maybe I didn’t phrase my issue very well, so thank you for clarifying.

This can all be found in the other link, but I’ll reiterate the relevant code below. There is a slight difference as I have changed the message Type from Empty to FindWall but that is all:

wall_following.py

import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
# import the LaserScan module from sensor_msgs interface
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from std_srvs.srv import Empty
from custom_interfaces.srv import FindWall
# import Quality of Service library, to set the correct profile and reliability to read sensor data.
from rclpy.qos import ReliabilityPolicy, QoSProfile


class WallFollower(Node):
    def __init__(self):

        super().__init__('wall_follower')

        self.dist_right = 0.0
        self.dist_fwd = 0.0

        self.laser_subscriber = self.create_subscription( 
            LaserScan, '/scan', 
            self.wall_dist, 
            QoSProfile(depth=10, 
            reliability=ReliabilityPolicy.RELIABLE))

        self.move_pub_ = self.create_publisher(
            Twist, 'cmd_vel', 10)

        # Find Wall Behaviour
        self.client = self.create_client(FindWall, 'find_wall')
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = FindWall.Request()

        timer_period = 0.5

        self.timer = self.create_timer(timer_period, self.move)


    def send_request(self):
        self.future = self.client.call(self.req)
        
        
        
    def wall_dist(self, msg):
        # print the log info in the terminal
        self.dist_right = msg.ranges[520]
        self.dist_left = msg.ranges[180]
        self.dist_fwd = msg.ranges[0]
        self.all_dist = msg
        # self.get_logger().info('Dist fwd: "%s"' % str(self.dist_fwd))
        # self.get_logger().info('Dist to wall: "%s"' % str(self.dist_right))
        # self.get_logger().info('Dist to left: "%s"' % str(self.dist_left))


    def move(self):

        cmd = Twist()
        self.get_logger().info('Dist fwd YA MADFE IT HERE: "%s"' % str(self.dist_fwd))

        if self.dist_fwd < 0.5:
            cmd.angular.z = 0.35
            cmd.linear.x = 0.01
        elif self.dist_right > 0.3:
            cmd.angular.z = -0.2
            cmd.linear.x = 0.03
        elif self.dist_right < 0.3 and self.dist_right > 0.2:
            cmd.angular.z = 0.0
            cmd.linear.x = 0.05
        elif self.dist_right < 0.2:
            cmd.angular.z = 0.2
            cmd.linear.x = 0.03


        self.move_pub_.publish(cmd)


def main(args=None):
    # initialize the ROS communication
    rclpy.init(args=args)
    # declare the node constructor
    wall_follower = WallFollower()
    # pause the program execution, waits for a request to kill the node (ctrl+c)

    wall_follower.send_request()
    while rclpy.ok():
        if wall_follower.future.done():
            try:
                response = wall_follower.future.result()
            
            except Exception as e:
                # Display the message on the console
                wall_follower.get_logger().info(
                    'Service call failed %r' % (e,))
            break
    rclpy.spin(wall_follower)

    # Explicity destroy the node
    wall_follower.destroy_node()
    # shutdown the ROS communication
    rclpy.shutdown()


if __name__ == '__main__':
    main()

wall_finder.py

import rclpy
# import the ROS2 python libraries
from rclpy.node import Node
# import the LaserScan module from sensor_msgs interface
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from std_srvs.srv import Empty
from custom_interfaces.srv import FindWall
# import Quality of Service library, to set the correct profile and reliability to read sensor data.
from rclpy.qos import ReliabilityPolicy, QoSProfile
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor



RIGHT = 520
LEFT = 180
FWD = 0


class WallFinder(Node):
    def __init__(self):

        super().__init__('wall_finder')

        self.mutual_1 = MutuallyExclusiveCallbackGroup()
        self.mutual_2 = MutuallyExclusiveCallbackGroup()

        self.dist_right = 0.0
        self.dist_fwd = 0.0

        self.laser_subscriber = self.create_subscription(
            LaserScan, '/scan', 
            self.wall_dist, 
            QoSProfile(depth=10, 
            reliability=ReliabilityPolicy.RELIABLE), callback_group=self.mutual_2)

        self.move_order = "TURN 1"

        self.srv = self.create_service(FindWall, 'find_wall', self.find_wall_callback, callback_group=self.mutual_1)


        self.move_pub_ = self.create_publisher(
            Twist, 'cmd_vel', 10)
        
        
        
    def wall_dist(self, msg):
        # print the log info in the terminal
        self.dist_left = msg.ranges[LEFT]
        self.dist_right = msg.ranges[RIGHT]

        self.dist_fwd = msg.ranges[FWD]
        self.all_dist = msg
        self.get_logger().info('Dist fwd: "%s"' % str(self.dist_fwd))
        self.get_logger().info('Dist to wall: "%s"' % str(self.dist_right))
        # self.get_logger().info('Dist to left: "%s"' % str(self.dist_left))


    def find_wall_callback(self, request, response):

        cmd = Twist()


        response.wallfound = False
        
        while self.move_order != "FINISHED":
            self.get_logger().info('MOVEMENT: "%s"' % str(self.move_order))

            if self.move_order == "TURN 1":

                index = self.all_dist.ranges.index(min(self.all_dist.ranges))
                if index < 360:
                    cmd.angular.z = 0.2
                elif index >= 360:
                    # turn right
                    cmd.angular.z = -0.2
                
                if index == FWD:
                    cmd.angular.z = 0.0
                    self.move_order = "MOVE 1"

            elif self.move_order == "MOVE 1":
                if min(self.all_dist.ranges) > 0.305:
                    cmd.linear.x = 0.05
                elif min(self.all_dist.ranges) <= 0.295:
                    cmd.linear.x = -0.05
                else:
                    cmd.linear.x = 0.0
                    self.move_order = "TURN 2"


            elif self.move_order == "TURN 2":

                index = self.all_dist.ranges.index(min(self.all_dist.ranges))
                if index >= LEFT and index <= (LEFT+360):
                    cmd.angular.z = -0.2
                else:
                    cmd.angular.z = 0.2

                if index == RIGHT:
                    cmd.angular.z = 0.0
                    self.move_order = "FINISHED"
                    response.wallfound = True
                    # response.message = "Finished"

            self.move_pub_.publish(cmd)

        return response


def main(args=None):
    # initialize the ROS communication
    rclpy.init(args=args)
    # declare the node constructor
    wall_finder = WallFinder()
    # pause the program execution, waits for a request to kill the node (ctrl+c)

    executor = MultiThreadedExecutor(num_threads=2)
    executor.add_node(wall_finder)

    try:
        executor.spin()
        wall_finder.get_logger().info('Result of add_two_ints: test5')
    finally:
    # shutdown the ROS communication
        wall_finder.destroy_node()
        executor.shutdown()


if __name__ == '__main__':
    main()

Somebody please help, I’ve been stuck on this for a while now and I’d appreciate it greatly. I’ve also tried using Mutually Exclusive callback groups.

5

I’m not the type that quickly checks code, because that can be a distraction to learning. It seems that your service call is never “done”, so your troubleshooting steps should be as follows:

  1. Can you successfully call the wall finder service from the terminal? It’s good practice to test the service from the terminal before writing code to call it.
  2. If you can, then it means you need to check how you are calling it in the code. It must be called correctly, and that part was covered in the notes.
  3. If you can’t, then you need to fix the service server, depending on the problems you get while trying to call it from the terminal.
6

Hello! Thank you for the response.

  1. Yes, I can successfully call it from the terminal.
  2. I believe I have implemented the code for calling the service server from a service client in the same manner as depicted in the notes. If I have done something wrong, I would need someone else to tell me, as it appears the same to me. The error may be here. It definitely exits the callback function of the server, I’ve checked. But the client does not seem to be receiving the response that the callback function has ended? I’m not sure why.
7

This seems to be the culprit. The right call should be self.client.call_async(self.req), as indicated in the notes.

You are trying to call the service asynchronously using the “sync” method.

I recommend that you carefully review the Understanding ROS2 Services unit to reinforce the sync/async nuances for services.

1 Like
9

Thank you so much! This has resolved the issue.

The section of the unit that I’m doing right now asks us to call the service synchronously so I edited the function to achieve this, but it seems I did that incorrectly. The notes on synchronous callbacks is somewhat limited (intentionally, I believe, as they heavily recommend always using asynchronous) but after reviewing it, I notice that it does a very good job and provides links to other useful resources. With your comment it works asynchronously, which makes me wonder why the notes ask us to use a synchronous call.

Anyway, thank you so much! I hope you have a great week :slight_smile:

Section in the notes where it asks us to write the synchronous service client:

ros2_screenshot
ros2_screenshot892×114 9.48 KB

10

Good job getting it working!

Perhaps related to this, but let me ask them.

11

Dear @bayodesegun,

I go even further:
I think, that the wall_finder “service” should not be a service, rather an action.
This was the point when I gave up this task and created my own project, because I think,
the material should teach good practices also, not only “raw” material.
And for creating a long running task an action is better than a service.
And usually going to the wall takes time.

Péter

12

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