update script with latest zenoh api (#149)

* rebase with latest change Signed-off-by: thomasung <thomas.ung@pal-robotics.com> * add missing \ in readme Signed-off-by: thomasung <thomas.ung@pal-robotics.com> * update readme informations about zenoh version Signed-off-by: thomasung <thomas.ung@pal-robotics.com> clean dangling code * fix linting issues and test example test files Signed-off-by: thomasung <thomas.ung@pal-robotics.com> --------- Signed-off-by: thomasung <thomas.ung@pal-robotics.com>
open-rmf · Nov 7, 2024 · 8cdc298 · 8cdc298
1 parent 57b17fc
commit 8cdc298
Show file tree

Hide file tree

Showing 5 changed files with 78 additions and 67 deletions.
diff --git a/README.md b/README.md
@@ -23,7 +23,7 @@ Supports
 * [ROS 2 Jazzy](https://docs.ros.org/en/jazzy/index.html)
 * [rmw-cyclonedds-cpp](https://github.com/ros2/rmw_cyclonedds)
 * [Open-RMF on main](https://github.com/open-rmf/rmf)
-* [zenoh-bridge-ros2dds v0.11](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/releases/tag/0.11.0)
+* [zenoh-bridge-ros2dds v1.0.0-beta.4](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds/releases/tag/1.0.0-beta.4)
 
 We recommend setting up `zenoh-bridge-ros2dds` with the standalone binaries. After downloading the appropriate released version and platform, extract and use the standalone binaries as is. For source builds of `zenoh-bridge-ros2dds`, please follow the [official guides](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds).
 
@@ -43,7 +43,7 @@ sudo apt install python3-pip ros-jazzy-rmw-cyclonedds-cpp
 The dependencies `eclipse-zenoh` and `pycdr2` are available through `pip`. Users can choose to set up a virtual environment, or `--break-system-packages` by performing the installation directly.
 
 ```bash
-pip3 install eclipse-zenoh==0.11.0 pycdr2 --break-system-packages
+pip3 install pip install eclipse-zenoh==1.0.0b4 pycdr2 --break-system-packages
 ```
 
 > [!NOTE]
@@ -124,7 +124,7 @@ source ~/ff_ws/install/setup.bash
 ros2 run free_fleet_examples test_navigate_to_pose.py \
     --frame-id map \
     --namespace turtlebot3_1 \
-    -x 1.808
+    -x 1.808 \
     -y 0.503
 ```
 
@@ -256,7 +256,6 @@ ros2 run rmf_demos_tasks dispatch_patrol \
 * attempt to optimize tf messages (not all are needed)
 * ROS 1 nav support
 * map switching support
-* update to use zenoh 1.0
 * static testing
 * end-to-end testing
 * support for Rolling

diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py
@@ -72,7 +72,7 @@ def __init__(
 
         def _tf_callback(sample: zenoh.Sample):
             try:
-                transform = TFMessage.deserialize(sample.payload)
+                transform = TFMessage.deserialize(sample.payload.to_bytes())
             except Exception as e:
                 self.node.get_logger().debug(
                     f'Failed to deserialize TF payload: {type(e)}: {e}'
@@ -105,7 +105,9 @@ def _tf_callback(sample: zenoh.Sample):
         )
 
         def _battery_state_callback(sample: zenoh.Sample):
-            battery_state = SensorMsgs_BatteryState.deserialize(sample.payload)
+            battery_state = SensorMsgs_BatteryState.deserialize(
+                sample.payload.to_bytes()
+            )
             self.battery_soc = battery_state.percentage
 
         self.battery_state_sub = self.zenoh_session.declare_subscriber(
@@ -124,15 +126,14 @@ def _is_navigation_done(self) -> bool:
         # TODO(ac): parameterize the service call timeout
         replies = self.zenoh_session.get(
             namespacify('navigate_to_pose/_action/get_result', self.name),
-            zenoh.Queue(),
-            value=req.serialize(),
+            payload=req.serialize(),
             # timeout=0.5
         )
-        for reply in replies.receiver:
+        for reply in replies:
             try:
                 # Deserialize the response
                 rep = NavigateToPose_GetResult_Response.deserialize(
-                    reply.ok.payload
+                    reply.ok.payload.to_bytes()
                 )
                 self.node.get_logger().debug(f'Result: {rep.status}')
                 if rep.status == GoalStatus.STATUS_EXECUTING:
@@ -151,7 +152,7 @@ def _is_navigation_done(self) -> bool:
                 self.node.get_logger().debug(
                     'Received (ERROR: "{}"): {}: {}'
                     .format(
-                        reply.err.payload.decode('utf-8'),
+                        reply.err.payload.to_string(),
                         type(e),
                         e
                     ))
@@ -223,14 +224,14 @@ def navigate(self, destination, execution):
 
         replies = self.zenoh_session.get(
             namespacify('navigate_to_pose/_action/send_goal', self.name),
-            zenoh.Queue(),
-            value=req.serialize(),
+            payload=req.serialize(),
             # timeout=0.5
         )
-        for reply in replies.receiver:
+
+        for reply in replies:
             try:
                 rep = NavigateToPose_SendGoal_Response.deserialize(
-                    reply.ok.payload)
+                    reply.ok.payload.to_bytes())
                 if rep.accepted:
                     self.node.get_logger().info(
                         f'Navigation goal {nav_goal_id} accepted'
@@ -244,7 +245,7 @@ def navigate(self, destination, execution):
                 self.nav_goal_id = None
                 return
             except Exception as e:
-                payload = reply.err.payload.decode('utf-8')
+                payload = reply.err.payload.to_string()
                 self.node.get_logger().error(
                     f'Received (ERROR: {payload}: {type(e)}: {e})'
                 )
@@ -266,13 +267,12 @@ def stop(self, activity):
                         'navigate_to_pose/_action/cancel_goal',
                         self.name,
                     ),
-                    zenoh.Queue(),
-                    value=req.serialize(),
+                    payload=req.serialize(),
                     # timeout=0.5
                 )
-                for reply in replies.receiver:
+                for reply in replies:
                     rep = ActionMsgs_CancelGoal_Response.deserialize(
-                        reply.ok.payload
+                        reply.ok.payload.to_bytes()
                     )
                     self.node.get_logger().info(
                         'Return code: %d' % rep.return_code

diff --git a/free_fleet_examples/free_fleet_examples/tests/test_cancel_all_goals.py b/free_fleet_examples/free_fleet_examples/tests/test_cancel_all_goals.py
@@ -47,14 +47,15 @@ def main(argv=sys.argv):
             'navigate_to_pose/_action/cancel_goal',
             args.namespace
         ),
-        zenoh.Queue(),
-        value=req.serialize()
+        payload=req.serialize()
     )
     # Zenoh could get several replies for a request (e.g. from several
     # 'Service Servers' using the same name)
-    for reply in replies.receiver:
+    for reply in replies:
         # Deserialize the response
-        rep = ActionMsgs_CancelGoal_Response.deserialize(reply.ok.payload)
+        rep = ActionMsgs_CancelGoal_Response.deserialize(
+            reply.ok.payload.to_bytes()
+        )
         print('Return code: %d' % rep.return_code)
 
     session.close()

diff --git a/free_fleet_examples/free_fleet_examples/tests/test_navigate_to_pose.py b/free_fleet_examples/free_fleet_examples/tests/test_navigate_to_pose.py
@@ -39,7 +39,7 @@
 
 
 def feedback_callback(sample: zenoh.Sample):
-    feedback = NavigateToPose_Feedback.deserialize(sample.payload)
+    feedback = NavigateToPose_Feedback.deserialize(sample.payload.to_bytes())
     print(f'Distance remaining: {feedback.distance_remaining}')
 
 
@@ -90,19 +90,20 @@ def main(argv=sys.argv):
     print('Sending goal')
     replies = session.get(
         namespacify('navigate_to_pose/_action/send_goal', args.namespace),
-        zenoh.Queue(),
-        value=req.serialize()
+        payload=req.serialize(),
     )
 
     # Zenoh could get several replies for a request (e.g. from several
     # 'Service Servers' using the same name)
-    for reply in replies.receiver:
+    for reply in replies:
         if not reply.ok:
             print('Reply was not ok!')
             continue
         print('handling a reply!')
         # Deserialize the response
-        rep = NavigateToPose_SendGoal_Response.deserialize(reply.ok.payload)
+        rep = NavigateToPose_SendGoal_Response.deserialize(
+            reply.ok.payload.to_bytes()
+        )
         if not rep.accepted:
             print('Goal rejected')
             return
@@ -117,27 +118,30 @@ def main(argv=sys.argv):
                 namespacify(
                     'navigate_to_pose/_action/get_result',
                     args.namespace),
-                zenoh.Queue(),
-                value=req.serialize(),
-                timeout=0.5
+                payload=req.serialize(),
+                timeout=5.5
             )
 
             # Zenoh could get several replies for a request (e.g. from several
             # 'Service Servers' using the same name)
-            for reply in replies.receiver:
+            for reply in replies:
                 try:
                     # Deserialize the response
                     rep = NavigateToPose_GetResult_Response.deserialize(
-                        reply.ok.payload
+                        reply.ok.payload.to_bytes()
                     )
-                    # print('Result: {0}'.format(rep.sequence))
-                    print(f'Result: {rep.status}')
+                    # print("Result: {0}".format(rep.sequence))
+                    print(f"Result: {rep.status}")
+                    if rep.status == GoalStatus.STATUS_ABORTED:
+                        print(
+                        "Received (ERROR: 'Plan aborted by planner_server')"
+                        )
+                        break
                     if rep.status == GoalStatus.STATUS_SUCCEEDED:
                         break
-                except Exception as e:
-                    e  # to prevent unused variable
-                    print('Received (ERROR: "{}")'.format(
-                        reply.err.payload.decode('utf-8')))
+                except Exception as _:
+                    print("Received (ERROR: '{}')".format(
+                        reply.err.payload.to_string()))
                     continue
 
             time.sleep(1)

diff --git a/free_fleet_examples/free_fleet_examples/tests/test_tf.py b/free_fleet_examples/free_fleet_examples/tests/test_tf.py
@@ -31,7 +31,7 @@
 
 
 def tf_callback(sample: zenoh.Sample):
-    transform = TFMessage.deserialize(sample.payload)
+    transform = TFMessage.deserialize(sample.payload.to_bytes())
     for zt in transform.transforms:
         t = transform_stamped_to_ros2_msg(zt)
         tf_buffer.set_transform(t, 'free_fleet_examples_test_tf')
@@ -54,34 +54,41 @@ def main(argv=sys.argv):
     # Create Zenoh Config from file if provoded, or a default one otherwise
     conf = zenoh.Config.from_file(args.config) \
         if args.config is not None else zenoh.Config()
-    # Open Zenoh Session
-    session = zenoh.open(conf)
 
-    # Subscribe to TF
-    pub = session.declare_subscriber(
-        namespacify('tf', args.namespace),
-        tf_callback
-    )
+    zenoh.try_init_log_from_env()
 
-    try:
-        while True:
-            try:
-                transform = tf_buffer.lookup_transform(
-                    args.base_footprint_frame,
-                    args.map_frame,
-                    Time()
-                )
-                print(transform)
-            except Exception as err:
-                print(f'Unable to get transform between base_footprint and '
-                      f'map: {type(err)}: {err}')
-
-            time.sleep(1)
-    except (KeyboardInterrupt):
-        pass
-    finally:
-        pub.undeclare()
-        session.close()
+    # Open Zenoh Session
+    with zenoh.open(conf) as session:
+        info = session.info
+        print(f"zid: {info.zid()}")
+        print(f"routers: {info.routers_zid()}")
+        print(f"peers: {info.peers_zid()}")
+
+        # Subscribe to TF
+        pub = session.declare_subscriber(
+            namespacify('tf', args.namespace),
+            tf_callback
+        )
+
+        try:
+            while True:
+                try:
+                    transform = tf_buffer.lookup_transform(
+                        args.base_footprint_frame,
+                        args.map_frame,
+                        Time()
+                    )
+                    print(transform)
+                except Exception as err:
+                    print(f'Unable to get transform between base_footprint and '
+                        f'map: {type(err)}: {err}')
+
+                time.sleep(1)
+        except (KeyboardInterrupt):
+            pass
+        finally:
+            pub.undeclare()
+            session.close()
 
 
 if __name__ == '__main__':