13 KiB
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 , -p , -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 , -p , -d , -l <max_linear>, -a <max_angular>, -z
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
- Build: make — should compile without warnings
- 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))
- Gamepad test: Connect Xbox controller, run build/gamepad_controller, verify stick input produces correct UDP packets
- Bridge test: Run udp_ros_bridge.py, then ros2 topic echo /hric/robot/cmd_vel to verify TwistStamped messages
- Safety: Stop controller, confirm bridge sends zero velocity after 0.5s timeout
- End-to-end: Controller → Bridge → robot moves