diff --git a/dimos/msgs/geometry_msgs/Twist.py b/dimos/msgs/geometry_msgs/Twist.py index b1320bf986..be5d9a34a0 100644 --- a/dimos/msgs/geometry_msgs/Twist.py +++ b/dimos/msgs/geometry_msgs/Twist.py @@ -88,6 +88,24 @@ def is_zero(self) -> bool: """Check if this is a zero twist (no linear or angular velocity).""" return self.linear.is_zero() and self.angular.is_zero() + def __sub__(self, other: Twist) -> Twist: + """Component-wise subtraction: self - other.""" + if not isinstance(other, Twist): + return NotImplemented + return Twist( + linear=self.linear - other.linear, + angular=self.angular - other.angular, + ) + + def __add__(self, other: Twist) -> Twist: + """Component-wise addition: self + other.""" + if not isinstance(other, Twist): + return NotImplemented + return Twist( + linear=self.linear + other.linear, + angular=self.angular + other.angular, + ) + def __bool__(self) -> bool: """Boolean conversion for Twist. diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 4f1d488eca..14e45b230d 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -16,11 +16,11 @@ # Run `pytest dimos/robot/test_all_blueprints_generation.py` to regenerate. all_blueprints = { - "arm-teleop": "dimos.teleop.blueprints:arm_teleop", - "arm-teleop-dual": "dimos.teleop.blueprints:arm_teleop_dual", - "arm-teleop-piper": "dimos.teleop.blueprints:arm_teleop_piper", - "arm-teleop-visualizing": "dimos.teleop.blueprints:arm_teleop_visualizing", - "arm-teleop-xarm6": "dimos.teleop.blueprints:arm_teleop_xarm6", + "arm-teleop": "dimos.teleop.quest.blueprints:arm_teleop", + "arm-teleop-dual": "dimos.teleop.quest.blueprints:arm_teleop_dual", + "arm-teleop-piper": "dimos.teleop.quest.blueprints:arm_teleop_piper", + "arm-teleop-visualizing": "dimos.teleop.quest.blueprints:arm_teleop_visualizing", + "arm-teleop-xarm6": "dimos.teleop.quest.blueprints:arm_teleop_xarm6", "coordinator-basic": "dimos.control.blueprints:coordinator_basic", "coordinator-cartesian-ik-mock": "dimos.control.blueprints:coordinator_cartesian_ik_mock", "coordinator-cartesian-ik-piper": "dimos.control.blueprints:coordinator_cartesian_ik_piper", @@ -47,6 +47,8 @@ "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "dual-xarm6-planner": "dimos.manipulation.manipulation_blueprints:dual_xarm6_planner", + "phone-go2-teleop": "dimos.teleop.phone.blueprints:phone_go2_teleop", + "simple-phone-teleop": "dimos.teleop.phone.blueprints:simple_phone_teleop", "uintree-g1-primitive-no-nav": "dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav:uintree_g1_primitive_no_nav", "unitree-g1": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree.g1.blueprints.agentic.unitree_g1_agentic:unitree_g1_agentic", @@ -107,11 +109,13 @@ "osm_skill": "dimos.agents.skills.osm", "person_follow_skill": "dimos.agents.skills.person_follow", "person_tracker_module": "dimos.perception.detection.person_tracker", + "phone_teleop_module": "dimos.teleop.phone.phone_teleop_module", "quest_teleop_module": "dimos.teleop.quest.quest_teleop_module", "realsense_camera": "dimos.hardware.sensors.camera.realsense.camera", "replanning_a_star_planner": "dimos.navigation.replanning_a_star.module", "rerun_bridge": "dimos.visualization.rerun.bridge", "ros_nav": "dimos.navigation.rosnav", + "simple_phone_teleop_module": "dimos.teleop.phone.phone_extensions", "simulation": "dimos.simulation.manipulators.sim_module", "spatial_memory": "dimos.perception.spatial_perception", "speak_skill": "dimos.agents.skills.speak_skill", diff --git a/dimos/teleop/README.md b/dimos/teleop/README.md index 3c29790cc5..e64a7b43ea 100644 --- a/dimos/teleop/README.md +++ b/dimos/teleop/README.md @@ -56,8 +56,6 @@ Adds Rerun visualization for debugging. Extends ArmTeleopModule (toggle engage). ``` teleop/ -├── base/ -│ └── teleop_protocol.py # TeleopProtocol interface ├── quest/ │ ├── quest_teleop_module.py # Base Quest teleop module │ ├── quest_extensions.py # ArmTeleop, TwistTeleop, VisualizingTeleop @@ -65,6 +63,13 @@ teleop/ │ └── web/ # Deno bridge + WebXR client │ ├── teleop_server.ts │ └── static/index.html +├── phone/ +│ ├── phone_teleop_module.py # Base Phone teleop module +│ ├── phone_extensions.py # SimplePhoneTeleop +│ ├── blueprints.py # Pre-wired configurations +│ └── web/ # Deno bridge + mobile web app +│ ├── teleop_server.ts +│ └── static/index.html ├── utils/ │ ├── teleop_transforms.py # WebXR → robot frame math │ └── teleop_visualization.py # Rerun visualization helpers diff --git a/dimos/teleop/__init__.py b/dimos/teleop/__init__.py index a8c3c0b21a..8324113111 100644 --- a/dimos/teleop/__init__.py +++ b/dimos/teleop/__init__.py @@ -13,7 +13,3 @@ # limitations under the License. """Teleoperation modules for DimOS.""" - -from dimos.teleop.base import TeleopProtocol - -__all__ = ["TeleopProtocol"] diff --git a/dimos/teleop/base/teleop_protocol.py b/dimos/teleop/base/teleop_protocol.py deleted file mode 100644 index 9e1647d64d..0000000000 --- a/dimos/teleop/base/teleop_protocol.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Teleoperation specifications: Protocol. - -Defines the interface that all teleoperation modules must implement. -No implementation - just method signatures. -""" - -from typing import Any, Protocol, runtime_checkable - -# ============================================================================ -# TELEOP PROTOCOL -# ============================================================================ - - -@runtime_checkable -class TeleopProtocol(Protocol): - """Protocol defining the teleoperation interface. - - All teleop modules (Quest, keyboard, joystick, etc.) should implement these methods. - No state or implementation here - just the contract. - """ - - # --- Lifecycle --- - - def start(self) -> None: - """Start the teleoperation module.""" - ... - - def stop(self) -> None: - """Stop the teleoperation module.""" - ... - - # --- Engage / Disengage --- - - def engage(self, hand: Any = None) -> bool: - """Engage teleoperation. Hand type is device-specific (e.g., Hand enum for Quest).""" - ... - - def disengage(self, hand: Any = None) -> None: - """Disengage teleoperation. Hand type is device-specific.""" - ... - - -__all__ = ["TeleopProtocol"] diff --git a/dimos/teleop/phone/README.md b/dimos/teleop/phone/README.md new file mode 100644 index 0000000000..dd2af02281 --- /dev/null +++ b/dimos/teleop/phone/README.md @@ -0,0 +1,70 @@ +# Phone Teleop + +Teleoperation via smartphone motion sensors. Tilt to drive. + +## Architecture + +``` +Phone Browser (DeviceOrientation + DeviceMotion) + | + | TwistStamped + Bool via WebSocket + v +Deno Bridge (teleop_server.ts) + | + | LCM topics + v +PhoneTeleopModule + | Orientation delta from home pose + | Gains -> velocity commands + v +TwistStamped / Twist outputs +``` + +## Modules + +### PhoneTeleopModule +Base module. Receives raw sensor data and button state. On engage (button hold), captures home orientation and publishes deltas as TwistStamped. Launches the Deno bridge server automatically. + +### SimplePhoneTeleop +Filters to mobile-base axes (linear.x, linear.y, angular.z) and publishes as `Twist` on `cmd_vel` for direct autoconnect wiring with any module that has `cmd_vel: In[Twist]`. + +## Subclassing + +Override these methods: + +| Method | Purpose | +|--------|---------| +| `_handle_engage()` | Customize engage/disengage logic | +| `_should_publish()` | Add conditions for when to publish | +| `_publish_msg()` | Change output format | + +**Do not acquire `self._lock` in overrides.** The control loop already holds it. + +## LCM Topics + +| Topic | Type | Description | +|-------|------|-------------| +| `/phone_sensors` | TwistStamped | linear=(roll,pitch,yaw) deg, angular=(gyro) deg/s | +| `/phone_button` | Bool | Teleop engage button (1=held) | +| `/teleop/twist` | TwistStamped | Output velocity command | + +## Running + +```bash +dimos run phone-go2-teleop # Go2 +dimos run simple-phone-teleop # Generic ground robot +``` + +Server starts on port `8444`. Open `https://:8444` on phone, accept the self-signed certificate, allow sensor permissions, connect, hold button to drive. + +## File Structure + +``` +phone/ +├── phone_teleop_module.py # Base phone teleop module +├── phone_extensions.py # SimplePhoneTeleop +├── blueprints.py # Pre-wired configurations +└── web/ + ├── teleop_server.ts # Deno WSS-to-LCM bridge + └── static/index.html # Mobile web app +``` diff --git a/dimos/teleop/base/__init__.py b/dimos/teleop/phone/__init__.py similarity index 57% rename from dimos/teleop/base/__init__.py rename to dimos/teleop/phone/__init__.py index cf3b18d597..552032a47b 100644 --- a/dimos/teleop/base/__init__.py +++ b/dimos/teleop/phone/__init__.py @@ -12,8 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Teleoperation protocol.""" +"""Phone teleoperation module for DimOS.""" -from dimos.teleop.base.teleop_protocol import TeleopProtocol +from dimos.teleop.phone.phone_extensions import ( + SimplePhoneTeleop, + simple_phone_teleop_module, +) +from dimos.teleop.phone.phone_teleop_module import ( + PhoneTeleopConfig, + PhoneTeleopModule, + phone_teleop_module, +) -__all__ = ["TeleopProtocol"] +__all__ = [ + "PhoneTeleopConfig", + "PhoneTeleopModule", + "SimplePhoneTeleop", + "phone_teleop_module", + "simple_phone_teleop_module", +] diff --git a/dimos/teleop/phone/blueprints.py b/dimos/teleop/phone/blueprints.py new file mode 100644 index 0000000000..6328af8612 --- /dev/null +++ b/dimos/teleop/phone/blueprints.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos.core.blueprints import autoconnect +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic +from dimos.teleop.phone.phone_extensions import simple_phone_teleop_module + +# Simple phone teleop (mobile base axis filtering + cmd_vel output) +simple_phone_teleop = autoconnect( + simple_phone_teleop_module(), +) + +# Phone teleop wired to Unitree Go2 +phone_go2_teleop = autoconnect( + simple_phone_teleop_module(), + unitree_go2_basic, +) + + +__all__ = ["phone_go2_teleop", "simple_phone_teleop"] diff --git a/dimos/teleop/phone/phone_extensions.py b/dimos/teleop/phone/phone_extensions.py new file mode 100644 index 0000000000..f0a8fd4d01 --- /dev/null +++ b/dimos/teleop/phone/phone_extensions.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Phone teleop module extensions. + +Available subclasses: + - SimplePhoneTeleop: Filters to ground robot axes and outputs cmd_vel: Out[Twist] +""" + +from dimos.core import Out +from dimos.msgs.geometry_msgs import Twist, TwistStamped, Vector3 +from dimos.teleop.phone.phone_teleop_module import PhoneTeleopModule + + +class SimplePhoneTeleop(PhoneTeleopModule): + """Phone teleop for ground robots. + + Filters the raw 6-axis twist to mobile base axes (linear.x, linear.y, angular.z) + and publishes as Twist on cmd_vel for direct autoconnect wiring with any + module that has cmd_vel: In[Twist]. + """ + + cmd_vel: Out[Twist] + + def _publish_msg(self, output_msg: TwistStamped) -> None: + self.cmd_vel.publish( + Twist( + linear=Vector3(x=output_msg.linear.x, y=output_msg.linear.y, z=0.0), + angular=Vector3(x=0.0, y=0.0, z=output_msg.linear.z), + ) + ) + + +simple_phone_teleop_module = SimplePhoneTeleop.blueprint + +__all__ = [ + "SimplePhoneTeleop", + "simple_phone_teleop_module", +] diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py new file mode 100644 index 0000000000..c0da85c27c --- /dev/null +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -0,0 +1,308 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Phone Teleoperation Module. + +Receives raw sensor data (TwistStamped) and button state (Bool) from the +phone web app via the Deno LCM bridge. Computes orientation deltas from +a initial orientation captured on engage, converts to TwistStamped velocity +commands via configurable gains, and publishes. + +""" + +from dataclasses import dataclass +from pathlib import Path +import shutil +import signal +import subprocess +import threading +import time +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.core import In, Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.msgs.geometry_msgs import Twist, TwistStamped, Vector3 +from dimos.msgs.std_msgs.Bool import Bool +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +@dataclass +class PhoneTeleopConfig(ModuleConfig): + control_loop_hz: float = 50.0 + linear_gain: float = 1.0 / 30.0 # Gain: maps degrees of tilt to m/s. 30 deg -> 1.0 m/s + angular_gain: float = 1.0 / 30.0 # Gain: maps gyro deg/s to rad/s. 30 deg/s -> 1.0 rad/s + + +class PhoneTeleopModule(Module[PhoneTeleopConfig]): + """ + Receives raw sensor data from the phone web app: + - TwistStamped: linear=(roll, pitch, yaw) deg, angular=(gyro) deg/s + - Bool: teleop button state (True = held) + + Outputs: + - twist_output: TwistStamped (velocity command for robot) + """ + + default_config = PhoneTeleopConfig + + # Inputs from Deno bridge + phone_sensors: In[TwistStamped] + phone_button: In[Bool] + # Output: velocity command to robot + twist_output: Out[TwistStamped] + + # ------------------------------------------------------------------------- + # Initialization + # ------------------------------------------------------------------------- + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + + self._is_engaged: bool = False + self._teleop_button: bool = False + self._current_sensors: TwistStamped | None = None + self._initial_sensors: TwistStamped | None = None + self._lock = threading.RLock() + + # Control loop + self._control_loop_thread: threading.Thread | None = None + self._stop_event = threading.Event() + + # Deno bridge server + self._server_process: subprocess.Popen[bytes] | None = None + self._server_script = Path(__file__).parent / "web" / "teleop_server.ts" + + # ------------------------------------------------------------------------- + # Lifecycle + # ------------------------------------------------------------------------- + + @rpc + def start(self) -> None: + super().start() + for stream, handler in ( + (self.phone_sensors, self._on_sensors), + (self.phone_button, self._on_button), + ): + self._disposables.add(Disposable(stream.subscribe(handler))) # type: ignore[attr-defined] + self._start_server() + self._start_control_loop() + + @rpc + def stop(self) -> None: + self._stop_control_loop() + self._stop_server() + super().stop() + + # ------------------------------------------------------------------------- + # Internal engage / disengage (assumes lock is held) + # ------------------------------------------------------------------------- + + def _engage(self) -> bool: + """Engage: capture current sensors as initial""" + if self._current_sensors is None: + logger.error("Engage failed: no sensor data yet") + return False + self._initial_sensors = self._current_sensors + self._is_engaged = True + logger.info("Phone teleop engaged") + return True + + def _disengage(self) -> None: + """Disengage: stop publishing""" + self._is_engaged = False + self._initial_sensors = None + logger.info("Phone teleop disengaged") + + # ------------------------------------------------------------------------- + # Callbacks + # ------------------------------------------------------------------------- + + def _on_sensors(self, msg: TwistStamped) -> None: + """Callback for raw sensor TwistStamped from the phone""" + with self._lock: + self._current_sensors = msg + + def _on_button(self, msg: Bool) -> None: + """Callback for teleop button state.""" + with self._lock: + self._teleop_button = bool(msg.data) + + # ------------------------------------------------------------------------- + # Deno Bridge Server + # ------------------------------------------------------------------------- + + def _start_server(self) -> None: + """Launch the Deno WebSocket-to-LCM bridge server as a subprocess.""" + if self._server_process is not None and self._server_process.poll() is None: + logger.warning("Deno bridge already running", pid=self._server_process.pid) + return + + if shutil.which("deno") is None: + logger.error( + "Deno is not installed. Install it with: curl -fsSL https://deno.land/install.sh | sh" + ) + return + + script = str(self._server_script) + cmd = [ + "deno", + "run", + "--allow-net", + "--allow-read", + "--allow-run", + "--allow-write", + "--unstable-net", + script, + ] + try: + self._server_process = subprocess.Popen(cmd) + logger.info(f"Deno bridge server started (pid {self._server_process.pid})") + except OSError as e: + logger.error(f"Failed to start Deno bridge: {e}") + + def _stop_server(self) -> None: + """Terminate the Deno bridge server subprocess.""" + if self._server_process is None or self._server_process.poll() is not None: + self._server_process = None + return + + logger.info("Stopping Deno bridge server", pid=self._server_process.pid) + self._server_process.send_signal(signal.SIGTERM) + try: + self._server_process.wait(timeout=3) + except subprocess.TimeoutExpired: + logger.warning( + "Deno bridge did not exit, sending SIGKILL", pid=self._server_process.pid + ) + self._server_process.kill() + try: + self._server_process.wait(timeout=5) + except subprocess.TimeoutExpired: + logger.error("Deno bridge did not exit after SIGKILL") + logger.info("Deno bridge server stopped") + self._server_process = None + + # ------------------------------------------------------------------------- + # Control Loop + # ------------------------------------------------------------------------- + + def _start_control_loop(self) -> None: + if self._control_loop_thread is not None and self._control_loop_thread.is_alive(): + return + + self._stop_event.clear() + self._control_loop_thread = threading.Thread( + target=self._control_loop, + daemon=True, + name="PhoneTeleopControlLoop", + ) + self._control_loop_thread.start() + logger.info(f"Control loop started at {self.config.control_loop_hz} Hz") + + def _stop_control_loop(self) -> None: + self._stop_event.set() + if self._control_loop_thread is not None: + self._control_loop_thread.join(timeout=1.0) + self._control_loop_thread = None + logger.info("Control loop stopped") + + def _control_loop(self) -> None: + period = 1.0 / self.config.control_loop_hz + + while not self._stop_event.is_set(): + loop_start = time.perf_counter() + with self._lock: + self._handle_engage() + + if self._is_engaged: + output_twist = self._get_output_twist() + if output_twist is not None: + self._publish_msg(output_twist) + + elapsed = time.perf_counter() - loop_start + sleep_time = period - elapsed + if sleep_time > 0: + self._stop_event.wait(sleep_time) + + # ------------------------------------------------------------------------- + # Control Loop Internal Methods + # ------------------------------------------------------------------------- + + def _handle_engage(self) -> None: + """ + Override to customize engagement logic. + Default: button hold = engaged, release = disengaged. + """ + if self._teleop_button: + if not self._is_engaged: + self._engage() + else: + if self._is_engaged: + self._disengage() + + def _get_output_twist(self) -> TwistStamped | None: + """Compute twist from orientation delta. + Override to customize twist computation (e.g., apply scaling, filtering). + Default: Computes delta angles from initial orientation, applies gains. + """ + current = self._current_sensors + initial = self._initial_sensors + if current is None or initial is None: + return None + + delta: Twist = Twist(current) - Twist(initial) + + # Handle yaw wraparound (linear.z = yaw, 0-360 degrees) + d_yaw = delta.linear.z + if d_yaw > 180: + d_yaw -= 360 + elif d_yaw < -180: + d_yaw += 360 + + cfg = self.config + return TwistStamped( + ts=current.ts, + frame_id="phone", + linear=Vector3( + x=-delta.linear.y * cfg.linear_gain, # pitch forward -> drive forward + y=-delta.linear.x * cfg.linear_gain, # roll right -> strafe right + z=d_yaw * cfg.linear_gain, # yaw delta + ), + angular=Vector3( + x=current.angular.x * cfg.angular_gain, + y=current.angular.y * cfg.angular_gain, + z=current.angular.z * cfg.angular_gain, + ), + ) + + def _publish_msg(self, output_msg: TwistStamped) -> None: + """ + Override to customize output (e.g., apply limits, remap axes). + """ + self.twist_output.publish(output_msg) + + +phone_teleop_module = PhoneTeleopModule.blueprint + +__all__ = [ + "PhoneTeleopConfig", + "PhoneTeleopModule", + "phone_teleop_module", +] diff --git a/dimos/teleop/phone/web/static/index.html b/dimos/teleop/phone/web/static/index.html new file mode 100644 index 0000000000..6fad23b6c8 --- /dev/null +++ b/dimos/teleop/phone/web/static/index.html @@ -0,0 +1,393 @@ + + + + + + DimOS Phone Teleop + + + +

DimOS Phone Teleop

+ + +
+ Sensors: off + WS: disconnected +
+ + +
+ + +
+ + +
+

Sensors

+
+
+
X
+
Y
+
Z
+ +
Ori
+
0.0°
+
0.0°
+
0.0°
+ +
Gyro
+
0.0
+
0.0
+
0.0
+
+
+ + + + + + + diff --git a/dimos/teleop/phone/web/teleop_server.ts b/dimos/teleop/phone/web/teleop_server.ts new file mode 100755 index 0000000000..26202cf166 --- /dev/null +++ b/dimos/teleop/phone/web/teleop_server.ts @@ -0,0 +1,85 @@ +#!/usr/bin/env -S deno run --allow-net --allow-read --allow-run --allow-write --unstable-net + +// WebSocket to LCM Bridge for Phone Teleop +// Forwards twist data from Phone browser to LCM + +import { LCM } from "jsr:@dimos/lcm"; +import { dirname, fromFileUrl, join } from "jsr:@std/path"; + +const PORT = 8444; + +// Resolve paths relative to script location +const scriptDir = dirname(fromFileUrl(import.meta.url)); +const certsDir = join(scriptDir, "../../../../assets/teleop_certs"); +const certPath = join(certsDir, "cert.pem"); +const keyPath = join(certsDir, "key.pem"); + +// Auto-generate self-signed certificates if they don't exist +async function ensureCerts(): Promise<{ cert: string; key: string }> { + try { + const cert = await Deno.readTextFile(certPath); + const key = await Deno.readTextFile(keyPath); + return { cert, key }; + } catch { + console.log("Generating self-signed certificates..."); + await Deno.mkdir(certsDir, { recursive: true }); + const cmd = new Deno.Command("openssl", { + args: [ + "req", "-x509", "-newkey", "rsa:2048", + "-keyout", keyPath, "-out", certPath, + "-days", "365", "-nodes", "-subj", "/CN=localhost" + ], + }); + const { code } = await cmd.output(); + if (code !== 0) { + throw new Error("Failed to generate certificates. Is openssl installed?"); + } + console.log("Certificates generated in assets/teleop_certs/"); + return { + cert: await Deno.readTextFile(certPath), + key: await Deno.readTextFile(keyPath), + }; + } +} + +const { cert, key } = await ensureCerts(); + +const lcm = new LCM(); +await lcm.start(); + +// Binds to all interfaces so the phone can reach the server over LAN. +Deno.serve({ port: PORT, cert, key }, async (req) => { + const url = new URL(req.url); + + if (req.headers.get("upgrade") === "websocket") { + const { socket, response } = Deno.upgradeWebSocket(req); + socket.onopen = () => console.log("Phone client connected"); + socket.onclose = () => console.log("Phone client disconnected"); + + // Forward binary LCM packets from browser directly to UDP + socket.binaryType = "arraybuffer"; + socket.onmessage = async (event) => { + if (event.data instanceof ArrayBuffer) { + const packet = new Uint8Array(event.data); + try { + await lcm.publishPacket(packet); + } catch (e) { + console.error("Forward error:", e); + } + } + }; + + return response; + } + + if (url.pathname === "/" || url.pathname === "/index.html") { + const html = await Deno.readTextFile(new URL("./static/index.html", import.meta.url)); + return new Response(html, { headers: { "content-type": "text/html" } }); + } + + return new Response("Not found", { status: 404 }); +}); + +console.log(`Phone Teleop Server: https://localhost:${PORT}`); + +await lcm.run(); diff --git a/dimos/teleop/blueprints.py b/dimos/teleop/quest/blueprints.py similarity index 100% rename from dimos/teleop/blueprints.py rename to dimos/teleop/quest/blueprints.py diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index d7525e50ad..ea77bb5fc0 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -22,6 +22,10 @@ from dataclasses import dataclass from enum import IntEnum +from pathlib import Path +import shutil +import signal +import subprocess import threading import time from typing import Any @@ -32,7 +36,6 @@ from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.sensor_msgs import Joy -from dimos.teleop.base import TeleopProtocol from dimos.teleop.quest.quest_types import Buttons, QuestControllerState from dimos.teleop.utils.teleop_transforms import webxr_to_robot from dimos.utils.logging_config import setup_logger @@ -65,14 +68,12 @@ class QuestTeleopConfig(ModuleConfig): control_loop_hz: float = 50.0 -class QuestTeleopModule(Module[QuestTeleopConfig], TeleopProtocol): +class QuestTeleopModule(Module[QuestTeleopConfig]): """Quest Teleoperation Module for Meta Quest controllers. Gets controller data from Deno bridge, computes output poses, and publishes them. Subclass to customize pose computation, output format, and engage behavior. - Implements TeleopProtocol. - Outputs: - left_controller_output: PoseStamped (output pose for left hand) - right_controller_output: PoseStamped (output pose for right hand) @@ -111,17 +112,18 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Control loop self._control_loop_thread: threading.Thread | None = None - self._control_loop_running = False + self._stop_event = threading.Event() - logger.info("QuestTeleopModule initialized") + # Deno bridge server + self._server_process: subprocess.Popen[bytes] | None = None + self._server_script = Path(__file__).parent / "web" / "teleop_server.ts" # ------------------------------------------------------------------------- - # Public RPC Methods + # Lifecycle # ------------------------------------------------------------------------- @rpc def start(self) -> None: - """Start the Quest teleoperation module.""" super().start() input_streams = { @@ -141,28 +143,15 @@ def start(self) -> None: if connected: logger.info(f"Subscribed to: {', '.join(connected)}") - self._start_control_loop() + self._start_server() logger.info("Quest Teleoperation Module started") @rpc def stop(self) -> None: - """Stop the Quest teleoperation module.""" - logger.info("Stopping Quest Teleoperation Module...") self._stop_control_loop() + self._stop_server() super().stop() - @rpc - def engage(self, hand: Hand | None = None) -> bool: - """Engage teleoperation for a hand. If hand is None, engage both.""" - with self._lock: - return self._engage(hand) - - @rpc - def disengage(self, hand: Hand | None = None) -> None: - """Disengage teleoperation for a hand. If hand is None, disengage both.""" - with self._lock: - self._disengage(hand) - # ------------------------------------------------------------------------- # Internal engage/disengage (assumes lock is held) # ------------------------------------------------------------------------- @@ -187,9 +176,7 @@ def _disengage(self, hand: Hand | None = None) -> None: self._is_engaged[h] = False logger.info(f"{h.name} disengaged.") - @rpc def get_status(self) -> QuestTeleopStatus: - """Get current teleoperation status.""" with self._lock: left = self._controllers.get(Hand.LEFT) right = self._controllers.get(Hand.RIGHT) @@ -225,12 +212,67 @@ def _on_joy(self, hand: Hand, joy: Joy) -> None: with self._lock: self._controllers[hand] = controller + # ------------------------------------------------------------------------- + # Deno Bridge Server + # ------------------------------------------------------------------------- + + def _start_server(self) -> None: + """Launch the Deno WebSocket-to-LCM bridge server as a subprocess.""" + if self._server_process is not None and self._server_process.poll() is None: + logger.warning("Deno bridge already running", pid=self._server_process.pid) + return + + if shutil.which("deno") is None: + logger.error( + "Deno is not installed. Install it with: curl -fsSL https://deno.land/install.sh | sh" + ) + return + + script = str(self._server_script) + cmd = [ + "deno", + "run", + "--allow-net", + "--allow-read", + "--allow-run", + "--allow-write", + "--unstable-net", + script, + ] + try: + self._server_process = subprocess.Popen(cmd) + logger.info(f"Deno bridge server started (pid {self._server_process.pid})") + except OSError as e: + logger.error(f"Failed to start Deno bridge: {e}") + + def _stop_server(self) -> None: + """Terminate the Deno bridge server subprocess.""" + if self._server_process is None or self._server_process.poll() is not None: + self._server_process = None + return + + logger.info("Stopping Deno bridge server", pid=self._server_process.pid) + self._server_process.send_signal(signal.SIGTERM) + try: + self._server_process.wait(timeout=3) + except subprocess.TimeoutExpired: + logger.warning( + "Deno bridge did not exit, sending SIGKILL", pid=self._server_process.pid + ) + self._server_process.kill() + try: + self._server_process.wait(timeout=5) + except subprocess.TimeoutExpired: + logger.error("Deno bridge did not exit after SIGKILL") + logger.info("Deno bridge server stopped") + self._server_process = None + def _start_control_loop(self) -> None: """Start the control loop thread.""" - if self._control_loop_running: + if self._control_loop_thread is not None and self._control_loop_thread.is_alive(): return - self._control_loop_running = True + self._stop_event.clear() self._control_loop_thread = threading.Thread( target=self._control_loop, daemon=True, @@ -241,21 +283,20 @@ def _start_control_loop(self) -> None: def _stop_control_loop(self) -> None: """Stop the control loop thread.""" - self._control_loop_running = False + self._stop_event.set() if self._control_loop_thread is not None: self._control_loop_thread.join(timeout=1.0) self._control_loop_thread = None logger.info("Control loop stopped") def _control_loop(self) -> None: - """Main control loop: compute deltas and publish at fixed rate. - + """ Holds self._lock for the entire iteration so overridable methods don't need to acquire it themselves. """ period = 1.0 / self.config.control_loop_hz - while self._control_loop_running: + while not self._stop_event.is_set(): loop_start = time.perf_counter() try: with self._lock: @@ -279,7 +320,7 @@ def _control_loop(self) -> None: elapsed = time.perf_counter() - loop_start sleep_time = period - elapsed if sleep_time > 0: - time.sleep(sleep_time) + self._stop_event.wait(sleep_time) # ------------------------------------------------------------------------- # Control Loop Internals