221 lines
13 KiB
Markdown
221 lines
13 KiB
Markdown
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 |