fix: enable added week deletion and preserve learning concepts when cloning courses

resources/slam-ros-bridge/bridge.py

@@ -27,6 +27,7 @@ from nav_msgs.msg import OccupancyGrid, Odometry, Path as NavPath
 from nav2_msgs.action import NavigateToPose
 from pydantic import BaseModel
 from rclpy.action import ActionClient
+from rclpy.clock import Clock, ClockType
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from rclpy.parameter import Parameter
@@ -132,6 +133,7 @@ def resolve_2d_transform(transforms: dict, target_frame: str, source_frame: str)
 class VelocityPayload(BaseModel):
     linear: float = 0.0
     angular: float = 0.0
+    deadman: bool = False
 
 
 class SteeringPayload(BaseModel):
@@ -257,6 +259,8 @@ class SlamNode(Node):
         if self.color_point_cloud_topic not in (self.point_cloud_topic, self.fallback_point_cloud_topic):
             self.create_subscription(PointCloud2, self.color_point_cloud_topic, self._on_color_point_cloud, qos_profile_sensor_data)
         self.create_subscription(CompressedImage, self.camera_image_topic, self._on_camera_image, qos_profile_sensor_data)
+        if not os.environ.get("CAMERA_IMAGE_TOPIC"):
+            self.create_subscription(CompressedImage, "/image_raw/compressed", self._on_camera_image, qos_profile_sensor_data)
         self.create_subscription(Odometry, self.icp_odom_topic, self._on_icp_odom, best_effort_qos)
         self.create_subscription(Odometry, self.icp_odom_topic, self._on_icp_odom, reliable_qos)
         self.create_subscription(OccupancyGrid, self.map_topic, self._on_map, map_qos)
@@ -268,6 +272,10 @@ class SlamNode(Node):
         self.create_subscription(TFMessage, "/tf", self._on_tf, qos_profile_sensor_data)
         self.create_subscription(TFMessage, "/tf_static", self._on_tf, tf_static_qos)
         self._cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10)
+        self._teleop_lock = threading.Lock()
+        self._teleop_deadline = None
+        self._teleop_timer = self.create_timer(
+            0.1, self._check_teleop_timeout, clock=Clock(clock_type=ClockType.STEADY_TIME))
         self._steering_pub = self.create_publisher(Float64, self.steering_topic, 10)
         self._initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 10)
         self._navigate_to_pose_client = ActionClient(self, NavigateToPose, self.navigate_to_pose_action)
@@ -817,13 +825,27 @@ class SlamNode(Node):
                 }
                 self._tf_count += 1
 
-    def publish_cmd_vel(self, linear: float, angular: float):
+    def publish_cmd_vel(self, linear: float, angular: float, deadman: bool = False):
         if self.read_only:
             raise RuntimeError("읽기 전용 3D SLAM 화면에서는 차량 제어를 지원하지 않습니다.")
+        if not math.isfinite(linear) or not math.isfinite(angular):
+            raise RuntimeError("Invalid velocity")
+        if deadman:
+            linear = max(-0.1, min(0.1, linear))
+            angular = max(-0.5, min(0.5, angular))
         msg = Twist()
         msg.linear.x = float(linear)
         msg.angular.z = float(angular)
-        self._cmd_vel_pub.publish(msg)
+        with self._teleop_lock:
+            self._teleop_deadline = time.monotonic() + 0.5 if deadman and (linear or angular) else None
+            self._cmd_vel_pub.publish(msg)
+
+    def _check_teleop_timeout(self):
+        # Only stop expired manual commands, never idle autonomous navigation.
+        with self._teleop_lock:
+            if self._teleop_deadline is not None and time.monotonic() >= self._teleop_deadline:
+                self._teleop_deadline = None
+                self._cmd_vel_pub.publish(Twist())
 
     def set_simulator_steering(self, angle: float) -> None:
         if self.read_only:
@@ -1192,6 +1214,7 @@ class SlamNode(Node):
 
         return {
             "ok": True,
+            "teleop_deadman": not self.read_only,
             "topics": {
                 "odom": self.odom_topic,
                 "scan": self.scan_topic,
@@ -1549,7 +1572,7 @@ def debug_topics():
 def cmd_vel(payload: VelocityPayload):
     if not slam_node:
         raise HTTPException(status_code=503, detail="ROS node is not ready")
-    slam_node.publish_cmd_vel(payload.linear, payload.angular)
+    slam_node.publish_cmd_vel(payload.linear, payload.angular, payload.deadman)
     return {"ok": True}
 
 
@@ -1643,11 +1666,12 @@ async def ws_control(websocket: WebSocket):
                 velocity = VelocityPayload(
                     linear=payload.get("linear", 0.0),
                     angular=payload.get("angular", 0.0),
+                    deadman=payload.get("deadman", False),
                 )
             except (TypeError, ValueError):
                 continue
             try:
-                slam_node.publish_cmd_vel(velocity.linear, velocity.angular)
+                slam_node.publish_cmd_vel(velocity.linear, velocity.angular, velocity.deadman)
             except RuntimeError:
                 continue
     except WebSocketDisconnect:

resources/slam-ros-bridge/test_teleop.py

@@ -0,0 +1,70 @@
+import ast
+import math
+from pathlib import Path
+import threading
+from types import SimpleNamespace
+import unittest
+
+
+class Twist:
+    def __init__(self):
+        self.linear = SimpleNamespace(x=0.0)
+        self.angular = SimpleNamespace(z=0.0)
+
+
+class TeleopTest(unittest.TestCase):
+    def setUp(self):
+        self.now = 10.0
+        self.messages = []
+        source = ast.parse(Path(__file__).with_name('bridge.py').read_text())
+        node = next(item for item in source.body if isinstance(item, ast.ClassDef) and item.name == 'SlamNode')
+        methods = [item for item in node.body if isinstance(item, ast.FunctionDef)
+                   and item.name in ('publish_cmd_vel', '_check_teleop_timeout')]
+        scope = {'math': math, 'Twist': Twist, 'time': SimpleNamespace(monotonic=lambda: self.now)}
+        exec(compile(ast.Module(body=methods, type_ignores=[]), 'bridge-teleop', 'exec'), scope)
+        cls = type('Teleop', (), {name: scope[name] for name in ('publish_cmd_vel', '_check_teleop_timeout')})
+        self.node = cls()
+        self.node.read_only = False
+        self.node._teleop_lock = threading.Lock()
+        self.node._teleop_deadline = None
+        self.node._cmd_vel_pub = SimpleNamespace(publish=lambda msg: self.messages.append((msg.linear.x, msg.angular.z)))
+
+    def test_low_speed_and_expiry(self):
+        self.node.publish_cmd_vel(5.0, -5.0, True)
+        self.assertEqual(self.messages[-1], (0.1, -0.5))
+        self.now += 0.6
+        self.node._check_teleop_timeout()
+        self.assertEqual(self.messages[-1], (0.0, 0.0))
+        count = len(self.messages)
+        self.node._check_teleop_timeout()
+        self.assertEqual(len(self.messages), count)
+
+    def test_heartbeat_and_release(self):
+        self.node.publish_cmd_vel(0.1, 0, True)
+        self.now += 0.4
+        self.node.publish_cmd_vel(0.1, 0, True)
+        self.now += 0.2
+        self.node._check_teleop_timeout()
+        self.assertEqual(len(self.messages), 2)
+        self.node.publish_cmd_vel(0, 0, True)
+        self.assertIsNone(self.node._teleop_deadline)
+
+    def test_simulation_and_idle_navigation_unchanged(self):
+        self.node._check_teleop_timeout()
+        self.assertEqual(self.messages, [])
+        self.node.publish_cmd_vel(0.22, 2.84)
+        self.now += 2
+        self.node._check_teleop_timeout()
+        self.assertEqual(self.messages, [(0.22, 2.84)])
+
+    def test_invalid_and_readonly_rejected(self):
+        with self.assertRaises(RuntimeError):
+            self.node.publish_cmd_vel(float('nan'), 0, True)
+        self.node.read_only = True
+        with self.assertRaises(RuntimeError):
+            self.node.publish_cmd_vel(0.1, 0, True)
+        self.assertEqual(self.messages, [])
+
+
+if __name__ == '__main__':
+    unittest.main()