362 lines
11 KiB
C
362 lines
11 KiB
C
/*
|
|
* 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;
|
|
}
|