eclipse-zenoh / zenoh-plugin-ros2dds

A Zenoh plug-in for ROS2 with a DDS RMW.
https://zenoh.io
Other
112 stars 26 forks source link

Unable to call a ROS2 service from Zenoh #249

Open sumitpaulde opened 2 weeks ago

sumitpaulde commented 2 weeks ago

Describe the bug

I am using 2 physical machines. node1 host ROS2 service node2 host Zenoh client

Both running zenoh_bridge_ros2dds to establish communication between nodes.

ROS2 service requires a simple string as its service call. Service description

string taskname
---
string response

But I am trying to call the service with zenoh

@cdr
class Request:
       taskname: str

request_data = Request(taskname='ooo').serialize()
resource_name = "set_task_service"
for reply in session.get(resource_name, zenoh.Queue(), value=request_data):
    try:
        print(f"Received '{reply.ok.key_expr}': '{reply.ok.payload.decode('utf-8')}'")
    except:
        print(f"Received ERROR: '{reply.err.payload.decode('utf-8')}'")

the client request is not reaching to the ROS2 service server.

 rx-2 ThreadId(20) zenoh_plugin_ros2dds::route_service_srv: Route Service Server (ROS:/set_task_service <-> Zenoh:set_task_service): received invalid request: [7b, 22, 74, 61, 73, 6b, 6e, 61, 6d, 65, 22, 3a, 20, 22, 62, 6c, 61, 62, 6c, 61, 22, 7d]

The ros2 service code

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from hare_robot_interfaces.srv import SetTask
from functools import partial

class SetTaskCleintNode(Node):

    def __init__(self):
        super().__init__("set_task_client")
        for x in range(10):
            self.call_set_task_service("image detection " + str(x))

    def call_set_task_service(self, value1):

        client = self.create_client(SetTask, "set_task_service")
        while not client.wait_for_service(1.0):
            self.get_logger().warn("Waiting for servers response!!")
        request = SetTask.Request()
        request.taskname = value1

        future = client.call_async(request)

        future.add_done_callback(partial(self.callback_call_set_task, value1 = value1))

    def callback_call_set_task(self, future, value1):
        try:
            response = future.result()
            self.get_logger().info("task send: " + value1 + " received " + response)

        except Exception as e:
            self.get_logger().error("Service call failed %r" % (e,))

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

if __name__ == "__main__":
    main()

To reproduce

to reproduce you can use the ROS2 service running on one machine and try to send the server the request from zenoh.

System info

Ubuntu 22.04 arm 64 zenoh-bridge-ros2dds v0.11.0-dev-127-gb147cc7

TanJunKiat commented 6 days ago

Are you using ROS2 humble?

sumitpaulde commented 6 days ago

Ya I am using ROS2 humble