teleop arm stow

teleoperation/basestation_gui/backend/models_pydantic.py

@@ -84,6 +84,13 @@ class RecordingWaypointRequest(BaseModel):
 class RAModeRequest(BaseModel):
     mode: str
 
+class StowPositionRequest(BaseModel):
+    x: float
+    y: float
+    z: float
+    pitch: float
+    roll: float
+
 class ServoPositionRequest(BaseModel):
     names: List[str]
     positions: List[float]

teleoperation/basestation_gui/backend/ra_controls.py

@@ -1,37 +1,82 @@
+import asyncio
+import json
+import math
 from enum import Enum
+from pathlib import Path
 import threading
 
+import rclpy.time
+import tf2_ros
+from tf2_ros import LookupException, ConnectivityException, ExtrapolationException
 from rclpy.publisher import Publisher
 
 from backend.input import filter_input, simulated_axis, safe_index, DeviceInputs
 from backend.mappings import ControllerAxis, ControllerButton
 from backend.managers.ros import get_service_client
 from backend.utils.ros_service import call_service_async
+from lie import SE3
 from mrover.msg import Throttle, IK
 from mrover.srv import IkMode
 from geometry_msgs.msg import Twist
 
+STOW_CONFIG_PATH = Path(__file__).parent / '../../stow_config.json'
+
 ra_mode = "disabled"
 ra_mode_lock = threading.Lock()
 
+ik_pos_pub: Publisher | None = None
+stow_task: asyncio.Task | None = None
+tf_buffer: tf2_ros.Buffer | None = None
+
+
+async def stow_publish_loop() -> None:
+    try:
+        while get_ra_mode() == "stow" and ik_pos_pub is not None:
+            ik_pos_pub.publish(STOW_POSITION)
+            await asyncio.sleep(0.1)
+    except asyncio.CancelledError:
+        pass
+
+
+def register_ik_pos_pub(pub: Publisher) -> None:
+    global ik_pos_pub
+    ik_pos_pub = pub
+
+
+def register_tf_buffer(buffer: tf2_ros.Buffer) -> None:
+    global tf_buffer
+    tf_buffer = buffer
+
 
 def get_ra_mode() -> str:
     with ra_mode_lock:
         return ra_mode
 
 
-async def set_ra_mode(new_ra_mode: str):
-    global ra_mode
-    if new_ra_mode == "ik-pos":
+async def set_ra_mode(new_ra_mode: str) -> bool:
+    global ra_mode, stow_task
+    if new_ra_mode in ("ik-pos", "stow"):
         if not await call_ik_mode_service(IK_MODE_POSITION_CONTROL):
-            return
+            return False
     elif new_ra_mode == "ik-vel":
         if not await call_ik_mode_service(IK_MODE_VELOCITY_CONTROL):
-            return
+            return False
 
     with ra_mode_lock:
         ra_mode = new_ra_mode
 
+    if new_ra_mode == "stow":
+        if ik_pos_pub is not None:
+            ik_pos_pub.publish(STOW_POSITION)
+        if stow_task is None or stow_task.done():
+            stow_task = asyncio.create_task(stow_publish_loop())
+    else:
+        if stow_task is not None and not stow_task.done():
+            stow_task.cancel()
+        stow_task = None
+
+    return True
+
 
 async def call_ik_mode_service(mode: int) -> bool:
     client = get_service_client(IkMode, "/ik_mode")
@@ -45,6 +90,86 @@ async def call_ik_mode_service(mode: int) -> bool:
 IK_MODE_VELOCITY_CONTROL = 1
 IK_MODE_TYPING = 2
 
+STOW_POSITION_DEFAULTS = {
+    "x": 1.124319,
+    "y": 0.0,
+    "z": 0.042229,
+    "pitch": 0.072694,
+    "roll": 0.0,
+}
+STOW_POSITION = IK()
+
+
+def _quat_to_pitch_roll(qx: float, qy: float, qz: float, qw: float) -> tuple[float, float]:
+    sin_pitch = 2.0 * (qw * qy - qz * qx)
+    sin_pitch = max(-1.0, min(1.0, sin_pitch))
+    pitch = math.asin(sin_pitch)
+    roll = math.atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy))
+    return pitch, roll
+
+
+def _apply_stow_fields(values: dict) -> None:
+    STOW_POSITION.pos.x = float(values["x"])
+    STOW_POSITION.pos.y = float(values["y"])
+    STOW_POSITION.pos.z = float(values["z"])
+    STOW_POSITION.pitch = float(values.get("pitch", STOW_POSITION_DEFAULTS["pitch"]))
+    STOW_POSITION.roll = float(values.get("roll", STOW_POSITION_DEFAULTS["roll"]))
+
+
+def stow_position_dict() -> dict:
+    return {
+        "x": STOW_POSITION.pos.x,
+        "y": STOW_POSITION.pos.y,
+        "z": STOW_POSITION.pos.z,
+        "pitch": STOW_POSITION.pitch,
+        "roll": STOW_POSITION.roll,
+    }
+
+
+def load_stow_position() -> None:
+    try:
+        stored = json.loads(STOW_CONFIG_PATH.read_text())
+    except (FileNotFoundError, json.JSONDecodeError):
+        stored = STOW_POSITION_DEFAULTS
+    _apply_stow_fields(stored)
+
+
+def update_stow_position(x: float, y: float, z: float, pitch: float, roll: float) -> dict:
+    values = {"x": x, "y": y, "z": z, "pitch": pitch, "roll": roll}
+    _apply_stow_fields(values)
+    STOW_CONFIG_PATH.write_text(json.dumps(values, indent=2))
+    return stow_position_dict()
+
+
+def reset_stow_position() -> dict:
+    _apply_stow_fields(STOW_POSITION_DEFAULTS)
+    STOW_CONFIG_PATH.unlink(missing_ok=True)
+    return stow_position_dict()
+
+
+def capture_current_arm_pose() -> dict | None:
+    if tf_buffer is None:
+        return None
+    try:
+        if not tf_buffer.can_transform("arm_base_link", "arm_fk", rclpy.time.Time()):
+            return None
+        arm_in_base = SE3.from_tf_tree(tf_buffer, "arm_fk", "arm_base_link")
+    except (LookupException, ConnectivityException, ExtrapolationException):
+        return None
+    tx, ty, tz = arm_in_base.translation()
+    qx, qy, qz, qw = arm_in_base.quat()
+    pitch, roll = _quat_to_pitch_roll(float(qx), float(qy), float(qz), float(qw))
+    return {
+        "x": float(tx),
+        "y": float(ty),
+        "z": float(tz),
+        "pitch": pitch,
+        "roll": roll,
+    }
+
+
+_apply_stow_fields(STOW_POSITION_DEFAULTS)
+
 
 class Joint(Enum):
     A = 0
@@ -158,8 +283,8 @@ def send_ra_controls(
                     ik_vel_msg.linear.x = (-1.0) * filter_input(safe_index(inputs.axes, ControllerAxis.LEFT_Y), deadzone=CONTROLLER_STICK_DEADZONE)
                     ik_vel_msg.linear.y = (-1.0) * filter_input(safe_index(inputs.axes, ControllerAxis.LEFT_X), deadzone=CONTROLLER_STICK_DEADZONE)
                     ik_vel_msg.linear.z = (-1.0) * filter_input(safe_index(inputs.axes, ControllerAxis.RIGHT_Y), deadzone=CONTROLLER_STICK_DEADZONE)
-                    ik_vel_msg.angular.y = 1.0 * simulated_axis(inputs.buttons, ControllerButton.RIGHT_BUMPER, ControllerButton.LEFT_BUMPER)
-                    ik_vel_msg.angular.x = 1.0 * simulated_axis(inputs.buttons, ControllerButton.RIGHT_TRIGGER, ControllerButton.LEFT_TRIGGER)
+                    ik_vel_msg.angular.y = 1.0 * simulated_axis(inputs.buttons, ControllerButton.RIGHT_TRIGGER, ControllerButton.LEFT_TRIGGER)
+                    ik_vel_msg.angular.x = 1.0 * simulated_axis(inputs.buttons, ControllerButton.RIGHT_BUMPER, ControllerButton.LEFT_BUMPER)
                     ee_vel_pub.publish(ik_vel_msg)
 
                     cam_throttle = filter_input(

teleoperation/basestation_gui/backend/routes/arm.py

@@ -1,10 +1,17 @@
 from fastapi import APIRouter, HTTPException
-from backend.ra_controls import set_ra_mode as update_ra_mode
-from backend.models_pydantic import RAModeRequest
+from backend.ra_controls import (
+    set_ra_mode as update_ra_mode,
+    STOW_POSITION,
+    stow_position_dict,
+    update_stow_position,
+    reset_stow_position,
+    capture_current_arm_pose,
+)
+from backend.models_pydantic import RAModeRequest, StowPositionRequest
 
 router = APIRouter(prefix="/api/arm", tags=["arm"])
 
-VALID_RA_MODES = ["disabled", "throttle", "ik-pos", "ik-vel"]
+VALID_RA_MODES = ["disabled", "throttle", "ik-pos", "ik-vel", "stow"]
 
 
 @router.post("/ra_mode/")
@@ -15,5 +22,55 @@ async def change_ra_mode(data: RAModeRequest):
             status_code=400, detail=f"Invalid mode '{mode}'. Must be one of: {', '.join(VALID_RA_MODES)}"
         )
 
-    await update_ra_mode(mode)
+    if not await update_ra_mode(mode):
+        raise HTTPException(status_code=503, detail="Failed to set RA mode (ROS service might be unavailable)")
+
     return {"status": "success", "mode": mode}
+
+
+@router.post("/stow/")
+async def change_to_stow():
+    mode = "stow"
+    if not await update_ra_mode(mode):
+        raise HTTPException(status_code=503, detail="Failed to start stow sequence (ROS service might be unavailable)")
+
+    return {
+        "status": "success",
+        "mode": mode,
+        "stow_target": {
+            "pos": {
+                "x": STOW_POSITION.pos.x,
+                "y": STOW_POSITION.pos.y,
+                "z": STOW_POSITION.pos.z,
+            },
+            "pitch": STOW_POSITION.pitch,
+            "roll": STOW_POSITION.roll,
+        },
+    }
+
+
+@router.get("/stow/config/")
+async def get_stow_config():
+    return {"status": "success", "stow_position": stow_position_dict()}
+
+
+@router.post("/stow/capture/")
+async def capture_stow_pose():
+    pose = capture_current_arm_pose()
+    if pose is None:
+        raise HTTPException(
+            status_code=503,
+            detail="TF transform arm_base_link -> arm_fk is not available yet.",
+        )
+    return {"status": "success", "stow_position": pose}
+
+
+@router.post("/stow/config/")
+async def save_stow_config(data: StowPositionRequest):
+    saved = update_stow_position(data.x, data.y, data.z, data.pitch, data.roll)
+    return {"status": "success", "stow_position": saved}
+
+
+@router.post("/stow/config/reset/")
+async def reset_stow_config():
+    return {"status": "success", "stow_position": reset_stow_position()}

teleoperation/basestation_gui/backend/ws/arm_ws.py

@@ -1,28 +1,58 @@
+import rclpy.time
+import tf2_ros
+from tf2_ros import LookupException, ConnectivityException, ExtrapolationException
+from lie import SE3
 from backend.ws.base_ws import WebSocketHandler
 from backend.managers.ros import get_logger
 from backend.input import DeviceInputs
-from backend.ra_controls import send_ra_controls
+from backend.ra_controls import send_ra_controls, register_ik_pos_pub, register_tf_buffer
 from mrover.msg import Throttle, IK, ControllerState
 from geometry_msgs.msg import Twist
 from rclpy.publisher import Publisher
 
+
 class ArmHandler(WebSocketHandler):
     arm_thr_pub: Publisher
     ik_pos_pub: Publisher
     ik_vel_pub: Publisher
 
     def __init__(self, websocket):
         super().__init__(websocket, 'arm')
+        self.buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.buffer, self.node, spin_thread=False)
 
     async def setup(self):
         self.arm_thr_pub = self.node.create_publisher(Throttle, "/arm_thr_cmd", 1)
         self.ik_pos_pub = self.node.create_publisher(IK, "/ik_pos_cmd", 1)
         self.ik_vel_pub = self.node.create_publisher(Twist, "/ik_vel_cmd", 1)
         self.publishers.extend([self.arm_thr_pub, self.ik_pos_pub, self.ik_vel_pub])
+        register_ik_pos_pub(self.ik_pos_pub)
+        register_tf_buffer(self.buffer)
 
         self.forward_ros_topic("/arm_controller_state", ControllerState, "arm_state")
         self.forward_ros_topic("/arm_ik", IK, "ik_target")
 
+        self.timers.append(self.node.create_timer(0.1, self.send_arm_feedback_callback))
+
+    def send_arm_feedback_callback(self):
+        try:
+            if not self.buffer.can_transform("arm_base_link", "arm_fk", rclpy.time.Time()):
+                return
+            arm_in_base = SE3.from_tf_tree(self.buffer, "arm_fk", "arm_base_link")
+            pos = arm_in_base.translation()
+            self.schedule_send({
+                "type": "ik_feedback",
+                "pos": {
+                    "x": float(pos[0]),
+                    "y": float(pos[1]),
+                    "z": float(pos[2]),
+                },
+            })
+        except (LookupException, ConnectivityException, ExtrapolationException):
+            pass
+        except Exception as e:
+            get_logger().error(f"ArmHandler feedback error: {e}")
+
     async def handle_message(self, data):
         msg_type = data.get('type')