WIP add subscriber

Author: fabid

artefacts.yaml

@@ -41,7 +41,7 @@ jobs:
         params:
           launch/world: ["bookstore.sdf"]
           amcl/update_min_a: [0.2]
-          amcl/update_min_d: [0.25, 0.05]
+          #amcl/update_min_d: [0.25, 0.05]
       settings:
         - name: reach_goal
           ros_testfile: "src/sam_bot_nav2_gz/test/test_reach_goal.launch.py"

src/sam_bot_nav2_gz/test/gazebo.py

@@ -0,0 +1,117 @@
+import subprocess
+import re
+import math
+import time
+import threading
+
+from typing import Dict, Optional
+from xml.etree import ElementTree as ET
+import sys
+sys.path = sys.path + ['/usr/lib/python3/dist-packages']
+from mcap_protobuf.writer import Writer
+from gz.msgs10.pose_v_pb2 import Pose_V
+from gz.transport13 import Node
+
+
+class Pose:
+    def __init__(self, name: str, position: Dict[str, float], orientation: Dict[str, float]):
+        self.name = name
+        self.position = position
+        self.orientation = orientation
+
+    def distance_to(self, other: "Pose") -> float:
+        dx = self.position.get("x", 0) - other.position.get("x", 0)
+        dy = self.position.get("y", 0) - other.position.get("y", 0)
+        dz = self.position.get("z", 0) - other.position.get("z", 0)
+        return math.sqrt(dx*dx + dy*dy + dz*dz)
+
+    def is_near(self, other: "Pose", threshold: float = 0.5) -> bool:
+        return self.distance_to(other) < threshold
+
+
+
+class Gazebo:
+    def __init__(self, recording_path: str = None):
+        self.poses: Dict[str, Pose] = {}
+        self.node = Node()
+        world_name = "collision_test"
+        self.topic_dynamicposes = f"/world/{world_name}/dynamic_pose/info"
+        self._recording = False
+        self._mcap_writer = None
+        self._mcap_file = None
+        if recording_path is not None:
+            self._mcap_file = open(recording_path, "wb")
+            self._mcap_writer = Writer(self._mcap_file)
+            self._recording = True
+        self.node.subscribe(Pose_V, self.topic_dynamicposes, self.posev_cb)
+ 
+    def update_pose(self, name: str, position: Dict[str, float], orientation: Dict[str, float]):
+        self.poses[name] = Pose(name, position, orientation)
+
+    def get(self, name: str) -> Optional[Pose]:
+        return self.poses.get(name)
+
+    def posev_cb(self, msg: Pose_V):
+        if self._recording and self._mcap_writer:
+            now = int(time.time() * 1e9)
+            self._mcap_writer.write_message(
+                topic=self.topic_dynamicposes,
+                message=msg,
+                log_time=now,
+                publish_time=now, # do we compute from msg.header.stamp instead?, or even a sim time topic?
+            )
+            print("wrote 1 message")
+        for pose_msg in msg.pose:
+            name = pose_msg.name
+            position = {
+                "x": getattr(pose_msg.position, "x", 0.0),
+                "y": getattr(pose_msg.position, "y", 0.0),
+                "z": getattr(pose_msg.position, "z", 0.0),
+            }
+            orientation = {
+                "x": getattr(pose_msg.orientation, "x", 0.0),
+                "y": getattr(pose_msg.orientation, "y", 0.0),
+                "z": getattr(pose_msg.orientation, "z", 0.0),
+                "w": getattr(pose_msg.orientation, "w", 1.0),
+            }
+            self.update_pose(name, position, orientation)
+
+    def start(self):
+        self._running = True
+        self._thread = threading.Thread(target=self._spin_loop, daemon=True)
+        self._thread.start()
+
+    def stop(self):
+        self._running = False
+        if hasattr(self, "_thread"):
+            self._thread.join()
+
+        if self._recording:
+            if self._mcap_file:
+                self._mcap_writer.finish()
+                self._mcap_file.close()
+            self._recording = False
+
+    def _spin_loop(self):
+        while self._running:
+            time.sleep(0.001)
+
+def main():
+    if len(sys.argv) != 2:
+        print("Usage: python recorder.py <output.mcap>")
+        sys.exit(1)
+
+    output_path = sys.argv[1]
+    gz = Gazebo(recording_path=output_path)
+    gz.start()
+    print(f"Recording simulation to {output_path}. Press Ctrl+C to stop.")
+
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        print("Stopping recording...")
+        gz.stop()
+
+if __name__ == "__main__":
+    main()

src/sam_bot_nav2_gz/test/test_reach_goal.launch.py

@@ -14,6 +14,15 @@
 from artefacts_toolkit.chart import make_chart
 from artefacts_toolkit.config import get_artefacts_param
 
+# show python sys path
+import sys
+print("Python sys path:", sys.path)
+# add current directory to path
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+print("Python sys path:", sys.path)
+from gazebo import Gazebo
+
+
 
 # This function specifies the processes to be run for our test
 @pytest.mark.launch_test
@@ -54,18 +63,18 @@ def generate_test_description():
         )
 
     # Gazebo ros bridge
-    gz_bridge = Node(
-        package="ros_gz_bridge",
-        executable="parameter_bridge",
-        parameters=[{
-            "config_file": os.path.join(
-                "src",
-                "sam_bot_nav2_gz",
-                "test",
-                 "bridge.yaml"
-                )}],
-        output="screen",
-        )
+    #gz_bridge = Node(
+    #    package="ros_gz_bridge",
+    #    executable="parameter_bridge",
+    #    parameters=[{
+    #        "config_file": os.path.join(
+    #            "src",
+    #            "sam_bot_nav2_gz",
+    #            "test",
+    #             "bridge.yaml"
+    #            )}],
+    #    output="screen",
+    #    )
 
     test_odometry_node = ExecuteProcess(
         cmd=[
@@ -78,6 +87,8 @@ def generate_test_description():
             ),
         ]
     )
+    gz_interface = Gazebo(recording_path="recording.mcap")
+    gz_interface.start()
 
     return LaunchDescription(
         [
@@ -89,11 +100,11 @@ def generate_test_description():
             launch_navigation_stack,
             reach_goal,
             test_odometry_node,
-            gz_bridge,
+            #gz_bridge,
             bag_recorder,
             ReadyToTest(),
         ]
-    ), { "rosbag_filepath": rosbag_filepath}
+        ), { "rosbag_filepath": rosbag_filepath, "gz_interface": gz_interface}
 
 
 # This is our test fixture. Each method is a test case.
@@ -106,13 +117,14 @@ def test_nav2_started(self, proc_output):
             # replace the exception message with a more informative one
             raise AssertionError("Nav2 apparently failed to start") from e
 
-    def test_reached_goal(self, proc_output):
+    def test_reached_goal(self, proc_output, gz_interface):
         """Check the logs to see if the navigation task is completed"""
         # 'proc_output' is an object added automatically by the launch_testing framework.
         # It captures the outputs of the processes launched in generate_test_description()
         # Refer to the documentation for further details.
         try:
             proc_output.assertWaitFor("Goal succeeded!", timeout=240, stream="stdout")
+            assert gz_interface.get("sam_bot") > 0
         except AssertionError as e:
             # replace the exception message with a more informative one
             raise AssertionError("Goal was not reached") from e
@@ -121,7 +133,7 @@ def test_reached_goal(self, proc_output):
 
 @launch_testing.post_shutdown_test()
 class TestProcOutputAfterShutdown(unittest.TestCase):
-    def test_exit_code(self, rosbag_filepath):
+    def test_exit_code(self, rosbag_filepath, gz_interface):
         print(rosbag_filepath)
         make_chart(
             rosbag_filepath,
@@ -130,5 +142,9 @@ def test_exit_code(self, rosbag_filepath):
             field_unit="m",
             chart_name="odometry_position",
         )
+        final_pose = gz_interface.get("sam_bot")
+        print(final_pose)
+        assert final_pose.position["x"] > 10000.0
+        gz_interface.stop()
         image_topics.extract_camera_image(rosbag_filepath, "/sky_cam")
         image_topics.extract_video(rosbag_filepath, "/sky_cam", "output/sky_cam.webm")