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

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;
}