limingjie/OmniSocketGo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: C控制程序对接KCP

Browse Source
This commit is contained in:
Mock
2026-04-03 12:04:39 +08:00
parent 628583f79d
commit 8a1baa64c0
13 changed files with 1720 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
ros-control-c/Makefile Normal file
View File

@@ -0,0 +1,34 @@
CC = gcc
CFLAGS = -std=c11 -Wall -Wextra -O2 -pthread -D_GNU_SOURCE -I../include -I../third_party/cjson -I../third_party/kcp -I./common
LDFLAGS = -pthread -lm
OMNI_SRCS = \
../src/omni_common.c \
../src/protocol.c \
../src/latencylog.c \
../src/kcp_packet_debug.c \
../src/kcp_session_stats.c \
../src/linux_timestamping.c \
../src/transport_kcp.c \
../src/peer_kcp_client.c \
../third_party/cjson/cJSON.c \
../third_party/kcp/ikcp.c
BUILDDIR = build
TARGETS = $(BUILDDIR)/keyboard_controller $(BUILDDIR)/gamepad_controller
.PHONY: all clean
all: $(TARGETS)
$(BUILDDIR)/keyboard_controller: remote/keyboard_controller.c common/protocol.h common/teleop_transport.h common/teleop_transport.c $(OMNI_SRCS)
@mkdir -p $(BUILDDIR)
$(CC) $(CFLAGS) -o $@ remote/keyboard_controller.c common/teleop_transport.c $(OMNI_SRCS) $(LDFLAGS)
$(BUILDDIR)/gamepad_controller: remote/gamepad_controller.c common/protocol.h common/teleop_transport.h common/teleop_transport.c $(OMNI_SRCS)
@mkdir -p $(BUILDDIR)
$(CC) $(CFLAGS) -o $@ remote/gamepad_controller.c common/teleop_transport.c $(OMNI_SRCS) $(LDFLAGS)
clean:
rm -rf $(BUILDDIR)

71
ros-control-c/README.md Normal file
View File

@@ -0,0 +1,71 @@
# ros-control-c
`ros-control-c` keeps the original 24-byte `twist_cmd_t` control payload and now supports two runtime transports:
- `udp` (default): unchanged from the original implementation
- `kcp`: sent through OmniSocket using `MSG_TYPE_BINARY`
## Build
On Linux:
```bash
make -C ros-control-c
```
If the robot-side Python bridge will use KCP, build and install the OmniSocket Python extension from the repo root first:
```bash
make python-ext
make python-install
```
## UDP Mode
Sender:
```bash
./ros-control-c/build/keyboard_controller -i 192.168.1.100 -p 9870
./ros-control-c/build/gamepad_controller -i 192.168.1.100 -p 9870
```
Robot bridge:
```bash
python3 ros-control-c/robot/udp_ros_bridge.py
```
## KCP Mode
Start the existing OmniSocket KCP hub from the repo root:
```bash
./bin/kcpserver -listen :9002
```
Sender:
```bash
./ros-control-c/build/keyboard_controller -t kcp -s 192.168.1.50:9002 -I ros-keyboard-ctrl -T ros-bridge-ctrl
./ros-control-c/build/gamepad_controller -t kcp -s 192.168.1.50:9002 -I ros-gamepad-ctrl -T ros-bridge-ctrl
```
If a relay is needed, add `-r <relay_addr>` to the controller command.
Robot bridge:
```bash
python3 ros-control-c/robot/udp_ros_bridge.py --ros-args \
-p transport:=kcp \
-p kcp_server:=192.168.1.50:9002 \
-p peer_id:=ros-bridge-ctrl
```
Optional sender filtering:
```bash
python3 ros-control-c/robot/udp_ros_bridge.py --ros-args \
-p transport:=kcp \
-p peer_id:=ros-bridge-ctrl \
-p expected_sender:=ros-keyboard-ctrl
```

221
ros-control-c/Robot Remote Control via UDP - Implementation Plan.md Normal file
View File

@@ -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

BIN
ros-control-c/build/gamepad_controller Normal file
View File

Binary file not shown.

BIN
ros-control-c/build/keyboard_controller Normal file
View File

Binary file not shown.

26
ros-control-c/common/protocol.h Normal file
View File

@@ -0,0 +1,26 @@
#ifndef PROTOCOL_H
#define PROTOCOL_H
#include <stdint.h>
#define DEFAULT_PORT 9870
#define DEFAULT_IP "127.0.0.1"
#define SEND_RATE_HZ 20
#define SEND_INTERVAL_US (1000000 / SEND_RATE_HZ)
#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 TWIST_CMD_SIZE sizeof(twist_cmd_t) /* 24 bytes */
static inline void twist_cmd_zero(twist_cmd_t *cmd)
{
cmd->lx = cmd->ly = cmd->lz = 0.0f;
cmd->ax = cmd->ay = cmd->az = 0.0f;
}
#endif /* PROTOCOL_H */

300
ros-control-c/common/teleop_transport.c Normal file
View File

@@ -0,0 +1,300 @@
#include "teleop_transport.h"
#include <arpa/inet.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <sys/socket.h>
#include <unistd.h>
static void teleop_transport_clear(teleop_transport_t *transport)
{
if (transport == NULL) {
return;
}
memset(transport, 0, sizeof(*transport));
transport->mode = TELEOP_TRANSPORT_MODE_UDP;
transport->udp_fd = -1;
}
int teleop_transport_parse_mode(const char *raw, teleop_transport_mode_t *out_mode)
{
if (raw == NULL || out_mode == NULL) {
errno = EINVAL;
return -1;
}
if (strcmp(raw, "udp") == 0) {
*out_mode = TELEOP_TRANSPORT_MODE_UDP;
return 0;
}
if (strcmp(raw, "kcp") == 0) {
*out_mode = TELEOP_TRANSPORT_MODE_KCP;
return 0;
}
errno = EINVAL;
return -1;
}
const char *teleop_transport_mode_name(teleop_transport_mode_t mode)
{
return mode == TELEOP_TRANSPORT_MODE_KCP ? "kcp" : "udp";
}
static void teleop_transport_log_incoming(const message_t *msg)
{
if (msg == NULL) {
return;
}
switch (msg->type) {
case MSG_TYPE_ERROR:
fprintf(stderr,
"teleop transport: server error from %s to %s: %.*s\n",
msg->from,
msg->to,
(int)msg->body_len,
msg->body == NULL ? "" : (const char *)msg->body);
break;
case MSG_TYPE_TEXT:
fprintf(stderr,
"teleop transport: dropped unexpected text from %s to %s: %.*s\n",
msg->from,
msg->to,
(int)msg->body_len,
msg->body == NULL ? "" : (const char *)msg->body);
break;
case MSG_TYPE_BINARY:
fprintf(stderr,
"teleop transport: dropped unexpected binary payload from %s to %s (%lu bytes)\n",
msg->from,
msg->to,
(unsigned long)msg->body_len);
break;
case MSG_TYPE_FILE:
fprintf(stderr,
"teleop transport: dropped unexpected file from %s to %s: %s (%lu bytes)\n",
msg->from,
msg->to,
msg->file_name,
(unsigned long)msg->body_len);
break;
case MSG_TYPE_REGISTER:
fprintf(stderr,
"teleop transport: dropped unexpected register message from %s to %s\n",
msg->from,
msg->to);
break;
default:
fprintf(stderr,
"teleop transport: dropped unexpected message type %s from %s\n",
protocol_message_type_name(msg->type),
msg->from);
break;
}
}
static void *teleop_transport_kcp_recv_thread_main(void *arg)
{
teleop_transport_t *transport = (teleop_transport_t *)arg;
for (;;) {
message_t msg;
int rc;
if (transport->stop_requested) {
return NULL;
}
protocol_message_init(&msg);
rc = kcp_client_receive_timed(transport->kcp_client, &msg, 100);
if (rc == 1) {
protocol_message_clear(&msg);
continue;
}
if (rc != 0) {
protocol_message_clear(&msg);
if (!transport->stop_requested) {
int saved_errno = errno;
fprintf(stderr,
"teleop transport: KCP receive loop stopped: %s (errno=%d)\n",
saved_errno != 0 ? strerror(saved_errno) : "unknown error",
saved_errno);
}
return NULL;
}
teleop_transport_log_incoming(&msg);
protocol_message_clear(&msg);
}
}
static int teleop_transport_open_udp(teleop_transport_t *transport, const teleop_transport_config_t *config)
{
int sockfd;
if (transport == NULL || config == NULL || config->udp_ip == NULL) {
errno = EINVAL;
return -1;
}
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) {
perror("socket");
return -1;
}
memset(&transport->udp_dest, 0, sizeof(transport->udp_dest));
transport->udp_dest.sin_family = AF_INET;
transport->udp_dest.sin_port = htons(config->udp_port);
if (inet_pton(AF_INET, config->udp_ip, &transport->udp_dest.sin_addr) <= 0) {
fprintf(stderr, "Invalid IP: %s\n", config->udp_ip);
close(sockfd);
errno = EINVAL;
return -1;
}
transport->udp_fd = sockfd;
return 0;
}
static int teleop_transport_open_kcp(teleop_transport_t *transport, const teleop_transport_config_t *config)
{
kcp_conn_options_t options;
const char *relay_via;
if (transport == NULL || config == NULL ||
config->server_addr == NULL || config->peer_id == NULL || config->target_peer == NULL) {
errno = EINVAL;
return -1;
}
kcp_conn_options_set_control_defaults(&options);
relay_via = (config->relay_via != NULL && config->relay_via[0] != '\0') ? config->relay_via : NULL;
transport->kcp_client = kcp_client_dial_with_options(
config->server_addr,
relay_via,
config->peer_id,
"",
"",
&options,
NULL,
NULL,
NULL,
KCP_DEFAULT_STATS_INTERVAL_MS
);
if (transport->kcp_client == NULL) {
int saved_errno = errno;
fprintf(stderr,
"teleop transport: failed to open KCP session as %s via %s%s%s: %s (errno=%d)\n",
config->peer_id,
config->server_addr,
relay_via != NULL ? ", relay=" : "",
relay_via != NULL ? relay_via : "",
saved_errno != 0 ? strerror(saved_errno) : "unknown error",
saved_errno);
errno = saved_errno;
return -1;
}
{
int thread_rc = pthread_create(&transport->recv_thread, NULL, teleop_transport_kcp_recv_thread_main, transport);
if (thread_rc != 0) {
fprintf(stderr,
"teleop transport: failed to start KCP receive thread: %s (errno=%d)\n",
strerror(thread_rc),
thread_rc);
kcp_client_close(transport->kcp_client);
kcp_client_free(transport->kcp_client);
transport->kcp_client = NULL;
errno = thread_rc;
return -1;
}
}
transport->recv_thread_started = 1;
return 0;
}
int teleop_transport_open(teleop_transport_t *transport, const teleop_transport_config_t *config)
{
if (transport == NULL || config == NULL) {
errno = EINVAL;
return -1;
}
teleop_transport_clear(transport);
transport->mode = config->mode;
snprintf(transport->server_addr, sizeof(transport->server_addr), "%s",
config->server_addr == NULL ? "" : config->server_addr);
snprintf(transport->relay_via, sizeof(transport->relay_via), "%s",
config->relay_via == NULL ? "" : config->relay_via);
snprintf(transport->peer_id, sizeof(transport->peer_id), "%s",
config->peer_id == NULL ? "" : config->peer_id);
snprintf(transport->target_peer, sizeof(transport->target_peer), "%s",
config->target_peer == NULL ? "" : config->target_peer);
if (config->mode == TELEOP_TRANSPORT_MODE_KCP) {
return teleop_transport_open_kcp(transport, config);
}
return teleop_transport_open_udp(transport, config);
}
int teleop_transport_send_twist(teleop_transport_t *transport, const twist_cmd_t *cmd)
{
if (transport == NULL || cmd == NULL) {
errno = EINVAL;
return -1;
}
if (transport->mode == TELEOP_TRANSPORT_MODE_KCP) {
if (kcp_client_send_binary(transport->kcp_client, transport->target_peer, cmd, TWIST_CMD_SIZE) != 0) {
int saved_errno = errno;
fprintf(stderr,
"teleop transport: failed to send KCP payload to %s: %s (errno=%d)\n",
transport->target_peer,
saved_errno != 0 ? strerror(saved_errno) : "unknown error",
saved_errno);
errno = saved_errno;
return -1;
}
return 0;
}
{
ssize_t sent = sendto(transport->udp_fd, cmd, TWIST_CMD_SIZE, 0,
(const struct sockaddr *)&transport->udp_dest, sizeof(transport->udp_dest));
if (sent < 0) {
perror("sendto");
return -1;
}
if ((size_t)sent != TWIST_CMD_SIZE) {
fprintf(stderr, "sendto: short send (%zd/%zu)\n", sent, (size_t)TWIST_CMD_SIZE);
errno = EIO;
return -1;
}
}
return 0;
}
void teleop_transport_close(teleop_transport_t *transport)
{
if (transport == NULL) {
return;
}
transport->stop_requested = 1;
if (transport->kcp_client != NULL) {
kcp_client_close(transport->kcp_client);
}
if (transport->recv_thread_started) {
pthread_join(transport->recv_thread, NULL);
transport->recv_thread_started = 0;
}
if (transport->kcp_client != NULL) {
kcp_client_free(transport->kcp_client);
transport->kcp_client = NULL;
}
if (transport->udp_fd >= 0) {
close(transport->udp_fd);
transport->udp_fd = -1;
}
}

59
ros-control-c/common/teleop_transport.h Normal file
View File

@@ -0,0 +1,59 @@
#ifndef TELEOP_TRANSPORT_H
#define TELEOP_TRANSPORT_H
#include <netinet/in.h>
#include <pthread.h>
#include "protocol.h"
#include "peer_kcp_client.h"
#ifdef __cplusplus
extern "C" {
#endif
#define DEFAULT_KCP_SERVER_ADDR "127.0.0.1:9002"
#define DEFAULT_KCP_KEYBOARD_PEER_ID "ros-keyboard-ctrl"
#define DEFAULT_KCP_GAMEPAD_PEER_ID "ros-gamepad-ctrl"
#define DEFAULT_KCP_TARGET_PEER_ID "ros-bridge-ctrl"
typedef enum teleop_transport_mode {
TELEOP_TRANSPORT_MODE_UDP = 0,
TELEOP_TRANSPORT_MODE_KCP = 1
} teleop_transport_mode_t;
typedef struct teleop_transport_config {
teleop_transport_mode_t mode;
const char *udp_ip;
int udp_port;
const char *server_addr;
const char *relay_via;
const char *peer_id;
const char *target_peer;
} teleop_transport_config_t;
typedef struct teleop_transport {
teleop_transport_mode_t mode;
int udp_fd;
struct sockaddr_in udp_dest;
kcp_client_t *kcp_client;
pthread_t recv_thread;
int recv_thread_started;
volatile int stop_requested;
char server_addr[OMNI_MAX_ADDR_TEXT];
char relay_via[OMNI_MAX_ADDR_TEXT];
char peer_id[OMNI_MAX_PEER_ID];
char target_peer[OMNI_MAX_PEER_ID];
} teleop_transport_t;
int teleop_transport_parse_mode(const char *raw, teleop_transport_mode_t *out_mode);
const char *teleop_transport_mode_name(teleop_transport_mode_t mode);
int teleop_transport_open(teleop_transport_t *transport, const teleop_transport_config_t *config);
int teleop_transport_send_twist(teleop_transport_t *transport, const twist_cmd_t *cmd);
void teleop_transport_close(teleop_transport_t *transport);
#ifdef __cplusplus
}
#endif
#endif /* TELEOP_TRANSPORT_H */

293
ros-control-c/remote/gamepad_controller.c Normal file
View File

@@ -0,0 +1,293 @@
/*
* gamepad_controller.c — Gamepad/joystick teleop over UDP or KCP
*
* Uses the Linux joystick API (/dev/input/js*).
* Zero external dependencies.
*
* Xbox controller mapping (xpad driver):
* Left stick Y (axis 1) → linear.x (forward/back, inverted)
* Left stick X (axis 0) → linear.y (strafe)
* Right stick X (axis 3) → angular.z (turn)
* Button A (0) → emergency stop
* Button B (1) → quit
*
* Build: gcc -Wall -O2 -I../common -o gamepad_controller gamepad_controller.c -lm
* Usage: ./gamepad_controller [-i IP] [-p PORT] [-d /dev/input/js0]
* [-l MAX_LIN] [-a MAX_ANG] [-z DEADZONE]
* [-t udp|kcp] [-s SERVER] [-r RELAY]
* [-I PEER_ID] [-T TARGET_PEER]
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <signal.h>
#include <math.h>
#include <errno.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <getopt.h>
#include <linux/joystick.h>
#include "../common/protocol.h"
#include "../common/teleop_transport.h"
/* ── config ─────────────────────────────────────────────────────────── */
#define MAX_AXES 16
#define MAX_BUTTONS 16
#define JS_AXIS_MAX 32767.0f
/* Xbox mapping indices */
#define AXIS_LX 0 /* left stick X → strafe */
#define AXIS_LY 1 /* left stick Y → fwd/back (inverted) */
#define AXIS_RX 3 /* right stick X → turn */
#define BTN_STOP 0 /* A → emergency stop */
#define BTN_QUIT 1 /* B → quit */
static volatile sig_atomic_t g_running = 1;
static void sigint_handler(int sig) { (void)sig; g_running = 0; }
static int parse_port(const char *text, int *port_out)
{
char *end = NULL;
long value = strtol(text, &end, 10);
if (end == text || *end != '\0' || value < 1 || value > 65535)
return -1;
*port_out = (int)value;
return 0;
}
/* ── apply deadzone ─────────────────────────────────────────────────── */
static float apply_deadzone(float v, float dz)
{
if (fabsf(v) < dz) return 0.0f;
/* rescale so the output starts from 0 just outside the deadzone */
float sign = (v > 0) ? 1.0f : -1.0f;
return sign * (fabsf(v) - dz) / (1.0f - dz);
}
/* ── usage ──────────────────────────────────────────────────────────── */
static void usage(const char *prog)
{
fprintf(stderr,
"Usage: %s [options]\n"
" -i IP target IP (default %s)\n"
" -p PORT target port (default %d)\n"
" -d DEVICE joystick device (default /dev/input/js0)\n"
" -l SPEED max linear m/s (default 0.5)\n"
" -a SPEED max angular rad/s (default 0.5)\n"
" -z DZ deadzone 0<=DZ<1 (default 0.1)\n"
" -t MODE transport mode udp|kcp (default udp)\n"
" -s ADDR KCP server addr (default %s)\n"
" -r ADDR KCP relay addr (default none)\n"
" -I ID local KCP peer id (default %s)\n"
" -T ID target KCP peer id (default %s)\n"
" -h show help\n",
prog, DEFAULT_IP, DEFAULT_PORT,
DEFAULT_KCP_SERVER_ADDR,
DEFAULT_KCP_GAMEPAD_PEER_ID,
DEFAULT_KCP_TARGET_PEER_ID);
}
/* ──────────────────────────────────────────────────────────────────── */
int main(int argc, char *argv[])
{
char ip[64] = DEFAULT_IP;
int port = DEFAULT_PORT;
char device[128] = "/dev/input/js0";
char kcp_server[OMNI_MAX_ADDR_TEXT] = DEFAULT_KCP_SERVER_ADDR;
char kcp_relay[OMNI_MAX_ADDR_TEXT] = "";
char peer_id[OMNI_MAX_PEER_ID] = DEFAULT_KCP_GAMEPAD_PEER_ID;
char target_peer[OMNI_MAX_PEER_ID] = DEFAULT_KCP_TARGET_PEER_ID;
float max_lin = 0.5f;
float max_ang = 0.5f;
float deadzone = 0.1f;
teleop_transport_mode_t transport_mode = TELEOP_TRANSPORT_MODE_UDP;
teleop_transport_t transport;
teleop_transport_config_t transport_config;
int opt;
while ((opt = getopt(argc, argv, "i:p:d:l:a:z:t:s:r:I:T:h")) != -1) {
switch (opt) {
case 'i': strncpy(ip, optarg, sizeof(ip)-1); ip[sizeof(ip)-1] = '\0'; break;
case 'p':
if (parse_port(optarg, &port) != 0) {
fprintf(stderr, "Invalid port: %s (expected 1-65535)\n", optarg);
return 1;
}
break;
case 'd': strncpy(device, optarg, sizeof(device)-1); device[sizeof(device)-1] = '\0'; break;
case 'l': max_lin = strtof(optarg, NULL); break;
case 'a': max_ang = strtof(optarg, NULL); break;
case 'z': deadzone = strtof(optarg, NULL); break;
case 't':
if (teleop_transport_parse_mode(optarg, &transport_mode) != 0) {
fprintf(stderr, "Invalid transport mode: %s (expected udp or kcp)\n", optarg);
return 1;
}
break;
case 's': strncpy(kcp_server, optarg, sizeof(kcp_server)-1); kcp_server[sizeof(kcp_server)-1] = '\0'; break;
case 'r': strncpy(kcp_relay, optarg, sizeof(kcp_relay)-1); kcp_relay[sizeof(kcp_relay)-1] = '\0'; break;
case 'I': strncpy(peer_id, optarg, sizeof(peer_id)-1); peer_id[sizeof(peer_id)-1] = '\0'; break;
case 'T': strncpy(target_peer, optarg, sizeof(target_peer)-1); target_peer[sizeof(target_peer)-1] = '\0'; break;
default: usage(argv[0]); return (opt == 'h') ? 0 : 1;
}
}
if (deadzone < 0.0f || deadzone >= 1.0f) {
fprintf(stderr, "Invalid deadzone %.3f: expected 0 <= dz < 1\n", deadzone);
return 1;
}
signal(SIGINT, sigint_handler);
/* ── open joystick ───────────────────────────────────────────── */
int jsfd = open(device, O_RDONLY | O_NONBLOCK);
if (jsfd < 0) {
fprintf(stderr, "Cannot open %s: %s\n"
" Hint: connect Xbox controller, check 'ls /dev/input/js*'\n",
device, strerror(errno));
return 1;
}
char js_name[128] = "Unknown";
ioctl(jsfd, JSIOCGNAME(sizeof(js_name)), js_name);
int num_axes = 0, num_buttons = 0;
ioctl(jsfd, JSIOCGAXES, &num_axes);
ioctl(jsfd, JSIOCGBUTTONS, &num_buttons);
printf("========================================\n");
printf(" Gamepad Teleop Controller\n");
printf("========================================\n");
printf(" Device : %s\n", device);
printf(" Name : %s\n", js_name);
printf(" Axes : %d Buttons: %d\n", num_axes, num_buttons);
printf(" Transport: %s\n", teleop_transport_mode_name(transport_mode));
if (transport_mode == TELEOP_TRANSPORT_MODE_KCP) {
printf(" KCP server: %s\n", kcp_server);
if (kcp_relay[0] != '\0')
printf(" Relay via : %s\n", kcp_relay);
printf(" Peer ID : %s -> %s\n", peer_id, target_peer);
} else {
printf(" Target : %s:%d\n", ip, port);
}
printf(" Linear : %.2f m/s Angular: %.2f rad/s\n", max_lin, max_ang);
printf(" Deadzone: %.2f\n", deadzone);
printf("----------------------------------------\n");
printf(" Left stick → forward/back + strafe\n");
printf(" Right stick → turn\n");
printf(" A button → emergency stop\n");
printf(" B button → quit\n");
printf("========================================\n\n");
memset(&transport_config, 0, sizeof(transport_config));
transport_config.mode = transport_mode;
transport_config.udp_ip = ip;
transport_config.udp_port = port;
transport_config.server_addr = kcp_server;
transport_config.relay_via = kcp_relay;
transport_config.peer_id = peer_id;
transport_config.target_peer = target_peer;
if (teleop_transport_open(&transport, &transport_config) != 0) {
close(jsfd);
return 1;
}
/* ── state ───────────────────────────────────────────────────── */
float axes[MAX_AXES];
int buttons[MAX_BUTTONS];
memset(axes, 0, sizeof(axes));
memset(buttons, 0, sizeof(buttons));
twist_cmd_t cmd;
twist_cmd_zero(&cmd);
struct timeval last_send;
gettimeofday(&last_send, NULL);
int e_stop = 0;
/* ── main loop ───────────────────────────────────────────────── */
while (g_running) {
/* read all pending joystick events */
struct js_event ev;
while (read(jsfd, &ev, sizeof(ev)) == sizeof(ev)) {
ev.type &= ~JS_EVENT_INIT; /* strip init flag */
if (ev.type == JS_EVENT_AXIS && ev.number < MAX_AXES) {
axes[ev.number] = (float)ev.value / JS_AXIS_MAX;
} else if (ev.type == JS_EVENT_BUTTON && ev.number < MAX_BUTTONS) {
buttons[ev.number] = ev.value;
if (ev.number == BTN_QUIT && ev.value) {
g_running = 0;
break;
}
if (ev.number == BTN_STOP && ev.value) {
e_stop = !e_stop;
if (e_stop)
printf("\r ** EMERGENCY STOP ** ");
else
printf("\r ** E-STOP released ** ");
fflush(stdout);
}
}
}
/* EAGAIN is expected in non-blocking mode */
if (errno != EAGAIN && errno != 0) {
perror("read joystick");
break;
}
errno = 0;
/* map axes → twist (skip if e-stopped) */
if (e_stop) {
twist_cmd_zero(&cmd);
} else {
float lx_raw = apply_deadzone(-axes[AXIS_LY], deadzone); /* Y inverted */
float ly_raw = apply_deadzone(-axes[AXIS_LX], deadzone);
float az_raw = apply_deadzone(-axes[AXIS_RX], deadzone);
cmd.lx = lx_raw * max_lin;
cmd.ly = ly_raw * max_lin;
cmd.lz = 0.0f;
cmd.ax = 0.0f;
cmd.ay = 0.0f;
cmd.az = az_raw * max_ang;
}
/* rate-limit sending */
struct timeval now;
gettimeofday(&now, NULL);
long elapsed = (now.tv_sec - last_send.tv_sec) * 1000000
+ (now.tv_usec - last_send.tv_usec);
if (elapsed < SEND_INTERVAL_US) {
usleep(5000); /* 5 ms sleep to avoid busy-spin */
continue;
}
last_send = now;
teleop_transport_send_twist(&transport, &cmd);
printf("\r cmd: lx=%+.2f ly=%+.2f az=%+.2f | raw: LY=%+.2f LX=%+.2f RX=%+.2f ",
cmd.lx, cmd.ly, cmd.az,
axes[AXIS_LY], axes[AXIS_LX], axes[AXIS_RX]);
fflush(stdout);
}
/* send final stop */
twist_cmd_zero(&cmd);
teleop_transport_send_twist(&transport, &cmd);
close(jsfd);
teleop_transport_close(&transport);
printf("\nStopped.\n");
return 0;
}

361
ros-control-c/remote/keyboard_controller.c Normal file
View File

@@ -0,0 +1,361 @@
/*
* keyboard_controller.c - Keyboard teleop over UDP or KCP
*
* Keys:
* W/Up forward S/Down backward
* A/Left turn left D/Right turn right
* Q strafe left E strafe right
* Space stop
* [ / ] linear speed down/up
* - / = angular speed down/up
* Ctrl-C quit
*
* Build: gcc -Wall -O2 -I../common -o keyboard_controller keyboard_controller.c
* Usage: ./keyboard_controller [-i IP] [-p PORT] [-l MAX_LIN] [-a MAX_ANG]
* [-t udp|kcp] [-s SERVER] [-r RELAY]
* [-I PEER_ID] [-T TARGET_PEER]
*/
#include <getopt.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/select.h>
#include <sys/time.h>
#include <termios.h>
#include <unistd.h>
#include "../common/protocol.h"
#include "../common/teleop_transport.h"
/*
* Terminals do not provide key-release events, so keep the last motion command
* alive briefly to bridge the initial auto-repeat delay while a key is held.
*/
#define KEY_HOLD_TIMEOUT_US 500000L
static struct termios g_orig_termios;
static volatile sig_atomic_t g_running = 1;
static long elapsed_us(const struct timeval *start, const struct timeval *end)
{
return (end->tv_sec - start->tv_sec) * 1000000L
+ (end->tv_usec - start->tv_usec);
}
static int parse_port(const char *text, int *port_out)
{
char *end = NULL;
long value = strtol(text, &end, 10);
if (end == text || *end != '\0' || value < 1 || value > 65535)
return -1;
*port_out = (int)value;
return 0;
}
static void restore_terminal(void)
{
tcsetattr(STDIN_FILENO, TCSANOW, &g_orig_termios);
printf("\n\033[?25h");
fflush(stdout);
}
static void sigint_handler(int sig)
{
(void)sig;
g_running = 0;
}
static void set_raw_mode(void)
{
struct termios raw;
tcgetattr(STDIN_FILENO, &g_orig_termios);
atexit(restore_terminal);
raw = g_orig_termios;
raw.c_lflag &= ~(ICANON | ECHO | ISIG);
raw.c_cc[VMIN] = 0;
raw.c_cc[VTIME] = 0;
tcsetattr(STDIN_FILENO, TCSANOW, &raw);
}
static int read_key(long timeout_us)
{
fd_set fds;
struct timeval tv;
unsigned char c;
FD_ZERO(&fds);
FD_SET(STDIN_FILENO, &fds);
tv.tv_sec = timeout_us / 1000000L;
tv.tv_usec = timeout_us % 1000000L;
if (select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv) <= 0)
return -1;
if (read(STDIN_FILENO, &c, 1) != 1)
return -1;
if (c == 0x1B) {
unsigned char seq[2];
tv.tv_sec = 0;
tv.tv_usec = 20000;
FD_ZERO(&fds);
FD_SET(STDIN_FILENO, &fds);
if (select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv) <= 0)
return 0x1B;
if (read(STDIN_FILENO, &seq[0], 1) != 1)
return 0x1B;
FD_ZERO(&fds);
FD_SET(STDIN_FILENO, &fds);
tv.tv_sec = 0;
tv.tv_usec = 20000;
if (select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv) <= 0)
return 0x1B;
if (read(STDIN_FILENO, &seq[1], 1) != 1)
return 0x1B;
if (seq[0] == '[') {
switch (seq[1]) {
case 'A': return 'W';
case 'B': return 'S';
case 'D': return 'A';
case 'C': return 'D';
default: break;
}
}
return 0x1B;
}
if (c >= 'a' && c <= 'z')
c = (unsigned char)(c - ('a' - 'A'));
return c;
}
static void print_banner(void)
{
printf("\033[2J\033[H");
printf("========================================\n");
printf(" Keyboard Teleop Controller\n");
printf("========================================\n");
printf(" W/Up : forward S/Down : back\n");
printf(" A/Left : turn left D/Right: turn right\n");
printf(" Q : strafe left E : strafe right\n");
printf(" Space : stop\n");
printf(" [ / ] : linear speed -/+\n");
printf(" - / = : angular speed -/+\n");
printf(" Ctrl-C : quit\n");
printf("========================================\n\n");
}
static void usage(const char *prog)
{
fprintf(stderr,
"Usage: %s [options]\n"
" -i IP target IP (default %s)\n"
" -p PORT target port (default %d)\n"
" -l SPEED max linear speed m/s (default 0.5)\n"
" -a SPEED max angular speed rad/s (default 0.5)\n"
" -t MODE transport mode udp|kcp (default udp)\n"
" -s ADDR KCP server addr (default %s)\n"
" -r ADDR KCP relay addr (default none)\n"
" -I ID local KCP peer id (default %s)\n"
" -T ID target KCP peer id (default %s)\n"
" -h show help\n",
prog, DEFAULT_IP, DEFAULT_PORT,
DEFAULT_KCP_SERVER_ADDR,
DEFAULT_KCP_KEYBOARD_PEER_ID,
DEFAULT_KCP_TARGET_PEER_ID);
}
int main(int argc, char *argv[])
{
char ip[64] = DEFAULT_IP;
int port = DEFAULT_PORT;
char kcp_server[OMNI_MAX_ADDR_TEXT] = DEFAULT_KCP_SERVER_ADDR;
char kcp_relay[OMNI_MAX_ADDR_TEXT] = "";
char peer_id[OMNI_MAX_PEER_ID] = DEFAULT_KCP_KEYBOARD_PEER_ID;
char target_peer[OMNI_MAX_PEER_ID] = DEFAULT_KCP_TARGET_PEER_ID;
float max_lin = 0.5f;
float max_ang = 0.5f;
const float speed_step = 0.1f;
teleop_transport_mode_t transport_mode = TELEOP_TRANSPORT_MODE_UDP;
teleop_transport_t transport;
teleop_transport_config_t transport_config;
int opt;
while ((opt = getopt(argc, argv, "i:p:l:a:t:s:r:I:T:h")) != -1) {
switch (opt) {
case 'i':
strncpy(ip, optarg, sizeof(ip) - 1);
ip[sizeof(ip) - 1] = '\0';
break;
case 'p':
if (parse_port(optarg, &port) != 0) {
fprintf(stderr, "Invalid port: %s (expected 1-65535)\n", optarg);
return 1;
}
break;
case 'l':
max_lin = strtof(optarg, NULL);
break;
case 'a':
max_ang = strtof(optarg, NULL);
break;
case 't':
if (teleop_transport_parse_mode(optarg, &transport_mode) != 0) {
fprintf(stderr, "Invalid transport mode: %s (expected udp or kcp)\n", optarg);
return 1;
}
break;
case 's':
strncpy(kcp_server, optarg, sizeof(kcp_server) - 1);
kcp_server[sizeof(kcp_server) - 1] = '\0';
break;
case 'r':
strncpy(kcp_relay, optarg, sizeof(kcp_relay) - 1);
kcp_relay[sizeof(kcp_relay) - 1] = '\0';
break;
case 'I':
strncpy(peer_id, optarg, sizeof(peer_id) - 1);
peer_id[sizeof(peer_id) - 1] = '\0';
break;
case 'T':
strncpy(target_peer, optarg, sizeof(target_peer) - 1);
target_peer[sizeof(target_peer) - 1] = '\0';
break;
default:
usage(argv[0]);
return (opt == 'h') ? 0 : 1;
}
}
memset(&transport_config, 0, sizeof(transport_config));
transport_config.mode = transport_mode;
transport_config.udp_ip = ip;
transport_config.udp_port = port;
transport_config.server_addr = kcp_server;
transport_config.relay_via = kcp_relay;
transport_config.peer_id = peer_id;
transport_config.target_peer = target_peer;
if (teleop_transport_open(&transport, &transport_config) != 0) {
return 1;
}
set_raw_mode();
signal(SIGINT, sigint_handler);
print_banner();
printf(" Transport: %s\n", teleop_transport_mode_name(transport_mode));
if (transport_mode == TELEOP_TRANSPORT_MODE_KCP) {
printf(" KCP server: %s\n", kcp_server);
if (kcp_relay[0] != '\0')
printf(" Relay via : %s\n", kcp_relay);
printf(" Peer ID : %s -> %s\n", peer_id, target_peer);
} else {
printf(" Target: %s:%d\n", ip, port);
}
printf(" Linear: %.2f m/s Angular: %.2f rad/s\n\n", max_lin, max_ang);
printf("\033[?25l");
twist_cmd_t cmd;
twist_cmd_zero(&cmd);
struct timeval last_send;
struct timeval last_motion_key;
gettimeofday(&last_send, NULL);
last_motion_key = last_send;
while (g_running) {
int key = read_key(SEND_INTERVAL_US);
if (key >= 0) {
twist_cmd_zero(&cmd);
switch (key) {
case 'W':
cmd.lx = max_lin;
gettimeofday(&last_motion_key, NULL);
break;
case 'S':
cmd.lx = -max_lin;
gettimeofday(&last_motion_key, NULL);
break;
case 'A':
cmd.az = max_ang;
gettimeofday(&last_motion_key, NULL);
break;
case 'D':
cmd.az = -max_ang;
gettimeofday(&last_motion_key, NULL);
break;
case 'Q':
cmd.ly = max_lin;
gettimeofday(&last_motion_key, NULL);
break;
case 'E':
cmd.ly = -max_lin;
gettimeofday(&last_motion_key, NULL);
break;
case ' ':
break;
case ']':
max_lin += speed_step;
printf("\r Linear speed: %.2f m/s ", max_lin);
fflush(stdout);
continue;
case '[':
max_lin = (max_lin > speed_step) ? max_lin - speed_step : speed_step;
printf("\r Linear speed: %.2f m/s ", max_lin);
fflush(stdout);
continue;
case '=':
max_ang += speed_step;
printf("\r Angular speed: %.2f rad/s ", max_ang);
fflush(stdout);
continue;
case '-':
max_ang = (max_ang > speed_step) ? max_ang - speed_step : speed_step;
printf("\r Angular speed: %.2f rad/s ", max_ang);
fflush(stdout);
continue;
case 0x03:
g_running = 0;
continue;
default:
continue;
}
} else {
struct timeval now;
gettimeofday(&now, NULL);
if (elapsed_us(&last_motion_key, &now) > KEY_HOLD_TIMEOUT_US)
twist_cmd_zero(&cmd);
}
{
struct timeval now;
long elapsed;
gettimeofday(&now, NULL);
elapsed = elapsed_us(&last_send, &now);
if (elapsed < SEND_INTERVAL_US)
continue;
last_send = now;
}
teleop_transport_send_twist(&transport, &cmd);
printf("\r cmd: lx=%+.2f ly=%+.2f az=%+.2f | lin=%.2f ang=%.2f ",
cmd.lx, cmd.ly, cmd.az, max_lin, max_ang);
fflush(stdout);
}
twist_cmd_zero(&cmd);
teleop_transport_send_twist(&transport, &cmd);
teleop_transport_close(&transport);
printf("\nStopped.\n");
return 0;
}

247
ros-control-c/robot/udp_ros_bridge.py Normal file
View File

@@ -0,0 +1,247 @@
#!/usr/bin/env python3
"""
udp_ros_bridge.py — UDP/KCP → ROS2 TwistStamped bridge
Receives 24-byte binary twist commands from keyboard/gamepad controllers
via UDP or OmniSocket/KCP and publishes geometry_msgs/msg/TwistStamped to
/hric/robot/cmd_vel.
Usage:
ros2 run <your_pkg> udp_ros_bridge (if installed as a ROS2 package)
python3 udp_ros_bridge.py (standalone)
ROS2 parameters:
transport (string) — udp or kcp (default udp)
udp_port (int) — UDP listen port (default 9870)
kcp_server (string) — KCP hub addr (default 127.0.0.1:9002)
kcp_relay_via (string) — optional relay addr (default "")
peer_id (string) — local KCP peer id (default ros-bridge-ctrl)
expected_sender (string) — optional sender filter (default "")
topic (string) — publish topic (default /hric/robot/cmd_vel)
frame_id (string) — TwistStamped frame_id (default pelvis)
timeout (float) — watchdog timeout seconds (default 0.5)
"""
from pathlib import Path
import struct
import socket
import sys
import threading
import time
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TwistStamped
TWIST_CMD_FMT = '<6f' # 6 little-endian floats, 24 bytes
TWIST_CMD_SIZE = struct.calcsize(TWIST_CMD_FMT)
def _load_omnisocket():
try:
from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_BINARY, MSG_TYPE_ERROR, Session
return CONTROL_DEFAULTS, MSG_TYPE_BINARY, MSG_TYPE_ERROR, Session
except ImportError:
root = Path(__file__).resolve().parents[2]
python_dir = root / 'python'
if str(python_dir) not in sys.path:
sys.path.insert(0, str(python_dir))
from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_BINARY, MSG_TYPE_ERROR, Session
return CONTROL_DEFAULTS, MSG_TYPE_BINARY, MSG_TYPE_ERROR, Session
class UdpTeleopBridge(Node):
def __init__(self):
super().__init__('udp_teleop_bridge')
# declare parameters
self.declare_parameter('transport', 'udp')
self.declare_parameter('udp_port', 9870)
self.declare_parameter('kcp_server', '127.0.0.1:9002')
self.declare_parameter('kcp_relay_via', '')
self.declare_parameter('peer_id', 'ros-bridge-ctrl')
self.declare_parameter('expected_sender', '')
self.declare_parameter('topic', '/hric/robot/cmd_vel')
self.declare_parameter('frame_id', 'pelvis')
self.declare_parameter('timeout', 0.5)
self._transport = str(self.get_parameter('transport').value).strip().lower()
self._port = self.get_parameter('udp_port').value
self._kcp_server = str(self.get_parameter('kcp_server').value)
self._kcp_relay_via = str(self.get_parameter('kcp_relay_via').value)
self._peer_id = str(self.get_parameter('peer_id').value)
self._expected_sender = str(self.get_parameter('expected_sender').value)
self._topic = self.get_parameter('topic').value
self._frame_id = self.get_parameter('frame_id').value
self._timeout = self.get_parameter('timeout').value
if self._transport not in ('udp', 'kcp'):
raise ValueError(f"Unsupported transport '{self._transport}', expected 'udp' or 'kcp'")
# publisher
self._pub = self.create_publisher(TwistStamped, self._topic, 10)
# watchdog timer
self._last_recv = time.monotonic()
self._lock = threading.Lock()
self._latest_cmd = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
self._timer = self.create_timer(1.0 / 20.0, self._timer_cb)
self._sock = None
self._session = None
self._msg_type_binary = None
self._msg_type_error = None
self._closing = False
if self._transport == 'kcp':
control_defaults, self._msg_type_binary, self._msg_type_error, session_cls = _load_omnisocket()
self._session = session_cls()
self._session.connect(
server_addr=self._kcp_server,
peer_id=self._peer_id,
relay_via=self._kcp_relay_via,
**control_defaults,
)
else:
self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self._sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self._sock.bind(('0.0.0.0', self._port))
self._sock.settimeout(0.1)
# receive thread
recv_target = self._recv_loop_kcp if self._transport == 'kcp' else self._recv_loop_udp
self._recv_thread = threading.Thread(target=recv_target, daemon=True)
self._recv_thread.start()
if self._transport == 'kcp':
self.get_logger().info(
f'Bridge ready — KCP {self._kcp_server} as {self._peer_id} → {self._topic} '
f'(frame_id={self._frame_id}, timeout={self._timeout}s)'
)
else:
self.get_logger().info(
f'Bridge ready — UDP 0.0.0.0:{self._port} → {self._topic} '
f'(frame_id={self._frame_id}, timeout={self._timeout}s)'
)
def _recv_loop_udp(self):
"""Background thread: receive UDP packets and update latest command."""
while rclpy.ok():
try:
data, addr = self._sock.recvfrom(TWIST_CMD_SIZE + 64)
except socket.timeout:
continue
except OSError:
break
if len(data) != TWIST_CMD_SIZE:
self.get_logger().warn(
f'Packet has invalid size {len(data)} bytes from {addr}, '
f'expected {TWIST_CMD_SIZE}'
)
continue
values = struct.unpack(TWIST_CMD_FMT, data)
with self._lock:
self._latest_cmd = values
self._last_recv = time.monotonic()
def _recv_loop_kcp(self):
"""Background thread: receive KCP packets and update latest command."""
while rclpy.ok():
try:
result = self._session.recv(timeout_ms=100)
except OSError as exc:
if not self._closing:
self.get_logger().error(f'KCP receive failed: {exc}')
break
if result is None:
continue
from_peer, msg_type, payload = result
if msg_type == self._msg_type_error:
self.get_logger().error(
f'KCP server error from {from_peer}: {payload.decode("utf-8", errors="replace")}'
)
continue
if self._expected_sender and from_peer != self._expected_sender:
self.get_logger().warn(
f'Ignoring KCP packet from unexpected sender {from_peer}, '
f'expected {self._expected_sender}'
)
continue
if msg_type != self._msg_type_binary:
self.get_logger().warn(
f'Ignoring non-binary KCP message type {msg_type} from {from_peer}'
)
continue
if len(payload) != TWIST_CMD_SIZE:
self.get_logger().warn(
f'KCP payload has invalid size {len(payload)} bytes from {from_peer}, '
f'expected {TWIST_CMD_SIZE}'
)
continue
values = struct.unpack(TWIST_CMD_FMT, payload)
with self._lock:
self._latest_cmd = values
self._last_recv = time.monotonic()
def _timer_cb(self):
"""20 Hz: publish TwistStamped from latest received command."""
with self._lock:
elapsed = time.monotonic() - self._last_recv
if elapsed > self._timeout:
lx, ly, lz, ax, ay, az = 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
else:
lx, ly, lz, ax, ay, az = self._latest_cmd
msg = TwistStamped()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = self._frame_id
msg.twist.linear.x = float(lx)
msg.twist.linear.y = float(ly)
msg.twist.linear.z = float(lz)
msg.twist.angular.x = float(ax)
msg.twist.angular.y = float(ay)
msg.twist.angular.z = float(az)
self._pub.publish(msg)
def destroy_node(self):
self._closing = True
if self._sock is not None:
self._sock.close()
self._sock = None
if self._session is not None:
try:
self._session.close()
except OSError as exc:
self.get_logger().warn(f'Closing KCP session failed: {exc}')
self._session = None
if hasattr(self, '_recv_thread') and self._recv_thread.is_alive():
self._recv_thread.join(timeout=0.2)
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = None
try:
node = UdpTeleopBridge()
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
if node is not None:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()