diff --git a/phone/INSTALL_MOTOR_TEST.md b/phone/INSTALL_MOTOR_TEST.md new file mode 100644 index 0000000..3b71eaa --- /dev/null +++ b/phone/INSTALL_MOTOR_TEST.md @@ -0,0 +1,120 @@ +# Motor Test Joystick Installation (Issue #513) + +Quick setup guide for installing motor_test_joystick.py on Termux. + +## Prerequisites + +- Android phone with Termux installed +- Python 3.9+ (installed via termux-bootstrap.sh) +- ROS2 Humble OR Jetson bridge running on networked Jetson Orin + +## Installation + +### 1. Copy script to phone + +```bash +# Option A: Via git +cd ~ +git clone https://gitea.vayrette.com/seb/saltylab-firmware.git + +# Option B: Via adb +adb push phone/motor_test_joystick.py /data/data/com.termux/files/home/ +``` + +### 2. Make executable + +```bash +chmod +x ~/saltylab-firmware/phone/motor_test_joystick.py +``` + +## Quick Test + +### 1. Start on phone (Termux) + +**ROS2 mode** (requires Jetson ros_core running): +```bash +python3 ~/saltylab-firmware/phone/motor_test_joystick.py +``` + +**WebSocket mode** (if Jetson IP is 192.168.1.100): +```bash +python3 ~/saltylab-firmware/phone/motor_test_joystick.py \ + --backend websocket \ + --host 192.168.1.100 +``` + +### 2. Verify on Jetson + +```bash +ros2 topic echo /cmd_vel +``` + +### 3. Safety test + +1. Move joystick (W key) +2. Watch /cmd_vel values change +3. Press spacebar (E-stop) +4. Verify velocities go to 0.0 +5. Press Q to quit + +## Configuration + +### Adjust velocity limits + +```bash +# Conservative (default) +python3 motor_test_joystick.py # 0.1 m/s, 0.3 rad/s + +# Moderate +python3 motor_test_joystick.py --linear-max 0.3 --angular-max 0.8 + +# Aggressive +python3 motor_test_joystick.py --linear-max 0.5 --angular-max 1.5 +``` + +### Change Jetson address + +```bash +# Static IP +python3 motor_test_joystick.py --backend websocket --host 192.168.1.100 + +# mDNS hostname +python3 motor_test_joystick.py --backend websocket --host saltybot.local + +# Different port +python3 motor_test_joystick.py --backend websocket --host 192.168.1.100 --port 8080 +``` + +## Troubleshooting + +### "ROS2 module not found" +→ Use WebSocket backend: `--backend websocket --host ` + +### "Connection refused" (WebSocket mode) +→ Check Jetson IP, verify bridge listening on :9090 + +### Motors not responding +1. Verify e-stop status (should show "Inactive") +2. Check timeout warning (>500ms = zero velocity) +3. Monitor Jetson: `ros2 topic echo /cmd_vel` +4. Verify network connectivity + +### Terminal display issues +- Try `reset` or `stty sane` in Termux +- Increase terminal size (pinch-zoom) +- Use external keyboard (more reliable) + +## Safety Checklist + +- [ ] Phone connected to Jetson (WiFi/tether) +- [ ] Motors disconnected or isolated (bench testing) +- [ ] E-stop accessible (spacebar) +- [ ] Terminal window visible +- [ ] Max velocities appropriate (conservative defaults) +- [ ] Kill switch ready (Ctrl+C) + +## Support + +- Main documentation: `MOTOR_TEST_JOYSTICK.md` +- Issue tracker: https://gitea.vayrette.com/seb/saltylab-firmware/issues/513 +- Termux wiki: https://wiki.termux.com/ diff --git a/phone/MOTOR_TEST_JOYSTICK.md b/phone/MOTOR_TEST_JOYSTICK.md index 841b64d..95a02e2 100644 --- a/phone/MOTOR_TEST_JOYSTICK.md +++ b/phone/MOTOR_TEST_JOYSTICK.md @@ -4,13 +4,9 @@ Terminal-based interactive joystick for bench testing SaltyBot motors via Termux ## Quick Start -On phone (Termux): ```bash -# With ROS2 (default, requires ros_core running on Jetson) -python3 ~/saltylab-firmware/phone/motor_test_joystick.py - -# With WebSocket (if ROS2 unavailable) -python3 ~/saltylab-firmware/phone/motor_test_joystick.py --backend websocket --host +python3 motor_test_joystick.py # ROS2 mode +python3 motor_test_joystick.py --backend websocket --host # WebSocket mode ``` ## Controls @@ -21,157 +17,47 @@ python3 ~/saltylab-firmware/phone/motor_test_joystick.py --backend websocket --h | **S** / **↓** | Reverse (linear -X) | | **A** / **←** | Turn left (angular +Z) | | **D** / **→** | Turn right (angular -Z) | -| **SPACE** | E-stop toggle (hold disables motors) | -| **R** | Reset velocities to zero | +| **SPACE** | E-stop toggle | +| **R** | Reset velocities | | **Q** | Quit | ## Features -### Real-Time Feedback -- Live velocity displays (linear X, angular Z) -- Velocity bar graphs with ● indicator -- Current input state (before clamping) -- Timeout warning (>500ms since last command) -- Status message line - -### Safety Features -- **E-stop button** (spacebar): Instantly zeros velocity, toggle on/off -- **Timeout safety**: 500ms without command → sends zero velocity -- **Velocity ramping**: Input decays exponentially (95% per frame) -- **Conservative defaults**: 0.1 m/s linear, 0.3 rad/s angular -- **Graceful fallback**: WebSocket if ROS2 unavailable - -### Dual Backend Support -- **ROS2 (primary)**: Publishes directly to `/cmd_vel` topic -- **WebSocket (fallback)**: JSON messages to Jetson bridge (port 9090) +- Real-time velocity feedback with bar graphs +- Spacebar e-stop (instantly zeros velocity) +- 500ms timeout safety (sends zero if idle) +- Conservative defaults: 0.1 m/s linear, 0.3 rad/s angular +- Dual backend: ROS2 (/cmd_vel) or WebSocket +- Graceful fallback if ROS2 unavailable ## Usage Examples -### Standard ROS2 mode (Jetson has ros_core) ```bash +# Standard ROS2 mode python3 motor_test_joystick.py -``` -Sends Twist messages to `/cmd_vel` at ~20Hz -### WebSocket mode (fallback if no ROS2) -```bash +# WebSocket mode (fallback) python3 motor_test_joystick.py --backend websocket --host 192.168.1.100 -``` -Sends JSON: `{"type": "twist", "linear_x": 0.05, "angular_z": 0.0, "timestamp": 1234567890}` -### Custom velocity limits -```bash +# Custom velocity limits python3 motor_test_joystick.py --linear-max 0.5 --angular-max 1.0 ``` -Max forward: 0.5 m/s, max turn: 1.0 rad/s - -### Combine options -```bash -python3 motor_test_joystick.py \ - --backend websocket \ - --host saltybot.local \ - --linear-max 0.2 \ - --angular-max 0.5 -``` ## Architecture -### MotorTestController -Main state machine: -- Manages velocity state (linear_x, angular_z) -- Handles e-stop state -- Enforces 500ms timeout -- Clamps velocities to max limits -- Sends commands to backend +- **MotorTestController**: State machine, velocity limiting, timeout enforcement +- **MotorTestNode** (ROS2): Twist publisher to /cmd_vel +- **WebSocketController** (fallback): JSON messages to Jetson +- **Curses UI**: Non-blocking input, real-time feedback, status display -### Backend Options +## Safety -**ROS2Backend**: Direct Twist publisher -- Requires `geometry_msgs` / `rclpy` -- Topic: `/cmd_vel` -- Spin thread for ros2.spin() - -**WebSocketBackend**: JSON over TCP socket -- No ROS2 dependencies -- Connects to Jetson:9090 (configurable) -- Fallback if ROS2 unavailable - -### Curses UI -- Non-blocking input (getch timeout) -- 20Hz refresh rate -- Color-coded status (green=ok, red=estop/timeout, yellow=bars) -- Real-time velocity bars -- Exponential input decay (95% per frame) - -## Terminal Requirements - -- **Size**: Minimum 80×25 characters (larger is better for full feedback) -- **Colors**: 256-color support (curses.init_pair) -- **Non-blocking I/O**: ncurses.nodelay() -- **Unicode**: ● symbol (optional, falls back to ASCII) - -### Test in Termux -```bash -stty size # Should show >= 25 lines -echo $TERM # Should be xterm-256color or similar -``` - -## Performance - -| Metric | Value | Notes | -|--------|-------|-------| -| **UI Refresh** | 20 Hz | Non-blocking, timeout-based | -| **Command Rate** | 20 Hz | Updated per frame | -| **Timeout Safety** | 500ms | Zero velocity if no input | -| **Input Decay** | 95% per frame | Smooth ramp-down | -| **Max Linear** | 0.1 m/s (default) | Conservative for bench testing | -| **Max Angular** | 0.3 rad/s (default) | ~17°/s rotation | - -## Troubleshooting - -### "ROS2 module not found" -→ Run with `--backend websocket` instead - -### "Connection refused" (WebSocket mode) -→ Check Jetson IP with `--host `, verify bridge listening on :9090 - -### Motors not responding -1. Check e-stop status (should show "Inactive") -2. Verify timeout warning (>500ms = zero velocity sent) -3. Check Jetson `/cmd_vel` subscription: `ros2 topic echo /cmd_vel` -4. Verify network connectivity (WiFi/tethering) - -### Terminal artifacts / display issues -- Try `reset` or `stty sane` in Termux -- Increase terminal size (pinch-zoom) -- Use `--backend websocket` (simpler UI fallback) - -## Safety Checklist Before Testing - -- [ ] Phone connected to Jetson (WiFi or USB tether) -- [ ] Motors disconnected or isolated (bench testing mode) -- [ ] E-stop accessible (spacebar, always reachable) -- [ ] Terminal window visible (no hide/scroll) -- [ ] Max velocities appropriate (start conservative: 0.1/0.3) -- [ ] Kill switch ready (Ctrl+C, or `ros2 topic pub --once /cmd_vel ...`) - -## Future Enhancements - -- [ ] Gamepad/joystick input (evdev) instead of keyboard -- [ ] Configurable button mappings -- [ ] Velocity profile presets (slow/medium/fast) -- [ ] Motor current feedback from motor driver -- [ ] Telemetry logging (CSV) for bench analysis -- [ ] Multi-motor independent control +- Conservative defaults (0.1/0.3 m/s) +- E-stop button (spacebar) +- 500ms timeout (auto-zero velocity) +- Input clamping & exponential decay ## Related Issues -- **#420** — Termux bootstrap & Android phone node -- **#508** — Face LCD animations (separate system) -- **#512** — Autonomous arming (uses /cmd_vel via motor bridge) - -## References - -- [ROS2 Humble - geometry_msgs/Twist](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Different-Distributions.html) -- [Termux - Python environment](https://wiki.termux.com/wiki/Python) -- [ncurses - Curses module](https://docs.python.org/3/library/curses.html) +- #420 — Termux bootstrap & Android phone node +- #512 — Autonomous arming (uses /cmd_vel) diff --git a/phone/motor_test_joystick.py b/phone/motor_test_joystick.py index 27f01d2..437840a 100644 --- a/phone/motor_test_joystick.py +++ b/phone/motor_test_joystick.py @@ -12,10 +12,9 @@ Controls: Features: - Conservative velocity defaults: 0.1 m/s linear, 0.3 rad/s angular - - Real-time velocity feedback display (current, max, min) + - Real-time velocity feedback display - 500ms timeout safety: stops motors if no command received - - Graceful fallback if ROS2 unavailable (WebSocket to Jetson) - - Terminal UI: velocity bars, input prompt, status line + - Terminal UI: velocity bars, status line, e-stop indicator """ import curses @@ -24,7 +23,6 @@ import time import argparse import sys from dataclasses import dataclass -from typing import Optional from enum import Enum # Try to import ROS2; fall back to WebSocket if unavailable @@ -48,8 +46,8 @@ POLL_RATE_HZ = 20 # UI update rate @dataclass class VelocityState: """Current velocity state""" - linear_x: float = 0.0 # m/s - angular_z: float = 0.0 # rad/s + linear_x: float = 0.0 + angular_z: float = 0.0 max_linear: float = DEFAULT_LINEAR_VEL max_angular: float = DEFAULT_ANGULAR_VEL estop_active: bool = False @@ -103,7 +101,6 @@ class WebSocketController: """Send Twist via JSON over WebSocket""" if not self.connected: return - try: msg = { "type": "twist", @@ -160,7 +157,7 @@ class MotorTestController: linear_x = 0.0 angular_z = 0.0 - # Check timeout (500ms) + # Check timeout if time.time() - self.state.last_command_time > TIMEOUT_MS / 1000.0: linear_x = 0.0 angular_z = 0.0 @@ -186,11 +183,9 @@ class MotorTestController: def shutdown(self): """Clean shutdown""" self.running = False - # Send zero velocity self.update_velocity(0.0, 0.0) time.sleep(0.1) - # Cleanup backend if isinstance(self.backend_obj, MotorTestNode): self.node.destroy_node() rclpy.shutdown() @@ -201,9 +196,9 @@ class MotorTestController: # === Curses UI === def run_joystick_ui(stdscr, controller: MotorTestController): """Main curses event loop""" - curses.curs_set(0) # Hide cursor - stdscr.nodelay(1) # Non-blocking getch() - stdscr.timeout(int(1000 / POLL_RATE_HZ)) # Refresh rate + curses.curs_set(0) + stdscr.nodelay(1) + stdscr.timeout(int(1000 / POLL_RATE_HZ)) # Color pairs curses.init_pair(1, curses.COLOR_GREEN, curses.COLOR_BLACK) @@ -212,7 +207,7 @@ def run_joystick_ui(stdscr, controller: MotorTestController): linear_input = 0.0 angular_input = 0.0 - status_msg = "Ready for motor test. Press W/A/S/D to control, SPACE to e-stop, Q to quit." + status_msg = "Ready. W/A/S/D=control, SPACE=estop, Q=quit" try: while controller.running: @@ -241,13 +236,12 @@ def run_joystick_ui(stdscr, controller: MotorTestController): angular_input = min(angular_input + 0.02, DEFAULT_ANGULAR_VEL) status_msg = f"Left: {angular_input:.2f} rad/s" elif key == ord('r') or key == ord('R'): - # Reset velocities linear_input = 0.0 angular_input = 0.0 - status_msg = "Velocities reset to zero" + status_msg = "Velocities reset" - # Apply exponential decay to input (if no new input, ramp down) - if key == -1: # No input + # Exponential decay + if key == -1: linear_input *= 0.95 angular_input *= 0.95 if abs(linear_input) < 0.01: @@ -255,29 +249,25 @@ def run_joystick_ui(stdscr, controller: MotorTestController): if abs(angular_input) < 0.01: angular_input = 0.0 - # Send updated velocity + # Send velocity controller.update_velocity(linear_input, angular_input) # Render UI stdscr.clear() height, width = stdscr.getmaxyx() - # Title title = "SaltyBot Motor Test Joystick (Issue #513)" - stdscr.addstr(0, (width - len(title)) // 2, title, + stdscr.addstr(0, max(0, (width - len(title)) // 2), title, curses.color_pair(1) | curses.A_BOLD) - # Status line y = 2 estop_color = curses.color_pair(2) if controller.state.estop_active else curses.color_pair(1) estop_text = f"E-STOP: {'ACTIVE' if controller.state.estop_active else 'Inactive'}" stdscr.addstr(y, 2, estop_text, estop_color) y += 2 - # Velocity displays stdscr.addstr(y, 2, f"Linear X: {controller.state.linear_x:+7.3f} m/s (max: {DEFAULT_LINEAR_VEL})") y += 1 - # Bar for linear bar_width = 30 bar_fill = int((controller.state.linear_x / DEFAULT_LINEAR_VEL) * (bar_width / 2)) bar_fill = max(-bar_width // 2, min(bar_width // 2, bar_fill)) @@ -287,34 +277,19 @@ def run_joystick_ui(stdscr, controller: MotorTestController): stdscr.addstr(y, 2, f"Angular Z: {controller.state.angular_z:+7.3f} rad/s (max: {DEFAULT_ANGULAR_VEL})") y += 1 - # Bar for angular bar_fill = int((controller.state.angular_z / DEFAULT_ANGULAR_VEL) * (bar_width / 2)) bar_fill = max(-bar_width // 2, min(bar_width // 2, bar_fill)) bar = "[" + " " * (bar_width // 2 + bar_fill) + "●" + " " * (bar_width // 2 - bar_fill) + "]" stdscr.addstr(y, 2, bar, curses.color_pair(3)) y += 2 - # Command input display - stdscr.addstr(y, 2, f"Input: Linear={linear_input:+.3f} Angular={angular_input:+.3f}") + stdscr.addstr(y, 2, f"Input: L={linear_input:+.3f} A={angular_input:+.3f}") y += 2 - # Controls legend - stdscr.addstr(y, 2, "Controls:") - y += 1 - stdscr.addstr(y, 2, " W/↑ = Forward S/↓ = Reverse A/← = Left D/→ = Right") - y += 1 - stdscr.addstr(y, 2, " SPACE = E-stop (toggle) R = Reset Q = Quit") + stdscr.addstr(y, 2, "W/↑=Forward S/↓=Reverse A/←=Left D/→=Right SPACE=E-stop R=Reset Q=Quit") y += 2 - # Status message stdscr.addstr(y, 2, f"Status: {status_msg[:width-10]}", curses.color_pair(1)) - y += 2 - - # Timeout warning - time_since_cmd = time.time() - controller.state.last_command_time - if time_since_cmd > (TIMEOUT_MS / 1000.0): - warning = f"TIMEOUT: Motors disabled ({time_since_cmd:.1f}s since last command)" - stdscr.addstr(y, 2, warning, curses.color_pair(2)) stdscr.refresh() @@ -327,47 +302,25 @@ def main(): parser = argparse.ArgumentParser( description="Terminal-based motor test joystick for SaltyBot (Issue #513)" ) - parser.add_argument( - "--backend", - choices=["ros2", "websocket"], - default="ros2", - help="Communication backend (default: ros2)" - ) - parser.add_argument( - "--host", - default="127.0.0.1", - help="Jetson hostname/IP (for WebSocket backend)" - ) - parser.add_argument( - "--port", - type=int, - default=9090, - help="Jetson port (for WebSocket backend)" - ) - parser.add_argument( - "--linear-max", - type=float, - default=DEFAULT_LINEAR_VEL, - help=f"Max linear velocity (default: {DEFAULT_LINEAR_VEL} m/s)" - ) - parser.add_argument( - "--angular-max", - type=float, - default=DEFAULT_ANGULAR_VEL, - help=f"Max angular velocity (default: {DEFAULT_ANGULAR_VEL} rad/s)" - ) + parser.add_argument("--backend", choices=["ros2", "websocket"], default="ros2", + help="Communication backend (default: ros2)") + parser.add_argument("--host", default="127.0.0.1", + help="Jetson hostname/IP (for WebSocket backend)") + parser.add_argument("--port", type=int, default=9090, + help="Jetson port (for WebSocket backend)") + parser.add_argument("--linear-max", type=float, default=DEFAULT_LINEAR_VEL, + help=f"Max linear velocity (default: {DEFAULT_LINEAR_VEL} m/s)") + parser.add_argument("--angular-max", type=float, default=DEFAULT_ANGULAR_VEL, + help=f"Max angular velocity (default: {DEFAULT_ANGULAR_VEL} rad/s)") args = parser.parse_args() - # Select backend backend = ControllerBackend.WEBSOCKET if args.backend == "websocket" else ControllerBackend.ROS2 - # Create controller controller = MotorTestController(backend=backend) controller.state.max_linear = args.linear_max controller.state.max_angular = args.angular_max - # Run UI try: curses.wrapper(run_joystick_ui, controller) except KeyboardInterrupt: