initial commit
This commit is contained in:
huangweifeng
@@ -0,0 +1,347 @@
|
||||
/**
|
||||
* @file gamepad_interface.hpp
|
||||
* @brief gamepad
|
||||
* @author DeepRobotics
|
||||
* @version 1.0
|
||||
* @date 2026-01-07
|
||||
*
|
||||
* @copyright Copyright (c) 2025 DeepRobotics
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <functional>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <list>
|
||||
#include <set>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "common_types.h"
|
||||
#include "custom_types.h"
|
||||
#include "heartbeat.hpp"
|
||||
#include "json.hpp"
|
||||
#include "user_command_interface.h"
|
||||
|
||||
using json = nlohmann::json;
|
||||
|
||||
class GamepadInterface : public UserCommandInterface {
|
||||
public:
|
||||
GamepadInterface(RobotName robot_name) : UserCommandInterface(robot_name) {
|
||||
mHeartbeat_ = std::make_unique<Heartbeat>();
|
||||
|
||||
mHandleMap_[100][100] = std::bind(&GamepadInterface::Handle_Heartbeat, this, std::placeholders::_1,
|
||||
std::placeholders::_2, std::placeholders::_3);
|
||||
mHandleMap_[2][21] = std::bind(&GamepadInterface::Handle_Steer, this, std::placeholders::_1,
|
||||
std::placeholders::_2, std::placeholders::_3);
|
||||
mHandleMap_[1101][12] = std::bind(&GamepadInterface::Handle_Key, this, std::placeholders::_1,
|
||||
std::placeholders::_2, std::placeholders::_3);
|
||||
}
|
||||
|
||||
~GamepadInterface() {}
|
||||
|
||||
void Start() override {
|
||||
std::cout << "\n╔════════════════════════════════════════════════╗\n"
|
||||
<< "║ GAMEPAD TELEOP ║\n"
|
||||
<< "╚════════════════════════════════════════════════╝\n"
|
||||
<< " Movement: Left joystick\n"
|
||||
<< " Rotation: Right joystick\n"
|
||||
<< " Mode: R2 (damping) L1 (stand) L2 (control)\n"
|
||||
<< "\n";
|
||||
}
|
||||
|
||||
void Stop() override {}
|
||||
UserCommand* GetUserCommand() override { return usr_cmd_; }
|
||||
int ParseHeader(const char* buffer, int len) {
|
||||
const int headerSize = 16;
|
||||
if (len < headerSize) return -1;
|
||||
|
||||
if ((uint8_t)buffer[0] == 0xeb && (uint8_t)buffer[2] == 0xeb && (uint8_t)buffer[3] == 0x90) {
|
||||
if ((uint8_t)buffer[1] != 0x90 && (uint8_t)buffer[1] != 0x91) {
|
||||
std::cout<<"Header format error"<<std::endl;
|
||||
return -1;
|
||||
}
|
||||
int bodyLen = ((uint16_t)buffer[5] << 8) | (uint8_t)buffer[4];
|
||||
return bodyLen + headerSize;
|
||||
}
|
||||
std::cout<<"Header magic error"<<std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
bool ParseHeader(const char* buffer, int len, int& data_id, char* body, FromType& fromType) {
|
||||
const int headerSize = 16;
|
||||
if (len < headerSize) return false;
|
||||
|
||||
if ((uint8_t)buffer[0] == 0xeb && (uint8_t)buffer[2] == 0xeb && (uint8_t)buffer[3] == 0x90) {
|
||||
if ((uint8_t)buffer[1] == 0x90) {
|
||||
fromType = FromType::inside;
|
||||
} else if ((uint8_t)buffer[1] == 0x91) {
|
||||
fromType = FromType::user;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
int bodyLen = ((uint16_t)buffer[5] << 8) | (uint8_t)buffer[4];
|
||||
data_id = ((uint16_t)buffer[7] << 8) | (uint8_t)buffer[6];
|
||||
|
||||
if (bodyLen != len - headerSize) {
|
||||
std::cout << "recv lost! bodyLen=" << bodyLen << " real=" << len - headerSize << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t dateFlag = buffer[8] & 0x03;
|
||||
if (dateFlag != 0x01) {
|
||||
std::cout << "Data format error" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
memcpy(body, buffer + headerSize, bodyLen);
|
||||
body[bodyLen] = '\0';
|
||||
} else {
|
||||
std::cout << "Header sync error" << std::endl;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ParseData(const DataInfo& dataInfo, std::string& response) {
|
||||
int type = 0, command = 0;
|
||||
bool ret = false;
|
||||
|
||||
mHeartbeat_->isConntected_ = true;
|
||||
mHeartbeat_->lastHeartbeatTime_ = std::chrono::steady_clock::now();
|
||||
|
||||
json document;
|
||||
json items;
|
||||
|
||||
ret = ParseJson(dataInfo.buffer, dataInfo.len, document, type, command, items);
|
||||
if (!ret) {
|
||||
std::cout<<"JSON parse failed"<<std::endl;
|
||||
response = "JSON format parsing failed";
|
||||
return false;
|
||||
}
|
||||
|
||||
CmdInfo cmd_info;
|
||||
cmd_info.ip = dataInfo.ip;
|
||||
cmd_info.port = dataInfo.port;
|
||||
cmd_info.fromType = dataInfo.fromType;
|
||||
cmd_info.type = type;
|
||||
cmd_info.command = command;
|
||||
|
||||
if (mHandleMap_[type][command]) {
|
||||
mHandleMap_[type][command](items, response, cmd_info);
|
||||
} else {
|
||||
response = buildResponse(cmd_info, 1, "Unknown command");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void AddDataHeader(const std::string& in, std::string& out, int data_id = 0, NetInfo perNetInfo = {0}) {
|
||||
out.resize(16);
|
||||
|
||||
out[0] = 0xeb;
|
||||
if (perNetInfo.fromType == FromType::inside) {
|
||||
out[1] = 0x90;
|
||||
} else {
|
||||
out[1] = 0x91;
|
||||
}
|
||||
out[2] = 0xeb;
|
||||
out[3] = 0x90;
|
||||
|
||||
auto size = in.size();
|
||||
|
||||
out[4] = (uint8_t)(size & 0xFF);
|
||||
out[5] = (uint8_t)((size >> 8) & 0xFF);
|
||||
|
||||
out[6] = (uint8_t)(data_id & 0xFF);
|
||||
out[7] = (uint8_t)((data_id >> 8) & 0xFF);
|
||||
|
||||
out[8] = 1;
|
||||
|
||||
for (int i = 9; i < 16; i++) out[i] = 0x00;
|
||||
|
||||
out += in;
|
||||
}
|
||||
|
||||
private:
|
||||
std::unique_ptr<Heartbeat> mHeartbeat_;
|
||||
|
||||
using HANDLE_FUNC = std::function<void(json& item, std::string& res, const CmdInfo& cmd_info)>;
|
||||
|
||||
std::unordered_map<int, std::unordered_map<int, HANDLE_FUNC>> mHandleMap_;
|
||||
|
||||
bool ParseJson(const char* buf, int len, json& document, int& type, int& command, json& items) {
|
||||
if (!buf || len == 0) return false;
|
||||
|
||||
try {
|
||||
document = json::parse(buf, buf + len);
|
||||
|
||||
if (!document.is_object() || !document.contains("PatrolDevice"))
|
||||
throw std::runtime_error("No PatrolDevice");
|
||||
|
||||
json& root = document["PatrolDevice"];
|
||||
if (!root.is_object()) throw std::runtime_error("PatrolDevice not object");
|
||||
|
||||
if (!root.contains("Items")) throw std::runtime_error("No Items");
|
||||
|
||||
items = root["Items"];
|
||||
|
||||
if (!root.contains("Type") || !root.contains("Command")) throw std::runtime_error("No Type/Command");
|
||||
|
||||
if (!root["Type"].is_number_integer() || !root["Command"].is_number_integer())
|
||||
throw std::runtime_error("Type/Command not Int");
|
||||
|
||||
type = root["Type"].get<int>();
|
||||
command = root["Command"].get<int>();
|
||||
|
||||
} catch (const json::parse_error& e) {
|
||||
std::cerr << "JSON Parse Error: " << e.what() << std::endl;
|
||||
return false;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "JSON Logic Error: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void JSONRootToBroker(const int& type, const int& command, json& root, bool isNew = false) {
|
||||
if (isNew) {
|
||||
root = json::object();
|
||||
root["PatrolDevice"] = {
|
||||
{"Items", json::object()}, {"Type", type}, {"Command", command}, {"Time", getCurrentDateTime()}};
|
||||
} else {
|
||||
root["PatrolDevice"]["Type"] = type;
|
||||
root["PatrolDevice"]["Command"] = command;
|
||||
root["PatrolDevice"]["Time"] = getCurrentDateTime();
|
||||
}
|
||||
}
|
||||
|
||||
std::string buildResponse(const CmdInfo& cmd_info, const int& errorCode, std::string errorMsg = "") {
|
||||
std::string res;
|
||||
json document;
|
||||
|
||||
JSONRootToBroker(cmd_info.type, cmd_info.command, document, true);
|
||||
|
||||
document["PatrolDevice"]["Items"]["ErrorCode"] = errorCode;
|
||||
|
||||
if (!errorMsg.empty()) {
|
||||
document["PatrolDevice"]["Items"]["ErrorMessage"] = errorMsg;
|
||||
}
|
||||
|
||||
res = document.dump();
|
||||
return res;
|
||||
}
|
||||
|
||||
void Handle_Heartbeat(json& items, std::string& res, const CmdInfo& cmd_info) {
|
||||
NetInfo netInfo;
|
||||
netInfo.ip = cmd_info.ip;
|
||||
netInfo.port = cmd_info.port;
|
||||
netInfo.fromType = cmd_info.fromType;
|
||||
mHeartbeat_->wakeUp(netInfo);
|
||||
|
||||
res = buildResponse(cmd_info, 0);
|
||||
}
|
||||
|
||||
void Handle_Steer(json& items, std::string& res, const CmdInfo& cmd_info) {
|
||||
if (!items.contains("X") || !items.contains("Y")) {
|
||||
res = buildResponse(cmd_info, 2, "Missing fields");
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
if (items["X"].is_number()) usr_cmd_->forward_vel_scale = items["X"].get<float>();
|
||||
if (items["Y"].is_number()) usr_cmd_->side_vel_scale = items["Y"].get<float>();
|
||||
if (items.contains("Yaw") && items["Yaw"].is_number())
|
||||
usr_cmd_->turnning_vel_scale = items["Yaw"].get<float>();
|
||||
} catch (const std::exception& e) {
|
||||
res = buildResponse(cmd_info, 3, "Data type error");
|
||||
return;
|
||||
}
|
||||
|
||||
static std::string LastPerIp = "";
|
||||
static int64_t LastPerTime = 0;
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
auto timestamp = std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count();
|
||||
|
||||
if (LastPerIp == "" || LastPerIp == cmd_info.ip) {
|
||||
LastPerIp = cmd_info.ip;
|
||||
LastPerTime = timestamp;
|
||||
} else if (timestamp - LastPerTime > 2) {
|
||||
LastPerIp = cmd_info.ip;
|
||||
LastPerTime = timestamp;
|
||||
} else {
|
||||
std::cout << "IP Conflict" << std::endl;
|
||||
res = buildResponse(cmd_info, 4, "IP Conflict");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void Handle_Key(json& items, std::string& res, const CmdInfo& cmd_info) {
|
||||
static std::unordered_map<std::string, KeyCode> keyMap = {{"G20_KEY_L1", KeyCode::L1},
|
||||
{"G20_KEY_L2", KeyCode::L2},
|
||||
{"G20_KEY_R1", KeyCode::R1},
|
||||
{"G20_KEY_R2", KeyCode::R2}};
|
||||
|
||||
if (items.contains("KeyList") && items["KeyList"].is_array()) {
|
||||
for (const auto& key : items["KeyList"]) {
|
||||
if (key.is_string()) {
|
||||
std::string keyStr = key.get<std::string>();
|
||||
|
||||
if (keyMap.count(keyStr)) {
|
||||
KeyCode code = keyMap[keyStr];
|
||||
switch (code) {
|
||||
case KeyCode::L1:
|
||||
if (msfb_->GetCurrentState() == RobotMotionState::WaitingForStand
|
||||
|| msfb_->GetCurrentState() == RobotMotionState::LieDown) {
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::StandingUp);
|
||||
std::cout << "[MODE] Standing Up\n";
|
||||
}
|
||||
break;
|
||||
case KeyCode::L2:
|
||||
if (msfb_->GetCurrentState() == RobotMotionState::StandingUp) {
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::RLControlMode);
|
||||
std::cout << "[MODE] RL Control\n";
|
||||
}
|
||||
break;
|
||||
case KeyCode::R1:
|
||||
if (msfb_->GetCurrentState() == RobotMotionState::StandingUp
|
||||
|| msfb_->GetCurrentState() == RobotMotionState::RLControlMode) {
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::LieDown);
|
||||
std::cout << "[MODE] Lie Down\n";
|
||||
}
|
||||
break;
|
||||
case KeyCode::R2:
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::JointDamping);
|
||||
std::cout << "[MODE] Joint Damping\n";
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
res = buildResponse(cmd_info, 0);
|
||||
}
|
||||
|
||||
std::string getCurrentDateTime() {
|
||||
auto now = std::chrono::system_clock::now();
|
||||
std::time_t currentTime = std::chrono::system_clock::to_time_t(now);
|
||||
std::tm* localTime = std::localtime(¤tTime);
|
||||
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
|
||||
std::stringstream ss;
|
||||
ss << std::put_time(localTime, "%Y-%m-%d %H:%M:%S") << "." << std::setw(3) << std::setfill('0') << ms.count();
|
||||
return ss.str();
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,242 @@
|
||||
// keyboard_interface.hpp
|
||||
#pragma once
|
||||
|
||||
#include "user_command_interface.h"
|
||||
#include "custom_types.h"
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <unordered_map>
|
||||
#include <unordered_set>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <iostream>
|
||||
#include <chrono>
|
||||
#include <cctype>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
||||
using namespace interface;
|
||||
using namespace types;
|
||||
|
||||
class KeyboardInterface : public UserCommandInterface
|
||||
{
|
||||
private:
|
||||
std::atomic<bool> running_{false};
|
||||
std::thread kb_thread_;
|
||||
mutable std::mutex keys_mutex_;
|
||||
|
||||
float max_forward_ = 0.7f;
|
||||
float max_side_ = 0.5f;
|
||||
float max_yaw_ = 0.7f;
|
||||
|
||||
std::unordered_set<char> held_keys_;
|
||||
std::unordered_map<char, double> last_seen_time_;
|
||||
|
||||
const std::unordered_set<char> velocity_keys_ = {'w', 's', 'a', 'd', 'q', 'e'};
|
||||
const double key_timeout_ms_ = 500.0;
|
||||
|
||||
void ClipNumber(float& num, float low, float high)
|
||||
{
|
||||
if (num < low) num = low;
|
||||
if (num > high) num = high;
|
||||
}
|
||||
|
||||
double GetCurrentTimeStamp()
|
||||
{
|
||||
static auto start = std::chrono::steady_clock::now();
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
return std::chrono::duration<double, std::milli>(now - start).count();
|
||||
}
|
||||
|
||||
static void setup_raw_mode()
|
||||
{
|
||||
termios t{};
|
||||
tcgetattr(STDIN_FILENO, &t);
|
||||
termios raw = t;
|
||||
raw.c_lflag &= ~(ECHO | ICANON);
|
||||
raw.c_cc[VMIN] = 0;
|
||||
raw.c_cc[VTIME] = 0;
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &raw);
|
||||
|
||||
int flags = fcntl(STDIN_FILENO, F_GETFL, 0);
|
||||
fcntl(STDIN_FILENO, F_SETFL, flags | O_NONBLOCK);
|
||||
}
|
||||
|
||||
static void restore_terminal()
|
||||
{
|
||||
termios t{};
|
||||
tcgetattr(STDIN_FILENO, &t);
|
||||
t.c_lflag |= (ECHO | ICANON);
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &t);
|
||||
}
|
||||
|
||||
void compute_velocity_from_held_keys(float& fwd, float& side, float& yaw)
|
||||
{
|
||||
fwd = 0.0f;
|
||||
side = 0.0f;
|
||||
yaw = 0.0f;
|
||||
|
||||
std::lock_guard<std::mutex> lock(keys_mutex_);
|
||||
|
||||
if (held_keys_.count('w')) fwd += max_forward_;
|
||||
if (held_keys_.count('s')) fwd -= max_forward_;
|
||||
if (held_keys_.count('a')) side += max_side_;
|
||||
if (held_keys_.count('d')) side -= max_side_;
|
||||
if (held_keys_.count('q')) yaw += max_yaw_;
|
||||
if (held_keys_.count('e')) yaw -= max_yaw_;
|
||||
|
||||
ClipNumber(fwd, -max_forward_, max_forward_);
|
||||
ClipNumber(side, -max_side_, max_side_);
|
||||
ClipNumber(yaw, -max_yaw_, max_yaw_);
|
||||
}
|
||||
|
||||
void process_mode_command(char k)
|
||||
{
|
||||
if (k == 'r') {
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::JointDamping);
|
||||
std::cout << "[MODE] Joint Damping\n";
|
||||
}
|
||||
else if (k == 'z' && (msfb_->GetCurrentState() == RobotMotionState::WaitingForStand
|
||||
|| msfb_->GetCurrentState() == RobotMotionState::LieDown)) {
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::StandingUp);
|
||||
std::cout << "[MODE] Standing Up\n";
|
||||
}
|
||||
else if (k == 'c' && msfb_->GetCurrentState() == RobotMotionState::StandingUp) {
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::RLControlMode);
|
||||
std::cout << "[MODE] RL Control\n";
|
||||
}
|
||||
else if (k == 'x' && (msfb_->GetCurrentState() == RobotMotionState::StandingUp
|
||||
|| msfb_->GetCurrentState() == RobotMotionState::RLControlMode)) {
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::LieDown);
|
||||
std::cout << "[MODE] Lie Down\n";
|
||||
}
|
||||
}
|
||||
|
||||
void keyboard_loop()
|
||||
{
|
||||
setup_raw_mode();
|
||||
|
||||
std::cout << "\n╔════════════════════════════════════════════════╗\n"
|
||||
<< "║ KEYBOARD TELEOP - MULTI-KEY READY ║\n"
|
||||
<< "╚════════════════════════════════════════════════╝\n"
|
||||
<< " Movement: W/S (forward/back) A/D (left/right)\n"
|
||||
<< " Rotation: Q (CCW) E (CW)\n"
|
||||
<< " Mode: R (damping) Z (stand) C (control)\n"
|
||||
<< "\n";
|
||||
|
||||
char ch;
|
||||
|
||||
while (running_) {
|
||||
double now = GetCurrentTimeStamp();
|
||||
usr_cmd_->time_stamp = now;
|
||||
|
||||
// Read all available keyboard input
|
||||
while (read(STDIN_FILENO, &ch, 1) == 1) {
|
||||
char k = std::tolower(static_cast<unsigned char>(ch));
|
||||
|
||||
// Handle mode commands
|
||||
if (k == 'r' || k == 'z' || k == 'c' || k == 'x') {
|
||||
process_mode_command(k);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Track velocity keys
|
||||
if (velocity_keys_.count(k)) {
|
||||
std::lock_guard<std::mutex> lock(keys_mutex_);
|
||||
held_keys_.insert(k);
|
||||
last_seen_time_[k] = now;
|
||||
}
|
||||
}
|
||||
|
||||
// Remove keys that haven't been seen recently (released)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(keys_mutex_);
|
||||
std::vector<char> to_remove;
|
||||
|
||||
for (char k : held_keys_) {
|
||||
if (now - last_seen_time_[k] > key_timeout_ms_) {
|
||||
to_remove.push_back(k);
|
||||
}
|
||||
}
|
||||
|
||||
for (char k : to_remove) {
|
||||
held_keys_.erase(k);
|
||||
last_seen_time_.erase(k);
|
||||
}
|
||||
}
|
||||
|
||||
// Compute velocity from all currently held keys
|
||||
float fwd = 0.0f, side = 0.0f, yaw = 0.0f;
|
||||
|
||||
if (msfb_->GetCurrentState() == RobotMotionState::RLControlMode) {
|
||||
compute_velocity_from_held_keys(fwd, side, yaw);
|
||||
}
|
||||
|
||||
usr_cmd_->forward_vel_scale = fwd;
|
||||
usr_cmd_->side_vel_scale = side;
|
||||
usr_cmd_->turnning_vel_scale = yaw;
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||||
}
|
||||
|
||||
restore_terminal();
|
||||
std::cout << "\n[KEYBOARD] Stopped.\n";
|
||||
}
|
||||
|
||||
public:
|
||||
KeyboardInterface(RobotName robot_name) : UserCommandInterface(robot_name)
|
||||
{
|
||||
if (robot_name == RobotName::FireAnt) {
|
||||
// Stage-2 was trained for forward motion and yaw, with lateral command fixed at zero.
|
||||
max_forward_ = 1.2f;
|
||||
max_side_ = 0.0f;
|
||||
max_yaw_ = 0.6f;
|
||||
}
|
||||
std::cout << "[KeyboardInterface] Initialized with multi-key support\n";
|
||||
std::memset(usr_cmd_, 0, sizeof(UserCommand));
|
||||
}
|
||||
|
||||
~KeyboardInterface()
|
||||
{
|
||||
Stop();
|
||||
}
|
||||
|
||||
void Start() override
|
||||
{
|
||||
if (running_) return;
|
||||
running_ = true;
|
||||
kb_thread_ = std::thread(&KeyboardInterface::keyboard_loop, this);
|
||||
}
|
||||
|
||||
void Stop() override
|
||||
{
|
||||
running_ = false;
|
||||
if (kb_thread_.joinable()) {
|
||||
kb_thread_.join();
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(keys_mutex_);
|
||||
held_keys_.clear();
|
||||
last_seen_time_.clear();
|
||||
|
||||
usr_cmd_->forward_vel_scale = 0.0f;
|
||||
usr_cmd_->side_vel_scale = 0.0f;
|
||||
usr_cmd_->turnning_vel_scale = 0.0f;
|
||||
}
|
||||
|
||||
UserCommand* GetUserCommand() override
|
||||
{
|
||||
return usr_cmd_;
|
||||
}
|
||||
|
||||
void set_max_velocities(float fwd, float side, float yaw)
|
||||
{
|
||||
max_forward_ = std::abs(fwd);
|
||||
max_side_ = std::abs(side);
|
||||
max_yaw_ = std::abs(yaw);
|
||||
std::cout << "[CONFIG] Max velocities: fwd=" << max_forward_
|
||||
<< " side=" << max_side_
|
||||
<< " yaw=" << max_yaw_ << "\n";
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,284 @@
|
||||
// keyboard_interface.hpp
|
||||
// sudo apt install libevdev-dev
|
||||
// sudo adduser $USER input
|
||||
// newgrp input
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "user_command_interface.h"
|
||||
#include "custom_types.h"
|
||||
#include <libevdev-1.0/libevdev/libevdev.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <dirent.h>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <unordered_set>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <chrono>
|
||||
|
||||
using namespace interface;
|
||||
using namespace types;
|
||||
|
||||
class KeyboardInterface : public UserCommandInterface
|
||||
{
|
||||
private:
|
||||
std::atomic<bool> running_{false};
|
||||
std::thread kb_thread_;
|
||||
|
||||
float max_forward_ = 0.7f;
|
||||
float max_side_ = 0.5f;
|
||||
float max_yaw_ = 0.7f;
|
||||
|
||||
// Current velocity commands (ramp-up on repeat, zero on release)
|
||||
float fwd = 0.0f, side = 0.0f, yaw = 0.0f;
|
||||
|
||||
std::unordered_set<int> pressed_keys_;
|
||||
mutable std::mutex keys_mutex_;
|
||||
|
||||
struct EvDevKeyboard {
|
||||
int fd;
|
||||
struct libevdev* dev;
|
||||
std::string name;
|
||||
};
|
||||
std::vector<EvDevKeyboard> keyboards_;
|
||||
|
||||
void ClipNumber(float& num, float low, float high)
|
||||
{
|
||||
if (num < low) num = low;
|
||||
if (num > high) num = high;
|
||||
}
|
||||
|
||||
bool isKeyPressed(int keycode) const
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(keys_mutex_);
|
||||
return pressed_keys_.count(keycode);
|
||||
}
|
||||
|
||||
// Called on press (value=1) and repeat (value=2)
|
||||
void apply_key_direction(int keycode)
|
||||
{
|
||||
const float step = 0.1f;
|
||||
|
||||
if (keycode == KEY_W) fwd += step * max_forward_;
|
||||
else if (keycode == KEY_S) fwd -= step * max_forward_;
|
||||
else if (keycode == KEY_A) side += step * max_side_;
|
||||
else if (keycode == KEY_D) side -= step * max_side_;
|
||||
else if (keycode == KEY_Q) yaw += step * max_yaw_;
|
||||
else if (keycode == KEY_E) yaw -= step * max_yaw_;
|
||||
|
||||
ClipNumber(fwd, -max_forward_, max_forward_);
|
||||
ClipNumber(side, -max_side_, max_side_);
|
||||
ClipNumber(yaw, -max_yaw_, max_yaw_);
|
||||
}
|
||||
|
||||
// Called exactly on key release (value=0)
|
||||
void handle_key_release(int keycode)
|
||||
{
|
||||
if ((keycode == KEY_W || keycode == KEY_S) &&
|
||||
!isKeyPressed(KEY_W) && !isKeyPressed(KEY_S)) {
|
||||
fwd = 0.0f;
|
||||
}
|
||||
if ((keycode == KEY_A || keycode == KEY_D) &&
|
||||
!isKeyPressed(KEY_A) && !isKeyPressed(KEY_D)) {
|
||||
side = 0.0f;
|
||||
}
|
||||
if ((keycode == KEY_Q || keycode == KEY_E) &&
|
||||
!isKeyPressed(KEY_Q) && !isKeyPressed(KEY_E)) {
|
||||
yaw = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void process_mode_key(int keycode)
|
||||
{
|
||||
if (keycode == KEY_R) {
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::JointDamping);
|
||||
std::cout << "[MODE] Joint Damping\n";
|
||||
}
|
||||
else if (keycode == KEY_Z && msfb_->GetCurrentState() == RobotMotionState::WaitingForStand) {
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::StandingUp);
|
||||
std::cout << "[MODE] Standing Up\n";
|
||||
}
|
||||
else if (keycode == KEY_C && msfb_->GetCurrentState() == RobotMotionState::StandingUp) {
|
||||
usr_cmd_->target_mode = uint8_t(RobotMotionState::RLControlMode);
|
||||
std::cout << "[MODE] RL Control\n";
|
||||
}
|
||||
}
|
||||
|
||||
bool init_all_keyboards()
|
||||
{
|
||||
DIR* dir = opendir("/dev/input");
|
||||
if (!dir) return false;
|
||||
|
||||
struct dirent* ent;
|
||||
while ((ent = readdir(dir)) != nullptr) {
|
||||
if (strncmp(ent->d_name, "event", 5) != 0) continue;
|
||||
|
||||
std::string path = "/dev/input/" + std::string(ent->d_name);
|
||||
int fd = open(path.c_str(), O_RDONLY | O_NONBLOCK);
|
||||
if (fd < 0) continue;
|
||||
|
||||
struct libevdev* dev = nullptr;
|
||||
if (libevdev_new_from_fd(fd, &dev) < 0) {
|
||||
close(fd);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!libevdev_has_event_type(dev, EV_KEY) ||
|
||||
!libevdev_has_event_code(dev, EV_KEY, KEY_A)) {
|
||||
libevdev_free(dev);
|
||||
close(fd);
|
||||
continue;
|
||||
}
|
||||
|
||||
const char* name = libevdev_get_name(dev);
|
||||
std::string name_str = name ? name : "Unknown";
|
||||
|
||||
if (name_str.find("Mouse") != std::string::npos ||
|
||||
name_str.find("Touchpad") != std::string::npos) {
|
||||
libevdev_free(dev);
|
||||
close(fd);
|
||||
continue;
|
||||
}
|
||||
|
||||
keyboards_.push_back({fd, dev, name_str});
|
||||
std::cout << "[KB] Detected: " << name_str << " → " << path << "\n";
|
||||
}
|
||||
closedir(dir);
|
||||
|
||||
if (keyboards_.empty()) {
|
||||
std::cerr << "[KeyboardInterface] ERROR: No keyboards found!\n";
|
||||
return false;
|
||||
}
|
||||
|
||||
std::cout << "[KeyboardInterface] Initialized " << keyboards_.size() << " keyboard(s).\n";
|
||||
return true;
|
||||
}
|
||||
|
||||
void keyboard_loop()
|
||||
{
|
||||
if (!init_all_keyboards()) {
|
||||
std::cerr << "[KeyboardInterface] Failed to initialize keyboards!\n";
|
||||
return;
|
||||
}
|
||||
|
||||
std::cout << "\n╔════════════════════════════════════════════════╗\n"
|
||||
<< "║ KEYBOARD TELEOP – RAMP-UP + INSTANT STOP ║\n"
|
||||
<< "╚════════════════════════════════════════════════╝\n"
|
||||
<< " Hold W/S/A/D/Q/E → velocity ramps up\n"
|
||||
<< " Release key → instant stop on that axis\n"
|
||||
<< " Modes: R (damping) Z (stand) C (RL control)\n\n";
|
||||
|
||||
struct input_event ev;
|
||||
|
||||
while (running_) {
|
||||
bool got_event = false;
|
||||
|
||||
for (auto& kb : keyboards_) {
|
||||
while (libevdev_next_event(kb.dev, LIBEVDEV_READ_FLAG_NORMAL, &ev) == 0) {
|
||||
got_event = true;
|
||||
if (ev.type != EV_KEY) continue;
|
||||
|
||||
bool was_pressed = pressed_keys_.count(ev.code);
|
||||
|
||||
// Update pressed state
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(keys_mutex_);
|
||||
if (ev.value == 1 || ev.value == 2) {
|
||||
pressed_keys_.insert(ev.code);
|
||||
} else if (ev.value == 0) {
|
||||
pressed_keys_.erase(ev.code);
|
||||
}
|
||||
}
|
||||
|
||||
// Press or repeat → ramp up
|
||||
if (ev.value == 1 || ev.value == 2) {
|
||||
if (ev.code == KEY_W || ev.code == KEY_S ||
|
||||
ev.code == KEY_A || ev.code == KEY_D ||
|
||||
ev.code == KEY_Q || ev.code == KEY_E) {
|
||||
apply_key_direction(ev.code);
|
||||
}
|
||||
}
|
||||
|
||||
// Release → zero axis if needed
|
||||
if (was_pressed && ev.value == 0) {
|
||||
handle_key_release(ev.code);
|
||||
}
|
||||
|
||||
// Mode keys (only on real press)
|
||||
if (ev.value == 1) {
|
||||
if (ev.code == KEY_R || ev.code == KEY_Z || ev.code == KEY_C) {
|
||||
process_mode_key(ev.code);
|
||||
}
|
||||
if (ev.code == KEY_ESC) {
|
||||
std::cout << "\n[KEYBOARD] ESC pressed → stopping.\n";
|
||||
running_ = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ─────── CRITICAL: ALWAYS publish current values every loop ───────
|
||||
usr_cmd_->forward_vel_scale = fwd;
|
||||
usr_cmd_->side_vel_scale = side;
|
||||
usr_cmd_->turnning_vel_scale = yaw;
|
||||
usr_cmd_->time_stamp = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::steady_clock::now().time_since_epoch()).count() / 1000.0;
|
||||
|
||||
// Optional nice live display
|
||||
if (msfb_->GetCurrentState() == RobotMotionState::RLControlMode) {
|
||||
std::cout << "\rvel: " << fwd << " " << side << " " << yaw << std::flush;
|
||||
}
|
||||
|
||||
if (!got_event) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(4));
|
||||
}
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
public:
|
||||
KeyboardInterface(RobotName robot_name) : UserCommandInterface(robot_name)
|
||||
{
|
||||
std::cout << "[KeyboardInterface] Ramp-up + instant-stop version ready!\n";
|
||||
std::memset(usr_cmd_, 0, sizeof(UserCommand));
|
||||
}
|
||||
|
||||
~KeyboardInterface() { Stop(); }
|
||||
|
||||
void Start() override
|
||||
{
|
||||
if (running_) return;
|
||||
running_ = true;
|
||||
kb_thread_ = std::thread(&KeyboardInterface::keyboard_loop, this);
|
||||
}
|
||||
|
||||
void Stop() override
|
||||
{
|
||||
running_ = false;
|
||||
if (kb_thread_.joinable()) kb_thread_.join();
|
||||
|
||||
for (auto& kb : keyboards_) {
|
||||
libevdev_free(kb.dev);
|
||||
close(kb.fd);
|
||||
}
|
||||
keyboards_.clear();
|
||||
|
||||
usr_cmd_->forward_vel_scale = usr_cmd_->side_vel_scale = usr_cmd_->turnning_vel_scale = 0.0f;
|
||||
}
|
||||
|
||||
UserCommand* GetUserCommand() override { return usr_cmd_; }
|
||||
|
||||
void set_max_velocities(float fwd, float side, float yaw)
|
||||
{
|
||||
max_forward_ = std::abs(fwd);
|
||||
max_side_ = std::abs(side);
|
||||
max_yaw_ = std::abs(yaw);
|
||||
std::cout << "[CONFIG] Max velocities → fwd:" << max_forward_
|
||||
<< " side:" << max_side_
|
||||
<< " yaw:" << max_yaw_ << "\n";
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,62 @@
|
||||
/**
|
||||
* @file user_command_interface.h
|
||||
* @brief this file is used for robot's user command input
|
||||
* @author DeepRobotics
|
||||
* @version 1.0
|
||||
* @date 2025-11-07
|
||||
*
|
||||
* @copyright Copyright (c) 2025 DeepRobotics
|
||||
*
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "common_types.h"
|
||||
#include "custom_types.h"
|
||||
#include "motion_state_feedback.hpp"
|
||||
|
||||
using namespace types;
|
||||
|
||||
namespace interface{
|
||||
|
||||
class UserCommandInterface{
|
||||
private:
|
||||
/* data */
|
||||
public:
|
||||
UserCommandInterface(RobotName robot_name){
|
||||
robot_name_ = robot_name;
|
||||
usr_cmd_ = new UserCommand();
|
||||
std::memset(usr_cmd_, 0, sizeof(UserCommand));
|
||||
}
|
||||
virtual ~UserCommandInterface(){
|
||||
delete usr_cmd_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief start the thread to process user command
|
||||
*/
|
||||
virtual void Start() = 0;
|
||||
|
||||
/**
|
||||
* @brief stop the thread
|
||||
*/
|
||||
virtual void Stop() = 0;
|
||||
|
||||
/**
|
||||
* @brief return your user command
|
||||
* @return UserCommand ptr
|
||||
*/
|
||||
virtual UserCommand* GetUserCommand() = 0;
|
||||
|
||||
/**
|
||||
* @brief set the motion state feedback
|
||||
* @param msfb motion state feedback
|
||||
*/
|
||||
virtual void SetMotionStateFeedback(MotionStateFeedback* msfb){
|
||||
msfb_ = msfb;
|
||||
}
|
||||
|
||||
MotionStateFeedback *msfb_;
|
||||
RobotName robot_name_;
|
||||
UserCommand *usr_cmd_;
|
||||
};
|
||||
};
|
||||
Reference in New Issue
Block a user