diff --git a/AGENT.md b/AGENT.md new file mode 100644 index 0000000..12528c3 --- /dev/null +++ b/AGENT.md @@ -0,0 +1,54 @@ +# 通信场景说明 + +## 系统拓扑 +- 系统中有 `A/B/C/D` 四个终端。 +- `A` 与 `C` 在同一侧，`B` 与 `D` 在同一侧。 +- 实际运行时不是一条端到端 KCP，而是两条独立 KCP 链路： + - `B <-> D` + - `D <-> A` +- `C` 只负责 UDP 端口转发。 +- 整体目标是让 `A` 与 `B` 双向通信： + - `A -> B` 发送控制命令 + - `B -> A` 发送视频和其他反馈数据 + +## 网络情况 +- 主要瓶颈在 `B` 侧 5G 链路，`A` 侧是有线网络，不是主要约束。 +- `B <-> D` 的时延抖动较大： + - 常见约 `15-60 ms` + - 最差约 `80-90 ms` +- 带宽是明显非对称的： + - `B -> D` 约 `8 Mbps`，偏低且不稳定 + - `D -> B` 约 `27-50 Mbps`，更高但也有波动 +- 工程含义： + - 控制流必须优先保障 + - 视频流必须限速并具备自适应能力 + - 不能默认把所有流合成一个共享传输就一定更优 + +## 三个项目的角色 +- `OmniSocketGo` + - 底层传输项目 + - 使用 C 编写 + - 提供 OmniSocket/KCP 传输能力，以及视频发送等原生传输程序 +- `robot-command-center` + - A 侧上层应用 + - 主要负责接收视频帧，并提供监控/展示能力 + - 当前通过本机 `A-side OmniDaemon` 使用传输能力 +- `tienkung-szu` + - 上层控制项目 + - 同时包含 A 侧控制发送和 B 侧控制执行逻辑 + - A 侧典型入口是键盘/Xbox sender + - B 侧典型入口是 `rl_control_node.py` / `rl_control_node_sim.py`，负责接收控制并驱动机器人行为 + +## 当前架构方向 +- `A` 侧通过 `A-side OmniDaemon` 统一管理本机的控制发送和视频接收。 +- `B` 侧通过 `B-side OmniDaemon` 统一管理本机的控制接收和视频发送。 +- 核心原则： + - 控制流与视频流逻辑分离 + - 每台主机通过本地 daemon 统一持有传输会话 + - 后续围绕真实网络瓶颈做 telemetry、调度和速率自适应 + +## 代码原则 +- 一定要精简，不能写过于复杂和冗余的代码，因为必须确保各个环节可控，在低延迟的要求下，需要细化各个环节的消耗 + +## SSH原则 +- 如果你需要SSH到某个远程端执行操作，一定要确保不做删除文件操作，确保留有记录 \ No newline at end of file diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..12528c3 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,54 @@ +# 通信场景说明 + +## 系统拓扑 +- 系统中有 `A/B/C/D` 四个终端。 +- `A` 与 `C` 在同一侧，`B` 与 `D` 在同一侧。 +- 实际运行时不是一条端到端 KCP，而是两条独立 KCP 链路： + - `B <-> D` + - `D <-> A` +- `C` 只负责 UDP 端口转发。 +- 整体目标是让 `A` 与 `B` 双向通信： + - `A -> B` 发送控制命令 + - `B -> A` 发送视频和其他反馈数据 + +## 网络情况 +- 主要瓶颈在 `B` 侧 5G 链路，`A` 侧是有线网络，不是主要约束。 +- `B <-> D` 的时延抖动较大： + - 常见约 `15-60 ms` + - 最差约 `80-90 ms` +- 带宽是明显非对称的： + - `B -> D` 约 `8 Mbps`，偏低且不稳定 + - `D -> B` 约 `27-50 Mbps`，更高但也有波动 +- 工程含义： + - 控制流必须优先保障 + - 视频流必须限速并具备自适应能力 + - 不能默认把所有流合成一个共享传输就一定更优 + +## 三个项目的角色 +- `OmniSocketGo` + - 底层传输项目 + - 使用 C 编写 + - 提供 OmniSocket/KCP 传输能力，以及视频发送等原生传输程序 +- `robot-command-center` + - A 侧上层应用 + - 主要负责接收视频帧，并提供监控/展示能力 + - 当前通过本机 `A-side OmniDaemon` 使用传输能力 +- `tienkung-szu` + - 上层控制项目 + - 同时包含 A 侧控制发送和 B 侧控制执行逻辑 + - A 侧典型入口是键盘/Xbox sender + - B 侧典型入口是 `rl_control_node.py` / `rl_control_node_sim.py`，负责接收控制并驱动机器人行为 + +## 当前架构方向 +- `A` 侧通过 `A-side OmniDaemon` 统一管理本机的控制发送和视频接收。 +- `B` 侧通过 `B-side OmniDaemon` 统一管理本机的控制接收和视频发送。 +- 核心原则： + - 控制流与视频流逻辑分离 + - 每台主机通过本地 daemon 统一持有传输会话 + - 后续围绕真实网络瓶颈做 telemetry、调度和速率自适应 + +## 代码原则 +- 一定要精简，不能写过于复杂和冗余的代码，因为必须确保各个环节可控，在低延迟的要求下，需要细化各个环节的消耗 + +## SSH原则 +- 如果你需要SSH到某个远程端执行操作，一定要确保不做删除文件操作，确保留有记录 \ No newline at end of file diff --git a/ros-control-c/Makefile b/ros-control-c/Makefile new file mode 100644 index 0000000..26b692e --- /dev/null +++ b/ros-control-c/Makefile @@ -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) diff --git a/ros-control-c/README.md b/ros-control-c/README.md new file mode 100644 index 0000000..98b4726 --- /dev/null +++ b/ros-control-c/README.md @@ -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 ` 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 +``` diff --git a/ros-control-c/Robot Remote Control via UDP - Implementation Plan.md b/ros-control-c/Robot Remote Control via UDP - Implementation Plan.md new file mode 100644 index 0000000..350852b --- /dev/null +++ b/ros-control-c/Robot Remote Control via UDP - Implementation Plan.md @@ -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 , -p , -l , -a + + --- + 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 , -a , -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 + + 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 \ No newline at end of file diff --git a/ros-control-c/build/gamepad_controller b/ros-control-c/build/gamepad_controller new file mode 100644 index 0000000..3aa3cde Binary files /dev/null and b/ros-control-c/build/gamepad_controller differ diff --git a/ros-control-c/build/keyboard_controller b/ros-control-c/build/keyboard_controller new file mode 100644 index 0000000..a9d37a3 Binary files /dev/null and b/ros-control-c/build/keyboard_controller differ diff --git a/ros-control-c/common/protocol.h b/ros-control-c/common/protocol.h new file mode 100644 index 0000000..91adf5b --- /dev/null +++ b/ros-control-c/common/protocol.h @@ -0,0 +1,26 @@ +#ifndef PROTOCOL_H +#define PROTOCOL_H + +#include + +#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 */ diff --git a/ros-control-c/common/teleop_transport.c b/ros-control-c/common/teleop_transport.c new file mode 100644 index 0000000..c5d875d --- /dev/null +++ b/ros-control-c/common/teleop_transport.c @@ -0,0 +1,300 @@ +#include "teleop_transport.h" + +#include +#include +#include +#include +#include +#include + +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; + } +} diff --git a/ros-control-c/common/teleop_transport.h b/ros-control-c/common/teleop_transport.h new file mode 100644 index 0000000..6061aff --- /dev/null +++ b/ros-control-c/common/teleop_transport.h @@ -0,0 +1,59 @@ +#ifndef TELEOP_TRANSPORT_H +#define TELEOP_TRANSPORT_H + +#include +#include + +#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 */ diff --git a/ros-control-c/remote/gamepad_controller.c b/ros-control-c/remote/gamepad_controller.c new file mode 100644 index 0000000..db96133 --- /dev/null +++ b/ros-control-c/remote/gamepad_controller.c @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/ros-control-c/remote/keyboard_controller.c b/ros-control-c/remote/keyboard_controller.c new file mode 100644 index 0000000..239dad8 --- /dev/null +++ b/ros-control-c/remote/keyboard_controller.c @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/ros-control-c/robot/udp_ros_bridge.py b/ros-control-c/robot/udp_ros_bridge.py new file mode 100644 index 0000000..1b435d8 --- /dev/null +++ b/ros-control-c/robot/udp_ros_bridge.py @@ -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 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()