LinkNexus/huoyi_deploy
20
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
huangweifeng
2026-10-22 09:23:54 +08:00
commit 63f557a39f
4448 changed files with 584487 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
interface/CMakeLists.txt
+31
View File
@@ -0,0 +1,31 @@
cmake_minimum_required(VERSION 3.10)
# project(interface)
set(CMAKE_BUILD_TYPE Release)
add_definitions(-w) # warning ignore
add_compile_options(-fPIC)
set(CMAKE_CXX_STANDARD 17)
include_directories(
./robot
./user_command
)
# file(GLOB_RECURSE SIMULATOIN_SRC "robot/simulation/*.hpp")
file(GLOB_RECURSE USER_COMMAND_SRC "user_command/*.cpp")
set(GAMEPAD_SDK_PATH "${THIRD_PARTY}/gamepad")
include_directories(${GAMEPAD_SDK_PATH}/include)
file(GLOB_RECURSE GAMEPAD_SDK_SRC "${GAMEPAD_SDK_PATH}/src/*.cpp")
add_library(interface STATIC ${USER_COMMAND_SRC} ${GAMEPAD_SDK_SRC})
target_link_libraries(interface PUBLIC -lpthread -lm -lrt -ldl -lstdc++)
# add_library(user_command SHARED ${USER_COMMAND_SRC})
# target_link_libraries(user_command gamepad_sdk)
# add_executable(test ./test.cpp)
# target_link_libraries(test simulation gamepad_sdk)
# add_subdirectory(simulation)
interface/network/heartbeat.hpp
+112
View File
@@ -0,0 +1,112 @@
/**
* @file heartbeat.hpp
* @brief heartbeat
* @author DeepRobotics
* @version 1.0
* @date 2026-01-07
*
* @copyright Copyright (c) 2025 DeepRobotics
*
*/
#pragma once
#include <chrono>
#include <list>
#include <mutex>
#include <thread>
class Heartbeat {
public:
Heartbeat() {
mIsRunning_ = true;
heartbeatCheckThread_ = std::make_unique<std::thread>(&Heartbeat::heartbeatCheckThread, this);
}
~Heartbeat() {
mIsRunning_ = false;
heartbeatCheckThread_.get()->join();
}
void wakeUp(const NetInfo& netInfo) {
std::lock_guard<std::mutex> lock(mutex_);
for (auto& clientInfo : clientInfoList_) {
if (clientInfo.ip == netInfo.ip && clientInfo.port == netInfo.port) {
clientInfo.isLive = true;
return;
}
}
ClientInfo clientInfo;
clientInfo.ip = netInfo.ip;
clientInfo.port = netInfo.port;
clientInfo.fromType = netInfo.fromType;
clientInfo.isLive = true;
clientInfoList_.push_back(clientInfo);
}
void GetLiveList(std::list<NetInfo>& netInfos) {
netInfos.clear();
std::lock_guard<std::mutex> lock(mutex_);
for (auto& clientInfo : clientInfoList_) {
NetInfo netInfo;
netInfo.ip = clientInfo.ip;
netInfo.port = clientInfo.port;
netInfo.fromType = clientInfo.fromType;
netInfos.push_back(netInfo);
}
}
void DeleteClient(const NetInfo& netInfo) {
std::lock_guard<std::mutex> lock(mutex_);
for (auto it = clientInfoList_.begin(); it != clientInfoList_.end();) {
if (it->ip == netInfo.ip && it->port == netInfo.port) {
it = clientInfoList_.erase(it);
} else {
it++;
}
}
}
private:
void heartbeatCheckThread() {
bool flag = false;
while (mIsRunning_) {
std::this_thread::sleep_for(std::chrono::seconds(2));
auto now = std::chrono::steady_clock::now();
auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - lastHeartbeatTime_).count();
if (elapsed >= 2) {
if (!flag) std::cout<<"Heartbeat timeout!"<<std::endl;
isConntected_ = false;
flag = true;
clientInfoList_.clear();
} else {
isConntected_ = true;
flag = false;
}
if (clientInfoList_.size() > 0) {
std::lock_guard<std::mutex> lock(mutex_);
for (auto it = clientInfoList_.begin(); it != clientInfoList_.end();) {
if (it->isLive == false) {
it = clientInfoList_.erase(it);
} else {
it->isLive = false;
it++;
}
}
}
}
}
public:
bool mIsRunning_;
std::chrono::time_point<std::chrono::steady_clock> lastHeartbeatTime_;
std::unique_ptr<std::thread> heartbeatCheckThread_;
bool isConntected_ = false;
struct ClientInfo : public NetInfo {
bool isLive;
};
std::mutex mutex_;
std::list<ClientInfo> clientInfoList_;
};
interface/network/udp_server.hpp
+180
View File
@@ -0,0 +1,180 @@
/**
* @file udp_server.hpp
* @brief udp server
* @author DeepRobotics
* @version 1.0
* @date 2026-01-07
*
* @copyright Copyright (c) 2025 DeepRobotics
*
*/
#pragma once
#include <arpa/inet.h>
#include <fcntl.h>
#include <unistd.h>
#include <atomic>
#include <cstring>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <vector>
#include "common_types.h"
#include "gamepad_interface.hpp"
#include "thread_pool.hpp"
#define UDP_PORT 30000
#define IP "0.0.0.0"
#define UDP_BUFFER_SIZE 1024 * 32
class ThreadPool;
class GamepadInterface;
class UdpServer {
public:
UdpServer(GamepadInterface* gp) : mGamepadInterface_(gp) {
mThreadPool_ = std::make_unique<ThreadPool>(10);
if (!Init()) {
std::cerr << "[UdpServer] Init Failed!" << std::endl;
}
mIsRunning_ = true;
mServerThread_ = std::thread(&UdpServer::RecvLoop, this);
std::cout << "UDP Server started, listening on port: " << UDP_PORT << std::endl;
}
~UdpServer() {
mIsRunning_ = false;
if (mServerThread_.joinable()) {
mServerThread_.join();
}
if (mFd >= 0) {
std::cout << "Closing UDP socket..." << std::endl;
close(mFd);
mFd = -1;
}
}
private:
bool Init() {
mFd = socket(AF_INET, SOCK_DGRAM, 0);
if (mFd < 0) {
perror("Create socket failed");
return false;
}
int bufSize = 1024 * 1024 * 25;
setsockopt(mFd, SOL_SOCKET, SO_RCVBUF, &bufSize, sizeof(bufSize));
setsockopt(mFd, SOL_SOCKET, SO_SNDBUF, &bufSize, sizeof(bufSize));
struct timeval timeout;
timeout.tv_sec = 5;
timeout.tv_usec = 0;
if (setsockopt(mFd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) < 0) {
perror("Set timeout failed");
close(mFd);
return false;
}
int opt = 1;
setsockopt(mFd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
sockaddr_in serverAddr{};
serverAddr.sin_family = AF_INET;
serverAddr.sin_addr.s_addr = inet_addr(IP);
serverAddr.sin_port = htons(UDP_PORT);
if (bind(mFd, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) {
perror("Bind failed");
close(mFd);
return false;
}
return true;
}
void RecvLoop() {
struct sockaddr_in client_addr;
socklen_t client_addr_len = sizeof(client_addr);
std::vector<char> buffer(UDP_BUFFER_SIZE);
while (mIsRunning_) {
ssize_t recvLen =
recvfrom(mFd, buffer.data(), UDP_BUFFER_SIZE, 0, (struct sockaddr*)&client_addr, &client_addr_len);
if (recvLen > 0) {
auto data = std::make_shared<std::vector<char>>(buffer.begin(), buffer.begin() + recvLen);
auto headerLen = mGamepadInterface_->ParseHeader(data->data(), recvLen);
if (headerLen > 0 && headerLen == recvLen) {
dataHandle(mFd, data, headerLen, client_addr);
} else {
std::cout<<"Invalid header"<<std::endl;
}
}
}
}
void dataHandle(int socket, std::shared_ptr<std::vector<char>> data, int dataLen, const sockaddr_in& clientAddr) {
mThreadPool_->enqueue([=]() {
FromType from_type;
bool ret = false;
int dataId = -1;
std::string res;
DataInfo dataInfo;
if (mGamepadInterface_->ParseHeader(data->data(), dataLen, dataId, data->data(), from_type)) {
std::string ip = inet_ntoa(clientAddr.sin_addr);
int port = ntohs(clientAddr.sin_port);
dataInfo.ip = ip;
dataInfo.port = port;
dataInfo.fromType = from_type;
dataInfo.buffer = data->data();
dataInfo.len = dataLen;
ret = mGamepadInterface_->ParseData(dataInfo, res);
} else {
res = "Protocol header error,error code:0x2";
}
if (res.length() > 0) {
std::string Response;
mGamepadInterface_->AddDataHeader(res, Response, dataId, dataInfo);
sendData(socket, Response.c_str(), Response.length(), clientAddr);
}
});
}
void sendData(int socket, const char* data, int dataLen, const sockaddr_in& clientAddr) {
if (dataLen > 0) {
ssize_t sentBytes = 0;
while (sentBytes < dataLen) {
ssize_t sendNum = sendto(socket, data + sentBytes, dataLen - sentBytes, 0,
(struct sockaddr*)&clientAddr, sizeof(clientAddr));
if (sendNum < 0) {
std::cout << "Send data failure: " << strerror(errno) << std::endl;
return;
}
sentBytes += sendNum;
}
}
}
private:
int mFd = -1;
std::atomic<bool> mIsRunning_{false};
std::thread mServerThread_;
std::unique_ptr<ThreadPool> mThreadPool_;
std::unique_ptr<GamepadInterface> mGamepadInterface_;
};
interface/robot/hardware/dds_interface.hpp
+311
View File
@@ -0,0 +1,311 @@
/**
* @file dds_hardware_interface.hpp
* @brief hardware interface for dds
* @author DeepRobotics
* @version 1.0
* @date 2025-11-07
*
* @copyright Copyright (c) 2025 DeepRobotics
*
*/
#pragma once
#include "common_types.h"
#include "robot_interface.h"
#include "dds_types.h"
#include "drdds/msg/joints_data.hpp"
#include "drdds/msg/joints_data_cmd.hpp"
#include "drdds/msg/imu_data.hpp"
#include "drdds/msg/battery_data.hpp"
using namespace dds;
struct JointConfig {
float dir;
float offset;
};
class DdsInterface : public RobotInterface{
protected:
double ri_ts_, imu_ts_;
Vec3f omega_body_, rpy_, acc_, base_lin_vel_;
VecXf joint_pos_, joint_vel_, joint_tau_;
VecXf contact_force_;
VecXf motor_temperture_, driver_temperture_;
float *pos_offset_;
float *joint_dir_;
bool *data_updated_;
uint16_t *control_word;
JointConfig *joint_config_;
std::vector<uint16_t> driver_status_;
std::vector<uint16_t> joint_data_id_;
BatteryInfo_dds battery_info_[2];
std::vector<uint16_t> battery_data_;
double time_stamp_;
float remain_bat;
rclcpp::Publisher<drdds::msg::JointsDataCmd>::SharedPtr joint_cmd_pub_;
rclcpp::Subscription<drdds::msg::JointsData>::SharedPtr joint_data_sub_;
rclcpp::Subscription<drdds::msg::ImuData>::SharedPtr imu_data_sub_;
rclcpp::Subscription<drdds::msg::BatteryData>::SharedPtr health_data_sub_;
bool IsDataUpdatedFinished() {
bool res = data_updated_[0];
for (int i = 0; i < dof_num_; ++i) {
res = res & data_updated_[i];
}
return res;
}
void ResetPositionOffset() {
memset(data_updated_, 0, dof_num_ * sizeof(bool));
this->SetJointCommand(MatXf::Zero(dof_num_, 5));
usleep(100 * 1000);
VecXf last_joint_pos = this->GetJointPosition();
VecXf current_joint_pos = this->GetJointPosition();
int cnt = 0;
while (!IsDataUpdatedFinished()) {
++cnt;
this->RefreshRobotData();
rclcpp::spin_some(this->get_node());
current_joint_pos = this->GetJointPosition();
for (int i = 0; i < dof_num_; ++i) {
if (!data_updated_[i] && current_joint_pos(i) != last_joint_pos(i)) {
data_updated_[i] = true;
}
}
last_joint_pos = current_joint_pos;
usleep(1000);
if (cnt == 10000) {
std::cout << data_updated_ << std::endl;
std::cout << "joint data update is not finished\n";
}
}
for (int i = 1; i < dof_num_; i += 4) {
if (current_joint_pos(i) < -210. / 180. * M_PI) {
pos_offset_[i] = pos_offset_[i] + 360.;
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
} else if (current_joint_pos(i) > 90. / 180. * M_PI) {
pos_offset_[i] = pos_offset_[i] - 360.;
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
}
}
for (int i = 0; i < dof_num_; ++i) {
joint_config_[i].dir = joint_dir_[i];
joint_config_[i].offset = Deg2Rad(pos_offset_[i]);
}
}
public:
int run_cnt_ = 0;
DdsInterface(const std::string &robot_name, const int &dof_num) : RobotInterface(robot_name, dof_num) {
std::cout << robot_name << " is using DDS Interface" << std::endl;
joint_pos_ = VecXf::Zero(dof_num_);
joint_vel_ = VecXf::Zero(dof_num_);
joint_tau_ = VecXf::Zero(dof_num_);
motor_temperture_ = VecXf::Zero(dof_num_);
driver_temperture_ = VecXf::Zero(dof_num_);
omega_body_.setZero();
rpy_.setZero();
acc_.setZero();
base_lin_vel_.setZero();
contact_force_ = VecXf::Zero(9);
ri_ts_ = 0.0;
imu_ts_ = 0.0;
time_stamp_ = 0.0;
remain_bat = 0.0f;
pos_offset_ = new float[dof_num_];
joint_dir_ = new float[dof_num_];
data_updated_ = new bool[dof_num_];
joint_config_ = new JointConfig[dof_num_];
control_word = new uint16_t[4];
for (int i = 0; i < dof_num_; ++i) {
pos_offset_[i] = 0.0f;
joint_dir_[i] = 1.0f;
data_updated_[i] = false;
joint_config_[i].dir = 1.0f;
joint_config_[i].offset = 0.0f;
}
driver_status_.assign(dof_num_, 1);
joint_data_id_.resize(dof_num_);
battery_data_.resize(BATTERY_DATA_SIZE);
joint_cmd_pub_ = node_->create_publisher<drdds::msg::JointsDataCmd>("/JOINTS_CMD", 10);
joint_data_sub_ = node_->create_subscription<drdds::msg::JointsData>("/JOINTS_DATA", 10,
std::bind(&DdsInterface::Handler, this,std::placeholders::_1));
imu_data_sub_ = node_->create_subscription<drdds::msg::ImuData>("/IMU_DATA", 10,
std::bind(&DdsInterface::HandlerIMU, this, std::placeholders::_1));
health_data_sub_ = node_->create_subscription<drdds::msg::BatteryData>("/BATTERY_DATA", 10,
std::bind(&DdsInterface::HandlerHealth, this, std::placeholders::_1));
sleep(1);
control_word[0] = kIndexDisable;
control_word[1] = kIndexErrorReset;
control_word[2] = kIndexEnable;
control_word[3] = kIndexGetStatusWord;
ResetJointError();
}
virtual ~DdsInterface() {
delete[] pos_offset_;
delete[] joint_dir_;
delete[] data_updated_;
delete[] joint_config_;
}
virtual void Start() {
}
virtual void Stop() {
}
virtual double GetInterfaceTimeStamp() {
return ri_ts_;
}
virtual VecXf GetJointPosition() {
return joint_pos_;
}
virtual VecXf GetJointVelocity() {
return joint_vel_;
}
virtual VecXf GetJointTorque() {
return joint_tau_;
}
virtual Vec3f GetImuRpy() {
return rpy_;
}
virtual Vec3f GetImuAcc() {
return acc_;
}
virtual Vec3f GetImuOmega() {
return omega_body_;
}
virtual Vec3f GetBaseLinearVelocity() {
return base_lin_vel_;
}
virtual VecXf GetContactForce() {
return contact_force_;
}
virtual void ResetJointError() {
auto msg = drdds::msg::JointsDataCmd();
for (int j = 0; j < 4; ++j) {
for (int i = 0; i < dof_num_; ++i) {
msg.data.joints_data[i].position = 0;
msg.data.joints_data[i].velocity = 0;
msg.data.joints_data[i].torque = 0;
msg.data.joints_data[i].kp = 0;
msg.data.joints_data[i].kd = 0;
msg.data.joints_data[i].control_word = control_word[j];
}
std::cout << "ResetJointError publish!! \n" << std::endl;
joint_cmd_pub_->publish(msg);
usleep(10 * 1000);
}
}
virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) {
joint_cmd_ = input;
auto msg = drdds::msg::JointsDataCmd();
for (int i = 0; i < dof_num_; ++i) {
msg.data.joints_data[i].position =
(input(i, 1) - joint_config_[i].offset) * joint_config_[i].dir;
msg.data.joints_data[i].velocity = input(i, 3) * joint_dir_[i];
msg.data.joints_data[i].torque = input(i, 4) * joint_dir_[i];
msg.data.joints_data[i].kp = input(i, 0);
msg.data.joints_data[i].kd = input(i, 2);
msg.data.joints_data[i].control_word = kIndexMotorControl;
}
std::cout << "SetJointCommand publish!! \n" << std::endl;
joint_cmd_pub_->publish(msg);
}
virtual VecXf GetMotorTemperture() {
return motor_temperture_;
}
virtual VecXf GetDriverTemperture() {
return driver_temperture_;
}
virtual double GetImuTimestamp() {
return imu_ts_;
}
virtual std::vector<uint16_t> GetBatteryData() {
return battery_data_;
}
virtual std::vector<uint16_t> GetDriverStatusWord() {
return driver_status_;
}
virtual std::vector<uint16_t> GetJointDataID() {
return joint_data_id_;
}
virtual void RefreshRobotData() {
}
virtual void Handler(const drdds::msg::JointsData::SharedPtr msg) {
++run_cnt_;
for (int i = 0; i < dof_num_; ++i) {
joint_pos_(i) = msg->data.joints_data[i].position * joint_dir_[i] + pos_offset_[i] / 180. * M_PI;
joint_vel_(i) = msg->data.joints_data[i].velocity * joint_dir_[i];
joint_tau_(i) = msg->data.joints_data[i].torque * joint_dir_[i];
motor_temperture_(i) = float(msg->data.joints_data[i].motion_temp);
driver_temperture_(i) = float(msg->data.joints_data[i].driver_temp);
driver_status_[i] = msg->data.joints_data[i].status_word;
joint_data_id_[i] = uint16_t(run_cnt_);
}
// FireAnt sim2sim telemetry is packed into unused entries of the fixed
// 16-joint DDS message by mujoco_simulation_ros2.py.
if (dof_num_ == 8 && msg->data.joints_data.size() >= 16) {
for (int i = 0; i < 4; ++i) {
contact_force_(i) = msg->data.joints_data[8 + i].position;
contact_force_(5 + i) = msg->data.joints_data[12 + i].position;
}
contact_force_(4) = msg->data.joints_data[12].velocity;
for (int i = 0; i < 3; ++i) {
base_lin_vel_(i) = msg->data.joints_data[8 + i].torque;
}
}
ri_ts_ = rclcpp::Time(msg->header.stamp).seconds();
}
void HandlerIMU(const drdds::msg::ImuData::SharedPtr msg) {
rpy_ = Vec3f(Deg2Rad(msg->data.roll), Deg2Rad(msg->data.pitch), Deg2Rad(msg->data.yaw));
acc_ << msg->data.acc_x, msg->data.acc_y, msg->data.acc_z;
omega_body_ << msg->data.omega_x, msg->data.omega_y, msg->data.omega_z;
imu_ts_ = rclcpp::Time(msg->header.stamp).seconds();
}
void HandlerHealth(const drdds::msg::BatteryData::SharedPtr msg) {
battery_data_[0] = uint8_t(msg->data[0].battery_level);
remain_bat = battery_data_[0] / 100;
battery_info_[0].battery_level = msg->data[0].battery_level;
}
};
interface/robot/hardware/fire_ant_interface.hpp
+41
View File
@@ -0,0 +1,41 @@
#pragma once
#include "dds_interface.hpp"
/** Identity-calibrated DDS interface for the 8-DoF FireAnt MuJoCo model. */
class FireAntInterface : public DdsInterface {
public:
explicit FireAntInterface(const std::string &robot_name)
: DdsInterface(robot_name, 8) {
for (int i = 0; i < dof_num_; ++i) {
pos_offset_[i] = 0.0f;
joint_dir_[i] = 1.0f;
data_updated_[i] = false;
joint_config_[i].dir = 1.0f;
joint_config_[i].offset = 0.0f;
}
}
void Start() override {
time_stamp_ = GetTimestampMs();
}
void Stop() override {}
void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) override {
if (input.rows() != dof_num_ || input.cols() != 5) {
throw std::runtime_error("FireAnt joint command must be 8x5");
}
joint_cmd_ = input;
auto msg = drdds::msg::JointsDataCmd();
for (int i = 0; i < dof_num_; ++i) {
msg.data.joints_data[i].position = input(i, 1);
msg.data.joints_data[i].velocity = input(i, 3);
msg.data.joints_data[i].torque = input(i, 4);
msg.data.joints_data[i].kp = (i % 2 == 0) ? input(i, 0) : 0.0f;
msg.data.joints_data[i].kd = input(i, 2);
msg.data.joints_data[i].control_word = kIndexMotorControl;
}
joint_cmd_pub_->publish(msg);
}
};
interface/robot/hardware/m20_interface.hpp
+121
View File
@@ -0,0 +1,121 @@
#pragma once
#include "robot_interface.h"
#include "dds_interface.hpp"
class M20Interface : public DdsInterface {
protected:
void ResetPositionOffset() {
memset(data_updated_, 0, dof_num_ * sizeof(bool));
this->SetJointCommand(MatXf::Zero(dof_num_, 5));
VecXf last_joint_pos = this->GetJointPosition();
VecXf current_joint_pos = this->GetJointPosition();
int cnt = 0;
while (!IsDataUpdatedFinished()) {
++cnt;
usleep(1000);
rclcpp::spin_some(this->get_node());
current_joint_pos = this->GetJointPosition();
for (int i = 0; i < dof_num_; ++i) {
if (!data_updated_[i] && current_joint_pos(i) != last_joint_pos(i) &&
!std::isnan(current_joint_pos(i))) {
data_updated_[i] = true;
std::cout << "joint " << i << " data updated at " << cnt << " cnt!" << std::endl;
}
}
last_joint_pos = current_joint_pos;
if (cnt > 10000) {
for (int i = 0; i < dof_num_; ++i) {
std::cout << i << " :" << data_updated_[i] << std::endl;
}
std::cout << "joint data update is not finished\n";
}
}
for (int i = 1; i < dof_num_; i += 4) {
if (current_joint_pos(i) < -140. / 180. * M_PI) {
pos_offset_[i] = pos_offset_[i] + 360.;
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
} else if (current_joint_pos(i) > 140. / 180. * M_PI) {
pos_offset_[i] = pos_offset_[i] - 360.;
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
}
}
for (int i = 2; i < dof_num_; i += 4) {
if (current_joint_pos(i) < -164. / 180. * M_PI) {
pos_offset_[i] = pos_offset_[i] + 360.;
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
} else if (current_joint_pos(i) > 164. / 180. * M_PI) {
pos_offset_[i] = pos_offset_[i] - 360.;
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
}
}
for (int i = 0; i < dof_num_; ++i) {
joint_config_[i].dir = joint_dir_[i];
joint_config_[i].offset = Deg2Rad(pos_offset_[i]);
}
}
public:
M20Interface(const std::string &robot_name) : DdsInterface(robot_name, 16) {
battery_data_.resize(2 * BATTERY_DATA_SIZE);
float init_pos_offset[16] = {-25, -131, 160, 0.,
25, -131, 160, 0,
-25, 131, -160, 0,
25, 131, -160, 0};
float joint_dir[16] = {1, 1, -1, 1,
1, -1, 1, -1,
-1, 1, -1, 1.,
-1, -1, 1, -1};
for (int i = 0; i < dof_num_; ++i) {
pos_offset_[i] = init_pos_offset[i];
joint_dir_[i] = joint_dir[i];
data_updated_[i] = false;
joint_config_[i].dir = joint_dir_[i];
joint_config_[i].offset = Deg2Rad(pos_offset_[i]);
}
}
~M20Interface() {
}
virtual void Start() {
time_stamp_ = GetTimestampMs();
ResetPositionOffset();
}
virtual void Stop() {
}
/**
* Sends joint commands to the robot.
* Input matrix layout: [dof × 5] → [kp, pos, kd, vel, torque_ff]
*
* ⚠ Torque values (column 4) are protected by low-level hardware torque limits
* → commands exceeding limits are saturated at the firmware/hardware level
*
* Joints with index % 4 == 3 have kp forced to 0 (velocity/torque mode)
*/
virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) {
auto msg = drdds::msg::JointsDataCmd();
for (int i = 0; i < dof_num_; ++i) {
msg.data.joints_data[i].position =
(input(i, 1) - joint_config_[i].offset) * joint_config_[i].dir;
msg.data.joints_data[i].velocity = input(i, 3) * joint_dir_[i];
msg.data.joints_data[i].torque = input(i, 4) * joint_dir_[i];
msg.data.joints_data[i].kp = input(i, 0);
if (i % 4 == 3) {
msg.data.joints_data[i].kp = 0;
}
msg.data.joints_data[i].kd = input(i, 2);
msg.data.joints_data[i].control_word = kIndexMotorControl;
}
joint_cmd_pub_->publish(msg);
}
};
interface/robot/robot_interface.h
+175
View File
@@ -0,0 +1,175 @@
/**
* @file robot_interface.h
* @brief this file is for applying robot's control interface
* @author DeepRobotics
* @version 1.0
* @date 2025-11-07
*
* @copyright Copyright (c) 2025 DeepRobotics
*
*/
#pragma once
#include "common_types.h"
#include <atomic>
#include "rclcpp/rclcpp.hpp"
using namespace types;
namespace interface{
const int BATTERY_DATA_SIZE=8;
class RobotInterface{
private:
/* data */
public:
/**
* @brief Construct a new Robot Interface object
* @param robot_name robot name
* @param dof_num The number of degrees of freedom of the robot.
*/
RobotInterface(const std::string& robot_name, int dof_num):robot_name_(robot_name), dof_num_(dof_num){
start_flag_ = true;
joint_cmd_ = Eigen::Matrix<float, Eigen::Dynamic, 5>::Zero(dof_num_, 5);
node_ = std::make_shared<rclcpp::Node>("rl_deploy");
}
virtual ~RobotInterface(){};
std::string robot_name_;
const int dof_num_;
Eigen::Matrix<float, Eigen::Dynamic, 5> joint_cmd_;
std::atomic<bool> start_flag_;
rclcpp::Node::SharedPtr node_;
rclcpp::Node::SharedPtr get_node() {
return node_;
}
/**
* @brief Start to get robot's state and send control command
*/
virtual void Start() = 0;
/**
* @brief Stop to control the robot
*/
virtual void Stop() = 0;
/**
* @brief Get the time stamp of the robot
* @return double time stamp
*/
virtual double GetInterfaceTimeStamp() = 0;
/**
* @brief Get the joint position of the robot
* @return VecXf joint position vector
*/
virtual VecXf GetJointPosition() = 0;
/**
* @brief Get the joint velocity of the robot
* @return VecXf joint velocity vector
*/
virtual VecXf GetJointVelocity() = 0;
/**
* @brief Get the joint torque of the robot
* @return VecXf joint torque vector
*/
virtual VecXf GetJointTorque() = 0;
/**
* @brief Get the roll-pitch-yaw angle of the robot base
* @return Vec3f roll-pitch-yaw(unit rad)
*/
virtual Vec3f GetImuRpy() = 0;
/**
* @brief Get the accleration of the robot base
* @return Vec3f accleration(unit m*s^-2)
*/
virtual Vec3f GetImuAcc() = 0;
/**
* @brief Get the angular velocity of robot base
* @return Vec3f angular velocity in body coordinate(unit rad/s)
*/
virtual Vec3f GetImuOmega() = 0;
/** Body-frame base linear velocity, when supplied by the simulator/estimator. */
virtual Vec3f GetBaseLinearVelocity(){
return Vec3f::Zero();
}
/**
* @brief Set the joint command in standard form
* torque = kp * (qDes - q) + kd * (vDes - v) + tff
* @param input a dof_num*5 matrix and each column represent kp, goal_angle_pos, kd, goal_vel, torque_feedforward
*/
virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) = 0;
virtual MatXf GetJointCommand(){
return joint_cmd_;
}
/**
* @brief Get the contact force if robot have any force sensor
* @return VecXf force vector
*/
virtual VecXf GetContactForce() = 0;
/**
* @brief Get the motor temperture
* @return VecXf motor temperture
*/
virtual VecXf GetMotorTemperture(){
return 30.*VecXf::Ones(dof_num_);
}
/**
* @brief Get the driver temperture
* @return VecXf driver temperture
*/
virtual VecXf GetDriverTemperture(){
return VecXf::Zero(dof_num_);
}
/**
* @brief Get the Battery Data
* @return std::vector<uint16_t> level(0~100), voltage(mV), current(mA), cycles, temperture*4(C)
*/
virtual std::vector<uint16_t> GetBatteryData(){
return std::vector<uint16_t>(BATTERY_DATA_SIZE, 0);
}
/**
* @brief Get the Imu Timestamp
* @return float
*/
virtual double GetImuTimestamp(){
return 0.;
}
/**
* @brief Get the Driver Status Word
* @return std::vector<uint16_t>
*/
virtual std::vector<uint16_t> GetDriverStatusWord(){
return std::vector<uint16_t>(dof_num_, 1);
}
/**
* @brief Get the Joint Data ID object
* @return std::vector<uint16_t>
*/
virtual std::vector<uint16_t> GetJointDataID(){
return std::vector<uint16_t>(dof_num_, 0);
}
virtual void RefreshRobotData(){
}
};
};
interface/robot/simulation/__pycache__/mujoco_simulation_ros2.cpython-313.pyc
BIN
View File
Binary file not shown.
interface/robot/simulation/mujoco_simulation_ros2.py
+361
View File
@@ -0,0 +1,361 @@
#!/usr/bin/env python3
"""ROS 2 MuJoCo bridge for the original M20 and the 8-DoF FireAnt model."""
from __future__ import annotations
import argparse
import time
from pathlib import Path
import mujoco
import mujoco.viewer
import numpy as np
import rclpy
from builtin_interfaces.msg import Time
from drdds.msg import (
ImuData,
ImuDataValue,
JointData,
JointsData,
JointsDataCmd,
JointsDataValue,
MetaType,
)
from rclpy.node import Node
CURRENT_DIR = Path(__file__).resolve().parent
PROJECT_ROOT = CURRENT_DIR.parents[2]
MESSAGE_JOINT_COUNT = 16
DT = 0.001
STATE_PUBLISH_DECIMATION = 5 # 200 Hz, matching the C++ controller loop.
FIRE_ANT_JOINTS = (
"fl_knee_joint", "fl_wheel_joint",
"fr_knee_joint", "fr_wheel_joint",
"hl_knee_joint", "hl_wheel_joint",
"hr_knee_joint", "hr_wheel_joint",
)
FIRE_ANT_HOME = np.array([0.5, 0.0, 0.5, 0.0, -0.5, 0.0, -0.5, 0.0], dtype=np.float64)
M20_DIR = np.array(
[1, 1, -1, 1, 1, -1, 1, -1, -1, 1, -1, 1, -1, -1, 1, -1],
dtype=np.float64,
)
M20_OFFSET = np.deg2rad(
np.array([-25, 229, 160, 0, 25, -131, -200, 0, -25, -229, -160, 0, 25, 131, 200, 0])
)
M20_HOME = np.array(
[-0.438, -1.16, 2.76, 0, 0.438, -1.16, 2.76, 0,
-0.438, 1.16, -2.76, 0, 0.438, 1.16, -2.76, 0],
dtype=np.float64,
)
def model_path(robot: str, terrain: str) -> Path:
if robot == "fire_ant":
name = "fire_ant_rough.xml" if terrain == "rough" else "fire_ant.xml"
return PROJECT_ROOT / "M20_description" / "fire_ant_mjcf" / "mjcf" / name
name = "M20_stair.xml" if terrain == "rough" else "M20.xml"
return PROJECT_ROOT / "M20_description" / "m20_mjcf" / "mjcf" / name
def quaternion_to_euler(q: np.ndarray) -> np.ndarray:
"""Convert a MuJoCo wxyz quaternion to roll, pitch and yaw."""
w, x, y, z = q
roll = np.arctan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))
pitch = np.arcsin(np.clip(2.0 * (w * y - z * x), -1.0, 1.0))
yaw = np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
return np.array([roll, pitch, yaw], dtype=np.float64)
class MuJoCoSimulationNode(Node):
def __init__(
self,
robot: str,
xml_path: Path,
use_viewer: bool,
randomize: bool,
seed: int,
realtime_factor: float,
) -> None:
super().__init__("mujoco_simulation")
if not xml_path.is_file():
raise FileNotFoundError(f"Cannot find MJCF: {xml_path}")
self.robot = robot
self.model = mujoco.MjModel.from_xml_path(str(xml_path))
self.model.opt.timestep = DT
self.data = mujoco.MjData(self.model)
self.realtime_factor = realtime_factor
self.dof_num = self.model.nu
expected_dof = 8 if robot == "fire_ant" else 16
if self.dof_num != expected_dof:
raise RuntimeError(f"{robot} expected {expected_dof} actuators, model has {self.dof_num}")
self.joint_names = list(FIRE_ANT_JOINTS) if robot == "fire_ant" else [
mujoco.mj_id2name(
self.model,
mujoco.mjtObj.mjOBJ_JOINT,
int(self.model.actuator_trnid[i, 0]),
)
for i in range(self.dof_num)
]
self.joint_ids = np.array(
[mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name) for name in self.joint_names],
dtype=np.int32,
)
if np.any(self.joint_ids < 0):
raise RuntimeError(f"Missing joints in MJCF: {self.joint_names}")
self.qpos_adr = self.model.jnt_qposadr[self.joint_ids]
self.dof_adr = self.model.jnt_dofadr[self.joint_ids]
self.actuator_ids = np.array(
[
next(i for i in range(self.model.nu) if int(self.model.actuator_trnid[i, 0]) == joint_id)
for joint_id in self.joint_ids
],
dtype=np.int32,
)
self.joint_dir = np.ones(self.dof_num) if robot == "fire_ant" else M20_DIR.copy()
self.pos_offset = np.zeros(self.dof_num) if robot == "fire_ant" else M20_OFFSET.copy()
self.kp_cmd = np.zeros(self.dof_num)
self.kd_cmd = np.zeros(self.dof_num)
self.pos_cmd = np.zeros(self.dof_num)
self.vel_cmd = np.zeros(self.dof_num)
self.tau_ff = np.zeros(self.dof_num)
self.applied_tau = np.zeros(self.dof_num)
self.velocity_limit = (
np.array([17.8024, 109.956] * 4)
if robot == "fire_ant"
else np.array([45.0, 22.4, 22.4, 100.0] * 4)
)
self.kp_scale = np.ones(self.dof_num)
self.kd_scale = np.ones(self.dof_num)
self.timestamp = 0.0
self.base_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "base_link")
self.wheel_body_ids: list[int] = []
self.knee_body_ids: list[int] = []
if robot == "fire_ant":
self.wheel_body_ids = [self._body_id(f"{leg}_wheel") for leg in ("fl", "fr", "hl", "hr")]
self.knee_body_ids = [self._body_id(f"{leg}_knee") for leg in ("fl", "fr", "hl", "hr")]
if randomize:
self._apply_training_randomization(seed)
self._set_initial_pose()
self.imu_pub = self.create_publisher(ImuData, "/IMU_DATA", 200)
self.joints_pub = self.create_publisher(JointsData, "/JOINTS_DATA", 200)
self.cmd_sub = self.create_subscription(JointsDataCmd, "/JOINTS_CMD", self._cmd_callback, 50)
self.viewer = mujoco.viewer.launch_passive(self.model, self.data) if use_viewer else None
mode = "randomized" if randomize else "nominal"
self.get_logger().info(
f"Loaded {robot} ({mode}) from {xml_path}; dof={self.dof_num}, dt={DT:.4f}s"
)
def _body_id(self, name: str) -> int:
body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, name)
if body_id < 0:
raise RuntimeError(f"Missing body in MJCF: {name}")
return body_id
def _apply_training_randomization(self, seed: int) -> None:
"""Apply one reproducible sample from the training-time domain ranges."""
rng = np.random.default_rng(seed)
original_mass = self.model.body_mass.copy()
for body_id in [*self.knee_body_ids, *self.wheel_body_ids]:
mass_scale = rng.uniform(0.85, 1.15)
inertia_scale = mass_scale * rng.uniform(0.85, 1.15)
self.model.body_mass[body_id] *= mass_scale
self.model.body_inertia[body_id] *= inertia_scale
base_mass = max(0.1, original_mass[self.base_id] + rng.uniform(-1.0, 3.0))
base_mass_scale = base_mass / original_mass[self.base_id]
self.model.body_mass[self.base_id] = base_mass
self.model.body_inertia[self.base_id] *= base_mass_scale * rng.uniform(0.85, 1.15)
self.model.body_ipos[self.base_id] += rng.uniform((-0.03, -0.03, -0.02), (0.03, 0.03, 0.02))
for body_id in self.wheel_body_ids:
geom_start = self.model.body_geomadr[body_id]
geom_count = self.model.body_geomnum[body_id]
self.model.geom_friction[geom_start:geom_start + geom_count, 0] = rng.uniform(0.3, 1.0)
self.kp_scale = rng.uniform(0.85, 1.15, self.dof_num)
self.kd_scale = rng.uniform(0.85, 1.15, self.dof_num)
def _set_initial_pose(self) -> None:
self.data.qpos[:] = 0.0
self.data.qvel[:] = 0.0
self.data.qpos[0:3] = (0.0, 0.0, 0.306 if self.robot == "fire_ant" else 0.2)
self.data.qpos[3:7] = (1.0, 0.0, 0.0, 0.0)
self.data.qpos[self.qpos_adr] = FIRE_ANT_HOME if self.robot == "fire_ant" else M20_HOME
mujoco.mj_forward(self.model, self.data)
def _cmd_callback(self, msg: JointsDataCmd) -> None:
if len(msg.data.joints_data) < self.dof_num:
self.get_logger().warn(
f"Ignoring short JointsDataCmd ({len(msg.data.joints_data)} < {self.dof_num})"
)
return
pub_pos = np.empty(self.dof_num)
pub_vel = np.empty(self.dof_num)
for i in range(self.dof_num):
command = msg.data.joints_data[i]
self.kp_cmd[i] = command.kp
self.kd_cmd[i] = command.kd
pub_pos[i] = command.position
pub_vel[i] = command.velocity
self.tau_ff[i] = command.torque
self.pos_cmd = pub_pos * self.joint_dir + self.pos_offset
self.vel_cmd = pub_vel * self.joint_dir
def _joint_state(self) -> tuple[np.ndarray, np.ndarray]:
return self.data.qpos[self.qpos_adr].copy(), self.data.qvel[self.dof_adr].copy()
def _apply_joint_torque(self) -> None:
q, dq = self._joint_state()
torque = (
self.kp_cmd * self.kp_scale * (self.pos_cmd - q)
+ self.kd_cmd * self.kd_scale * (self.vel_cmd - dq)
+ self.tau_ff
)
ctrl_range = self.model.actuator_ctrlrange[self.actuator_ids]
torque = np.clip(torque, ctrl_range[:, 0], ctrl_range[:, 1])
# The MuJoCo motor has a torque limit but no built-in speed limit. Prevent
# positive work beyond the datasheet/training actuator velocity limit.
overspeed_accelerating = (np.abs(dq) >= self.velocity_limit) & (torque * dq > 0.0)
torque[overspeed_accelerating] = 0.0
self.data.ctrl[self.actuator_ids] = torque
def _body_linear_velocity(self) -> np.ndarray:
# Free-joint translational qvel is in world axes; the policy uses body axes.
rotation_world_from_body = self.data.xmat[self.base_id].reshape(3, 3)
return rotation_world_from_body.T @ self.data.qvel[0:3]
def _contact_force_norms(self, body_ids: list[int]) -> np.ndarray:
# cfrc_ext stores [torque, force]; norm is frame invariant.
return np.linalg.norm(self.data.cfrc_ext[body_ids, 3:6], axis=1)
def _make_stamp(self) -> Time:
stamp = Time()
stamp.sec = int(self.timestamp)
stamp.nanosec = int((self.timestamp - stamp.sec) * 1e9)
return stamp
def _publish_robot_state(self) -> None:
q_world = self.data.sensordata[0:4]
rpy_deg = np.rad2deg(quaternion_to_euler(q_world))
body_acc = self.data.sensordata[4:7]
angular_velocity_body = self.data.sensordata[7:10]
imu_msg = ImuData()
imu_msg.header = MetaType()
imu_msg.header.frame_id = 0
imu_msg.header.stamp = self._make_stamp()
imu_msg.data = ImuDataValue()
imu_msg.data.roll, imu_msg.data.pitch, imu_msg.data.yaw = map(float, rpy_deg)
imu_msg.data.omega_x, imu_msg.data.omega_y, imu_msg.data.omega_z = map(
float, angular_velocity_body
)
imu_msg.data.acc_x, imu_msg.data.acc_y, imu_msg.data.acc_z = map(float, body_acc)
self.imu_pub.publish(imu_msg)
q, dq = self._joint_state()
pub_pos = (q - self.pos_offset) * self.joint_dir
pub_vel = dq * self.joint_dir
self.applied_tau = self.data.actuator_force[self.actuator_ids] * self.joint_dir
joints_msg = JointsData()
joints_msg.header = MetaType()
joints_msg.header.frame_id = 0
joints_msg.header.stamp = self._make_stamp()
joints_msg.data = JointsDataValue()
joints_msg.data.joints_data = [JointData() for _ in range(MESSAGE_JOINT_COUNT)]
for i in range(self.dof_num):
joint = joints_msg.data.joints_data[i]
joint.name = [32, 32, 32, 32]
joint.data_id = 0
joint.status_word = 1
joint.position = float(pub_pos[i])
joint.velocity = float(pub_vel[i])
joint.torque = float(self.applied_tau[i])
joint.motion_temp = 40.0
joint.driver_temp = 45.0
if self.robot == "fire_ant":
# Sim-only extension inside the eight unused fields of the fixed 16-joint DDS message.
wheel_force = self._contact_force_norms(self.wheel_body_ids)
knee_force = self._contact_force_norms(self.knee_body_ids)
base_force = self._contact_force_norms([self.base_id])[0]
base_velocity = self._body_linear_velocity()
for i in range(4):
joints_msg.data.joints_data[8 + i].position = float(wheel_force[i])
joints_msg.data.joints_data[12 + i].position = float(knee_force[i])
for i in range(3):
joints_msg.data.joints_data[8 + i].torque = float(base_velocity[i])
joints_msg.data.joints_data[12].velocity = float(base_force)
self.joints_pub.publish(joints_msg)
def start(self) -> None:
step = 0
next_step_time = time.perf_counter()
while rclpy.ok() and (self.viewer is None or self.viewer.is_running()):
rclpy.spin_once(self, timeout_sec=0.0)
self._apply_joint_torque()
mujoco.mj_step(self.model, self.data)
mujoco.mj_rnePostConstraint(self.model, self.data)
step += 1
self.timestamp = step * DT
if step % STATE_PUBLISH_DECIMATION == 0:
self._publish_robot_state()
if self.viewer is not None and step % 20 == 0:
self.viewer.sync()
if self.realtime_factor > 0.0:
next_step_time += DT / self.realtime_factor
remaining = next_step_time - time.perf_counter()
if remaining > 0:
time.sleep(remaining)
elif remaining < -0.1:
next_step_time = time.perf_counter()
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--robot", choices=("fire_ant", "m20"), default="fire_ant")
parser.add_argument("--terrain", choices=("flat", "rough"), default="rough")
parser.add_argument("--xml", type=Path, help="Override the selected MJCF path")
parser.add_argument("--headless", action="store_true", help="Run without the MuJoCo viewer")
parser.add_argument("--randomize", action="store_true", help="Sample training mass/friction/gain ranges once")
parser.add_argument("--seed", type=int, default=0)
parser.add_argument("--realtime-factor", type=float, default=1.0, help="0 runs as fast as possible")
return parser.parse_args()
def main() -> None:
args = parse_args()
xml_path = args.xml.resolve() if args.xml else model_path(args.robot, args.terrain).resolve()
rclpy.init()
node = MuJoCoSimulationNode(
robot=args.robot,
xml_path=xml_path,
use_viewer=not args.headless,
randomize=args.randomize,
seed=args.seed,
realtime_factor=args.realtime_factor,
)
try:
node.start()
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == "__main__":
np.set_printoptions(precision=4, suppress=True)
main()
interface/user_command/gamepad_interface.hpp
+347
View File
@@ -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(&currentTime);
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();
}
};
interface/user_command/keyboard_interface.hpp
+242
View File
@@ -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";
}
};
interface/user_command/keyboard_interface_sim.hpp
+284
View File
@@ -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";
}
};
interface/user_command/user_command_interface.h
+62
View File
@@ -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_;
};
};