Ask Your Question
0

Mistakes using service and client in same node (ROS2, Python)

asked 2021-03-02 21:30:27 -0500

masynthetic gravatar image

updated 2021-03-03 12:10:56 -0500

Hello, I am new to ROS2 and working on making a package for communicating with 8 Bit MCUs (arduinos) which seems to have been left behind by ROS2.

I have made a service which simply uses pyserial to send a message to the Arduino then receive and return the response. This service works fine however I am trying to make another layer of services which use that service as a client and that is where I am not quite getting it. Below is an example where I am trying to create another service for reading the oxygen sensor, which must use the serial service to request from the sensor and then get the return value. My problem is that I am not really getting how to make the async calls or use/setup the client node inside of the service node and haven't found a good example of this. Additionally, the message send (service request) works but it is specifically getting the return value back that I don't have down.

The code is below, I believe most of the issues and the solution should be in the callback_sense_oxygen method.

#!/usr/bin/env python3
import sys
import rclpy
from rclpy.node import Node
from functools import partial

from garden_interfaces.srv import OxygenSense
from garden_interfaces.srv import SerialMessage

class SenseOxygenServerNode(Node):
    def __init__(self):
        super().__init__("sense_oxygen_server") 
        self.server_ = self.create_service(
            OxygenSense, "sense_oxygen", self.callback_sense_oxygen)
        self.get_logger().info("starting sense oxygen server")

    def callback_sense_oxygen(self, request, response):
        # create and send serial service request
        msg = "rdo2"
        skey = "oxgn"
        self.call_send_recieve_canopy_server(msg, skey)
        call_result = future.result()

        # create sensor service response
        if (call_result.success):
            response.success = call_result.success
            response.concentration = call_result.reply_data
            response.unit = "%"

        return response

    def call_send_recieve_canopy_server(self, message, success_key):
        # create client 
        client = self.create_client(SerialMessage, "send_recieve_canopy")
        while not client.wait_for_service(1.0):
            self.get_logger().warn("waiting for canopy server...")
        # create request
        request = SerialMessage.Request()
        request.message = message
        request.success_key = success_key
        # send request asyncronously
        future = client.call_async(request)
        # create callback for the future
        future.add_done_callback(partial(self.callback_call_send_recieve_canopy_server, message=message, success_key=success_key))

        #return future

    def callback_call_send_recieve_canopy_server(self, future, message, success_key):
        try:
            response = future.result()
            self.get_logger().info("success message here")
            return response

        except Exception as err:
            self.get_logger().error("service call failed %r" % (err))


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


if __name__ == "__main__":
    main()
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-03-04 15:00:32 -0500

ChuiV gravatar image

updated 2021-03-04 19:31:52 -0500

You'll need to add a second node that you attach the "send_recieve_canopy" client to instead of attaching it to the "self" node object. The issue I think you're running into is that the "self" node cannot spin until that callback is finished. So by creating a second private node, you can independently spin that node inside the callback of the "self" node.

At least that's what I have to do in rclcpp. So it should be very similar in rclpy.

Let me know if you need a more concrete example.

Edit 1:

Here an example:

import rclpy
from rclpy.node import Node
from example_interfaces.srv import AddTwoInts


class ServiceNode(Node):
    def __init__(self):
        super().__init__('main_node')
        self.sub_node = rclpy.create_node('sub_node')
        self.server = self.create_service(AddTwoInts, 'example_service', self.add_two_ints_callback)
        self.sub_client = self.sub_node.create_client(AddTwoInts, 'add_two_ints')

    def add_two_ints_callback(self, request: AddTwoInts.Request, response: AddTwoInts.Response):
        self.sub_client.wait_for_service()
        sub_request = AddTwoInts.Request()
        sub_request.a = request.a
        sub_request.b = request.b
        future = self.sub_client.call_async(sub_request)
        rclpy.spin_until_future_complete(self.sub_node, future)
        if future.result() is not None:
            result: AddTwoInts.Response = future.result()
            self.get_logger().info('Result of add_two_ints: {}'.format(result.sum))
            response.sum = result.sum
            return response
        else:
            self.get_logger().error('Exception while calling service: {}'.format(future.exception()))


if __name__ == '__main__':
    rclpy.init()
    node = ServiceNode()
    rclpy.spin(node)
    rclpy.shutdown()

To show it works, first in a terminal start

$ ros2 run demo_nodes_cpp add_two_ints_server

in another terminal run this example node. In yet another terminal run

$ ros2 run demo_nodes_cpp add_two_ints_client -s example_service

Hope that answers your question.

edit flag offensive delete link more

Comments

thanks that makes sense, an example would be awesome. Do you mean to make a whole different "class" for that node?

masynthetic gravatar image masynthetic  ( 2021-03-04 16:03:31 -0500 )edit

Looks great thank you I will give that a shot

masynthetic gravatar image masynthetic  ( 2021-03-04 19:52:59 -0500 )edit

So I can run your example fine (although the last command worked but didn't actually hit the test node for me) so I just ran as:

$ ros2 service call /example_service example_interfaces/srv/AddTwoInts "{a: 1, b: 2}"

which returned 3 as expected. However what is strange is that I cannot get essentially the same code to work on my own inner service.

I thought it may be something simple due to your example using the same interface as the inner service and mine basically translating one into another so I made sure I wasn't making any mistakes there and then eventually just made them the same interface to test but still it just hangs after sending the request. The inner service processes the request fine but for some reason it never makes it back to the outer service.

Additionally the inner returns when called in a manner similar ...(more)

masynthetic gravatar image masynthetic  ( 2021-03-06 19:01:52 -0500 )edit

Just want to double check that you're calling the sub-service spin on the sub node.. ie from my example:

rclpy.spin_until_future_complete(self.sub_node, future)

instead of

rclpy.spin_until_future_complete(self, future)
ChuiV gravatar image ChuiV  ( 2021-03-08 08:31:28 -0500 )edit

It wasn't this but was very close, I was missing the .sub_node in the initialization

really appreciate the help!

masynthetic gravatar image masynthetic  ( 2021-03-09 02:50:16 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-03-02 21:30:27 -0500

Seen: 90 times

Last updated: Mar 04