diff --git a/dimos/robot/unitree/g1/blueprints/unitree_g1_blueprints.py b/dimos/robot/unitree/g1/blueprints/unitree_g1_blueprints.py new file mode 100644 index 000000000..f07a69b98 --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/unitree_g1_blueprints.py @@ -0,0 +1,269 @@ +#!/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. + +"""Blueprint configurations for Unitree G1 humanoid robot. + +This module provides pre-configured blueprints for various G1 robot setups, +from basic teleoperation to full autonomous agent configurations. +""" + +from dimos_lcm.foxglove_msgs import SceneUpdate +from dimos_lcm.foxglove_msgs.ImageAnnotations import ( + ImageAnnotations, +) +from dimos_lcm.sensor_msgs import CameraInfo + +from dimos.agents.agent import llm_agent +from dimos.agents.cli.human import human_input +from dimos.agents.skills.navigation import navigation_skill +from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport, pSHMTransport +from dimos.hardware.sensors.camera import zed +from dimos.hardware.sensors.camera.module import camera_module # type: ignore[attr-defined] +from dimos.hardware.sensors.camera.webcam import Webcam +from dimos.mapping.costmapper import cost_mapper +from dimos.mapping.voxels import voxel_mapper +from dimos.msgs.geometry_msgs import ( + PoseStamped, + Quaternion, + Transform, + Twist, + Vector3, +) +from dimos.msgs.nav_msgs import Odometry, Path +from dimos.msgs.sensor_msgs import Image, PointCloud2 +from dimos.msgs.std_msgs import Bool +from dimos.msgs.vision_msgs import Detection2DArray +from dimos.navigation.frontier_exploration import wavefront_frontier_explorer +from dimos.navigation.replanning_a_star.module import replanning_a_star_planner +from dimos.navigation.rosnav import ros_nav +from dimos.perception.detection.detectors.person.yolo import YoloPersonDetector +from dimos.perception.detection.module3D import Detection3DModule, detection3d_module +from dimos.perception.detection.moduleDB import ObjectDBModule, detectionDB_module +from dimos.perception.detection.person_tracker import PersonTracker, person_tracker_module +from dimos.perception.object_tracker import object_tracking +from dimos.perception.spatial_perception import spatial_memory +from dimos.robot.doom_teleop import doom_teleop +from dimos.robot.foxglove_bridge import foxglove_bridge +from dimos.robot.unitree.connection.g1 import g1_connection +from dimos.robot.unitree.connection.g1sim import g1_sim_connection +from dimos.robot.unitree_webrtc.keyboard_teleop import keyboard_teleop +from dimos.robot.unitree_webrtc.unitree_g1_skill_container import g1_skills +from dimos.utils.monitoring import utilization +from dimos.web.websocket_vis.websocket_vis_module import websocket_vis + +_basic_no_nav = ( + autoconnect( + camera_module( + transform=Transform( + translation=Vector3(0.05, 0.0, 0.0), + rotation=Quaternion.from_euler(Vector3(0.0, 0.2, 0.0)), + frame_id="sensor", + child_frame_id="camera_link", + ), + hardware=lambda: Webcam( + camera_index=0, + fps=15, + stereo_slice="left", + camera_info=zed.CameraInfo.SingleWebcam, + ), + ), + voxel_mapper(voxel_size=0.1), + cost_mapper(), + wavefront_frontier_explorer(), + # Visualization + websocket_vis(), + foxglove_bridge(), + ) + .global_config(n_dask_workers=4, robot_model="unitree_g1") + .transports( + { + # G1 uses Twist for movement commands + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + # State estimation from ROS + ("state_estimation", Odometry): LCMTransport("/state_estimation", Odometry), + # Odometry output from ROSNavigationModule + ("odom", PoseStamped): LCMTransport("/odom", PoseStamped), + # Navigation module topics from nav_bot + ("goal_req", PoseStamped): LCMTransport("/goal_req", PoseStamped), + ("goal_active", PoseStamped): LCMTransport("/goal_active", PoseStamped), + ("path_active", Path): LCMTransport("/path_active", Path), + ("pointcloud", PointCloud2): LCMTransport("/lidar", PointCloud2), + ("global_pointcloud", PointCloud2): LCMTransport("/map", PointCloud2), + # Original navigation topics for backwards compatibility + ("goal_pose", PoseStamped): LCMTransport("/goal_pose", PoseStamped), + ("goal_reached", Bool): LCMTransport("/goal_reached", Bool), + ("cancel_goal", Bool): LCMTransport("/cancel_goal", Bool), + # Camera topics (if camera module is added) + ("color_image", Image): LCMTransport("/g1/color_image", Image), + ("camera_info", CameraInfo): LCMTransport("/g1/camera_info", CameraInfo), + } + ) +) + +basic_ros = autoconnect( + _basic_no_nav, + g1_connection(), + ros_nav(), +) + +basic_sim = autoconnect( + _basic_no_nav, + g1_sim_connection(), + replanning_a_star_planner(), +) + +_perception_and_memory = autoconnect( + spatial_memory(), + object_tracking(frame_id="camera_link"), + utilization(), +) + +standard = autoconnect( + basic_ros, + _perception_and_memory, +).global_config(n_dask_workers=8) + +standard_sim = autoconnect( + basic_sim, + _perception_and_memory, +).global_config(n_dask_workers=8) + +# Optimized configuration using shared memory for images +standard_with_shm = autoconnect( + standard.transports( + { + ("color_image", Image): pSHMTransport( + "/g1/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE + ), + } + ), + foxglove_bridge( + shm_channels=[ + "/g1/color_image#sensor_msgs.Image", + ] + ), +) + +_agentic_skills = autoconnect( + llm_agent(), + human_input(), + navigation_skill(), + g1_skills(), +) + +# Full agentic configuration with LLM and skills +agentic = autoconnect( + standard, + _agentic_skills, +) + +agentic_sim = autoconnect( + standard_sim, + _agentic_skills, +) + +# Configuration with joystick/DOOM-style control for teleoperation +with_joystick = autoconnect( + basic_ros, + doom_teleop(), # Doom-style keyboard + mouse teleop (cmd_vel / goal_pose / cancel_goal) +) + +# Detection configuration with person tracking and 3D detection +detection = ( + autoconnect( + basic_ros, + # Person detection modules with YOLO + detection3d_module( + camera_info=zed.CameraInfo.SingleWebcam, + detector=YoloPersonDetector, + ), + detectionDB_module( + camera_info=zed.CameraInfo.SingleWebcam, + filter=lambda det: det.class_id == 0, # Filter for person class only + ), + person_tracker_module( + cameraInfo=zed.CameraInfo.SingleWebcam, + ), + ) + .global_config(n_dask_workers=8) + .remappings( + [ + # Connect detection modules to camera and lidar + (Detection3DModule, "image", "color_image"), + (Detection3DModule, "pointcloud", "pointcloud"), + (ObjectDBModule, "image", "color_image"), + (ObjectDBModule, "pointcloud", "pointcloud"), + (PersonTracker, "image", "color_image"), + (PersonTracker, "detections", "detections_2d"), + ] + ) + .transports( + { + # Detection 3D module outputs + ("detections", Detection3DModule): LCMTransport( + "/detector3d/detections", Detection2DArray + ), + ("annotations", Detection3DModule): LCMTransport( + "/detector3d/annotations", ImageAnnotations + ), + ("scene_update", Detection3DModule): LCMTransport( + "/detector3d/scene_update", SceneUpdate + ), + ("detected_pointcloud_0", Detection3DModule): LCMTransport( + "/detector3d/pointcloud/0", PointCloud2 + ), + ("detected_pointcloud_1", Detection3DModule): LCMTransport( + "/detector3d/pointcloud/1", PointCloud2 + ), + ("detected_pointcloud_2", Detection3DModule): LCMTransport( + "/detector3d/pointcloud/2", PointCloud2 + ), + ("detected_image_0", Detection3DModule): LCMTransport("/detector3d/image/0", Image), + ("detected_image_1", Detection3DModule): LCMTransport("/detector3d/image/1", Image), + ("detected_image_2", Detection3DModule): LCMTransport("/detector3d/image/2", Image), + # Detection DB module outputs + ("detections", ObjectDBModule): LCMTransport( + "/detectorDB/detections", Detection2DArray + ), + ("annotations", ObjectDBModule): LCMTransport( + "/detectorDB/annotations", ImageAnnotations + ), + ("scene_update", ObjectDBModule): LCMTransport("/detectorDB/scene_update", SceneUpdate), + ("detected_pointcloud_0", ObjectDBModule): LCMTransport( + "/detectorDB/pointcloud/0", PointCloud2 + ), + ("detected_pointcloud_1", ObjectDBModule): LCMTransport( + "/detectorDB/pointcloud/1", PointCloud2 + ), + ("detected_pointcloud_2", ObjectDBModule): LCMTransport( + "/detectorDB/pointcloud/2", PointCloud2 + ), + ("detected_image_0", ObjectDBModule): LCMTransport("/detectorDB/image/0", Image), + ("detected_image_1", ObjectDBModule): LCMTransport("/detectorDB/image/1", Image), + ("detected_image_2", ObjectDBModule): LCMTransport("/detectorDB/image/2", Image), + # Person tracker outputs + ("target", PersonTracker): LCMTransport("/person_tracker/target", PoseStamped), + } + ) +) + +# Full featured configuration with everything +full_featured = autoconnect( + standard_with_shm, + _agentic_skills, + keyboard_teleop(), +) diff --git a/dimos/robot/unitree/go2/blueprints/unitree_go2_blueprints.py b/dimos/robot/unitree/go2/blueprints/unitree_go2_blueprints.py new file mode 100644 index 000000000..c22348898 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/unitree_go2_blueprints.py @@ -0,0 +1,240 @@ +#!/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 pathlib import Path +import platform + +from dimos_lcm.foxglove_msgs.ImageAnnotations import ( + ImageAnnotations, # type: ignore[import-untyped] +) +from dimos_lcm.foxglove_msgs.SceneUpdate import SceneUpdate # type: ignore[import-untyped] + +from dimos.agents.agent import llm_agent +from dimos.agents.cli.human import human_input +from dimos.agents.cli.web import web_input +from dimos.agents.ollama_agent import ollama_installed +from dimos.agents.skills.navigation import navigation_skill +from dimos.agents.skills.person_follow import person_follow_skill +from dimos.agents.skills.speak_skill import speak_skill +from dimos.agents.spec import Provider +from dimos.agents.vlm_agent import vlm_agent +from dimos.agents.vlm_stream_tester import vlm_stream_tester +from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE +from dimos.core.blueprints import autoconnect +from dimos.core.transport import ( + JpegLcmTransport, + JpegShmTransport, + LCMTransport, + ROSTransport, + pSHMTransport, +) +from dimos.dashboard.tf_rerun_module import tf_rerun +from dimos.mapping.costmapper import cost_mapper +from dimos.mapping.voxels import voxel_mapper +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.sensor_msgs import Image, PointCloud2 +from dimos.msgs.vision_msgs import Detection2DArray +from dimos.navigation.frontier_exploration import ( + wavefront_frontier_explorer, +) +from dimos.navigation.replanning_a_star.module import ( + replanning_a_star_planner, +) +from dimos.perception.detection.module3D import Detection3DModule, detection3d_module +from dimos.perception.experimental.temporal_memory import temporal_memory +from dimos.perception.spatial_perception import spatial_memory +from dimos.protocol.mcp.mcp import MCPModule +from dimos.robot.doom_teleop import doom_teleop +from dimos.robot.foxglove_bridge import foxglove_bridge +import dimos.robot.unitree.connection.go2 as _go2_mod +from dimos.robot.unitree.connection.go2 import GO2Connection, go2_connection +from dimos.robot.unitree_webrtc.unitree_skill_container import unitree_skills +from dimos.utils.monitoring import utilization +from dimos.web.websocket_vis.websocket_vis_module import websocket_vis + +_GO2_URDF = Path(_go2_mod.__file__).parent.parent / "go2" / "go2.urdf" + +# Mac has some issue with high bandwidth UDP +# +# so we use pSHMTransport for color_image +# (Could we adress this on the system config layer? Is this fixable on mac?) +mac = autoconnect( + foxglove_bridge( + shm_channels=[ + "/color_image#sensor_msgs.Image", + ] + ), +).transports( + { + ("color_image", Image): pSHMTransport( + "color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE + ), + } +) + + +linux = autoconnect(foxglove_bridge()) + +basic = autoconnect( + go2_connection(), + linux if platform.system() == "Linux" else mac, + websocket_vis(), + tf_rerun( + urdf_path=str(_GO2_URDF), + cameras=[ + ("world/robot/camera", "camera_optical", GO2Connection.camera_info_static), + ], + ), +).global_config(n_dask_workers=4, robot_model="unitree_go2") + +go2_with_doom = autoconnect( + basic, + doom_teleop(), # Doom-style keyboard + mouse teleop (cmd_vel / goal_pose / cancel_goal) +) + +nav = autoconnect( + basic, + voxel_mapper(voxel_size=0.1), + cost_mapper(), + replanning_a_star_planner(), + wavefront_frontier_explorer(), +).global_config(n_dask_workers=6, robot_model="unitree_go2") + +ros = nav.transports( + { + ("lidar", PointCloud2): ROSTransport("lidar", PointCloud2), + ("global_map", PointCloud2): ROSTransport("global_map", PointCloud2), + ("odom", PoseStamped): ROSTransport("odom", PoseStamped), + ("color_image", Image): ROSTransport("color_image", Image), + } +) + +detection = ( + autoconnect( + nav, + detection3d_module( + camera_info=GO2Connection.camera_info_static, + ), + ) + .remappings( + [ + (Detection3DModule, "pointcloud", "global_map"), + ] + ) + .transports( + { + # Detection 3D module outputs + ("detections", Detection3DModule): LCMTransport( + "/detector3d/detections", Detection2DArray + ), + ("annotations", Detection3DModule): LCMTransport( + "/detector3d/annotations", ImageAnnotations + ), + ("scene_update", Detection3DModule): LCMTransport( + "/detector3d/scene_update", SceneUpdate + ), + ("detected_pointcloud_0", Detection3DModule): LCMTransport( + "/detector3d/pointcloud/0", PointCloud2 + ), + ("detected_pointcloud_1", Detection3DModule): LCMTransport( + "/detector3d/pointcloud/1", PointCloud2 + ), + ("detected_pointcloud_2", Detection3DModule): LCMTransport( + "/detector3d/pointcloud/2", PointCloud2 + ), + ("detected_image_0", Detection3DModule): LCMTransport("/detector3d/image/0", Image), + ("detected_image_1", Detection3DModule): LCMTransport("/detector3d/image/1", Image), + ("detected_image_2", Detection3DModule): LCMTransport("/detector3d/image/2", Image), + } + ) +) + + +spatial = autoconnect( + nav, + spatial_memory(), + utilization(), +).global_config(n_dask_workers=8) + +with_jpeglcm = nav.transports( + { + ("color_image", Image): JpegLcmTransport("/color_image", Image), + } +) + +with_jpegshm = autoconnect( + nav.transports( + { + ("color_image", Image): JpegShmTransport("/color_image", quality=75), + } + ), + foxglove_bridge( + jpeg_shm_channels=[ + "/color_image#sensor_msgs.Image", + ] + ), +) + +_common_agentic = autoconnect( + human_input(), + navigation_skill(), + person_follow_skill(camera_info=GO2Connection.camera_info_static), + unitree_skills(), + web_input(), + speak_skill(), +) + +agentic = autoconnect( + spatial, + llm_agent(), + _common_agentic, +) + +agentic_mcp = autoconnect( + agentic, + MCPModule.blueprint(), +) + +agentic_ollama = autoconnect( + spatial, + llm_agent( + model="qwen3:8b", + provider=Provider.OLLAMA, # type: ignore[attr-defined] + ), + _common_agentic, +).requirements( + ollama_installed, +) + +agentic_huggingface = autoconnect( + spatial, + llm_agent( + model="Qwen/Qwen2.5-1.5B-Instruct", + provider=Provider.HUGGINGFACE, # type: ignore[attr-defined] + ), + _common_agentic, +) + +vlm_stream_test = autoconnect( + basic, + vlm_agent(), + vlm_stream_tester(), +) + +temporal_memory = autoconnect( + agentic, + temporal_memory(), +) diff --git a/dimos/teleop/keyboard/doom_teleop.py b/dimos/teleop/keyboard/doom_teleop.py new file mode 100644 index 000000000..ffcf32f85 --- /dev/null +++ b/dimos/teleop/keyboard/doom_teleop.py @@ -0,0 +1,323 @@ +#!/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 __future__ import annotations + +import math +import os +import threading + +import pygame + +from dimos.core import In, Module, Out, rpc +from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Twist, Vector3 +from dimos.msgs.std_msgs.Bool import Bool + +# Match existing teleop modules: force X11 to avoid OpenGL threading issues. +os.environ.setdefault("SDL_VIDEODRIVER", "x11") + + +class DoomTeleop(Module): + """Keyboard + mouse teleoperation in a DOOM/FPS style. + + - Keyboard: W/S for forward/back, A/D for turn left/right, Space for e-stop. + - Mouse: + - MOUSEMOTION controls yaw based on horizontal motion. + - Optional: right-click sends a small forward goal pose, middle-click + rotates in place using a discrete goal (if goal topics are wired). + - Outputs: + - cmd_vel: continuous Twist on the standard /cmd_vel interface. + - goal_pose + cancel_goal: optional discrete pose goals on existing + navigation topics (e.g. /goal_pose, /cancel_goal). + + The module is robot-agnostic; it only publishes Twist / PoseStamped / + Bool messages and relies on existing transports in the blueprint. + """ + + # Continuous velocity interface + cmd_vel: Out[Twist] + + # Optional discrete navigation goal interface + goal_pose: Out[PoseStamped] + cancel_goal: Out[Bool] + odom: In[PoseStamped] + + _stop_event: threading.Event + _thread: threading.Thread | None = None + _screen: pygame.Surface | None = None + _clock: pygame.time.Clock | None = None + _font: pygame.font.Font | None = None + + _keys_held: set[int] | None = None + _mouse_buttons_held: set[int] | None = None + _has_focus: bool = True + + _current_pose: PoseStamped | None = None + + # Tunable parameters + _base_linear_speed: float = 0.6 # m/s + _base_angular_speed: float = 1.2 # rad/s + _mouse_yaw_sensitivity: float = 0.003 # rad per pixel + _goal_step_forward: float = 0.5 # m + _goal_step_degrees: float = 20.0 # deg + + def __init__(self) -> None: + super().__init__() + self._stop_event = threading.Event() + + @rpc + def start(self) -> bool: + super().start() + + self._stop_event.clear() + self._keys_held = set() + self._mouse_buttons_held = set() + + # Subscribe to odom if wired, to enable discrete pose goals + if self.odom: + self.odom.subscribe(self._on_odom) + + self._thread = threading.Thread(target=self._pygame_loop, daemon=True) + self._thread.start() + + return True + + @rpc + def stop(self) -> None: + # Publish a final stop twist + stop_twist = Twist() + stop_twist.linear = Vector3(0.0, 0.0, 0.0) + stop_twist.angular = Vector3(0.0, 0.0, 0.0) + self.cmd_vel.publish(stop_twist) + + # Optionally cancel any active goal + if self.cancel_goal: + cancel_msg = Bool(data=True) + self.cancel_goal.publish(cancel_msg) + + self._stop_event.set() + + if self._thread is None: + raise RuntimeError("Cannot stop: thread was never started") + self._thread.join(2) + + super().stop() + + def _on_odom(self, pose: PoseStamped) -> None: + self._current_pose = pose + + def _clear_motion(self) -> None: + """Clear key/mouse state and send a hard stop.""" + if self._keys_held is not None: + self._keys_held.clear() + if self._mouse_buttons_held is not None: + self._mouse_buttons_held.clear() + + stop_twist = Twist() + stop_twist.linear = Vector3(0.0, 0.0, 0.0) + stop_twist.angular = Vector3(0.0, 0.0, 0.0) + self.cmd_vel.publish(stop_twist) + + def _pygame_loop(self) -> None: + if self._keys_held is None or self._mouse_buttons_held is None: + raise RuntimeError("Internal state not initialized") + + pygame.init() + self._screen = pygame.display.set_mode((640, 320), pygame.SWSURFACE) + pygame.display.set_caption("Doom Teleop (WSAD + Mouse)") + self._clock = pygame.time.Clock() + self._font = pygame.font.Font(None, 24) + + # Center the mouse and start with relative motion + pygame.mouse.set_visible(True) + pygame.mouse.get_rel() # reset relative movement + + while not self._stop_event.is_set(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + self._stop_event.set() + + elif event.type == pygame.KEYDOWN: + self._handle_keydown(event.key) + + elif event.type == pygame.KEYUP: + self._handle_keyup(event.key) + + elif event.type == pygame.MOUSEBUTTONDOWN: + self._mouse_buttons_held.add(event.button) + self._handle_mouse_button_down(event.button) + + elif event.type == pygame.MOUSEBUTTONUP: + self._mouse_buttons_held.discard(event.button) + + elif event.type == pygame.ACTIVEEVENT: + # Lose focus → immediately stop and ignore motion until focus returns. + if getattr(event, "gain", 0) == 0: + self._has_focus = False + self._clear_motion() + else: + self._has_focus = True + + # Compute continuous Twist command + twist = Twist() + twist.linear = Vector3(0.0, 0.0, 0.0) + twist.angular = Vector3(0.0, 0.0, 0.0) + + if self._has_focus: + # Keyboard WSAD mapping (DOOM-style) + if pygame.K_w in self._keys_held: + twist.linear.x += self._base_linear_speed + if pygame.K_s in self._keys_held: + twist.linear.x -= self._base_linear_speed + + # A/D = turn left/right + if pygame.K_a in self._keys_held: + twist.angular.z += self._base_angular_speed + if pygame.K_d in self._keys_held: + twist.angular.z -= self._base_angular_speed + + # Mouse horizontal motion → yaw + dx, _dy = pygame.mouse.get_rel() + twist.angular.z += float(-dx) * self._mouse_yaw_sensitivity + + # Left mouse button acts as a "drive" enable: if held with no WS, + # move forward slowly; if released, rely on keys only. + if 1 in self._mouse_buttons_held and twist.linear.x == 0.0: + twist.linear.x = 0.3 + + # Always publish at a fixed rate, even when zero, so downstream + # modules see that control has stopped. + self.cmd_vel.publish(twist) + + self._update_display(twist) + + if self._clock is None: + raise RuntimeError("_clock not initialized") + self._clock.tick(50) + + pygame.quit() + + def _handle_keydown(self, key: int) -> None: + if self._keys_held is None: + raise RuntimeError("_keys_held not initialized") + + self._keys_held.add(key) + + if key == pygame.K_SPACE: + # Emergency stop: clear all motion and cancel any goal. + self._clear_motion() + if self.cancel_goal: + cancel_msg = Bool(data=True) + self.cancel_goal.publish(cancel_msg) + print("EMERGENCY STOP!") + elif key == pygame.K_ESCAPE: + # ESC quits the teleop module. + self._stop_event.set() + + def _handle_keyup(self, key: int) -> None: + if self._keys_held is None: + raise RuntimeError("_keys_held not initialized") + self._keys_held.discard(key) + + def _handle_mouse_button_down(self, button: int) -> None: + """Map mouse button clicks to optional discrete goals.""" + if self._current_pose is None: + return + + # Right click → small forward step goal + if button == 3 and self.goal_pose: + goal = self._relative_goal( + self._current_pose, + forward=self._goal_step_forward, + yaw_degrees=0.0, + ) + self.goal_pose.publish(goal) + print("Published forward step goal from right click.") + # Middle click → in-place rotation goal + elif button == 2 and self.goal_pose: + goal = self._relative_goal( + self._current_pose, + forward=0.0, + yaw_degrees=self._goal_step_degrees, + ) + self.goal_pose.publish(goal) + print("Published rotate-in-place goal from middle click.") + + @staticmethod + def _relative_goal( + current_pose: PoseStamped, + forward: float, + yaw_degrees: float, + ) -> PoseStamped: + """Generate a new PoseStamped goal in the global frame. + + - forward is measured in the robot's local x direction. + - yaw_degrees is the desired change in yaw at the goal. + """ + local_offset = Vector3(forward, 0.0, 0.0) + global_offset = current_pose.orientation.rotate_vector(local_offset) + goal_position = current_pose.position + global_offset + + current_euler = current_pose.orientation.to_euler() + goal_yaw = current_euler.yaw + math.radians(yaw_degrees) + goal_euler = Vector3(current_euler.roll, current_euler.pitch, goal_yaw) + goal_orientation = Quaternion.from_euler(goal_euler) + + return PoseStamped( + position=goal_position, + orientation=goal_orientation, + frame_id=current_pose.frame_id, + ) + + def _update_display(self, twist: Twist) -> None: + if self._screen is None or self._font is None or self._keys_held is None: + raise RuntimeError("Display not initialized correctly") + + self._screen.fill((20, 20, 20)) + + y = 20 + focus_text = "FOCUSED" if self._has_focus else "OUT OF FOCUS (stopped)" + lines = [ + f"Doom Teleop - {focus_text}", + "", + f"Linear X: {twist.linear.x:+.2f} m/s", + f"Angular Z: {twist.angular.z:+.2f} rad/s", + "", + "Keyboard: W/S = forward/back, A/D = turn", + "Mouse: move = look/turn, LMB = slow forward drive", + "Mouse: RMB = step goal, MMB = rotate goal", + "Space: E-stop (also cancels goal), ESC: quit", + ] + + for text in lines: + color = (0, 255, 255) if text.startswith("Doom Teleop") else (230, 230, 230) + surf = self._font.render(text, True, color) + self._screen.blit(surf, (20, y)) + y += 26 + + # Simple status LED + moving = ( + abs(twist.linear.x) > 1e-3 or abs(twist.linear.y) > 1e-3 or abs(twist.angular.z) > 1e-3 + ) + color = (255, 0, 0) if moving else (0, 200, 0) + pygame.draw.circle(self._screen, color, (600, 30), 12) + + pygame.display.flip() + + +doom_teleop = DoomTeleop.blueprint + +__all__ = ["DoomTeleop", "doom_teleop"]