refactor(log): 日志文案改 ASCII 英文,修 ROS_INFO 中文乱码

ROS_INFO/WARN/ERROR 走的不是 printf,而是 rosconsole -> log4cxx,后者按进程当前的
C 语言环境对消息做一次窄/宽字符转码。节点从未调用 setlocale,LC_CTYPE 停在默认的
"C"(ASCII),于是非 ASCII 的每个字节被替换成一个 '?'——一个汉字 3 字节正好 3 个
'?'。而 [config] 摘要那条 std::cout 路径直接写字节、不转码,所以之前只有 ROS_* 的
日志乱码。备选方案是 setlocale(LC_CTYPE, "")(3 行即可修),这里采用"运行时输出一律
英文":英文日志不依赖语言环境,systemd/robot_upstart 拉起时也不会因缺 LANG 复发。

改动与校验:
- invicta_hardware / invicta_remote / invicta_deploy / invicta_msgs 共 20 个文件、
  202 处字符串字面量改为 ASCII 英文。中文代码注释、标识符、话题名、JSON/TOML 键名、
  日志前缀一律未动(把字面量内容挖空后与改前做骨架比对,20 个文件零差异)。
- printf 格式说明符(%s/%d/%zu/%.1f/%02X ...)的数量、类型、顺序逐文件与改前一致。
- 顺带 ASCII 化非中文字符:° -> deg、± -> +/-、× -> x、→ -> ->、框线 -> "+==+",
  并修正了启动横幅框线宽度差 2 列的问题。
- motion_defs.h 的 GaitName / MotionParamName 显示名改英文(只用于日志与回包文案);
  协议字段仍传整数(bs["MotionState"] = int(...)),不影响上位机协议。

注意:invicta_remote 回给手柄的 Unsupported / "Not allowed operation" 文案也变为英文,
会出现在上位机错误列表里。

已在 SBC(aarch64, Noetic)编译确认。
This commit is contained in:
2026-10-23 15:39:55 +08:00
parent 46bbb06978
commit 944774eab3
20 changed files with 177 additions and 177 deletions
@@ -163,17 +163,17 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
try { try {
tbl = toml::parse_file(path); tbl = toml::parse_file(path);
} catch (const toml::parse_error& e) { } catch (const toml::parse_error& e) {
return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path); return fail(std::string("TOML parse failed: ") + std::string(e.description()) + " @ " + path);
} }
const toml::table* robot = detail::Section(tbl, "robot"); const toml::table* robot = detail::Section(tbl, "robot");
if (!robot) return fail("缺少 [robot] 段"); if (!robot) return fail("missing [robot] section");
detail::GetStr(robot, "name", robot_name); detail::GetStr(robot, "name", robot_name);
detail::GetStr(robot, "version", robot_version); detail::GetStr(robot, "version", robot_version);
detail::GetStrArr(robot, "joint_names", joint_names); detail::GetStrArr(robot, "joint_names", joint_names);
if (joint_names.empty()) return fail("[robot].joint_names 为空"); if (joint_names.empty()) return fail("[robot].joint_names is empty");
dof = int(joint_names.size()); dof = int(joint_names.size());
if (dof % 2 != 0) return fail("joint_names 数量必须是偶数(每腿 摆臂+轮)"); if (dof % 2 != 0) return fail("joint_names count must be even (one swing + one wheel per leg)");
std::vector<double> lower, upper, vel_limit; std::vector<double> lower, upper, vel_limit;
const toml::table* limits = detail::Section(tbl, "joint_limits"); const toml::table* limits = detail::Section(tbl, "joint_limits");
@@ -181,7 +181,7 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
detail::GetDoubleArr(limits, "upper", upper); detail::GetDoubleArr(limits, "upper", upper);
detail::GetDoubleArr(limits, "vel_limit", vel_limit); detail::GetDoubleArr(limits, "vel_limit", vel_limit);
if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) { if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) {
return fail("[joint_limits] 的 lower/upper/vel_limit 长度必须等于关节数量"); return fail("[joint_limits] lower/upper/vel_limit length must equal joint count");
} }
joints.assign(size_t(dof), JointSpec{}); joints.assign(size_t(dof), JointSpec{});
for (int i = 0; i < dof; ++i) { for (int i = 0; i < dof; ++i) {
@@ -204,7 +204,7 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
leg.swing_index = IndexOf(leg.swing_joint); leg.swing_index = IndexOf(leg.swing_joint);
leg.wheel_index = IndexOf(leg.wheel_joint); leg.wheel_index = IndexOf(leg.wheel_joint);
if (leg.swing_index < 0 || leg.wheel_index < 0) { if (leg.swing_index < 0 || leg.wheel_index < 0) {
return fail("[[legs]] " + leg.name + " 的 swing_joint/wheel_joint 不在 joint_names 里"); return fail("[[legs]] " + leg.name + " swing_joint/wheel_joint not in joint_names");
} }
joints[size_t(leg.swing_index)].leg = int(legs.size()); joints[size_t(leg.swing_index)].leg = int(legs.size());
joints[size_t(leg.wheel_index)].leg = int(legs.size()); joints[size_t(leg.wheel_index)].leg = int(legs.size());
@@ -212,8 +212,8 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
legs.push_back(leg); legs.push_back(leg);
} }
} }
if (legs.empty()) return fail("缺少 [[legs]] 段"); if (legs.empty()) return fail("missing [[legs]] section");
if (int(legs.size()) * 2 != dof) return fail("腿数与关节数不匹配(每腿 2 个关节)"); if (int(legs.size()) * 2 != dof) return fail("leg count does not match joint count (2 joints per leg)");
const toml::table* gc = detail::Section(tbl, "leg"); const toml::table* gc = detail::Section(tbl, "leg");
detail::Get(gc, "swing_link_len", swing_link_len); detail::Get(gc, "swing_link_len", swing_link_len);
@@ -235,12 +235,12 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
detail::Get(gains, "damping_kd_wheel", damping_kd_wheel); detail::Get(gains, "damping_kd_wheel", damping_kd_wheel);
detail::GetBool(gains, "wheel_kp_zero", wheel_kp_zero); detail::GetBool(gains, "wheel_kp_zero", wheel_kp_zero);
if (swing_link_len <= 1e-6) return fail("[leg].swing_link_len 必须 > 0"); if (swing_link_len <= 1e-6) return fail("[leg].swing_link_len must be > 0");
for (const auto& pair : {std::make_pair("stand_height", stand_height), for (const auto& pair : {std::make_pair("stand_height", stand_height),
std::make_pair("pre_height", pre_height), std::make_pair("pre_height", pre_height),
std::make_pair("lie_height", lie_height)}) { std::make_pair("lie_height", lie_height)}) {
if (pair.second > swing_link_len) { if (pair.second > swing_link_len) {
return fail(std::string("[leg].") + pair.first + " 大于 swing_link_len,物理上不可达"); return fail(std::string("[leg].") + pair.first + " greater than swing_link_len, physically unreachable");
} }
} }
@@ -264,14 +264,14 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
detail::GetDoubleArr(pol, "kp", policy_kp); detail::GetDoubleArr(pol, "kp", policy_kp);
detail::GetDoubleArr(pol, "kd", policy_kd); detail::GetDoubleArr(pol, "kd", policy_kd);
if (policy_joint_order.empty()) policy_joint_order = joint_names; if (policy_joint_order.empty()) policy_joint_order = joint_names;
if (int(policy_joint_order.size()) != dof) return fail("[policy].joint_order 长度必须等于关节数量"); if (int(policy_joint_order.size()) != dof) return fail("[policy].joint_order length must equal joint count");
for (const auto& n : policy_joint_order) { for (const auto& n : policy_joint_order) {
if (IndexOf(n) < 0) return fail("[policy].joint_order 里的 " + n + " 不在 joint_names 里"); if (IndexOf(n) < 0) return fail("[policy].joint_order entry " + n + " not in joint_names");
} }
action_dim = dof; action_dim = dof;
for (const auto& arr : {&action_scale, &default_joint_pos, &policy_kp, &policy_kd}) { for (const auto& arr : {&action_scale, &default_joint_pos, &policy_kp, &policy_kd}) {
if (int(arr->size()) != action_dim) { if (int(arr->size()) != action_dim) {
return fail("[policy] 的 action_scale/default_joint_pos/kp/kd 长度必须等于关节数量"); return fail("[policy] action_scale/default_joint_pos/kp/kd length must equal joint count");
} }
} }
obs_dim = 9 + 3 * action_dim; // 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角 + 关节速度 + 上一步动作 obs_dim = 9 + 3 * action_dim; // 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角 + 关节速度 + 上一步动作
@@ -285,7 +285,7 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
try { try {
gait_policies[uint32_t(std::stoul(std::string(k.str())))] = v.value_or(std::string{}); gait_policies[uint32_t(std::stoul(std::string(k.str())))] = v.value_or(std::string{});
} catch (const std::exception&) { } catch (const std::exception&) {
return fail("[gait.policies] 的键必须能转成整数步态值"); return fail("[gait.policies] key must convert to an integer gait value");
} }
} }
} }
@@ -320,7 +320,7 @@ inline std::string DeployConfig::Summary() const {
for (size_t i = 0; i < legs.size(); ++i) { for (size_t i = 0; i < legs.size(); ++i) {
const double sw = SwingAngleForHeight(stand_height, int(i)); const double sw = SwingAngleForHeight(stand_height, int(i));
const double li = SwingAngleForHeight(lie_height, int(i)); const double li = SwingAngleForHeight(lie_height, int(i));
snprintf(buf, sizeof(buf), "[config] %s splay=%+.0f 站立摆臂=%+.1f° 趴下=%+.1f°\n", snprintf(buf, sizeof(buf), "[config] %s splay=%+.0f stand swing=%+.1f deg lie down=%+.1f deg\n",
legs[i].name.c_str(), legs[i].splay_sign, sw * 180. / M_PI, li * 180. / M_PI); legs[i].name.c_str(), legs[i].splay_sign, sw * 180. / M_PI, li * 180. / M_PI);
s += buf; s += buf;
} }
@@ -216,7 +216,7 @@ private:
} }
if (robot_error_state_.motor_heat_warn) { if (robot_error_state_.motor_heat_warn) {
std::cout << "Motor overheat warning: motor " << max_temp_idx_ << " reached " << max_temp_ << "°C" std::cout << "Motor overheat warning: motor " << max_temp_idx_ << " reached " << max_temp_ << " C"
<< std::endl; << std::endl;
} }
@@ -34,7 +34,7 @@ public:
InvictaInterface(const std::string& robot_name, int dof_num) InvictaInterface(const std::string& robot_name, int dof_num)
: RobotInterface(robot_name, dof_num), cfg_(invicta::Config()) { : RobotInterface(robot_name, dof_num), cfg_(invicta::Config()) {
if (dof_num_ != cfg_.dof) { if (dof_num_ != cfg_.dof) {
ROS_WARN("[InvictaInterface] 传入 dof=%d 与配置的 %d 不一致,按配置执行", dof_num_, cfg_.dof); ROS_WARN("[InvictaInterface] passed dof=%d does not match configured %d, using config value", dof_num_, cfg_.dof);
} }
joint_pos_ = VecXf::Zero(dof_num_); joint_pos_ = VecXf::Zero(dof_num_);
joint_vel_ = VecXf::Zero(dof_num_); joint_vel_ = VecXf::Zero(dof_num_);
@@ -58,7 +58,7 @@ public:
SendStartupSequence(); SendStartupSequence();
std::cout << "[InvictaInterface] topics: " << cfg_.topic_joint_states << " / " << cfg_.topic_imu << " -> " std::cout << "[InvictaInterface] topics: " << cfg_.topic_joint_states << " / " << cfg_.topic_imu << " -> "
<< cfg_.topic_joint_commands << std::endl; << cfg_.topic_joint_commands << std::endl;
std::cout << "[InvictaInterface] 模型帧透传:dir/offset 由 invicta_hardware 施加" << std::endl; std::cout << "[InvictaInterface] model frame passthrough: dir/offset applied by invicta_hardware" << std::endl;
} }
virtual void Stop() {} virtual void Stop() {}
@@ -124,21 +124,21 @@ protected:
ros::Duration(0.01).sleep(); ros::Duration(0.01).sleep();
} }
if (cmd_pub_.getNumSubscribers() == 0) { if (cmd_pub_.getNumSubscribers() == 0) {
std::cout << "[InvictaInterface] 警告:joint_commands 还没有订阅者,启动时序可能丢失" << std::endl; std::cout << "[InvictaInterface] warning: joint_commands has no subscribers yet, startup sequence may be lost" << std::endl;
} }
const uint16_t seq[] = {invicta_msgs::kDisable, invicta_msgs::kErrorReset, invicta_msgs::kEnable}; const uint16_t seq[] = {invicta_msgs::kDisable, invicta_msgs::kErrorReset, invicta_msgs::kEnable};
for (uint16_t word : seq) { for (uint16_t word : seq) {
PublishRawCommand(word); PublishRawCommand(word);
ros::Duration(0.01).sleep(); ros::Duration(0.01).sleep();
} }
std::cout << "[InvictaInterface] 启动控制序列完成 (disable -> error_reset -> enable)" << std::endl; std::cout << "[InvictaInterface] startup control sequence complete (disable -> error_reset -> enable)" << std::endl;
} }
// ------------------------------------------------------------------ 回调 // ------------------------------------------------------------------ 回调
void JointStateHandler(const sensor_msgs::JointState::ConstPtr& msg) { void JointStateHandler(const sensor_msgs::JointState::ConstPtr& msg) {
const size_t n = size_t(dof_num_); const size_t n = size_t(dof_num_);
if (msg->position.size() != n || msg->velocity.size() != n || msg->effort.size() != n) { if (msg->position.size() != n || msg->velocity.size() != n || msg->effort.size() != n) {
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 长度应为 %d,收到 pos=%zu vel=%zu eff=%zu,丢帧", ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s length should be %d, got pos=%zu vel=%zu eff=%zu, dropping frame",
cfg_.topic_joint_states.c_str(), dof_num_, msg->position.size(), msg->velocity.size(), cfg_.topic_joint_states.c_str(), dof_num_, msg->position.size(), msg->velocity.size(),
msg->effort.size()); msg->effort.size());
return; return;
@@ -159,11 +159,11 @@ protected:
} }
if (!by_name && !warned_name_mismatch_) { if (!by_name && !warned_name_mismatch_) {
warned_name_mismatch_ = true; warned_name_mismatch_ = true;
ROS_WARN("[InvictaInterface] %s 未按约定给出 %d 个 name,按数组下标解析", ROS_WARN("[InvictaInterface] %s did not provide the agreed %d names, resolving by array index",
cfg_.topic_joint_states.c_str(), dof_num_); cfg_.topic_joint_states.c_str(), dof_num_);
} }
if (unknown > 0) { if (unknown > 0) {
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 有 %d 个关节名不认识,已忽略", ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s has %d unknown joint names, ignored",
cfg_.topic_joint_states.c_str(), unknown); cfg_.topic_joint_states.c_str(), unknown);
} }
@@ -182,7 +182,7 @@ protected:
if (msg->orientation_covariance[0] == -1.0) { if (msg->orientation_covariance[0] == -1.0) {
if (!warned_imu_no_orientation_) { if (!warned_imu_no_orientation_) {
warned_imu_no_orientation_ = true; warned_imu_no_orientation_ = true;
ROS_WARN("[InvictaInterface] %s 没有提供 orientation(covariance=-1),姿态观测无效", ROS_WARN("[InvictaInterface] %s did not provide orientation (covariance=-1), attitude observation invalid",
cfg_.topic_imu.c_str()); cfg_.topic_imu.c_str());
} }
return; return;
@@ -190,7 +190,7 @@ protected:
Eigen::Quaternionf q(float(msg->orientation.w), float(msg->orientation.x), float(msg->orientation.y), Eigen::Quaternionf q(float(msg->orientation.w), float(msg->orientation.x), float(msg->orientation.y),
float(msg->orientation.z)); float(msg->orientation.z));
if (q.norm() < 1e-6f) { if (q.norm() < 1e-6f) {
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 四元数模长近零,忽略该帧", cfg_.topic_imu.c_str()); ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s quaternion norm near zero, ignoring this frame", cfg_.topic_imu.c_str());
return; return;
} }
q.normalize(); q.normalize();
@@ -47,12 +47,12 @@ public:
void Start() override { void Start() override {
if (msfb_) msfb_->UpdateCurrentGait(uint32_t(default_gait_)); if (msfb_) msfb_->UpdateCurrentGait(uint32_t(default_gait_));
std::cout << "\n╔════════════════════════════════════════════════╗\n" std::cout << "\n+==================================================+\n"
<< "║ INVICTA MOTION COMMAND LINK ║\n" << "| INVICTA MOTION COMMAND LINK |\n"
<< "╚════════════════════════════════════════════════╝\n" << "+==================================================+\n"
<< " 指令来源: " << topic_ << "(由 invicta_remote 发布)\n" << " Command source: " << topic_ << " (published by invicta_remote)\n"
<< " 默认步态: " << GaitName(uint32_t(default_gait_)) << "\n" << " Default gait: " << GaitName(uint32_t(default_gait_)) << "\n"
<< " 已支持步态: " << SupportedGaitList() << "\n\n"; << " Supported gaits: " << SupportedGaitList() << "\n\n";
} }
void Stop() override {} void Stop() override {}
@@ -61,7 +61,7 @@ public:
private: private:
std::string SupportedGaitList() const { std::string SupportedGaitList() const {
if (!loaded_gaits_ || loaded_gaits_->empty()) return "(无)"; if (!loaded_gaits_ || loaded_gaits_->empty()) return "(none)";
std::string s; std::string s;
for (int gait : *loaded_gaits_) { for (int gait : *loaded_gaits_) {
char buf[16]; char buf[16];
@@ -83,39 +83,39 @@ private:
switch (int(target)) { switch (int(target)) {
case kMotionStanding: case kMotionStanding:
name = "站立"; name = "stand up";
next = uint8_t(RobotMotionState::StandingUp); next = uint8_t(RobotMotionState::StandingUp);
accepted = (cur == RobotMotionState::WaitingForStand || cur == RobotMotionState::LieDown); accepted = (cur == RobotMotionState::WaitingForStand || cur == RobotMotionState::LieDown);
break; break;
case kMotionJointDamping: case kMotionJointDamping:
name = "关节阻尼"; name = "joint damping";
next = uint8_t(RobotMotionState::JointDamping); next = uint8_t(RobotMotionState::JointDamping);
accepted = true; accepted = true;
break; break;
case kMotionBootDamping: case kMotionBootDamping:
name = "开机阻尼(按关节阻尼处理)"; name = "boot damping (handled as joint damping)";
next = uint8_t(RobotMotionState::JointDamping); next = uint8_t(RobotMotionState::JointDamping);
accepted = true; accepted = true;
break; break;
case kMotionLieDown: case kMotionLieDown:
name = "趴下"; name = "lie down";
next = uint8_t(RobotMotionState::LieDown); next = uint8_t(RobotMotionState::LieDown);
accepted = (cur == RobotMotionState::StandingUp || cur == RobotMotionState::RLControlMode); accepted = (cur == RobotMotionState::StandingUp || cur == RobotMotionState::RLControlMode);
break; break;
case kMotionRlControl: case kMotionRlControl:
name = "RL控制"; name = "RL control";
next = uint8_t(RobotMotionState::RLControlMode); next = uint8_t(RobotMotionState::RLControlMode);
accepted = (cur == RobotMotionState::StandingUp); accepted = (cur == RobotMotionState::StandingUp);
break; break;
case kMotionIdle: case kMotionIdle:
// 状态机没有「直接回空闲」的路径(RL / 站立只能经阻尼或趴下退出),不作为请求 // 状态机没有「直接回空闲」的路径(RL / 站立只能经阻尼或趴下退出),不作为请求
if (last_request_ != kMotionIdle) { if (last_request_ != kMotionIdle) {
std::cout << "[MODE] 空闲(0):不作为切换请求(退出 RL 请先切关节阻尼或趴下)" << std::endl; std::cout << "[MODE] idle (0): not treated as a switch request (to exit RL, switch to joint damping or lie down first)" << std::endl;
} }
last_request_ = kMotionIdle; last_request_ = kMotionIdle;
return false; return false;
default: default:
std::cout << "[MODE] 未知目标状态 " << int(target) << "(来自 " << src << ")" << std::endl; std::cout << "[MODE] unknown target state " << int(target) << " (from " << src << ")" << std::endl;
return false; return false;
} }
@@ -127,9 +127,9 @@ private:
if (accepted) { if (accepted) {
usr_cmd_->target_mode = next; usr_cmd_->target_mode = next;
if (last_request_ != target) std::cout << "[MODE] " << name << "(来自 " << src << ")" << std::endl; if (last_request_ != target) std::cout << "[MODE] " << name << " (from " << src << ")" << std::endl;
} else if (last_request_ != target) { } else if (last_request_ != target) {
std::cout << "[MODE] 拒绝 " << name << "(来自 " << src << ",当前状态 " << int(cur) << ")" << std::endl; std::cout << "[MODE] reject " << name << " (from " << src << ", current state " << int(cur) << ")" << std::endl;
} }
last_request_ = target; last_request_ = target;
return accepted; return accepted;
@@ -145,14 +145,14 @@ private:
if (gait == 0 || gait == last_gait_request_) return; if (gait == 0 || gait == last_gait_request_) return;
// 只有"启动时真的加载了模型"的步态才接受;否则状态会上报未切换,而不是假成功 // 只有"启动时真的加载了模型"的步态才接受;否则状态会上报未切换,而不是假成功
if (!loaded_gaits_ || loaded_gaits_->count(int(gait)) == 0) { if (!loaded_gaits_ || loaded_gaits_->count(int(gait)) == 0) {
std::cout << "[GAIT] 拒绝 " << GaitName(gait) << "(来自 " << src std::cout << "[GAIT] reject " << GaitName(gait) << " (from " << src
<< "):该步态没有对应的 RL 模型(启动时未加载,见 gait/policy_map)" << std::endl; << "): this gait has no corresponding RL model (not loaded at startup, see gait/policy_map)" << std::endl;
last_gait_request_ = gait; last_gait_request_ = gait;
return; return;
} }
usr_cmd_->gait = gait; usr_cmd_->gait = gait;
if (msfb_) msfb_->UpdateCurrentGait(gait); if (msfb_) msfb_->UpdateCurrentGait(gait);
std::cout << "[GAIT] " << GaitName(gait) << "(来自 " << src << "),实际生效" << std::endl; std::cout << "[GAIT] " << GaitName(gait) << " (from " << src << "), now effective" << std::endl;
last_gait_request_ = gait; last_gait_request_ = gait;
} }
@@ -160,7 +160,7 @@ private:
const std::string src = "invicta_remote"; const std::string src = "invicta_remote";
if (!has_command_) { if (!has_command_) {
has_command_ = true; has_command_ = true;
std::cout << "[MODE] 收到第一条运动指令(" << topic_ << ")" << std::endl; std::cout << "[MODE] received first motion command (" << topic_ << ")" << std::endl;
} }
// 轴指令:协议里是比例 [-1,1],策略直接用这三个分量 // 轴指令:协议里是比例 [-1,1],策略直接用这三个分量
@@ -171,7 +171,7 @@ private:
if (msg->usage_mode != usr_cmd_->usage_mode) { if (msg->usage_mode != usr_cmd_->usage_mode) {
usr_cmd_->usage_mode = msg->usage_mode; usr_cmd_->usage_mode = msg->usage_mode;
std::cout << "[MODE] 使用模式 -> " << int(msg->usage_mode) << "(0 常规 / 1 导航 / 2 辅助)" << std::endl; std::cout << "[MODE] usage mode -> " << int(msg->usage_mode) << " (0 normal / 1 navigation / 2 assist)" << std::endl;
} }
RequestMotionState(msg->target_state, src); RequestMotionState(msg->target_state, src);
+1 -1
View File
@@ -29,7 +29,7 @@ int main(int argc, char** argv) {
std::string err; std::string err;
auto& cfg = invicta::Config(); auto& cfg = invicta::Config();
if (!cfg.Load(config_path, &err)) { if (!cfg.Load(config_path, &err)) {
ROS_FATAL("[invicta_deploy] 配置加载失败:%s(%s)", err.c_str(), config_path.c_str()); ROS_FATAL("[invicta_deploy] failed to load config: %s (%s)", err.c_str(), config_path.c_str());
return 1; return 1;
} }
std::cout << cfg.Summary() << std::flush; std::cout << cfg.Summary() << std::flush;
@@ -37,7 +37,7 @@ public:
action_dim_ = cfg_.action_dim; action_dim_ = cfg_.action_dim;
obs_dim_ = cfg_.obs_dim; obs_dim_ = cfg_.obs_dim;
if (action_dim_ <= 0 || obs_dim_ <= 0) { if (action_dim_ <= 0 || obs_dim_ <= 0) {
throw std::runtime_error("策略维度为 0:配置未加载或 [policy] 段不完整"); throw std::runtime_error("policy dimension is 0: config not loaded or [policy] section incomplete");
} }
robot2policy_idx_ = Permutation(cfg_.joint_names, cfg_.policy_joint_order); robot2policy_idx_ = Permutation(cfg_.joint_names, cfg_.policy_joint_order);
@@ -88,7 +88,7 @@ public:
current_state_name_ = kIdle; current_state_name_ = kIdle;
next_state_name_ = kIdle; next_state_name_ = kIdle;
std::cout << "control period: " << cfg.control_period_ms << " ms, 默认模型: " << policy_path << std::endl; std::cout << "control period: " << cfg.control_period_ms << " ms, default model: " << policy_path << std::endl;
// ---- 状态回报(供 invicta_remote 判前提 + 状态上报)---- // ---- 状态回报(供 invicta_remote 判前提 + 状态上报)----
state_reporter_ = std::make_shared<interface::MotionStateReporter>( state_reporter_ = std::make_shared<interface::MotionStateReporter>(
@@ -34,13 +34,13 @@ public:
virtual void OnEnter() { virtual void OnEnter() {
start_time_ = std::chrono::steady_clock::now(); start_time_ = std::chrono::steady_clock::now();
std::cout << "[DAMPING] 进入关节阻尼,持续 " << invicta::Config().damping_hold_s << " s 后转入空闲" std::cout << "[DAMPING] entering joint damping, duration " << invicta::Config().damping_hold_s << " s then transition to idle"
<< std::endl; << std::endl;
msfb_.UpdateCurrentState(RobotMotionState::JointDamping); msfb_.UpdateCurrentState(RobotMotionState::JointDamping);
} }
virtual void OnExit() { virtual void OnExit() {
std::cout << "[DAMPING] 结束(实际 " << Elapsed() << " s),转入空闲" << std::endl; std::cout << "[DAMPING] exit (actual " << Elapsed() << " s), transition to idle" << std::endl;
} }
virtual void Run() { ri_ptr_->SetJointCommand(joint_cmd_); } virtual void Run() { ri_ptr_->SetJointCommand(joint_cmd_); }
@@ -44,7 +44,7 @@ public:
joint_cmd_(cfg.legs[l].wheel_index, 2) = float(cfg.damping_kd_wheel); joint_cmd_(cfg.legs[l].wheel_index, 2) = float(cfg.damping_kd_wheel);
char buf[160]; char buf[160];
snprintf(buf, sizeof(buf), "[GAIT] 趴下: %s 目标高度 %.3f m → 摆臂 %+.1f°\n", cfg.legs[l].name.c_str(), snprintf(buf, sizeof(buf), "[GAIT] lie down: %s target height %.3f m -> swing %+.1f deg\n", cfg.legs[l].name.c_str(),
cfg.lie_height, target_swing_(int(l)) * 180. / M_PI); cfg.lie_height, target_swing_(int(l)) * 180. / M_PI);
std::cout << buf; std::cout << buf;
} }
@@ -88,8 +88,8 @@ private:
auto next = PolicyForGait(gait, &switched); auto next = PolicyForGait(gait, &switched);
if (switched) { if (switched) {
next->OnEnter(); // 清 last_action / run_cnt,避免跨模型残留 next->OnEnter(); // 清 last_action / run_cnt,避免跨模型残留
std::cout << "[GAIT] 切换模型 -> " << invicta_msgs::GaitName(gait) std::cout << "[GAIT] switch model -> " << invicta_msgs::GaitName(gait)
<< (gait_runners_.count(int(gait)) ? "(已注册模型)" : "(默认模型)") << std::endl; << (gait_runners_.count(int(gait)) ? " (registered model)" : " (default model)") << std::endl;
} }
active_policy_ = next; active_policy_ = next;
active_gait_ = int(gait); active_gait_ = int(gait);
@@ -101,7 +101,7 @@ private:
MatXf res = ra.ConvertToMat(); MatXf res = ra.ConvertToMat();
if (!res.allFinite()) { if (!res.allFinite()) {
policy_output_bad_ = true; policy_output_bad_ = true;
std::cout << "[RL] 推理输出含 NaN/Inf,停止下发并转入关节阻尼" << std::endl; std::cout << "[RL] policy output contains NaN/Inf, stopping commands and switching to joint damping" << std::endl;
} else { } else {
ri_ptr_->SetJointCommand(res); ri_ptr_->SetJointCommand(res);
} }
@@ -123,13 +123,13 @@ public:
try { try {
gait_paths_[std::stoi(kv.first)] = kv.second; gait_paths_[std::stoi(kv.first)] = kv.second;
} catch (const std::exception&) { } catch (const std::exception&) {
ROS_WARN("[RLControlState] 步态表键 '%s' 不是整数,已忽略", kv.first.c_str()); ROS_WARN("[RLControlState] gait map key '%s' is not an integer, ignored", kv.first.c_str());
} }
} }
default_policy_ = std::make_shared<InvictaPolicyRunner>("invicta_policy", policy_path); default_policy_ = std::make_shared<InvictaPolicyRunner>("invicta_policy", policy_path);
active_policy_ = default_policy_; active_policy_ = default_policy_;
std::cout << "[GAIT] 默认模型: " << policy_path << std::endl; std::cout << "[GAIT] default model: " << policy_path << std::endl;
// 默认步态 = 默认模型 // 默认步态 = 默认模型
if (loaded_gaits_) loaded_gaits_->insert(default_gait_); if (loaded_gaits_) loaded_gaits_->insert(default_gait_);
@@ -138,14 +138,14 @@ public:
for (const auto& kv : gait_paths_) { for (const auto& kv : gait_paths_) {
if (kv.first == default_gait_) continue; // 默认步态已在上面处理 if (kv.first == default_gait_) continue; // 默认步态已在上面处理
if (kv.second.empty()) { if (kv.second.empty()) {
std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first)) std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first))
<< " 未提供模型路径,该步态不可用" << std::endl; << " has no model path, gait unavailable" << std::endl;
continue; continue;
} }
if (kv.second == policy_path) { if (kv.second == policy_path) {
gait_runners_[kv.first] = default_policy_; gait_runners_[kv.first] = default_policy_;
if (loaded_gaits_) loaded_gaits_->insert(kv.first); if (loaded_gaits_) loaded_gaits_->insert(kv.first);
std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first)) << " 复用默认模型" std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first)) << " reuses default model"
<< std::endl; << std::endl;
continue; continue;
} }
@@ -153,11 +153,11 @@ public:
gait_runners_[kv.first] = std::make_shared<InvictaPolicyRunner>("gait_" + std::to_string(kv.first), gait_runners_[kv.first] = std::make_shared<InvictaPolicyRunner>("gait_" + std::to_string(kv.first),
kv.second); kv.second);
if (loaded_gaits_) loaded_gaits_->insert(kv.first); if (loaded_gaits_) loaded_gaits_->insert(kv.first);
std::cout << "[GAIT] 预加载步态 " << invicta_msgs::GaitName(uint32_t(kv.first)) << " 模型 " std::cout << "[GAIT] preload gait " << invicta_msgs::GaitName(uint32_t(kv.first)) << " model "
<< kv.second << std::endl; << kv.second << std::endl;
} catch (const std::exception& e) { } catch (const std::exception& e) {
std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first)) std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first))
<< " 模型加载失败(该步态将不可用):" << e.what() << std::endl; << " failed to load model (gait unavailable): " << e.what() << std::endl;
} }
} }
policy_output_bad_ = false; policy_output_bad_ = false;
@@ -58,7 +58,7 @@ public:
joint_cmd_(cfg.legs[l].wheel_index, 0) = 0.f; // 轮 kp 恒 0 joint_cmd_(cfg.legs[l].wheel_index, 0) = 0.f; // 轮 kp 恒 0
char buf[160]; char buf[160];
snprintf(buf, sizeof(buf), "[GAIT] 起立: %s 收腿(%.3f m) %+.1f° → 站立(%.3f m) %+.1f°\n", snprintf(buf, sizeof(buf), "[GAIT] stand up: %s tuck leg(%.3f m) %+.1f deg -> stand(%.3f m) %+.1f deg\n",
cfg.legs[l].name.c_str(), cfg.pre_height, tuck_swing_(int(l)) * 180. / M_PI, cfg.stand_height, cfg.legs[l].name.c_str(), cfg.pre_height, tuck_swing_(int(l)) * 180. / M_PI, cfg.stand_height,
stand_swing_(int(l)) * 180. / M_PI); stand_swing_(int(l)) * 180. / M_PI);
std::cout << buf; std::cout << buf;
@@ -134,12 +134,12 @@ public:
bool ReadyForRl() { bool ReadyForRl() {
const auto& cfg = invicta::Config(); const auto& cfg = invicta::Config();
if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) { if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) {
LogNotReady("安全标志非 0"); LogNotReady("safe flag is not 0");
return false; return false;
} }
for (int i = 0; i < int(current_joint_pos_.size()); ++i) { for (int i = 0; i < int(current_joint_pos_.size()); ++i) {
if (std::isnan(current_joint_pos_(i)) || std::isnan(current_joint_vel_(i))) { if (std::isnan(current_joint_pos_(i)) || std::isnan(current_joint_vel_(i))) {
LogNotReady("关节数据含 NaN"); LogNotReady("joint data contains NaN");
return false; return false;
} }
} }
@@ -147,11 +147,11 @@ public:
for (size_t l = 0; l < cfg.legs.size(); ++l) { for (size_t l = 0; l < cfg.legs.size(); ++l) {
const int si = cfg.legs[l].swing_index; const int si = cfg.legs[l].swing_index;
if (std::fabs(current_joint_pos_(si) - stand_pose_(si)) > cfg.rl_joint_tolerance) { if (std::fabs(current_joint_pos_(si) - stand_pose_(si)) > cfg.rl_joint_tolerance) {
LogNotReady("关节 " + cfg.joint_names[size_t(si)] + " 未到站立位姿"); LogNotReady("joint " + cfg.joint_names[size_t(si)] + " not at stand pose");
return false; return false;
} }
if (std::fabs(current_joint_vel_(si)) > cfg.rl_velocity_tolerance) { if (std::fabs(current_joint_vel_(si)) > cfg.rl_velocity_tolerance) {
LogNotReady("关节 " + cfg.joint_names[size_t(si)] + " 速度过大"); LogNotReady("joint " + cfg.joint_names[size_t(si)] + " velocity too large");
return false; return false;
} }
} }
@@ -161,12 +161,12 @@ public:
const Vec3f acc = ri_ptr_->GetImuAcc(); const Vec3f acc = ri_ptr_->GetImuAcc();
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
if (std::isnan(rpy(i)) || std::isnan(acc(i))) { if (std::isnan(rpy(i)) || std::isnan(acc(i))) {
LogNotReady("IMU 数据含 NaN"); LogNotReady("IMU data contains NaN");
return false; return false;
} }
} }
if (acc.norm() < 0.1f * gravity || acc.norm() > 3.0f * gravity) { if (acc.norm() < 0.1f * gravity || acc.norm() > 3.0f * gravity) {
LogNotReady("IMU 加速度异常"); LogNotReady("IMU acceleration abnormal");
return false; return false;
} }
} }
@@ -176,7 +176,7 @@ public:
void LogNotReady(const std::string& why) { void LogNotReady(const std::string& why) {
if (run_time_ - last_ready_log_ < 1.0) return; if (run_time_ - last_ready_log_ < 1.0) return;
last_ready_log_ = run_time_; last_ready_log_ = run_time_;
std::cout << "[RL] 暂不进入 RL:" << why << std::endl; std::cout << "[RL] not entering RL yet: " << why << std::endl;
} }
virtual bool LoseControlJudge() { virtual bool LoseControlJudge() {
@@ -200,7 +200,7 @@ public:
if (elapsed >= ready_time + cfg.rl_enter_delay && ReadyForRl()) { if (elapsed >= ready_time + cfg.rl_enter_delay && ReadyForRl()) {
if (!rl_enter_logged_) { if (!rl_enter_logged_) {
rl_enter_logged_ = true; rl_enter_logged_ = true;
std::cout << "[RL] 站立完成且自检通过,进入 RL 控制" << std::endl; std::cout << "[RL] stand up complete and self-check passed, entering RL control" << std::endl;
} }
return StateName::kRLControl; return StateName::kRLControl;
} }
@@ -148,13 +148,13 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
try { try {
tbl = toml::parse_file(path); tbl = toml::parse_file(path);
} catch (const toml::parse_error& e) { } catch (const toml::parse_error& e) {
return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path); return fail(std::string("failed to parse TOML: ") + std::string(e.description()) + " @ " + path);
} }
const toml::table* robot = detail::Section(tbl, "robot"); const toml::table* robot = detail::Section(tbl, "robot");
if (!robot) return fail("缺少 [robot] 段"); if (!robot) return fail("missing [robot] section");
detail::GetStrArr(robot, "joint_names", joint_names); detail::GetStrArr(robot, "joint_names", joint_names);
if (joint_names.empty()) return fail("[robot].joint_names 为空"); if (joint_names.empty()) return fail("[robot].joint_names is empty");
dof = int(joint_names.size()); dof = int(joint_names.size());
std::vector<double> lower, upper, vel_limit, torque_limit; std::vector<double> lower, upper, vel_limit, torque_limit;
@@ -165,7 +165,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
detail::GetDoubleArr(limits, "torque_limit", torque_limit); detail::GetDoubleArr(limits, "torque_limit", torque_limit);
if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof || if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof ||
int(torque_limit.size()) != dof) { int(torque_limit.size()) != dof) {
return fail("[joint_limits] 的 lower/upper/vel_limit/torque_limit 长度必须等于关节数量"); return fail("[joint_limits] lower/upper/vel_limit/torque_limit length must equal joint count");
} }
joints.assign(size_t(dof), JointSpec{}); joints.assign(size_t(dof), JointSpec{});
for (int i = 0; i < dof; ++i) { for (int i = 0; i < dof; ++i) {
@@ -188,7 +188,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
leg.swing_index = IndexOf(leg.swing_joint); leg.swing_index = IndexOf(leg.swing_joint);
leg.wheel_index = IndexOf(leg.wheel_joint); leg.wheel_index = IndexOf(leg.wheel_joint);
if (leg.swing_index < 0 || leg.wheel_index < 0) { if (leg.swing_index < 0 || leg.wheel_index < 0) {
return fail("[[legs]] " + leg.name + " 的 swing_joint/wheel_joint 不在 joint_names 里"); return fail("[[legs]] " + leg.name + " swing_joint/wheel_joint not in joint_names");
} }
const toml::table* sw = detail::Section(*lt, "swing"); const toml::table* sw = detail::Section(*lt, "swing");
const toml::table* wh = detail::Section(*lt, "wheel"); const toml::table* wh = detail::Section(*lt, "wheel");
@@ -206,7 +206,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
legs.push_back(leg); legs.push_back(leg);
} }
} }
if (legs.empty()) return fail("缺少 [[legs]] 段"); if (legs.empty()) return fail("missing [[legs]] section");
if (const toml::table* mj = detail::Section(tbl, "motor")) { if (const toml::table* mj = detail::Section(tbl, "motor")) {
const auto load_motor = [](const toml::table* m, MotorSpec& out) { const auto load_motor = [](const toml::table* m, MotorSpec& out) {
@@ -247,13 +247,13 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
const JointSpec& j = joints[size_t(i)]; const JointSpec& j = joints[size_t(i)];
const MotorSpec& m = j.is_wheel ? motor_wheel : motor_joint; const MotorSpec& m = j.is_wheel ? motor_wheel : motor_joint;
if (!j.is_wheel && (j.lower < m.p_min || j.upper > m.p_max)) { if (!j.is_wheel && (j.lower < m.p_min || j.upper > m.p_max)) {
return fail("[joint_limits] " + j.name + " 的位置限幅超出归一化边界 [p_min, p_max]"); return fail("[joint_limits] " + j.name + " position limit outside normalized bounds [p_min, p_max]");
} }
if (j.vel_limit > std::min(std::fabs(m.v_max), std::fabs(m.v_min))) { if (j.vel_limit > std::min(std::fabs(m.v_max), std::fabs(m.v_min))) {
return fail("[joint_limits] " + j.name + " 的速度限幅超出归一化边界 [v_min, v_max]"); return fail("[joint_limits] " + j.name + " velocity limit outside normalized bounds [v_min, v_max]");
} }
if (j.torque_limit > std::min(std::fabs(m.t_max), std::fabs(m.t_min))) { if (j.torque_limit > std::min(std::fabs(m.t_max), std::fabs(m.t_min))) {
return fail("[joint_limits] " + j.name + " 的力矩限幅超出归一化边界 [t_min, t_max]"); return fail("[joint_limits] " + j.name + " torque limit outside normalized bounds [t_min, t_max]");
} }
} }
@@ -272,9 +272,9 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
if (hw) { if (hw) {
if (auto v = (*hw)["require_can"].value<bool>()) require_can = *v; if (auto v = (*hw)["require_can"].value<bool>()) require_can = *v;
} }
if (publish_rate_hz <= 0.) return fail("[hardware].publish_rate_hz 必须 > 0"); if (publish_rate_hz <= 0.) return fail("[hardware].publish_rate_hz must be > 0");
if (diagnostics_rate_hz <= 0.) return fail("[hardware].diagnostics_rate_hz 必须 > 0"); if (diagnostics_rate_hz <= 0.) return fail("[hardware].diagnostics_rate_hz must be > 0");
if (command_timeout_s < 0.) return fail("[hardware].command_timeout_s 不能为负"); if (command_timeout_s < 0.) return fail("[hardware].command_timeout_s must not be negative");
if (error) error->clear(); if (error) error->clear();
return true; return true;
@@ -283,7 +283,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
inline std::string HardwareConfig::Summary() const { inline std::string HardwareConfig::Summary() const {
char buf[512]; char buf[512];
std::string s; std::string s;
snprintf(buf, sizeof(buf), "[config] %d 关节,%zu 条腿,状态发布 %.0f Hz,IMU frame=%s\n", dof, legs.size(), snprintf(buf, sizeof(buf), "[config] %d joints, %zu legs, state publish %.0f Hz, IMU frame=%s\n", dof, legs.size(),
publish_rate_hz, imu_frame_id.c_str()); publish_rate_hz, imu_frame_id.c_str());
s += buf; s += buf;
for (const auto& leg : legs) { for (const auto& leg : legs) {
@@ -292,11 +292,11 @@ inline std::string HardwareConfig::Summary() const {
leg.wheel_bus.c_str(), leg.wheel_canid); leg.wheel_bus.c_str(), leg.wheel_canid);
s += buf; s += buf;
} }
snprintf(buf, sizeof(buf), "[config] 电机: 关节 %s(p±%.1f v±%.1f t±%.0f) 轮 %s(p±%.1f v±%.1f t±%.0f)\n", snprintf(buf, sizeof(buf), "[config] motor: joint %s(p+/-%.1f v+/-%.1f t+/-%.0f) wheel %s(p+/-%.1f v+/-%.1f t+/-%.0f)\n",
motor_joint.model.c_str(), motor_joint.p_max, motor_joint.v_max, motor_joint.t_max, motor_joint.model.c_str(), motor_joint.p_max, motor_joint.v_max, motor_joint.t_max,
motor_wheel.model.c_str(), motor_wheel.p_max, motor_wheel.v_max, motor_wheel.t_max); motor_wheel.model.c_str(), motor_wheel.p_max, motor_wheel.v_max, motor_wheel.t_max);
s += buf; s += buf;
snprintf(buf, sizeof(buf), "[config] CAN: %zu 路 %s.. @%.1fM/%.1fM, 使能 %d 帧 x %.3f s\n", snprintf(buf, sizeof(buf), "[config] CAN: %zu channels %s.. @%.1fM/%.1fM, enable %d frames x %.3f s\n",
can.channels.size(), can.channels.empty() ? "-" : can.channels.front().c_str(), can.bitrate / 1e6, can.channels.size(), can.channels.empty() ? "-" : can.channels.front().c_str(), can.bitrate / 1e6,
can.data_bitrate / 1e6, can.enable_frames, can.enable_interval_s); can.data_bitrate / 1e6, can.enable_frames, can.enable_interval_s);
s += buf; s += buf;
@@ -84,14 +84,14 @@ struct MotorFeedback {
/// 状态码非零故障位的中文名,用于日志 /// 状态码非零故障位的中文名,用于日志
inline const char* FaultName(uint8_t status) { inline const char* FaultName(uint8_t status) {
switch ((status >> 1) & 0x3F) { switch ((status >> 1) & 0x3F) {
case 0x00: return "无"; case 0x00: return "none";
case 0x01: return "过压"; case 0x01: return "overvoltage";
case 0x02: return "过流"; case 0x02: return "overcurrent";
case 0x04: return "过温"; case 0x04: return "overtemperature";
case 0x08: return "过速"; case 0x08: return "overspeed";
case 0x10: return "编码器"; case 0x10: return "encoder";
case 0x20: return "欠压/预驱"; case 0x20: return "undervoltage/pre-driver";
default: return "组合故障"; default: return "combined fault";
} }
} }
@@ -59,7 +59,7 @@ HardwareBridge::HardwareBridge(const ros::NodeHandle& pnh) : nh_() {
ready_ = OpenBuses(); ready_ = OpenBuses();
ROS_INFO("[invicta_hardware] %s -> %s/%s,订阅 %s @ %.0f Hz;诊断 %s @ %.0f Hz", joint_state_topic_.c_str(), ROS_INFO("[invicta_hardware] %s -> %s/%s, subscribing %s @ %.0f Hz; diagnostics %s @ %.0f Hz", joint_state_topic_.c_str(),
imu_topic_.c_str(), imu_frame_id_.c_str(), command_topic_.c_str(), publish_rate_hz_, imu_topic_.c_str(), imu_frame_id_.c_str(), command_topic_.c_str(), publish_rate_hz_,
diagnostics_topic_.c_str(), diagnostics_rate_hz_); diagnostics_topic_.c_str(), diagnostics_rate_hz_);
} }
@@ -69,7 +69,7 @@ bool HardwareBridge::OpenBuses() {
buses_.clear(); buses_.clear();
if (cfg.can.channels.empty()) { if (cfg.can.channels.empty()) {
ROS_ERROR("[invicta_hardware] 配置里没有 CAN 通道([motor.can].channels)"); ROS_ERROR("[invicta_hardware] no CAN channels in config ([motor.can].channels)");
return !require_can_; return !require_can_;
} }
@@ -79,11 +79,11 @@ bool HardwareBridge::OpenBuses() {
std::string err; std::string err;
if (!bus.can.Open(name, &err)) { if (!bus.can.Open(name, &err)) {
ROS_ERROR("[invicta_hardware] 打开 %s 失败:%s", name.c_str(), err.c_str()); ROS_ERROR("[invicta_hardware] failed to open %s: %s", name.c_str(), err.c_str());
ROS_ERROR("[invicta_hardware] 请先配置总线:sudo ip link set %s up type can bitrate %.0f dbitrate %.0f fd on", ROS_ERROR("[invicta_hardware] configure the bus first: sudo ip link set %s up type can bitrate %.0f dbitrate %.0f fd on",
name.c_str(), cfg.can.bitrate, cfg.can.data_bitrate); name.c_str(), cfg.can.bitrate, cfg.can.data_bitrate);
if (require_can_) return false; if (require_can_) return false;
ROS_WARN("[invicta_hardware] require_can=false,跳过 %s,只发布状态、不下发电机指令", name.c_str()); ROS_WARN("[invicta_hardware] require_can=false, skipping %s, state published only, no motor commands sent", name.c_str());
continue; continue;
} }
@@ -93,7 +93,7 @@ bool HardwareBridge::OpenBuses() {
if (leg.wheel_bus == name) bus.max_canid = std::max(bus.max_canid, leg.wheel_canid); if (leg.wheel_bus == name) bus.max_canid = std::max(bus.max_canid, leg.wheel_canid);
} }
if (bus.max_canid <= 0 || bus.max_canid > kMaxMotorsPerFrame) { if (bus.max_canid <= 0 || bus.max_canid > kMaxMotorsPerFrame) {
ROS_ERROR("[invicta_hardware] %s 上的 CANID 非法(最大 %d,协议要求 1~%d)", name.c_str(), bus.max_canid, ROS_ERROR("[invicta_hardware] invalid CANID on %s (max %d, protocol requires 1~%d)", name.c_str(), bus.max_canid,
kMaxMotorsPerFrame); kMaxMotorsPerFrame);
if (require_can_) return false; if (require_can_) return false;
continue; continue;
@@ -105,7 +105,7 @@ bool HardwareBridge::OpenBuses() {
} }
bus.frame_len = FrameLenForMaxCanId(bus.max_canid); bus.frame_len = FrameLenForMaxCanId(bus.max_canid);
ROS_INFO("[invicta_hardware] %s 已打开(CANID 1~%d,下发 %d 字节)", name.c_str(), bus.max_canid, bus.frame_len); ROS_INFO("[invicta_hardware] %s opened (CANID 1~%d, sending %d bytes)", name.c_str(), bus.max_canid, bus.frame_len);
buses_.push_back(std::move(bus)); buses_.push_back(std::move(bus));
} }
@@ -135,11 +135,11 @@ void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
joint_names_ = cfg.joint_names; // 顺序必须与 invicta_deploy 一致(同一份配置,天然一致) joint_names_ = cfg.joint_names; // 顺序必须与 invicta_deploy 一致(同一份配置,天然一致)
if (publish_rate_hz_ <= 0.) { if (publish_rate_hz_ <= 0.) {
ROS_ERROR("[invicta_hardware] publish_rate_hz 必须 > 0,回退到 200 Hz"); ROS_ERROR("[invicta_hardware] publish_rate_hz must be > 0, falling back to 200 Hz");
publish_rate_hz_ = 200.0; publish_rate_hz_ = 200.0;
} }
if (diagnostics_rate_hz_ <= 0.) { if (diagnostics_rate_hz_ <= 0.) {
ROS_ERROR("[invicta_hardware] diagnostics_rate_hz 必须 > 0,回退到 10 Hz"); ROS_ERROR("[invicta_hardware] diagnostics_rate_hz must be > 0, falling back to 10 Hz");
diagnostics_rate_hz_ = 10.0; diagnostics_rate_hz_ = 10.0;
} }
} }
@@ -182,18 +182,18 @@ void HardwareBridge::HandleControlWord(uint16_t word, const std::vector<uint8_t>
case invicta_msgs::kDisable: case invicta_msgs::kDisable:
SendCodeAll(kCodeDisable, 1); SendCodeAll(kCodeDisable, 1);
enabled_ = false; enabled_ = false;
ROS_INFO("[invicta_hardware] control_word -> kDisable:已发失能(0xFD),线圈断开"); ROS_INFO("[invicta_hardware] control_word -> kDisable: disable sent (0xFD), coils open");
break; break;
case invicta_msgs::kEnable: case invicta_msgs::kEnable:
SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s); SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
enabled_ = true; enabled_ = true;
ROS_INFO("[invicta_hardware] control_word -> kEnable:已发使能(0xFC × %d)", Config().can.enable_frames); ROS_INFO("[invicta_hardware] control_word -> kEnable: enable sent (0xFC x %d)", Config().can.enable_frames);
break; break;
case invicta_msgs::kErrorReset: case invicta_msgs::kErrorReset:
// 协议没有独立的清错指令(手册 §6:故障后靠重新使能或重启恢复) // 协议没有独立的清错指令(手册 §6:故障后靠重新使能或重启恢复)
SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s); SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
enabled_ = true; enabled_ = true;
ROS_INFO("[invicta_hardware] control_word -> kErrorReset:本协议无清错指令,按重新使能处理"); ROS_INFO("[invicta_hardware] control_word -> kErrorReset: no fault-reset command in this protocol, handled as re-enable");
break; break;
case invicta_msgs::kMotorControl: case invicta_msgs::kMotorControl:
if (!enabled_) { if (!enabled_) {
@@ -201,13 +201,13 @@ void HardwareBridge::HandleControlWord(uint16_t word, const std::vector<uint8_t>
// 协议要求先使能再运行,这里补一次使能,而不是永远不发帧。 // 协议要求先使能再运行,这里补一次使能,而不是永远不发帧。
SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s); SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s);
enabled_ = true; enabled_ = true;
ROS_WARN("[invicta_hardware] 收到 kMotorControl 但未使能:已补发使能序列(上游启动时序可能丢失)"); ROS_WARN("[invicta_hardware] kMotorControl received but not enabled: enable sequence re-sent (upstream startup sequence may have been lost)");
} }
ROS_INFO("[invicta_hardware] control_word -> kMotorControl:进入 kp/kd + 前馈控制"); ROS_INFO("[invicta_hardware] control_word -> kMotorControl: entering kp/kd + feedforward control");
break; break;
case invicta_msgs::kGetStatus: case invicta_msgs::kGetStatus:
// 每条指令都会回 12 字节状态帧,无需专门请求 // 每条指令都会回 12 字节状态帧,无需专门请求
ROS_INFO("[invicta_hardware] control_word -> kGetStatus:状态随每条指令自动返回,无需请求"); ROS_INFO("[invicta_hardware] control_word -> kGetStatus: status returns automatically with every command, no request needed");
break; break;
case invicta_msgs::kSetZero: { case invicta_msgs::kSetZero: {
// 把当前机械位置写成模组零位(0xEF)。只作用于指令 name 选中的关节子集, // 把当前机械位置写成模组零位(0xEF)。只作用于指令 name 选中的关节子集,
@@ -221,17 +221,17 @@ void HardwareBridge::HandleControlWord(uint16_t word, const std::vector<uint8_t>
if (mask[i]) sel += (sel.empty() ? "" : ",") + joint_names_[i]; if (mask[i]) sel += (sel.empty() ? "" : ",") + joint_names_[i];
} }
if (sel.empty()) { if (sel.empty()) {
ROS_WARN("[invicta_hardware] kSetZero:选中的关节里没有可置零的摆臂,忽略"); ROS_WARN("[invicta_hardware] kSetZero: no swing joints to set zero among the selected joints, ignoring");
break; break;
} }
SendCodeAll(kCodeZero, 1, 0.0, &mask); SendCodeAll(kCodeZero, 1, 0.0, &mask);
ROS_WARN("[invicta_hardware] control_word -> kSetZero:已发置零(0xEF)到 [%s];" ROS_WARN("[invicta_hardware] control_word -> kSetZero: set zero sent (0xEF) to [%s]; "
"此后这些摆臂该位姿的反馈角度约为 0,请把对应 offset_deg 设为 0(或据此重标)", "from now on the feedback angle of these swing joints at this pose is about 0, set the corresponding offset_deg to 0 (or recalibrate accordingly)",
sel.c_str()); sel.c_str());
break; break;
} }
default: default:
ROS_WARN("[invicta_hardware] 未知 control_word=%u,忽略", word); ROS_WARN("[invicta_hardware] unknown control_word=%u, ignore", word);
break; break;
} }
} }
@@ -263,7 +263,7 @@ void HardwareBridge::SendCodeAll(uint8_t code, int frames, double interval_s, co
if (!any) continue; if (!any) continue;
std::string err; std::string err;
if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) { if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 发送控制码 0x%02X 失败: %s", bus.name.c_str(), code, ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s failed to send control code 0x%02X: %s", bus.name.c_str(), code,
err.c_str()); err.c_str());
} }
} }
@@ -299,7 +299,7 @@ bool HardwareBridge::SendTimeoutFrame() {
std::string err; std::string err;
if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) { if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 发送阻尼帧失败: %s", bus.name.c_str(), err.c_str()); ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s failed to send damping frame: %s", bus.name.c_str(), err.c_str());
} }
} }
return true; return true;
@@ -318,12 +318,12 @@ bool HardwareBridge::ReadHardware() {
const int canid = int(frame.id); const int canid = int(frame.id);
const int joint = (canid > 0 && canid <= bus.max_canid) ? bus.canid_to_joint[size_t(canid)] : -1; const int joint = (canid > 0 && canid <= bus.max_canid) ? bus.canid_to_joint[size_t(canid)] : -1;
if (joint < 0) { if (joint < 0) {
ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s 收到未知帧 id=0x%X len=%u,忽略", bus.name.c_str(), ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s received unknown frame id=0x%X len=%u, ignore", bus.name.c_str(),
unsigned(frame.id), unsigned(frame.len)); unsigned(frame.id), unsigned(frame.len));
continue; continue;
} }
if (frame.len < 12) { if (frame.len < 12) {
ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s CANID=%d 反馈只有 %u 字节(协议要求 12)", bus.name.c_str(), ROS_WARN_THROTTLE(5.0, "[invicta_hardware] %s CANID=%d feedback only %u bytes (protocol requires 12)", bus.name.c_str(),
canid, unsigned(frame.len)); canid, unsigned(frame.len));
continue; continue;
} }
@@ -344,7 +344,7 @@ bool HardwareBridge::ReadHardware() {
has_feedback_[size_t(joint)] = 1; has_feedback_[size_t(joint)] = 1;
if (fb.fault() && last_status_[size_t(joint)] != fb.status) { if (fb.fault() && last_status_[size_t(joint)] != fb.status) {
ROS_ERROR("[invicta_hardware] %s 故障:%s(status=0x%02X,温度 %.0f ℃)", ROS_ERROR("[invicta_hardware] %s fault: %s (status=0x%02X, temperature %.0f C)",
joint_names_[size_t(joint)].c_str(), FaultName(fb.status), unsigned(fb.status), fb.temperature); joint_names_[size_t(joint)].c_str(), FaultName(fb.status), unsigned(fb.status), fb.temperature);
} }
last_status_[size_t(joint)] = fb.status; last_status_[size_t(joint)] = fb.status;
@@ -362,7 +362,7 @@ bool HardwareBridge::WriteHardware() {
if (!has_cmd_) { if (!has_cmd_) {
if (!warned_no_command_) { if (!warned_no_command_) {
warned_no_command_ = true; warned_no_command_ = true;
ROS_WARN("[invicta_hardware] 还没收到 %s,不下发任何指令", command_topic_.c_str()); ROS_WARN("[invicta_hardware] %s not received yet, not sending any command", command_topic_.c_str());
} }
return false; return false;
} }
@@ -376,7 +376,7 @@ bool HardwareBridge::WriteHardware() {
const std::vector<uint8_t> selected = SelectionMask(cmd.name, &had_unknown); const std::vector<uint8_t> selected = SelectionMask(cmd.name, &had_unknown);
if (had_unknown && !warned_unknown_joint_) { if (had_unknown && !warned_unknown_joint_) {
warned_unknown_joint_ = true; warned_unknown_joint_ = true;
ROS_WARN("[invicta_hardware] %s 的 name 里有不认识的关节名,已忽略(只控制能识别的)", ROS_WARN("[invicta_hardware] unrecognized joint name in %s name list, ignored (only recognized joints are controlled)",
command_topic_.c_str()); command_topic_.c_str());
} }
@@ -387,13 +387,13 @@ bool HardwareBridge::WriteHardware() {
} }
if (cmd.position.size() != n || cmd.velocity.size() != n || cmd.effort.size() != n || cmd.kp.size() != n || if (cmd.position.size() != n || cmd.velocity.size() != n || cmd.effort.size() != n || cmd.kp.size() != n ||
cmd.kd.size() != n) { cmd.kd.size() != n) {
ROS_WARN_THROTTLE(5.0, "[invicta_hardware] 指令数组长度与 %zu 个关节不匹配,丢弃该帧", n); ROS_WARN_THROTTLE(5.0, "[invicta_hardware] command array length does not match %zu joints, dropping frame", n);
return false; return false;
} }
for (size_t i = 0; i < n; ++i) { for (size_t i = 0; i < n; ++i) {
if (!std::isfinite(cmd.position[i]) || !std::isfinite(cmd.velocity[i]) || !std::isfinite(cmd.effort[i]) || if (!std::isfinite(cmd.position[i]) || !std::isfinite(cmd.velocity[i]) || !std::isfinite(cmd.effort[i]) ||
!std::isfinite(cmd.kp[i]) || !std::isfinite(cmd.kd[i])) { !std::isfinite(cmd.kp[i]) || !std::isfinite(cmd.kd[i])) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] 指令含非有限值,丢弃该帧"); ROS_WARN_THROTTLE(1.0, "[invicta_hardware] command contains non-finite value, dropping frame");
return false; return false;
} }
} }
@@ -410,14 +410,14 @@ bool HardwareBridge::WriteHardware() {
if (age > command_timeout_s_) { if (age > command_timeout_s_) {
if (!timeout_active_) { if (!timeout_active_) {
timeout_active_ = true; timeout_active_ = true;
ROS_ERROR("[invicta_hardware] %.0f ms 未收到 %s,转阻尼(kp=0, kd=%.1f/%.1f)", ROS_ERROR("[invicta_hardware] %.0f ms without %s, switching to damping (kp=0, kd=%.1f/%.1f)",
age * 1000.0, command_topic_.c_str(), timeout_joint_kd_, timeout_wheel_kd_); age * 1000.0, command_topic_.c_str(), timeout_joint_kd_, timeout_wheel_kd_);
} }
return SendTimeoutFrame(); return SendTimeoutFrame();
} }
if (timeout_active_) { if (timeout_active_) {
timeout_active_ = false; timeout_active_ = false;
ROS_WARN("[invicta_hardware] 指令恢复,退出阻尼"); ROS_WARN("[invicta_hardware] command recovered, exiting damping");
} }
} }
@@ -453,7 +453,7 @@ bool HardwareBridge::WriteHardware() {
const double tlim = spec.torque_limit > 0. ? spec.torque_limit : std::abs(range.t_max); const double tlim = spec.torque_limit > 0. ? spec.torque_limit : std::abs(range.t_max);
double tau = cmd.effort[j]; double tau = cmd.effort[j];
if (std::abs(tau) > tlim) { if (std::abs(tau) > tlim) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 前馈力矩 %.1f N·m 超过限幅 %.1f,已夹紧", name.c_str(), ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s feedforward torque %.1f N*m exceeds clamp %.1f, clamped", name.c_str(),
tau, tlim); tau, tlim);
tau = std::min(std::max(tau, -tlim), tlim); tau = std::min(std::max(tau, -tlim), tlim);
} }
@@ -466,13 +466,13 @@ bool HardwareBridge::WriteHardware() {
// 越界会被编码器静默饱和,这里显式夹紧并把配置错误报出来 // 越界会被编码器静默饱和,这里显式夹紧并把配置错误报出来
double kp = cmd.kp[j]; double kp = cmd.kp[j];
if (kp < 0. || kp > range.kp_max) { if (kp < 0. || kp > range.kp_max) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 的 kp=%.1f 超出模组范围 [0, %.1f],已夹紧", name.c_str(), ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s kp=%.1f out of module range [0, %.1f], clamped", name.c_str(),
kp, range.kp_max); kp, range.kp_max);
kp = std::min(std::max(kp, 0.0), range.kp_max); kp = std::min(std::max(kp, 0.0), range.kp_max);
} }
double kd = cmd.kd[j]; double kd = cmd.kd[j];
if (kd < 0. || kd > range.kd_max) { if (kd < 0. || kd > range.kd_max) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 的 kd=%.2f 超出模组范围 [0, %.1f],已夹紧", name.c_str(), ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s kd=%.2f out of module range [0, %.1f], clamped", name.c_str(),
kd, range.kd_max); kd, range.kd_max);
kd = std::min(std::max(kd, 0.0), range.kd_max); kd = std::min(std::max(kd, 0.0), range.kd_max);
} }
@@ -484,7 +484,7 @@ bool HardwareBridge::WriteHardware() {
std::string err; std::string err;
if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) { if (!bus.can.Send(Config().can.arbitration_id, payload, uint8_t(bus.frame_len), true, &err)) {
ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s 发送运行帧失败: %s", bus.name.c_str(), err.c_str()); ROS_WARN_THROTTLE(1.0, "[invicta_hardware] %s failed to send run frame: %s", bus.name.c_str(), err.c_str());
} }
} }
return true; return true;
@@ -572,14 +572,14 @@ int main(int argc, char** argv) {
if (config_path.empty()) config_path = ros::package::getPath("invicta_hardware") + "/config/invicta_hardware.toml"; if (config_path.empty()) config_path = ros::package::getPath("invicta_hardware") + "/config/invicta_hardware.toml";
std::string err; std::string err;
if (!invicta_hardware::Config().Load(config_path, &err)) { if (!invicta_hardware::Config().Load(config_path, &err)) {
ROS_FATAL("[invicta_hardware] 配置加载失败:%s(%s)", err.c_str(), config_path.c_str()); ROS_FATAL("[invicta_hardware] failed to load config: %s (%s)", err.c_str(), config_path.c_str());
return 1; return 1;
} }
std::cout << invicta_hardware::Config().Summary() << std::flush; std::cout << invicta_hardware::Config().Summary() << std::flush;
invicta_hardware::HardwareBridge bridge(pnh); invicta_hardware::HardwareBridge bridge(pnh);
if (!bridge.Ready()) { if (!bridge.Ready()) {
ROS_FATAL("[invicta_hardware] CAN 初始化失败,退出(无硬件调试可设 require_can:=false)"); ROS_FATAL("[invicta_hardware] CAN init failed, exiting (for hardware-free debug set require_can:=false)");
return 1; return 1;
} }
@@ -37,23 +37,23 @@ enum UsageMode : uint8_t {
inline const char* MotionParamName(uint8_t v) { inline const char* MotionParamName(uint8_t v) {
switch (v) { switch (v) {
case kMotionIdle: return "空闲"; case kMotionIdle: return "Idle";
case kMotionStanding: return "站立"; case kMotionStanding: return "Standing";
case kMotionJointDamping: return "关节阻尼"; case kMotionJointDamping: return "Joint damping";
case kMotionBootDamping: return "开机阻尼"; case kMotionBootDamping: return "Boot damping";
case kMotionLieDown: return "趴下"; case kMotionLieDown: return "Lie down";
case kMotionRlControl: return "RL控制"; case kMotionRlControl: return "RL control";
default: return "未知"; default: return "Unknown";
} }
} }
inline const char* GaitName(uint32_t v) { inline const char* GaitName(uint32_t v) {
switch (v) { switch (v) {
case kGaitBasic: return "基础(平地)"; case kGaitBasic: return "Basic (flat)";
case kGaitStairs: return "楼梯"; case kGaitStairs: return "Stairs";
case kGaitFlatAgile: return "平地(敏捷)"; case kGaitFlatAgile: return "Flat (agile)";
case kGaitStairsAgile: return "楼梯(敏捷)"; case kGaitStairsAgile: return "Stairs (agile)";
default: return "未知"; default: return "Unknown";
} }
} }
@@ -52,7 +52,7 @@ public:
c.port = port; c.port = port;
c.last_seen = std::chrono::steady_clock::now(); c.last_seen = std::chrono::steady_clock::now();
clients_.push_back(c); clients_.push_back(c);
std::cout << "[GAMEPAD] 新客户端 " << ip << ":" << port << std::endl; std::cout << "[GAMEPAD] new client " << ip << ":" << port << std::endl;
} }
/// 记录某客户端订阅了哪一路状态上报 /// 记录某客户端订阅了哪一路状态上报
@@ -91,8 +91,8 @@ private:
const double idle = const double idle =
std::chrono::duration_cast<std::chrono::duration<double>>(now - it->last_seen).count(); std::chrono::duration_cast<std::chrono::duration<double>>(now - it->last_seen).count();
if (idle > timeout_sec_) { if (idle > timeout_sec_) {
std::cout << "[GAMEPAD] 客户端 " << it->ip << ":" << it->port << " 超过 " << timeout_sec_ std::cout << "[GAMEPAD] client " << it->ip << ":" << it->port << " exceeded " << timeout_sec_
<< " s 无报文,标记离线" << std::endl; << " s without packets, marked offline" << std::endl;
it = clients_.erase(it); it = clients_.erase(it);
offline_logged = true; offline_logged = true;
} else { } else {
@@ -72,15 +72,15 @@ inline bool RemoteConfig::Load(const std::string& path, std::string* error) {
try { try {
tbl = toml::parse_file(path); tbl = toml::parse_file(path);
} catch (const toml::parse_error& e) { } catch (const toml::parse_error& e) {
return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path); return fail(std::string("TOML parse failed: ") + std::string(e.description()) + " @ " + path);
} }
const toml::table* proto = detail::Section(tbl, "protocol"); const toml::table* proto = detail::Section(tbl, "protocol");
if (!proto) return fail("缺少 [protocol] 段"); if (!proto) return fail("missing [protocol] section");
detail::Get(proto, "udp_port", udp_port); detail::Get(proto, "udp_port", udp_port);
detail::Get(proto, "heartbeat_timeout_s", heartbeat_timeout_s); detail::Get(proto, "heartbeat_timeout_s", heartbeat_timeout_s);
detail::GetBool(proto, "unsupported_as_success", unsupported_as_success); detail::GetBool(proto, "unsupported_as_success", unsupported_as_success);
if (udp_port <= 0 || udp_port > 65535) return fail("[protocol].udp_port 非法"); if (udp_port <= 0 || udp_port > 65535) return fail("[protocol].udp_port out of range");
const toml::table* topics = detail::Section(tbl, "topics"); const toml::table* topics = detail::Section(tbl, "topics");
detail::GetStr(topics, "motion_command", topic_motion_command); detail::GetStr(topics, "motion_command", topic_motion_command);
@@ -103,11 +103,11 @@ inline bool RemoteConfig::Load(const std::string& path, std::string* error) {
inline std::string RemoteConfig::Summary() const { inline std::string RemoteConfig::Summary() const {
char buf[256]; char buf[256];
snprintf(buf, sizeof(buf), "[config] UDP %d, 心跳超时 %.1f s, 未实现项回 0xE00A=%s\n", udp_port, snprintf(buf, sizeof(buf), "[config] UDP %d, heartbeat timeout %.1f s, unsupported item returns 0xE00A=%s\n", udp_port,
heartbeat_timeout_s, unsupported_as_success ? "否(回 0)" : "是"); heartbeat_timeout_s, unsupported_as_success ? "no (returns 0)" : "yes");
std::string s = buf; std::string s = buf;
snprintf(buf, sizeof(buf), "[config] 状态上报: %s 基础 %.0f Hz / 运控 %.0f Hz / 设备 %.0f Hz / 异常 %.0f Hz\n", snprintf(buf, sizeof(buf), "[config] telemetry: %s basic %.0f Hz / motion %.0f Hz / device %.0f Hz / error %.0f Hz\n",
telemetry_enabled ? "开" : "关", telemetry_basic_hz, telemetry_motion_hz, telemetry_device_hz, telemetry_enabled ? "on" : "off", telemetry_basic_hz, telemetry_motion_hz, telemetry_device_hz,
telemetry_error_hz); telemetry_error_hz);
s += buf; s += buf;
return s; return s;
@@ -86,13 +86,13 @@ public:
udp_server_ = std::make_unique<UdpServer>([this](const Frame& f) { return OnFrame(f); }, udp_port_); udp_server_ = std::make_unique<UdpServer>([this](const Frame& f) { return OnFrame(f); }, udp_port_);
StartTelemetry(); StartTelemetry();
ROS_INFO_STREAM("[invicta_remote] 协议节点就绪:" << motion_command_topic_ << " <- " << motion_state_topic_); ROS_INFO_STREAM("[invicta_remote] protocol node ready: " << motion_command_topic_ << " <- " << motion_state_topic_);
ROS_INFO("[invicta_remote] 已实现:心跳 / 轴指令(2,21) / 运动状态(2,22) / 步态(2,23) / " ROS_INFO("[invicta_remote] implemented: heartbeat / axis command(2,21) / motion state(2,22) / gait(2,23) / "
"使用模式(1101,5 仅常规) / 按键(1101,12)"); "usage mode(1101,5 normal only) / key(1101,12)");
ROS_INFO("[invicta_remote] 状态上报:1002/6 %.0f Hz、1002/4 %.0f Hz、1002/5 %.0f Hz、1002/3 %.0f Hz" 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_, "(%s)", basic_hz_, motion_hz_, device_hz_, error_hz_,
require_subscription_ ? "需先发查询订阅" : "主动推送"); require_subscription_ ? "query subscription required" : "push autonomously");
ROS_INFO("[invicta_remote] 未实现:照明灯 休眠 充电 导航 速度指令(回 0xE00A)"); ROS_INFO("[invicta_remote] not implemented: light, sleep, charge, navigation, velocity command (reply 0xE00A)");
} }
private: private:
@@ -122,7 +122,7 @@ private:
try { try {
doc = json::parse(frame.body); doc = json::parse(frame.body);
} catch (const std::exception& e) { } catch (const std::exception& e) {
std::cout << "[GAMEPAD] JSON 解析失败:" << e.what() << std::endl; std::cout << "[GAMEPAD] JSON parse failed: " << e.what() << std::endl;
return BuildResponse(0, 0, Result::Err(kErrDataParsingFailed)); return BuildResponse(0, 0, Result::Err(kErrDataParsingFailed));
} }
@@ -356,7 +356,7 @@ private:
double value[kM20Joints]; double value[kM20Joints];
FillM20Joints(value); FillM20Joints(value);
if (!have_joints_ && std::chrono::steady_clock::now() - start_time_ > std::chrono::seconds(2)) { if (!have_joints_ && std::chrono::steady_clock::now() - start_time_ > std::chrono::seconds(2)) {
ROS_WARN_THROTTLE(30.0, "[invicta_remote] 尚未收到 %s,1002/4 的关节按 0 上报", ROS_WARN_THROTTLE(30.0, "[invicta_remote] %s not received yet, 1002/4 joints reported as 0",
joint_states_topic_.c_str()); joint_states_topic_.c_str());
} }
motor["Joint"] = std::vector<double>(value, value + kM20Joints); motor["Joint"] = std::vector<double>(value, value + kM20Joints);
@@ -427,7 +427,7 @@ private:
heartbeat_->SetSubscribed(frame.ip, frame.port, command); heartbeat_->SetSubscribed(frame.ip, frame.port, command);
if (!subscribed_logged_.count({frame.ip, command})) { if (!subscribed_logged_.count({frame.ip, command})) {
subscribed_logged_.insert({frame.ip, command}); subscribed_logged_.insert({frame.ip, command});
std::cout << "[GAMEPAD] " << frame.ip << ":" << frame.port << " 查询状态上报 1002/" << command std::cout << "[GAMEPAD] " << frame.ip << ":" << frame.port << " query telemetry 1002/" << command
<< std::endl; << std::endl;
} }
switch (command) { switch (command) {
@@ -501,7 +501,7 @@ private:
const auto now = std::chrono::steady_clock::now(); const auto now = std::chrono::steady_clock::now();
if (now - last_heartbeat_log_ > std::chrono::seconds(2)) { if (now - last_heartbeat_log_ > std::chrono::seconds(2)) {
last_heartbeat_log_ = now; last_heartbeat_log_ = now;
std::cout << "[GAMEPAD] heartbeat from " << frame.ip << ":" << frame.port << "(状态上报已开始推送)" std::cout << "[GAMEPAD] heartbeat from " << frame.ip << ":" << frame.port << " (telemetry push started)"
<< std::endl; << std::endl;
} }
return Result::Ok(); return Result::Ok();
@@ -562,8 +562,8 @@ private:
std::lock_guard<std::mutex> lock(state_mutex_); std::lock_guard<std::mutex> lock(state_mutex_);
if (have_state_ && actual_motion_state_ != kMotionRlControl) { if (have_state_ && actual_motion_state_ != kMotionRlControl) {
return Result::Err(kErrNotAllowedOperation, return Result::Err(kErrNotAllowedOperation,
std::string("Not allowed operation: 步态切换仅 RL 控制状态下有效(当前 ") + std::string("Not allowed operation: gait switch only valid in RL control state (current ") +
MotionParamName(actual_motion_state_) + ")"); MotionParamName(actual_motion_state_) + ")");
} }
} }
latest_.gait = uint32_t(gait); latest_.gait = uint32_t(gait);
@@ -583,7 +583,7 @@ private:
PublishMotionCommand(); PublishMotionCommand();
return Result::Ok(); return Result::Ok();
} }
return Unsupported("使用模式 " + std::to_string(mode) + "(仅支持常规模式)"); return Unsupported("usage mode " + std::to_string(mode) + " (only normal mode supported)");
} }
/// 1101/12 按键事件:新版 App(≥ V1.5.11)用按键,这里映射到同一套运动状态 /// 1101/12 按键事件:新版 App(≥ V1.5.11)用按键,这里映射到同一套运动状态
@@ -601,7 +601,7 @@ private:
latest_.target_state = it->second; latest_.target_state = it->second;
PublishMotionCommand(); PublishMotionCommand();
} else { } else {
std::cout << "[GAMEPAD] 未知按键 " << name << std::endl; std::cout << "[GAMEPAD] unknown key " << name << std::endl;
} }
} }
} }
@@ -611,10 +611,10 @@ private:
/// 未实现的功能:统一返回 0xE00A(可用参数改成回 0) /// 未实现的功能:统一返回 0xE00A(可用参数改成回 0)
Result Unsupported(const std::string& what) { Result Unsupported(const std::string& what) {
if (unsupported_as_success_) { if (unsupported_as_success_) {
std::cout << "[GAMEPAD] " << what << " 未实现,按 unsupported_as_success=true 回 0" << std::endl; std::cout << "[GAMEPAD] " << what << " not implemented, reply 0 per unsupported_as_success=true" << std::endl;
return Result::Ok(); return Result::Ok();
} }
std::cout << "[GAMEPAD] " << what << " 未实现,回 0xE00A" << std::endl; std::cout << "[GAMEPAD] " << what << " not implemented, reply 0xE00A" << std::endl;
return Result::Err(kErrUnsupportedFunction, "Unsupported function: " + what); return Result::Err(kErrUnsupportedFunction, "Unsupported function: " + what);
} }
@@ -625,18 +625,18 @@ private:
handlers_[{kTypeMotion, kCmdAxisControl}] = [this](json& i, const Frame& f) { return HandleAxis(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, 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, kCmdGait}] = [this](json& i, const Frame& f) { return HandleGait(i, f); };
handlers_[{kTypeMotion, kCmdCharge}] = [this](json&, const Frame&) { return Unsupported("自主充电 2/24"); }; handlers_[{kTypeMotion, kCmdCharge}] = [this](json&, const Frame&) { return Unsupported("autonomous charging 2/24"); };
handlers_[{kTypeMotion, kCmdVelocity}] = [this](json&, const Frame&) { handlers_[{kTypeMotion, kCmdVelocity}] = [this](json&, const Frame&) {
return Unsupported("速度指令 2/25(导航模式)"); return Unsupported("velocity command 2/25 (navigation mode)");
}; };
handlers_[{kTypeDevice, kCmdLight}] = [this](json&, const Frame&) { handlers_[{kTypeDevice, kCmdLight}] = [this](json&, const Frame&) {
return Unsupported("照明灯 1101/2(本平台无灯硬件)"); 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, kCmdUsageMode}] = [this](json& i, const Frame& f) { return HandleUsageMode(i, f); };
handlers_[{kTypeDevice, kCmdSleepSet}] = [this](json&, const Frame&) { return Unsupported("休眠设置 1101/6"); }; handlers_[{kTypeDevice, kCmdSleepSet}] = [this](json&, const Frame&) { return Unsupported("sleep set 1101/6"); };
handlers_[{kTypeDevice, kCmdSleepQuery}] = [this](json&, const Frame&) { handlers_[{kTypeDevice, kCmdSleepQuery}] = [this](json&, const Frame&) {
return Unsupported("休眠查询 1101/7"); return Unsupported("sleep query 1101/7");
}; };
handlers_[{kTypeDevice, kCmdKey}] = [this](json& i, const Frame& f) { return HandleKey(i, f); }; handlers_[{kTypeDevice, kCmdKey}] = [this](json& i, const Frame& f) { return HandleKey(i, f); };
@@ -154,7 +154,7 @@ private:
uint8_t from_type = kFromTypeUser; uint8_t from_type = kFromTypeUser;
bool format_ok = true; bool format_ok = true;
if (ParseHeader(buffer.data(), int(recvLen), body_len, data_id, from_type, format_ok) < 0) { if (ParseHeader(buffer.data(), int(recvLen), body_len, data_id, from_type, format_ok) < 0) {
std::cout << "[UdpServer] 非法帧头,丢弃(长度 " << recvLen << ")" << std::endl; std::cout << "[UdpServer] invalid frame header, drop (length " << recvLen << ")" << std::endl;
continue; continue;
} }
@@ -19,7 +19,7 @@ int main(int argc, char** argv) {
if (config_path.empty()) config_path = ros::package::getPath("invicta_remote") + "/config/invicta_remote.toml"; if (config_path.empty()) config_path = ros::package::getPath("invicta_remote") + "/config/invicta_remote.toml";
std::string err; std::string err;
if (!invicta_remote::Config().Load(config_path, &err)) { if (!invicta_remote::Config().Load(config_path, &err)) {
ROS_FATAL("[invicta_remote] 配置加载失败:%s(%s)", err.c_str(), config_path.c_str()); ROS_FATAL("[invicta_remote] failed to load config: %s (%s)", err.c_str(), config_path.c_str());
return 1; return 1;
} }
std::cout << invicta_remote::Config().Summary() << std::flush; std::cout << invicta_remote::Config().Summary() << std::flush;