Rerun Route - ver0

pyproject.toml

@@ -0,0 +1,24 @@
+[project]
+name = "osgar-apps"
+version = "0.1.0"
+description = "Add your description here"
+authors = [
+    { name = "Elena Ai", email = "elena.ai@robotika.cz" }
+]
+requires-python = ">=3.9"
+dependencies = [
+    "bitstring>=4.3.1",
+    "librosa>=0.10.0",
+    "numpy==1.22.0",
+    "opencv-python==4.7.0.72",
+    "osgar",
+    "shapely==2.0.1",
+    "speechrecognition==3.14.3",
+]
+
+[build-system]
+requires = ["uv_build>=0.9.8,<0.10.0"]
+build-backend = "uv_build"
+
+[tool.uv.sources]
+osgar = { git = "https://github.com/robotika/osgar.git", rev = "master" }

rerun-route/README.md

@@ -0,0 +1,29 @@
+# Rerun Route
+
+This application allows a mobile robot to re-run a route recorded in a previous OSGAR log file.
+
+## Usage
+
+Update `config/matty-rerun-route.json` with the path to your log file and the desired stream name:
+
+```json
+"init": {
+  "logfile": "path/to/your/previous-run.log",
+  "pose2d_stream": "platform.pose2d",
+  "max_speed": 0.5
+}
+```
+
+Run the application with OSGAR:
+
+```bash
+python -m osgar.record config/matty-rerun-route.json
+```
+
+## How it works
+
+The `RerunRoute` module:
+1. Opens the specified OSGAR log file.
+2. Extracts the `pose2d` path.
+3. Initializes `osgar.followpath.FollowPath` with the extracted route.
+4. Executes the path following.

rerun-route/config/matty-rerun-route.json

@@ -0,0 +1,77 @@
+{
+  "version": 2,
+  "robot": {
+    "modules": {
+      "app": {
+          "driver": "rerun-route.main:RerunRoute",
+          "in": ["emergency_stop", "pose2d", "obstacle"],
+          "out": ["desired_speed"],
+          "init": {
+            "logfile": "last-run.log",
+            "pose2d_stream": "platform.pose2d",
+            "max_speed": 0.5,
+            "obstacle_stop_dist": 0.4,
+            "timeout": 30
+          }
+      },
+      "platform": {
+        "driver": "osgar.platforms.matty:Matty",
+        "in": ["esp_data"],
+        "out": ["esp_data"],
+        "init": {}
+      },
+      "timer": {
+        "driver": "timer",
+        "in": [],
+        "out": ["tick"],
+        "init": {
+          "sleep": 0.1
+        }
+      },
+      "serial": {
+          "driver": "serial",
+          "in": ["raw"],
+          "out": ["raw"],
+          "init": {"port": "/dev/ttyUSB0", "speed": 115200}
+      },
+      "oak": {
+        "driver": "osgar.drivers.oak_camera:OakCamera",
+        "init": {
+          "fps": 10,
+          "is_color": true,
+          "video_encoder": "h265",
+          "h264_bitrate": 2000,
+          "is_depth": true,
+          "laser_projector_current": 0,
+          "flood_light_current": 500,
+          "is_imu_enabled": true,
+          "number_imu_records": 10,
+          "disable_magnetometer_fusion": false,
+          "mono_resolution": "THE_400_P",
+          "color_resolution": "THE_1080_P",
+          "color_manual_focus": 130,
+          "stereo_median_filter": "KERNEL_3x3",
+          "stereo_mode": "HIGH_ACCURACY",
+          "stereo_extended_disparity": false,
+          "stereo_subpixel": false,
+          "stereo_left_right_check": true
+        }
+      },
+      "obstdet3d": {
+          "driver": "osgar.obstdet3d:ObstacleDetector3D",
+          "in": ["depth"],
+          "out": ["obstacle"],
+          "init": {}
+      }
+    },
+    "links": [
+      ["app.desired_speed", "platform.desired_steering"],
+      ["platform.pose2d", "app.pose2d"],
+      ["serial.raw", "platform.esp_data"],
+      ["platform.esp_data", "serial.raw"],
+      ["timer.tick", "platform.tick"],
+      ["oak.depth", "obstdet3d.depth"],
+      ["obstdet3d.obstacle", "app.obstacle"]
+    ]
+  }
+}

rerun-route/main.py

@@ -0,0 +1,83 @@
+"""
+  Rerun Route from OSGAR log
+"""
+import math
+from osgar.node import Node
+from osgar.bus import BusShutdownException
+from osgar.followpath import FollowPath, Route
+from osgar.logger import LogReader, lookup_stream_id
+from osgar.lib.serialize import deserialize
+
+
+class RerunRoute(Node):
+    def __init__(self, config, bus):
+        super().__init__(config, bus)
+        bus.register('desired_speed')
+        self.logfile = config.get('logfile')
+        self.pose2d_stream = config.get('pose2d_stream', 'platform.pose2d')
+
+        # Load path from log file
+        self.path = self.extract_path(self.logfile, self.pose2d_stream)
+        if not self.path:
+            # We can't raise exception here as it might crash the whole system
+            # but we can print and keep empty path
+            print(f"ERROR: No path extracted from {self.logfile}")
+        else:
+            print(f"Extracted {len(self.path)} points from {self.logfile}")
+
+        self.app = FollowPath(config, bus)
+        self.app.route = Route(pts=self.path)
+
+        # Override app methods to use this node's bus
+        self.app.publish = self.my_publish
+        self.app.listen = self.my_listen
+        self.app.update = self.my_update
+
+    def my_publish(self, name, data):
+        self.publish(name, data)
+
+    def my_listen(self):
+        return self.listen()
+
+    def my_update(self):
+        return self.update()
+
+    def extract_path(self, logfile, pose2d_stream):
+        if logfile is None:
+            return []
+        path = []
+        try:
+            stream_id = lookup_stream_id(logfile, pose2d_stream)
+        except Exception as e:
+            print(f"Error looking up stream {pose2d_stream} in {logfile}: {e}")
+            return []
+
+        with LogReader(logfile, only_stream_id=stream_id) as log:
+            for timestamp, stream_id, data in log:
+                pose = deserialize(data)
+                # pose is typically [x, y, heading] in mm and hundredths of degree
+                x, y = pose[0]/1000.0, pose[1]/1000.0
+                if len(path) == 0 or math.hypot(path[-1][0] - x, path[-1][1] - y) > 0.1:
+                    path.append((x, y))
+        return path
+
+    def on_pose2d(self, data):
+        self.app.on_pose2d(data)
+
+    def on_emergency_stop(self, data):
+        self.app.on_emergency_stop(data)
+
+    def on_obstacle(self, data):
+        if hasattr(self.app, 'on_obstacle'):
+            self.app.on_obstacle(data)
+
+    def run(self):
+        try:
+            while not self.app.finished:
+                self.update()
+        except BusShutdownException:
+            pass
+        print("Route finished, requesting stop.")
+        self.request_stop()
+
+# vim: expandtab sw=4 ts=4

rerun-route/test_rerun_route.py

@@ -0,0 +1,19 @@
+import unittest
+from unittest.mock import MagicMock
+from main import RerunRoute
+
+
+class RerunRouteTest(unittest.TestCase):
+    def test_init(self):
+        bus = MagicMock()
+        config = {
+            'logfile': None,
+            'pose2d_stream': 'platform.pose2d'
+        }
+        # It should at least not crash on init with no logfile
+        app = RerunRoute(config, bus)
+        self.assertEqual(app.path, [])
+
+
+if __name__ == '__main__':
+    unittest.main()