ROS2 rclpy - Call a Service from inside a Service

asked 2020-01-22 08:13:14 -0600

mikelynders gravatar image

I have an application where I would like to call a service (provided by different node) from inside a service provided by my node. However my service always hangs when waiting for the response even after the called service has completed.

Closest thing I could find is this: https://answers.ros.org/question/4676...

But rclpy does not have AsyncSpinner. It does have Multithreaded executor which I tried but it didn’t seem to change anything.

Here is a minimum example demonstrating the problem:

A_node

class A_node(Node):

        def __init__(self):
            super().__init__('A_node')
            self.cli = self.create_client(SetBool, 'set_C_from_B')
            print('started A_node')

        def set_b(self):
            req = SetBool.Request()
            req.data = True
            future = self.cli.call_async(req)
            rclpy.spin_until_future_complete(self, future)
            result = future.result()
            print(result)

    def main(args=None):
        rclpy.init(args=args)
        node = A_node()
        node.set_b()
        rclpy.shutdown()

    if __name__ == '__main__':
        main()

B_node

class B_node(Node):
    def __init__(self):
        super().__init__('B_node')
        self.cli = self.create_client(SetBool, 'set_C')
        self.set_C_srv = self.create_service(SetBool, 'set_C_from_B', self.set_C_from_B)
        print('started B_node')

    def set_C_from_B(self, request, response):
        print('recieved request')
        req = SetBool.Request()
        req.data = request.data
        print('calling C')
        future = self.cli.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        result = future.result()
        print('finished C')
        response.success = result.success
        print('Done')
        return response

def main(args=None):
    rclpy.init(args=args)
    node = B_node()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

C_node

class C_node(Node):
    def __init__(self):
        super().__init__('C_node')
        self.set_C_srv = self.create_service(SetBool, 'set_C', self.set_C)
        self.C = False
        print('started C_node')

    def set_C(self, request, response):
        print('recieved request')
        self.C = request.data
        print(f'self.C = {self.C}')
        response.success = True
        print('Done')
        return response

def main(args=None):
    rclpy.init(args=args)
    node = C_node()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

I would like A_node to call a service provided in B_node which in turn calls a service in C_node

Procedure:

  1. Start C_node
  2. Start B_node
  3. Start A_node

Output: A_node:

started A_node

B_node:

started B_node
recieved request
calling C

C_node:

started C_node
recieved request
self.C = True
Done

It seems that B_node receives the request from A_node. It calls the service in C_node but hangs when waiting for the response. It appears that the service in C_node completes successfully.

Background:

A_node is simulating a service call from a roslibjs client in a web-browser interface implemented by the ROS2-web-bridge. Click button-> call service

B_node represents a Node that provides a Service that receives a list of setpoints and loops through the list calling “set_C” for each setpoint.

C_node is simulating a Node that is effectively running a PID loop on a motor. The equivalent of the “set_C” Service sets the setpoint of the PID loop.

I had this application working fine when the Service in B_node was instead implemented with a ROS2 Action server. Unfortunately, my application requires that I can call this functionality from a web-browser interface provided by the ROS2-web-bridge which currently does not support the Action library, so I converted it ... (more)

edit retag flag offensive close merge delete

Comments

This might be related to you: ros2-how-to-call-a-service-from-the-callback-function-of-a-subscriber

But that question was asked ages ago, so maybe not.

johnconn gravatar imagejohnconn ( 2020-01-22 09:01:06 -0600 )edit

thanks for sharing. Although it does not provide a method for "calling a service within a service" I was able to adapt the example in your link to something that works for the time being. Basically the service in B_node sets a self.pending_action and returns immediately. I added code to the spin() loop to check whether self.pending_action has been set. If it has, my routine is run (including the service calls). As the routine is no longer inside a call back, spin_until_future_complete() does not hang. This solution would probably require more work to handle more than one service as in this simple case.

mikelynders gravatar imagemikelynders ( 2020-01-22 12:38:50 -0600 )edit