caowangrenbo
7ba2bba450
- MotionState 增加 error_code，新增 invicta_msgs/error_code.h 共用位定义；SafeController 写入、remote 展开成协议 ErrorList（component 暂为 0） - InvictaInterface 增加 IMU 断流/恢复判定（imu_timeout_s / imu_recover_s），covariance=-1 视为不可用；RL 丢失 IMU 走正常趴下 - invicta_hardware 不再发布硬编码单位四元数 IMU（含错误的 covariance[0]=0） - 更新 README 与 docs/TODO.md：记录已完成项、反馈帧 CANID 1~4 已确认、[joint_limits] 重复维护入待办
745 lines
34 KiB
C++
745 lines
34 KiB
C++
/**
|
||
* @file remote_node.h
|
||
* @brief 遥控/设备协议节点：UDP + JSON 报文 ↔ ROS 话题
|
||
*
|
||
* 职责边界（见 proto.md）：
|
||
* - 报文层：Header 校验、JSON 解析、错误码（0xE0xx）、MsgId 配对
|
||
* - 语义层：只做「语法/枚举/类型」校验，以及协议明确规定的状态前提（如步态仅 RL 下有效）；
|
||
* 运动状态是否真的切过去，由 invicta_deploy 决定，并通过 /motion_state 回报真实状态
|
||
* （协议里 ErrorCode=0 的含义是「命令已接收并触发，不代表业务成功」）
|
||
* - 状态上报：1002/6 基础 2 Hz、1002/4 运控 10 Hz、1002/5 设备 2 Hz、1002/3 异常 2 Hz。
|
||
* 默认对**所有在线客户端**推送（老版 App 不发 1002 查询，只靠推送）；`telemetry/require_subscription`
|
||
* 设为 true 则退回协议原文的「先查询再推送」。
|
||
*/
|
||
#pragma once
|
||
|
||
#include <ros/ros.h>
|
||
#include <sensor_msgs/Imu.h>
|
||
#include <sensor_msgs/JointState.h>
|
||
|
||
#include <algorithm>
|
||
#include <atomic>
|
||
#include <chrono>
|
||
#include <cmath>
|
||
#include <ctime>
|
||
#include <cstdint>
|
||
#include <functional>
|
||
#include <map>
|
||
#include <memory>
|
||
#include <mutex>
|
||
#include <string>
|
||
#include <thread>
|
||
#include <unordered_map>
|
||
#include <unordered_set>
|
||
#include <utility>
|
||
#include <vector>
|
||
|
||
#include "heartbeat.h"
|
||
#include "invicta_msgs/JointCommandArray.h"
|
||
#include "invicta_msgs/JointDiagnostics.h"
|
||
#include "invicta_msgs/MotionCommand.h"
|
||
#include "invicta_msgs/MotionState.h"
|
||
#include "invicta_msgs/error_code.h"
|
||
#include "remote_config.h"
|
||
#include "json.hpp"
|
||
#include "protocol_defs.h"
|
||
#include "udp_server.h"
|
||
|
||
namespace invicta_remote {
|
||
|
||
using namespace invicta_msgs; // MotionParam / GaitParam / UsageMode 及名字函数
|
||
using json = nlohmann::json;
|
||
|
||
class RemoteNode {
|
||
public:
|
||
explicit RemoteNode(const ros::NodeHandle& pnh) : pnh_(pnh), cfg_(invicta_remote::Config()) {
|
||
// 参数来自本节点自己的 config/invicta_remote.toml
|
||
udp_port_ = cfg_.udp_port;
|
||
motion_command_topic_ = cfg_.topic_motion_command;
|
||
motion_state_topic_ = cfg_.topic_motion_state;
|
||
joint_states_topic_ = cfg_.topic_joint_states;
|
||
imu_topic_ = cfg_.topic_imu;
|
||
joint_diagnostics_topic_ = cfg_.topic_joint_diagnostics;
|
||
heartbeat_timeout_s_ = cfg_.heartbeat_timeout_s;
|
||
unsupported_as_success_ = cfg_.unsupported_as_success;
|
||
|
||
telemetry_enabled_ = cfg_.telemetry_enabled;
|
||
require_subscription_ = cfg_.telemetry_require_subscription;
|
||
basic_hz_ = cfg_.telemetry_basic_hz;
|
||
motion_hz_ = cfg_.telemetry_motion_hz;
|
||
device_hz_ = cfg_.telemetry_device_hz;
|
||
error_hz_ = cfg_.telemetry_error_hz;
|
||
|
||
heartbeat_ = std::make_unique<Heartbeat>(heartbeat_timeout_s_);
|
||
motion_command_pub_ = nh_.advertise<invicta_msgs::MotionCommand>(motion_command_topic_, 1);
|
||
motion_state_sub_ = nh_.subscribe(motion_state_topic_, 10, &RemoteNode::MotionStateCallback, this);
|
||
joint_state_sub_ = nh_.subscribe(joint_states_topic_, 10, &RemoteNode::JointStateCallback, this);
|
||
imu_sub_ = nh_.subscribe(imu_topic_, 10, &RemoteNode::ImuCallback, this);
|
||
diagnostics_sub_ =
|
||
nh_.subscribe(joint_diagnostics_topic_, 10, &RemoteNode::JointDiagnosticsCallback, this);
|
||
|
||
RegisterHandlers();
|
||
}
|
||
|
||
~RemoteNode() { StopTelemetry(); }
|
||
|
||
void Start() {
|
||
udp_server_ = std::make_unique<UdpServer>([this](const Frame& f) { return OnFrame(f); }, udp_port_);
|
||
StartTelemetry();
|
||
|
||
ROS_INFO_STREAM("[invicta_remote] protocol node ready: " << motion_command_topic_ << " <- " << motion_state_topic_);
|
||
ROS_INFO("[invicta_remote] implemented: heartbeat / axis command(2,21) / motion state(2,22) / gait(2,23) / "
|
||
"usage mode(1101,5 normal only) / key(1101,12)");
|
||
ROS_INFO("[invicta_remote] telemetry: 1002/6 %.0f Hz, 1002/4 %.0f Hz, 1002/5 %.0f Hz, 1002/3 %.0f Hz"
|
||
"(%s)", basic_hz_, motion_hz_, device_hz_, error_hz_,
|
||
require_subscription_ ? "query subscription required" : "push autonomously");
|
||
ROS_INFO("[invicta_remote] not implemented: light, sleep, charge, navigation, velocity command (reply 0xE00A)");
|
||
}
|
||
|
||
private:
|
||
struct Result {
|
||
int code = kSuccess;
|
||
std::string message;
|
||
static Result Ok() { return Result{}; }
|
||
static Result Err(int c, const std::string& m = "") {
|
||
Result r;
|
||
r.code = c;
|
||
r.message = m.empty() ? ErrorMessage(c) : m;
|
||
return r;
|
||
}
|
||
};
|
||
|
||
using Handler = std::function<Result(json& items, const Frame& frame)>;
|
||
|
||
// ---------------------------------------------------------------- 帧入口
|
||
std::string OnFrame(const Frame& frame) {
|
||
heartbeat_->Touch(frame.ip, frame.port);
|
||
|
||
if (!frame.format_ok) {
|
||
return BuildResponse(0, 0, Result::Err(kErrUnsupportedDataFormat));
|
||
}
|
||
|
||
json doc;
|
||
try {
|
||
doc = json::parse(frame.body);
|
||
} catch (const std::exception& e) {
|
||
std::cout << "[GAMEPAD] JSON parse failed: " << e.what() << std::endl;
|
||
return BuildResponse(0, 0, Result::Err(kErrDataParsingFailed));
|
||
}
|
||
|
||
if (!doc.is_object() || !doc.contains("PatrolDevice") || !doc["PatrolDevice"].is_object()) {
|
||
return BuildResponse(0, 0, Result::Err(kErrDataParsingFailed));
|
||
}
|
||
json& root = doc["PatrolDevice"];
|
||
if (!root.contains("Type") || !root.contains("Command") || !root["Type"].is_number_integer() ||
|
||
!root["Command"].is_number_integer()) {
|
||
return BuildResponse(0, 0, Result::Err(kErrDataParsingFailed));
|
||
}
|
||
|
||
const int type = root["Type"].get<int>();
|
||
const int command = root["Command"].get<int>();
|
||
json items = root.contains("Items") && root["Items"].is_object() ? root["Items"] : json::object();
|
||
|
||
LogReceived(type, command, items);
|
||
|
||
auto it = handlers_.find({type, command});
|
||
if (it == handlers_.end()) {
|
||
return BuildResponse(type, command, Result::Err(kErrUnsupportedProtocolType));
|
||
}
|
||
|
||
// 状态上报类查询：直接回一帧当前数据（同时记录订阅，见 HandleStatusQuery）
|
||
if (type == kTypeStatus) {
|
||
return StatusQueryResponse(command, items, frame);
|
||
}
|
||
|
||
Result res = it->second(items, frame);
|
||
if (res.code != kSuccess) {
|
||
std::cout << "[GAMEPAD] Type=" << type << " Command=" << command << " -> 0x" << std::hex << res.code
|
||
<< std::dec << " " << res.message << std::endl;
|
||
}
|
||
return BuildResponse(type, command, res);
|
||
}
|
||
|
||
std::string BuildResponse(int type, int command, const Result& res) const {
|
||
json doc;
|
||
doc["PatrolDevice"]["Items"]["ErrorCode"] = res.code;
|
||
doc["PatrolDevice"]["Items"]["ErrorMessage"] = res.message;
|
||
doc["PatrolDevice"]["Type"] = type;
|
||
doc["PatrolDevice"]["Command"] = command;
|
||
doc["PatrolDevice"]["Time"] = CurrentDateTime();
|
||
return doc.dump();
|
||
}
|
||
|
||
static std::string CurrentDateTime() {
|
||
char buf[32];
|
||
std::time_t t = std::time(nullptr);
|
||
std::tm tm_buf{};
|
||
localtime_r(&t, &tm_buf);
|
||
std::strftime(buf, sizeof(buf), "%Y-%m-%d %H:%M:%S", &tm_buf);
|
||
return std::string(buf);
|
||
}
|
||
|
||
void LogReceived(int type, int command, const json& items) {
|
||
const auto now = std::chrono::steady_clock::now();
|
||
auto key = std::make_pair(type, command);
|
||
auto it = last_rx_log_.find(key);
|
||
if (it != last_rx_log_.end() && now - it->second < std::chrono::seconds(1)) return;
|
||
last_rx_log_[key] = now;
|
||
std::cout << "[GAMEPAD] recv Type=" << type << " Command=" << command << " items=" << items.dump()
|
||
<< std::endl;
|
||
}
|
||
|
||
// ---------------------------------------------------------------- 指令发布
|
||
void PublishMotionCommand() {
|
||
invicta_msgs::MotionCommand msg;
|
||
msg.header.stamp = ros::Time::now();
|
||
msg.target_state = latest_.target_state;
|
||
msg.gait = latest_.gait;
|
||
msg.usage_mode = latest_.usage_mode;
|
||
msg.x = latest_.x;
|
||
msg.y = latest_.y;
|
||
msg.yaw = latest_.yaw;
|
||
motion_command_pub_.publish(msg);
|
||
}
|
||
|
||
// ---------------------------------------------------------------- 状态缓存
|
||
void MotionStateCallback(const invicta_msgs::MotionState::ConstPtr& msg) {
|
||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||
actual_motion_state_ = msg->motion_state;
|
||
actual_gait_ = msg->gait;
|
||
actual_usage_mode_ = msg->usage_mode;
|
||
actual_charge_ = msg->charge;
|
||
actual_hes_ = msg->hes;
|
||
actual_sleep_ = msg->sleep;
|
||
actual_error_code_ = msg->error_code;
|
||
actual_version_ = msg->version;
|
||
have_state_ = true;
|
||
}
|
||
|
||
void JointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) {
|
||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||
joint_names_ = msg->name;
|
||
joint_pos_.assign(msg->position.begin(), msg->position.end());
|
||
joint_vel_.assign(msg->velocity.begin(), msg->velocity.end());
|
||
// 本机 8 关节（每腿 swing + wheel），不是 M20 的 16；上报时由 FillM20Joints 折叠到 16 槽
|
||
have_joints_ = !joint_pos_.empty();
|
||
}
|
||
|
||
void JointDiagnosticsCallback(const invicta_msgs::JointDiagnostics::ConstPtr& msg) {
|
||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||
diag_names_ = msg->name;
|
||
diag_temperature_.assign(msg->temperature.begin(), msg->temperature.end());
|
||
diag_status_.assign(msg->status.begin(), msg->status.end());
|
||
}
|
||
|
||
void ImuCallback(const sensor_msgs::Imu::ConstPtr& msg) {
|
||
const double w = msg->orientation.w, x = msg->orientation.x, y = msg->orientation.y,
|
||
z = msg->orientation.z;
|
||
const double n = std::sqrt(w * w + x * x + y * y + z * z);
|
||
if (n < 1e-6) return;
|
||
const double qw = w / n, qx = x / n, qy = y / n, qz = z / n;
|
||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||
rpy_[0] = std::atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy));
|
||
rpy_[1] = std::asin(std::max(-1.0, std::min(1.0, 2.0 * (qw * qy - qz * qx))));
|
||
rpy_[2] = std::atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
|
||
omega_z_ = msg->angular_velocity.z;
|
||
have_imu_ = true;
|
||
}
|
||
|
||
// ---------------------------------------------------------------- M20 16 关节布局
|
||
// 协议 §3.2 固定 16 槽：每腿 [HipX, HipY, Knee, Wheel]，腿序 前左/前右/后左/后右。
|
||
// 本机每腿只有 swing + wheel：HipY ← 摆臂角，Wheel ← 轮转速。
|
||
// HipX（侧摆）与 Knee（膝）在单连杆腿上不存在，恒为 0——这不是缺数据，而是当前机构
|
||
// 「无侧摆、腿笔直」的正确取值，客户端按 16 槽解析时姿态不会异常。
|
||
// 保留 16 槽而不是裁成 8 槽，是为了让同一套 App 不必区分机型。
|
||
static constexpr int kM20Joints = 16;
|
||
|
||
static const char* M20JointName(int slot) {
|
||
static const char* kNames[kM20Joints] = {
|
||
"LeftFrontHipX", "LeftFrontHipY", "LeftFrontKnee", "LeftFrontWheel",
|
||
"RightFrontHipX", "RightFrontHipY", "RightFrontKnee", "RightFrontWheel",
|
||
"LeftBackHipX", "LeftBackHipY", "LeftBackKnee", "LeftBackWheel",
|
||
"RightBackHipX", "RightBackHipY", "RightBackKnee", "RightBackWheel"};
|
||
return kNames[slot];
|
||
}
|
||
|
||
/// 关节名 → 腿序号（0 前左 fl、1 前右 fr、2 后左 hl、3 后右 hr）；不认识返回 -1
|
||
static int LegIndexFromJointName(const std::string& n) {
|
||
if (n.compare(0, 2, "fl") == 0) return 0;
|
||
if (n.compare(0, 2, "fr") == 0) return 1;
|
||
if (n.compare(0, 2, "hl") == 0) return 2;
|
||
if (n.compare(0, 2, "hr") == 0) return 3;
|
||
return -1;
|
||
}
|
||
|
||
/**
|
||
* @brief 一组关节名 → M20 的 16 槽
|
||
*
|
||
* @param names 关节名（/joint_states 或 /joint_diagnostics 的 name）
|
||
* @param value_count 该消息实际带的数据条数（无 name 时按它做下标回退）
|
||
* @param slot_index 输出：槽 → names 下标，-1 表示本机没有这个自由度
|
||
*
|
||
* HipX（侧摆）与 Knee（膝）在单连杆腿上不存在，永远 -1；swing → HipY，wheel → Wheel。
|
||
* 优先按 name 解析（fl_swing / fl_wheel …），无 name 时退回「每腿两个、先 swing 后 wheel」。
|
||
*/
|
||
static void BuildSlotToIndex(const std::vector<std::string>& names, size_t value_count,
|
||
int slot_index[kM20Joints]) {
|
||
for (int i = 0; i < kM20Joints; ++i) slot_index[i] = -1;
|
||
const bool by_name = !names.empty() && names.size() == value_count;
|
||
const size_t n = by_name ? names.size() : std::min(value_count, size_t(kM20Joints / 2));
|
||
for (size_t k = 0; k < n; ++k) {
|
||
int leg = -1;
|
||
bool is_wheel = false;
|
||
if (by_name) {
|
||
leg = LegIndexFromJointName(names[k]);
|
||
if (leg < 0) continue;
|
||
const bool wheel = names[k].find("wheel") != std::string::npos;
|
||
const bool swing = names[k].find("swing") != std::string::npos;
|
||
if (!wheel && !swing) continue;
|
||
is_wheel = wheel;
|
||
} else {
|
||
leg = int(k / 2);
|
||
is_wheel = (k % 2 == 1);
|
||
}
|
||
slot_index[leg * 4 + (is_wheel ? 3 : 1)] = int(k);
|
||
}
|
||
}
|
||
|
||
/// 16 槽取值：协议里 Joint 数组与具名字段的混合量（角度，轮为 rad/s）
|
||
void FillM20Joints(double value[kM20Joints]) const {
|
||
for (int i = 0; i < kM20Joints; ++i) value[i] = 0.0;
|
||
int slot_index[kM20Joints];
|
||
BuildSlotToIndex(joint_names_, std::min(joint_pos_.size(), joint_vel_.size()), slot_index);
|
||
for (int slot = 0; slot < kM20Joints; ++slot) {
|
||
const int k = slot_index[slot];
|
||
if (k < 0) continue;
|
||
value[slot] = (slot % 4 == 3) ? joint_vel_[size_t(k)] : joint_pos_[size_t(k)];
|
||
}
|
||
}
|
||
|
||
// ---------------------------------------------------------------- 状态上报
|
||
json BasicDoc(int type, int command) {
|
||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||
json doc;
|
||
json& items = doc["PatrolDevice"]["Items"];
|
||
auto& bs = items["BasicStatus"];
|
||
bs["MotionState"] = int(actual_motion_state_);
|
||
bs["Gait"] = int(actual_gait_ == 0 ? kGaitBasic : actual_gait_);
|
||
bs["Charge"] = int(actual_charge_);
|
||
bs["HES"] = int(actual_hes_);
|
||
bs["ControlUsageMode"] = int(actual_usage_mode_);
|
||
bs["Direction"] = 0;
|
||
bs["OOA"] = 0;
|
||
bs["PowerManagement"] = 0;
|
||
bs["Sleep"] = int(actual_sleep_);
|
||
bs["Version"] = actual_version_;
|
||
doc["PatrolDevice"]["Type"] = type;
|
||
doc["PatrolDevice"]["Command"] = command;
|
||
doc["PatrolDevice"]["Time"] = CurrentDateTime();
|
||
return doc;
|
||
}
|
||
|
||
json MotionDoc(int type, int command) {
|
||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||
json doc;
|
||
json& items = doc["PatrolDevice"]["Items"];
|
||
auto& ms = items["MotionStatus"];
|
||
ms["Roll"] = rpy_[0];
|
||
ms["Pitch"] = rpy_[1];
|
||
ms["Yaw"] = rpy_[2];
|
||
ms["OmegaZ"] = omega_z_;
|
||
ms["LinearX"] = 0.0; // 待硬件/里程计
|
||
ms["LinearY"] = 0.0;
|
||
ms["Height"] = 0.0; // 待运动学估计
|
||
ms["Payload"] = 0.0;
|
||
ms["RemainMile"] = 0.0; // 待电池/里程
|
||
|
||
auto& motor = items["MotorStatus"];
|
||
double value[kM20Joints];
|
||
FillM20Joints(value);
|
||
if (!have_joints_ && std::chrono::steady_clock::now() - start_time_ > std::chrono::seconds(2)) {
|
||
ROS_WARN_THROTTLE(30.0, "[invicta_remote] %s not received yet, 1002/4 joints reported as 0",
|
||
joint_states_topic_.c_str());
|
||
}
|
||
motor["Joint"] = std::vector<double>(value, value + kM20Joints);
|
||
for (int i = 0; i < kM20Joints; ++i) motor[M20JointName(i)] = value[i];
|
||
doc["PatrolDevice"]["Type"] = type;
|
||
doc["PatrolDevice"]["Command"] = command;
|
||
doc["PatrolDevice"]["Time"] = CurrentDateTime();
|
||
return doc;
|
||
}
|
||
|
||
json DeviceDoc(int type, int command) {
|
||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||
json doc;
|
||
json& items = doc["PatrolDevice"]["Items"];
|
||
// 电池 / CPU / GPS 还没有数据源，用 0 占位；电机温度来自 /joint_diagnostics
|
||
json battery = {{"Voltage", 0.0}, {"BatteryLevel", 0.0}, {"battery_temperature", 0.0},
|
||
{"charge", false}, {"serial", ""}};
|
||
items["BatteryList"] = json::array({battery, battery});
|
||
items["BatteryStatus"] = {{"VoltageLeft", 0.0}, {"VoltageRight", 0.0},
|
||
{"BatteryLevelLeft", 0.0}, {"BatteryLevelRight", 0.0},
|
||
{"battery_temperatureLeft", 0.0}, {"battery_temperatureRight", 0.0},
|
||
{"chargeLeft", false}, {"chargeRight", false}};
|
||
// 温度按 M20 布局展开：本机 8 个电机落在对应槽（swing → HipY、wheel → Wheel），
|
||
// 其余槽没有对应电机填 0；驱动器温度本协议不返回，恒 0。
|
||
int slot_index[kM20Joints];
|
||
BuildSlotToIndex(diag_names_, diag_temperature_.size(), slot_index);
|
||
json motor_temp = json::array();
|
||
json driver_temp = json::array();
|
||
for (int slot = 0; slot < kM20Joints; ++slot) {
|
||
const int k = slot_index[slot];
|
||
motor_temp.push_back((k >= 0 && size_t(k) < diag_temperature_.size()) ? diag_temperature_[size_t(k)] : 0.0);
|
||
driver_temp.push_back(0.0);
|
||
}
|
||
items["DeviceTemperature"] = {{"Motor", motor_temp}, {"Driver", driver_temp}};
|
||
auto& temps = items["DeviceTemperature"];
|
||
for (int slot = 0; slot < kM20Joints; ++slot) {
|
||
const std::string base = M20JointName(slot);
|
||
temps[base + "Motor"] = motor_temp[slot];
|
||
temps[base + "Driver"] = 0.0;
|
||
}
|
||
items["Led"] = {{"Fill", {{"Front", 0}, {"Back", 0}}}};
|
||
items["GPS"] = {{"Latitude", 0.0}, {"Longitude", 0.0}, {"Speed", 0.0}, {"Course", 0.0},
|
||
{"FixQuality", 0}, {"NumSatellites", 0}, {"Altitude", 0.0},
|
||
{"HDOP", 0.0}, {"VDOP", 0.0}, {"PDOP", 0.0}, {"VisibleSatellites", 0}};
|
||
items["DevEnable"] = {{"FanSpeed", 100}, {"LoadPower", 1}, {"LedHost", 1}, {"LedExt", 1}, {"FP", 1},
|
||
{"Lidar", {{"Front", 1}, {"Back", 1}}}, {"GPS", 1},
|
||
{"Video", {{"Front", 1}, {"Back", 1}}}};
|
||
items["CPU"] = {{"AOS", {{"Temperature", 0.0}, {"FrequencyInt", 0.0}, {"FrequencyApp", 0.0}}},
|
||
{"NOS", {{"Temperature", 0.0}, {"FrequencyInt", 0.0}, {"FrequencyApp", 0.0}}},
|
||
{"GOS", {{"Temperature", 0.0}, {"FrequencyInt", 0.0}, {"FrequencyApp", 0.0}}}};
|
||
doc["PatrolDevice"]["Type"] = type;
|
||
doc["PatrolDevice"]["Command"] = command;
|
||
doc["PatrolDevice"]["Time"] = CurrentDateTime();
|
||
return doc;
|
||
}
|
||
|
||
json ErrorDoc(int type, int command) {
|
||
json doc;
|
||
json list = json::array();
|
||
uint32_t mask = 0;
|
||
{
|
||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||
mask = actual_error_code_;
|
||
}
|
||
// 内部错误位 → 协议 errorCode（proto.md §3.4）；没有对应协议码的位（如心跳超时）只在上报里省略
|
||
for (const auto& item : RobotErrorList(mask)) {
|
||
json e;
|
||
e["errorCode"] = item.error_code;
|
||
e["component"] = item.component;
|
||
list.push_back(e);
|
||
}
|
||
if (list.empty()) list = json::array();
|
||
doc["PatrolDevice"]["Items"]["ErrorList"] = list;
|
||
doc["PatrolDevice"]["Type"] = type;
|
||
doc["PatrolDevice"]["Command"] = command;
|
||
doc["PatrolDevice"]["Time"] = CurrentDateTime();
|
||
return doc;
|
||
}
|
||
|
||
/// 查询响应：把对应上报的当前值作为响应返回（协议 §3 的「查询触发」）
|
||
std::string StatusQueryResponse(int command, const json&, const Frame& frame) {
|
||
heartbeat_->SetSubscribed(frame.ip, frame.port, command);
|
||
if (!subscribed_logged_.count({frame.ip, command})) {
|
||
subscribed_logged_.insert({frame.ip, command});
|
||
std::cout << "[GAMEPAD] " << frame.ip << ":" << frame.port << " query telemetry 1002/" << command
|
||
<< std::endl;
|
||
}
|
||
switch (command) {
|
||
case kCmdStatusBasic: return BasicDoc(kTypeStatus, command).dump();
|
||
case kCmdStatusMotion: return MotionDoc(kTypeStatus, command).dump();
|
||
case kCmdStatusDevice: return DeviceDoc(kTypeStatus, command).dump();
|
||
case kCmdStatusError: return ErrorDoc(kTypeStatus, command).dump();
|
||
default: return BuildResponse(kTypeStatus, command, Result::Err(kErrUnsupportedProtocolType));
|
||
}
|
||
}
|
||
|
||
void StartTelemetry() {
|
||
if (!telemetry_enabled_) return;
|
||
telemetry_running_ = true;
|
||
telemetry_thread_ = std::thread(&RemoteNode::TelemetryLoop, this);
|
||
}
|
||
|
||
void StopTelemetry() {
|
||
telemetry_running_ = false;
|
||
if (telemetry_thread_.joinable()) telemetry_thread_.join();
|
||
}
|
||
|
||
void TelemetryLoop() {
|
||
using clock = std::chrono::steady_clock;
|
||
const auto t0 = clock::now();
|
||
double next_basic = 0, next_motion = 0, next_device = 0, next_error = 0;
|
||
auto now = [&]() { return std::chrono::duration<double>(clock::now() - t0).count(); };
|
||
|
||
while (ros::ok() && telemetry_running_) {
|
||
const double t = now();
|
||
if (t >= next_basic) {
|
||
next_basic = t + (basic_hz_ > 0 ? 1.0 / basic_hz_ : 0.5);
|
||
PushToClients(BasicDoc(kTypeStatus, kCmdStatusBasic).dump(), kCmdStatusBasic);
|
||
}
|
||
if (t >= next_motion) {
|
||
next_motion = t + (motion_hz_ > 0 ? 1.0 / motion_hz_ : 0.1);
|
||
PushToClients(MotionDoc(kTypeStatus, kCmdStatusMotion).dump(), kCmdStatusMotion);
|
||
}
|
||
if (t >= next_device) {
|
||
next_device = t + (device_hz_ > 0 ? 1.0 / device_hz_ : 0.5);
|
||
PushToClients(DeviceDoc(kTypeStatus, kCmdStatusDevice).dump(), kCmdStatusDevice);
|
||
}
|
||
if (t >= next_error) {
|
||
next_error = t + (error_hz_ > 0 ? 1.0 / error_hz_ : 0.5);
|
||
PushToClients(ErrorDoc(kTypeStatus, kCmdStatusError).dump(), kCmdStatusError);
|
||
}
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
||
}
|
||
}
|
||
|
||
void PushToClients(const std::string& body, int status_command) {
|
||
if (!udp_server_ || body.empty()) return;
|
||
for (const auto& c : heartbeat_->GetClients()) {
|
||
if (require_subscription_ && !IsSubscribed(c, status_command)) continue;
|
||
udp_server_->SendTo(c.ip, c.port, body, next_push_id_++);
|
||
}
|
||
}
|
||
|
||
static bool IsSubscribed(const ClientInfo& c, int status_command) {
|
||
switch (status_command) {
|
||
case kCmdStatusBasic: return c.subscribed_basic;
|
||
case kCmdStatusMotion: return c.subscribed_motion;
|
||
case kCmdStatusDevice: return c.subscribed_device;
|
||
case kCmdStatusError: return c.subscribed_error;
|
||
default: return false;
|
||
}
|
||
}
|
||
|
||
// ---------------------------------------------------------------- 各命令处理
|
||
Result HandleHeartbeat(json&, const Frame& frame) {
|
||
const auto now = std::chrono::steady_clock::now();
|
||
if (now - last_heartbeat_log_ > std::chrono::seconds(2)) {
|
||
last_heartbeat_log_ = now;
|
||
std::cout << "[GAMEPAD] heartbeat from " << frame.ip << ":" << frame.port << " (telemetry push started)"
|
||
<< std::endl;
|
||
}
|
||
return Result::Ok();
|
||
}
|
||
|
||
/// 2/21 轴指令（常规模式），比例 [-1,1]，协议建议 ≥20 Hz
|
||
Result HandleAxis(json& items, const Frame& frame) {
|
||
if (!items.contains("X") || !items.contains("Y") || !items.contains("Yaw")) {
|
||
return Result::Err(kErrMissingFields, "Missing necessary fields: X/Y/Yaw");
|
||
}
|
||
if (!items["X"].is_number() || !items["Y"].is_number() || !items["Yaw"].is_number()) {
|
||
return Result::Err(kErrMismatchedDataType, "Mismatched data type: X/Y/Yaw must be float");
|
||
}
|
||
|
||
const std::string client = frame.ip + ":" + std::to_string(frame.port);
|
||
const auto now = std::chrono::steady_clock::now();
|
||
if (!axis_client_.empty() && axis_client_ != client &&
|
||
now - axis_client_time_ < std::chrono::seconds(2)) {
|
||
return Result::Err(kErrMismatchedClient, "Mismatched request client: " + axis_client_ + " busy");
|
||
}
|
||
axis_client_ = client;
|
||
axis_client_time_ = now;
|
||
|
||
latest_.x = items["X"].get<float>();
|
||
latest_.y = items["Y"].get<float>();
|
||
latest_.yaw = items["Yaw"].get<float>();
|
||
PublishMotionCommand();
|
||
return Result::Ok();
|
||
}
|
||
|
||
/// 2/22 运动状态转换
|
||
Result HandleMotionState(json& items, const Frame&) {
|
||
if (!items.contains("MotionParam"))
|
||
return Result::Err(kErrMissingFields, "Missing necessary fields: MotionParam");
|
||
if (!items["MotionParam"].is_number_integer()) {
|
||
return Result::Err(kErrMismatchedDataType, "Mismatched data type: MotionParam must be int");
|
||
}
|
||
const int param = items["MotionParam"].get<int>();
|
||
if (!IsKnownMotionParam(param)) {
|
||
return Result::Err(kErrUnsupportedFunction, "Unsupported MotionParam: " + std::to_string(param));
|
||
}
|
||
latest_.target_state = uint8_t(param);
|
||
PublishMotionCommand();
|
||
return Result::Ok();
|
||
}
|
||
|
||
/// 2/23 步态切换：不同步态对应不同 RL 模型，协议规定仅 RL 控制状态下有效
|
||
Result HandleGait(json& items, const Frame&) {
|
||
if (!items.contains("GaitParam")) return Result::Err(kErrMissingFields, "Missing necessary fields: GaitParam");
|
||
if (!items["GaitParam"].is_number_integer()) {
|
||
return Result::Err(kErrMismatchedDataType, "Mismatched data type: GaitParam must be int");
|
||
}
|
||
const int gait = items["GaitParam"].get<int>();
|
||
if (!IsKnownGait(uint32_t(gait))) {
|
||
return Result::Err(kErrUnsupportedFunction, "Unsupported GaitParam: " + std::to_string(gait));
|
||
}
|
||
{
|
||
std::lock_guard<std::mutex> lock(state_mutex_);
|
||
if (have_state_ && actual_motion_state_ != kMotionRlControl) {
|
||
return Result::Err(kErrNotAllowedOperation,
|
||
std::string("Not allowed operation: gait switch only valid in RL control state (current ") +
|
||
MotionParamName(actual_motion_state_) + ")");
|
||
}
|
||
}
|
||
latest_.gait = uint32_t(gait);
|
||
PublishMotionCommand();
|
||
return Result::Ok();
|
||
}
|
||
|
||
/// 1101/5 使用模式：0 常规 / 1 导航 / 2 辅助
|
||
Result HandleUsageMode(json& items, const Frame&) {
|
||
if (!items.contains("Mode")) return Result::Err(kErrMissingFields, "Missing necessary fields: Mode");
|
||
if (!items["Mode"].is_number_integer()) {
|
||
return Result::Err(kErrMismatchedDataType, "Mismatched data type: Mode must be int");
|
||
}
|
||
const int mode = items["Mode"].get<int>();
|
||
if (mode == kUsageNormal) {
|
||
latest_.usage_mode = uint8_t(mode);
|
||
PublishMotionCommand();
|
||
return Result::Ok();
|
||
}
|
||
return Unsupported("usage mode " + std::to_string(mode) + " (only normal mode supported)");
|
||
}
|
||
|
||
/// 1101/12 按键事件：新版 App（≥ V1.5.11）用按键，这里映射到同一套运动状态
|
||
Result HandleKey(json& items, const Frame&) {
|
||
static const std::map<std::string, uint8_t> key_to_motion = {{"G20_KEY_L1", kMotionStanding},
|
||
{"G20_KEY_L2", kMotionRlControl},
|
||
{"G20_KEY_R1", kMotionLieDown},
|
||
{"G20_KEY_R2", kMotionJointDamping}};
|
||
if (items.contains("KeyList") && items["KeyList"].is_array()) {
|
||
for (const auto& key : items["KeyList"]) {
|
||
if (!key.is_string()) continue;
|
||
const std::string name = key.get<std::string>();
|
||
auto it = key_to_motion.find(name);
|
||
if (it != key_to_motion.end()) {
|
||
latest_.target_state = it->second;
|
||
PublishMotionCommand();
|
||
} else {
|
||
std::cout << "[GAMEPAD] unknown key " << name << std::endl;
|
||
}
|
||
}
|
||
}
|
||
return Result::Ok();
|
||
}
|
||
|
||
/// 未实现的功能：统一返回 0xE00A（可用参数改成回 0）
|
||
Result Unsupported(const std::string& what) {
|
||
if (unsupported_as_success_) {
|
||
std::cout << "[GAMEPAD] " << what << " not implemented, reply 0 per unsupported_as_success=true" << std::endl;
|
||
return Result::Ok();
|
||
}
|
||
std::cout << "[GAMEPAD] " << what << " not implemented, reply 0xE00A" << std::endl;
|
||
return Result::Err(kErrUnsupportedFunction, "Unsupported function: " + what);
|
||
}
|
||
|
||
// ---------------------------------------------------------------- 注册
|
||
void RegisterHandlers() {
|
||
handlers_[{kTypeHeartbeat, kCmdHeartbeat}] = [this](json& i, const Frame& f) { return HandleHeartbeat(i, f); };
|
||
|
||
handlers_[{kTypeMotion, kCmdAxisControl}] = [this](json& i, const Frame& f) { return HandleAxis(i, f); };
|
||
handlers_[{kTypeMotion, kCmdMotionState}] = [this](json& i, const Frame& f) { return HandleMotionState(i, f); };
|
||
handlers_[{kTypeMotion, kCmdGait}] = [this](json& i, const Frame& f) { return HandleGait(i, f); };
|
||
handlers_[{kTypeMotion, kCmdCharge}] = [this](json&, const Frame&) { return Unsupported("autonomous charging 2/24"); };
|
||
handlers_[{kTypeMotion, kCmdVelocity}] = [this](json&, const Frame&) {
|
||
return Unsupported("velocity command 2/25 (navigation mode)");
|
||
};
|
||
|
||
handlers_[{kTypeDevice, kCmdLight}] = [this](json&, const Frame&) {
|
||
return Unsupported("light 1101/2 (no light hardware on this platform)");
|
||
};
|
||
handlers_[{kTypeDevice, kCmdUsageMode}] = [this](json& i, const Frame& f) { return HandleUsageMode(i, f); };
|
||
handlers_[{kTypeDevice, kCmdSleepSet}] = [this](json&, const Frame&) { return Unsupported("sleep set 1101/6"); };
|
||
handlers_[{kTypeDevice, kCmdSleepQuery}] = [this](json&, const Frame&) {
|
||
return Unsupported("sleep query 1101/7");
|
||
};
|
||
handlers_[{kTypeDevice, kCmdKey}] = [this](json& i, const Frame& f) { return HandleKey(i, f); };
|
||
|
||
// 状态上报：存在性用于入口判断，实际响应在 StatusQueryResponse 里生成
|
||
for (int cmd : {kCmdStatusBasic, kCmdStatusMotion, kCmdStatusDevice, kCmdStatusError}) {
|
||
handlers_[{kTypeStatus, cmd}] = [](json&, const Frame&) { return Result::Ok(); };
|
||
}
|
||
}
|
||
|
||
private:
|
||
struct handlers_key_hash {
|
||
size_t operator()(const std::pair<int, int>& p) const {
|
||
return std::hash<long long>()((static_cast<long long>(p.first) << 32) ^ static_cast<unsigned>(p.second));
|
||
}
|
||
};
|
||
struct ip_cmd_hash {
|
||
size_t operator()(const std::pair<std::string, int>& p) const {
|
||
return std::hash<std::string>()(p.first) * 131u + static_cast<unsigned>(p.second);
|
||
}
|
||
};
|
||
|
||
const invicta_remote::RemoteConfig& cfg_;
|
||
ros::NodeHandle nh_;
|
||
ros::NodeHandle pnh_;
|
||
ros::Publisher motion_command_pub_;
|
||
ros::Subscriber motion_state_sub_;
|
||
ros::Subscriber joint_state_sub_;
|
||
ros::Subscriber imu_sub_;
|
||
ros::Subscriber diagnostics_sub_;
|
||
|
||
std::unique_ptr<Heartbeat> heartbeat_;
|
||
std::unique_ptr<UdpServer> udp_server_;
|
||
std::unordered_map<std::pair<int, int>, Handler, handlers_key_hash> handlers_;
|
||
std::unordered_map<std::pair<int, int>, std::chrono::steady_clock::time_point, handlers_key_hash> last_rx_log_;
|
||
std::unordered_set<std::pair<std::string, int>, ip_cmd_hash> subscribed_logged_;
|
||
|
||
// 参数
|
||
int udp_port_ = kUdpDefaultPort;
|
||
std::string motion_command_topic_ = "motion_command";
|
||
std::string motion_state_topic_ = "motion_state";
|
||
std::string joint_states_topic_ = "joint_states";
|
||
std::string imu_topic_ = "imu/data";
|
||
std::string joint_diagnostics_topic_ = "joint_diagnostics";
|
||
double heartbeat_timeout_s_ = 3.0;
|
||
bool unsupported_as_success_ = false;
|
||
|
||
bool telemetry_enabled_ = true;
|
||
bool require_subscription_ = false;
|
||
double basic_hz_ = 2.0, motion_hz_ = 10.0, device_hz_ = 2.0, error_hz_ = 2.0;
|
||
std::atomic<bool> telemetry_running_{false};
|
||
std::thread telemetry_thread_;
|
||
std::atomic<int> next_push_id_{0};
|
||
|
||
// 最近一次「指令意图」，每条指令帧都会整体重发（deploy 侧对重复请求做去重）
|
||
struct Latest {
|
||
uint8_t target_state = kMotionIdle;
|
||
uint32_t gait = 0;
|
||
uint8_t usage_mode = kUsageNormal;
|
||
float x = 0.f, y = 0.f, yaw = 0.f;
|
||
} latest_;
|
||
|
||
std::string axis_client_;
|
||
std::chrono::steady_clock::time_point axis_client_time_{};
|
||
std::chrono::steady_clock::time_point last_heartbeat_log_{};
|
||
|
||
// 机器人实际状态（订阅 /motion_state、/joint_states、/imu/data）
|
||
std::mutex state_mutex_;
|
||
uint8_t actual_motion_state_ = kMotionIdle;
|
||
uint32_t actual_gait_ = kGaitBasic;
|
||
uint8_t actual_usage_mode_ = kUsageNormal;
|
||
uint8_t actual_charge_ = 0, actual_hes_ = 0, actual_sleep_ = 0;
|
||
/// 机器人内部错误位掩码（来自 /motion_state）；展开成协议 ErrorList 见 invicta_msgs/error_code.h
|
||
uint32_t actual_error_code_ = 0;
|
||
std::string actual_version_ = "INVICTA";
|
||
bool have_state_ = false;
|
||
std::vector<std::string> joint_names_;
|
||
std::vector<double> joint_pos_, joint_vel_;
|
||
bool have_joints_ = false;
|
||
/// 启动后 2 s 内不报「还没收到关节状态」：节点刚起来时上游本来就还没发
|
||
const std::chrono::steady_clock::time_point start_time_ = std::chrono::steady_clock::now();
|
||
std::vector<std::string> diag_names_;
|
||
std::vector<double> diag_temperature_;
|
||
std::vector<uint8_t> diag_status_;
|
||
double rpy_[3] = {0., 0., 0.};
|
||
double omega_z_ = 0.;
|
||
bool have_imu_ = false;
|
||
};
|
||
|
||
} // namespace invicta_remote
|