Robot Remote Control via UDP — Implementation Plan

 Context

 The robot subscribes to /hric/robot/cmd_vel with geometry_msgs/msg/TwistStamped (frame_id: pelvis). Standard ROS2 teleop tools (teleop_twist_keyboard, teleop_twist_joy) publish   
 plain Twist, not TwistStamped, so they won't work directly. We build custom keyboard and gamepad controllers in C (zero external dependencies, Linux-only) communicating over UDP  
 to a robot-side ROS2 bridge.

 How to Make the Robot Move

 Publish TwistStamped to /hric/robot/cmd_vel continuously (~20 Hz):

 ┌─────────────────────┬─────────────────┐
 │        Field        │     Effect      │
 ├─────────────────────┼─────────────────┤
 │ twist.linear.x > 0  │ Walk forward    │
 ├─────────────────────┼─────────────────┤
 │ twist.linear.x < 0  │ Walk backward   │
 ├─────────────────────┼─────────────────┤
 │ twist.linear.y > 0  │ Strafe left     │
 ├─────────────────────┼─────────────────┤
 │ twist.linear.y < 0  │ Strafe right    │
 ├─────────────────────┼─────────────────┤
 │ twist.angular.z > 0 │ Turn left (CCW) │
 ├─────────────────────┼─────────────────┤
 │ twist.angular.z < 0 │ Turn right (CW) │
 ├─────────────────────┼─────────────────┤
 │ All zeros           │ Stop            │
 └─────────────────────┴─────────────────┘

 Header must have frame_id = "pelvis" and current ROS timestamp.

 ---
 Architecture

 [PC: Keyboard/Gamepad (C)] --UDP binary struct--> [Robot: Bridge (Python/rclpy)] --> /hric/robot/cmd_vel

 ---
 Project Structure

 ros-control/
 ├── topic_example.yaml              # (existing)
 ├── Makefile                         # Build both C programs
 ├── common/
 │   └── protocol.h                   # Shared UDP protocol (binary struct)
 ├── remote/
 │   ├── keyboard_controller.c        # Keyboard teleop (C, termios)
 │   └── gamepad_controller.c         # Gamepad teleop (C, Linux joystick API)
 └── robot/
     └── udp_ros_bridge.py            # UDP → ROS2 TwistStamped (Python/rclpy)

 ---
 UDP Protocol (common/protocol.h)

 Binary packed struct — 24 bytes, no parsing overhead:

 #pragma pack(push, 1)
 typedef struct {
     float lx, ly, lz;   // linear velocity (m/s)
     float ax, ay, az;    // angular velocity (rad/s)
 } twist_cmd_t;
 #pragma pack(pop)

 #define DEFAULT_PORT 9870
 #define DEFAULT_IP   "127.0.0.1"

 On Python side, decode with struct.unpack('<6f', data).

 ---
 Program 1: Keyboard Controller (remote/keyboard_controller.c)

 Dependencies: None (POSIX + termios only)

 Technical approach:
 - termios.h: Set terminal to raw mode (~ICANON, ~ECHO, VMIN=0, VTIME=1)
 - select() with 50ms timeout for non-blocking key detection
 - Arrow keys: detect ESC sequence (\x1B[A/B/C/D)
 - UDP send via standard socket() / sendto()

 Key mapping:

 ┌────────┬───────────────────────────────────┐
 │  Key   │              Action               │
 ├────────┼───────────────────────────────────┤
 │ W / ↑  │ Forward (+linear.x)               │
 ├────────┼───────────────────────────────────┤
 │ S / ↓  │ Backward (-linear.x)              │
 ├────────┼───────────────────────────────────┤
 │ A / ←  │ Turn left (+angular.z)            │
 ├────────┼───────────────────────────────────┤
 │ D / →  │ Turn right (-angular.z)           │
 ├────────┼───────────────────────────────────┤
 │ Q      │ Strafe left (+linear.y)           │
 ├────────┼───────────────────────────────────┤
 │ E      │ Strafe right (-linear.y)          │
 ├────────┼───────────────────────────────────┤
 │ Space  │ Emergency stop (all zeros)        │
 ├────────┼───────────────────────────────────┤
 │ [ / ]  │ Decrease / increase linear speed  │
 ├────────┼───────────────────────────────────┤
 │ - / =  │ Decrease / increase angular speed │
 ├────────┼───────────────────────────────────┤
 │ Ctrl+C │ Quit (restore terminal)           │
 └────────┴───────────────────────────────────┘

 Behavior:
 - 20 Hz send loop in main thread
 - On key press: set velocity to ±max_speed
 - On no key (select timeout): gradually decay velocity to zero OR send zero immediately (configurable)
 - Print current velocity and speed settings to terminal (refresh in-place with \r)
 - signal(SIGINT) handler to restore terminal settings before exit
 - CLI args: -i <ip>, -p <port>, -l <max_linear>, -a <max_angular>

 ---
 Program 2: Gamepad Controller (remote/gamepad_controller.c)

 Dependencies: None (Linux joystick API only: linux/joystick.h)

 Technical approach:
 - Open /dev/input/js0 (configurable) with O_RDONLY | O_NONBLOCK
 - Read struct js_event (8 bytes: __u32 time, __s16 value, __u8 type, __u8 number)
 - Event types: JS_EVENT_AXIS (0x02), JS_EVENT_BUTTON (0x01)
 - select() for multiplexing joystick read + periodic UDP send

 Xbox controller axis mapping (xpad driver):

 ┌────────┬───────────────┬───────────────────────────────────┐
 │ Axis # │   Physical    │              Mapping              │
 ├────────┼───────────────┼───────────────────────────────────┤
 │ 0      │ Left stick X  │ linear.y (strafe)                 │
 ├────────┼───────────────┼───────────────────────────────────┤
 │ 1      │ Left stick Y  │ linear.x (forward/back, inverted) │
 ├────────┼───────────────┼───────────────────────────────────┤
 │ 3      │ Right stick X │ angular.z (turn)                  │
 └────────┴───────────────┴───────────────────────────────────┘

 Button mapping:

 ┌──────────┬──────────┬────────────────┐
 │ Button # │ Physical │     Action     │
 ├──────────┼──────────┼────────────────┤
 │ 0        │ A        │ Emergency stop │
 ├──────────┼──────────┼────────────────┤
 │ 1        │ B        │ Quit           │
 └──────────┴──────────┴────────────────┘

 Behavior:
 - Axis values: raw range [-32767, 32767] → normalized to [-1.0, 1.0] → scaled by max_speed
 - Deadzone: |normalized| < 0.1 → treat as 0 (configurable)
 - 20 Hz UDP send loop
 - Print gamepad name (via JSIOCGNAME ioctl), axes, and current velocities
 - Auto-detect controller disconnect / reconnect
 - CLI args: -i <ip>, -p <port>, -d <device>, -l <max_linear>, -a <max_angular>, -z <deadzone>

 ---
 Program 3: UDP-to-ROS2 Bridge (robot/udp_ros_bridge.py)

 Dependencies: rclpy, geometry_msgs (standard ROS2)

 Behavior:
 - ROS2 node: udp_teleop_bridge
 - Bind UDP on 0.0.0.0:9870
 - Receive 24-byte struct → struct.unpack('<6f', data) → build TwistStamped
 - Set header.stamp = current ROS time, header.frame_id = 'pelvis'
 - Publish to /hric/robot/cmd_vel at received rate
 - Watchdog: if no packet for 0.5s, publish zero velocity (safety stop)
 - UDP recv in separate threading.Thread, ROS2 spin() in main thread
 - ROS2 parameters: udp_port (int), topic (string), frame_id (string), timeout (float)

 ---
 Build System (Makefile)

 CC = gcc
 CFLAGS = -Wall -Wextra -O2 -I./common
 LDFLAGS = -lm

 all: build/keyboard_controller build/gamepad_controller

 build/keyboard_controller: remote/keyboard_controller.c common/protocol.h
        @mkdir -p build
        $(CC) $(CFLAGS) -o $@ $< $(LDFLAGS)

 build/gamepad_controller: remote/gamepad_controller.c common/protocol.h
        @mkdir -p build
        $(CC) $(CFLAGS) -o $@ $< $(LDFLAGS)

 clean:
        rm -rf build

 ---
 Files to Create (5 total)

 ┌─────┬──────────────────────────────┬────────┬───────────────────────────────────┐
 │  #  │             File             │  Lang  │              Purpose              │
 ├─────┼──────────────────────────────┼────────┼───────────────────────────────────┤
 │ 1   │ common/protocol.h            │ C      │ UDP protocol: struct + constants  │
 ├─────┼──────────────────────────────┼────────┼───────────────────────────────────┤
 │ 2   │ remote/keyboard_controller.c │ C      │ Keyboard → UDP (termios, select)  │
 ├─────┼──────────────────────────────┼────────┼───────────────────────────────────┤
 │ 3   │ remote/gamepad_controller.c  │ C      │ Gamepad → UDP (linux/joystick.h)  │
 ├─────┼──────────────────────────────┼────────┼───────────────────────────────────┤
 │ 4   │ robot/udp_ros_bridge.py      │ Python │ UDP → ROS2 TwistStamped publisher │
 ├─────┼──────────────────────────────┼────────┼───────────────────────────────────┤
 │ 5   │ Makefile                     │ Make   │ Build system                      │
 └─────┴──────────────────────────────┴────────┴───────────────────────────────────┘

 ---
 Verification

 1. Build: make — should compile without warnings
 2. Keyboard test: Run build/keyboard_controller -i 127.0.0.1, use a simple Python UDP listener to verify packets:
 import socket, struct
 s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 s.bind(('0.0.0.0', 9870))
 while True:
     data, _ = s.recvfrom(24)
     print(struct.unpack('<6f', data))
 3. Gamepad test: Connect Xbox controller, run build/gamepad_controller, verify stick input produces correct UDP packets
 4. Bridge test: Run udp_ros_bridge.py, then ros2 topic echo /hric/robot/cmd_vel to verify TwistStamped messages
 5. Safety: Stop controller, confirm bridge sends zero velocity after 0.5s timeout
 6. End-to-end: Controller → Bridge → robot moves