feat: C控制程序对接KCP
This commit is contained in:
@@ -0,0 +1,221 @@
|
||||
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
|
||||
Reference in New Issue
Block a user