Implement teleop node

README.md

@@ -19,6 +19,12 @@ git lfs pull
 uv sync
 ```
 
+## Run teleop demo
+Use **WASD** for linear motion and **QE** for turning. **R** reloads the scene and **F** jumps to the next one. 
+```sh
+uv run dataflow  --teleop
+```
+
 
 ## Testing with Artefacts
 
@@ -59,6 +65,7 @@ The `dora-rs` nodes are organized as separate Python packages under `nodes/*`
  - [`tester`](./nodes/tester/) contains the test nodes that should be executed with `pytest`
    - [test_waypoints_poses.py](./nodes/tester/tester/test_waypoints_poses.py) Executes multiple waypoint navigation scenarios and uses the robot and waypoint position data to determine if the waypoint mission was successful.
    - [test_waypoints_report.py](./nodes/tester/tester/test_waypoints_report.py) The simplified version of the test above, that uses the internal waypoint mission state from the simulation to determine if the waypoint mission was successful.
+ - [`teleop`](./nodes/teleop/) implements keyboard teleop control
 
 ### Other packages
  - [`msgs`](./msgs/) Implements the necessary messages as python classes using [`arrow-message`](https://github.com/hennzau/arrow-message) 

dataflow/pyproject.toml

@@ -5,8 +5,12 @@ description = "Add your description here"
 readme = "README.md"
 requires-python = ">=3.11,<3.12"
 dependencies = [
-    # "artefacts-cli>=0.9.9",
-    # "artefacts-click>=0.1.0",
+    "msgs",
+    "simulation",
+    "navigator",
+    "policy_controller",
+    "tester",
+    "teleop",
     "dora-rs-cli>=0.3.13",
     "typer>=0.20.0",
 ]

dataflow/src/dataflow/dataflow.py

@@ -4,15 +4,59 @@
 from typing_extensions import Annotated
 from pathlib import Path
 
+workspace_path = Path(__file__).parent.parent.parent.parent
+output_path = workspace_path / "outputs/artefacts"
+nodes_path = workspace_path / "nodes"
+temp_dataflow_path = output_path / "dataflow.yaml"
 
-def exec_dataflow(dataflow: DataflowBuilder, temp_dataflow_path: Path):
+
+def _exec_dataflow(dataflow: DataflowBuilder, temp_dataflow_path: Path):
     """Build and/or run the dataflow with dora-rs."""
     dataflow.to_yaml(temp_dataflow_path)
     dora.build(str(temp_dataflow_path), uv=True)
     dora.run(str(temp_dataflow_path))
 
 
+def _create_base_dataflow() -> DataflowBuilder:
+    dataflow = DataflowBuilder(name="go2-example-dataflow")
+
+    output_path.mkdir(parents=True, exist_ok=True)
+
+    # Set up simulation
+    simulation = dataflow.add_node(
+        id="simulation",
+        path="simulation",
+        args="--scene generated_pyramid --use-auto-pilot",
+        env={
+            "OMNI_KIT_ACCEPT_EULA": "YES",
+            "OUTPUT_DIR": str(output_path),
+        },
+    )
+    simulation.add_input("pub_status_tick", "dora/timer/millis/200")
+    simulation.add_input("joint_commands", "policy_controller/joint_commands")
+    simulation.add_output("robot_pose")
+    simulation.add_output("waypoints")
+    simulation.add_output("scene_info")
+    simulation.add_output("rtf")
+    simulation.add_output("observations")
+    simulation.add_output("simulation_time")
+
+    # Set up policy controller
+    policy_controller = dataflow.add_node(
+        id="policy_controller",
+        path="policy_controller",
+    )
+    policy_controller.add_input("observations", "simulation/observations")
+    policy_controller.add_input("clock", "simulation/simulation_time")
+    policy_controller.add_output("joint_commands")
+
+    return dataflow, simulation, policy_controller
+
+
 def run_dataflow(
+    teleop: Annotated[
+        bool, typer.Option(help="Use keyboard teleoperation to control the robot")
+    ] = False,
     test_waypoint_poses: Annotated[
         bool, typer.Option(help="Run the waypoint poses tests")
     ] = False,
@@ -23,6 +67,11 @@ def run_dataflow(
 ):
     """Compose the dataflow, and build/run it with dora-rs."""
 
+    # We either test or teleop for now
+    if teleop and (test_waypoint_poses or test_waypoint_report or test_all):
+        print("Cannot use teleop and testing options at the same time.")
+        return
+
     # List tests for running
     tests = []
     if test_waypoint_poses or test_all:
@@ -34,42 +83,24 @@ def run_dataflow(
         print("No tests selected to run. Use --help for options.")
         # TODO: run default dataflow without tester node (and optionally with teleop)
 
-    for test in tests:
-        dataflow = DataflowBuilder(name="go2-example-dataflow")
-        workspace_path = Path(__file__).parent.parent.parent.parent
-        output_path = workspace_path / "outputs/artefacts"
-        nodes_path = workspace_path / "nodes"
-        temp_dataflow_path = output_path / "dataflow.yaml"
-
-        output_path.mkdir(parents=True, exist_ok=True)
-
-        # Set up simulation
-        simulation = dataflow.add_node(
-            id="simulation",
-            path="simulation",
-            args="--scene generated_pyramid --use-auto-pilot",
-            env={
-                "OMNI_KIT_ACCEPT_EULA": "YES",
-                "OUTPUT_DIR": str(output_path),
-            },
-        )
-        simulation.add_input("pub_status_tick", "dora/timer/millis/200")
-        simulation.add_input("joint_commands", "policy_controller/joint_commands")
-        simulation.add_output("robot_pose")
-        simulation.add_output("waypoints")
-        simulation.add_output("scene_info")
-        simulation.add_output("rtf")
-        simulation.add_output("observations")
-        simulation.add_output("simulation_time")
-
-        policy_controller = dataflow.add_node(
-            id="policy_controller",
-            path="policy_controller",
+    if teleop:
+        dataflow, simulation, policy_controller = _create_base_dataflow()
+
+        teleop_node = dataflow.add_node(
+            id="teleop",
+            path="teleop",
         )
-        policy_controller.add_input("observations", "simulation/observations")
-        policy_controller.add_input("clock", "simulation/simulation_time")
-        policy_controller.add_input("command_2d", "navigator/command_2d")
-        policy_controller.add_output("joint_commands")
+        teleop_node.add_input("tick", "dora/timer/millis/100")
+        teleop_node.add_output("command_2d")
+        teleop_node.add_output("load_scene")
+
+        policy_controller.add_input("command_2d", "teleop/command_2d")
+        simulation.add_input("load_scene", "teleop/load_scene")
+
+        _exec_dataflow(dataflow, temp_dataflow_path)
+
+    for test in tests:
+        dataflow, simulation, policy_controller = _create_base_dataflow()
 
         # Add waypoint navigation
         navigator = dataflow.add_node(
@@ -80,6 +111,7 @@ def run_dataflow(
         navigator.add_input("robot_pose", "simulation/robot_pose")
         navigator.add_input("waypoints", "simulation/waypoints")
         navigator.add_output("command_2d")
+        policy_controller.add_input("command_2d", "navigator/command_2d")
 
         # Add the tester node
         tester = dataflow.add_node(
@@ -102,7 +134,7 @@ def run_dataflow(
         # Allow the tester to load scenes in the simulation
         simulation.add_input("load_scene", "tester/load_scene")
 
-        exec_dataflow(dataflow, temp_dataflow_path)
+        _exec_dataflow(dataflow, temp_dataflow_path)
 
 
 def main():

nodes/navigator/navigator/main.py

@@ -1,4 +1,4 @@
-"""TODO: Add docstring."""
+"""Simple navigator node that steers towards waypoints."""
 
 import pyarrow as pa
 import numpy as np
@@ -8,7 +8,7 @@
 
 
 def main():
-    """TODO: Add docstring."""
+    """Publish Twist2D commands to navigate to waypoints."""
     node = Node()
 
     print("Navigator node started.")

nodes/teleop/README.md

@@ -0,0 +1,40 @@
+# teleop
+
+## Getting started
+
+- Install it with uv:
+
+```bash
+uv venv -p 3.11 --seed
+uv pip install -e .
+```
+
+## Contribution Guide
+
+- Format with [ruff](https://docs.astral.sh/ruff/):
+
+```bash
+uv pip install ruff
+uv run ruff check . --fix
+```
+
+- Lint with ruff:
+
+```bash
+uv run ruff check .
+```
+
+- Test with [pytest](https://github.com/pytest-dev/pytest)
+
+```bash
+uv pip install pytest
+uv run pytest . # Test
+```
+
+## YAML Specification
+
+## Examples
+
+## License
+
+teleop's code are released under the MIT License

nodes/teleop/pyproject.toml

@@ -0,0 +1,26 @@
+[project]
+name = "teleop"
+version = "0.0.0"
+authors = [{ name = "Your Name", email = "email@email.com" }]
+description = "teleop"
+license = { text = "MIT" }
+readme = "README.md"
+requires-python = ">=3.8"
+
+dependencies = [
+    "msgs",
+    "dora-rs >= 0.3.9",
+    "pynput>=1.8.1",
+]
+
+[dependency-groups]
+dev = ["pytest >=8.1.1", "ruff >=0.9.1"]
+
+[project.scripts]
+teleop = "teleop.main:main"
+
+[tool.ruff.lint]
+extend-select = [
+  "D",   # pydocstyle
+  "UP"
+]

nodes/teleop/teleop/__init__.py

@@ -0,0 +1,13 @@
+"""Outputs a 2D twist command based on keyboard teleoperation."""
+
+import os
+
+# Define the path to the README file relative to the package directory
+readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.md")
+
+# Read the content of the README file
+try:
+    with open(readme_path, encoding="utf-8") as f:
+        __doc__ = f.read()
+except FileNotFoundError:
+    __doc__ = "README file not found."

nodes/teleop/teleop/__main__.py

@@ -0,0 +1,6 @@
+#  # noqa: D100
+
+from .main import main
+
+if __name__ == "__main__":
+    main()

nodes/teleop/teleop/main.py

@@ -0,0 +1,106 @@
+"""Keyboard teleoperation node with holonomic control and scene selection."""
+
+from __future__ import annotations
+
+from typing import Final
+
+from dora import Node
+from pynput import keyboard
+
+import msgs
+
+LINEAR_SPEED: Final[float] = 1.2  # m/s
+ANGULAR_SPEED: Final[float] = 1.2  # rad/s
+
+MOVEMENT_KEYS = {
+    keyboard.KeyCode.from_char("w"),
+    keyboard.KeyCode.from_char("a"),
+    keyboard.KeyCode.from_char("s"),
+    keyboard.KeyCode.from_char("d"),
+    keyboard.KeyCode.from_char("q"),
+    keyboard.KeyCode.from_char("e"),
+}
+
+KEY_NEXT_SCENE = keyboard.KeyCode.from_char("f")
+KEY_RELOAD_SCENE = keyboard.KeyCode.from_char("r")
+
+SCENES: Final[list[tuple[str, float]]] = [
+    ("generated_pyramid", 0.7),
+    ("rail_blocks", 1.0),
+    ("stone_stairs", 1.0),
+    ("excavator", 1.0),
+]
+
+
+def get_twist(movement_pressed: set[keyboard.KeyCode | keyboard.Key]) -> msgs.Twist2D:
+    """Calculate twist based on pressed keys."""
+    forward = int(keyboard.KeyCode.from_char("w") in movement_pressed) - int(
+        keyboard.KeyCode.from_char("s") in movement_pressed
+    )
+    strafe = int(keyboard.KeyCode.from_char("a") in movement_pressed) - int(
+        keyboard.KeyCode.from_char("d") in movement_pressed
+    )
+    rotate = int(keyboard.KeyCode.from_char("q") in movement_pressed) - int(
+        keyboard.KeyCode.from_char("e") in movement_pressed
+    )
+    return msgs.Twist2D(
+        linear_x=forward * LINEAR_SPEED,
+        linear_y=strafe * LINEAR_SPEED,
+        angular_z=rotate * ANGULAR_SPEED,
+    )
+
+
+def main() -> None:
+    """Entrypoint wiring the teleop publisher."""
+    node = Node()
+
+    movement_pressed: set[keyboard.KeyCode | keyboard.Key] = set()
+    scene_index = 0
+
+    def publish_scene_info() -> None:
+        name, difficulty = SCENES[scene_index]
+        scene = msgs.SceneInfo(name=name, difficulty=difficulty)
+        node.send_output("load_scene", scene.to_arrow())
+        print(f"[teleop] Loading scene '{name}' (difficulty {difficulty})")
+
+    # Initial scene publish
+    publish_scene_info()
+
+    try:
+        with keyboard.Events() as events:
+            while True:
+                # Check for dora events with a small timeout to keep the loop spinning
+                dora_event = node.next(timeout=0.01)
+
+                if dora_event is not None:
+                    if dora_event["type"] == "INPUT":
+                        if dora_event["id"] == "tick":
+                            twist = get_twist(movement_pressed)
+                            node.send_output("command_2d", twist.to_arrow())
+                        elif dora_event["id"] == "stop":
+                            break
+
+                # Drain all pending keyboard events
+                while True:
+                    key_event = events.get(0.0)
+                    if key_event is None:
+                        break
+
+                    if isinstance(key_event, keyboard.Events.Press):
+                        key = key_event.key
+                        if key in MOVEMENT_KEYS:
+                            movement_pressed.add(key)
+                        elif key == KEY_NEXT_SCENE:
+                            scene_index = (scene_index + 1) % len(SCENES)
+                            publish_scene_info()
+                        elif key == KEY_RELOAD_SCENE:
+                            publish_scene_info()
+
+                    elif isinstance(key_event, keyboard.Events.Release):
+                        key = key_event.key
+                        if key in MOVEMENT_KEYS:
+                            movement_pressed.discard(key)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.send_output("command_2d", msgs.Twist2D().to_arrow())