diff --git a/src/invicta_deploy/include/config/deploy_config.h b/src/invicta_deploy/include/config/deploy_config.h index 3817715..7585832 100644 --- a/src/invicta_deploy/include/config/deploy_config.h +++ b/src/invicta_deploy/include/config/deploy_config.h @@ -163,17 +163,17 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) { try { tbl = toml::parse_file(path); } 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"); - if (!robot) return fail("缺少 [robot] 段"); + if (!robot) return fail("missing [robot] section"); detail::GetStr(robot, "name", robot_name); detail::GetStr(robot, "version", robot_version); 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()); - 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 lower, upper, vel_limit; 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, "vel_limit", vel_limit); 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{}); 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.wheel_index = IndexOf(leg.wheel_joint); 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.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); } } - if (legs.empty()) return fail("缺少 [[legs]] 段"); - if (int(legs.size()) * 2 != dof) return fail("腿数与关节数不匹配(每腿 2 个关节)"); + if (legs.empty()) return fail("missing [[legs]] section"); + 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"); 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::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), std::make_pair("pre_height", pre_height), std::make_pair("lie_height", lie_height)}) { 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, "kd", policy_kd); 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) { - 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; for (const auto& arr : {&action_scale, &default_joint_pos, &policy_kp, &policy_kd}) { 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) + 关节角 + 关节速度 + 上一步动作 @@ -285,7 +285,7 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) { try { gait_policies[uint32_t(std::stoul(std::string(k.str())))] = v.value_or(std::string{}); } 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) { const double sw = SwingAngleForHeight(stand_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); s += buf; } diff --git a/src/invicta_deploy/include/utils/safe_controller.hpp b/src/invicta_deploy/include/utils/safe_controller.hpp index 9198873..9153243 100644 --- a/src/invicta_deploy/include/utils/safe_controller.hpp +++ b/src/invicta_deploy/include/utils/safe_controller.hpp @@ -216,7 +216,7 @@ private: } 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; } diff --git a/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp b/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp index 4790fdd..5b323b9 100644 --- a/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp +++ b/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp @@ -34,7 +34,7 @@ public: InvictaInterface(const std::string& robot_name, int dof_num) : RobotInterface(robot_name, dof_num), cfg_(invicta::Config()) { 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_vel_ = VecXf::Zero(dof_num_); @@ -58,7 +58,7 @@ public: SendStartupSequence(); std::cout << "[InvictaInterface] topics: " << cfg_.topic_joint_states << " / " << cfg_.topic_imu << " -> " << 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() {} @@ -124,21 +124,21 @@ protected: ros::Duration(0.01).sleep(); } 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}; for (uint16_t word : seq) { PublishRawCommand(word); 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) { const size_t n = size_t(dof_num_); 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(), msg->effort.size()); return; @@ -159,11 +159,11 @@ protected: } if (!by_name && !warned_name_mismatch_) { 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_); } 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); } @@ -182,7 +182,7 @@ protected: if (msg->orientation_covariance[0] == -1.0) { if (!warned_imu_no_orientation_) { 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()); } return; @@ -190,7 +190,7 @@ protected: Eigen::Quaternionf q(float(msg->orientation.w), float(msg->orientation.x), float(msg->orientation.y), float(msg->orientation.z)); 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; } q.normalize(); diff --git a/src/invicta_deploy/interface/user_command/ros_command_interface.hpp b/src/invicta_deploy/interface/user_command/ros_command_interface.hpp index 085d81d..ceed971 100644 --- a/src/invicta_deploy/interface/user_command/ros_command_interface.hpp +++ b/src/invicta_deploy/interface/user_command/ros_command_interface.hpp @@ -47,12 +47,12 @@ public: void Start() override { if (msfb_) msfb_->UpdateCurrentGait(uint32_t(default_gait_)); - std::cout << "\n╔════════════════════════════════════════════════╗\n" - << "║ INVICTA MOTION COMMAND LINK ║\n" - << "╚════════════════════════════════════════════════╝\n" - << " 指令来源: " << topic_ << "(由 invicta_remote 发布)\n" - << " 默认步态: " << GaitName(uint32_t(default_gait_)) << "\n" - << " 已支持步态: " << SupportedGaitList() << "\n\n"; + std::cout << "\n+==================================================+\n" + << "| INVICTA MOTION COMMAND LINK |\n" + << "+==================================================+\n" + << " Command source: " << topic_ << " (published by invicta_remote)\n" + << " Default gait: " << GaitName(uint32_t(default_gait_)) << "\n" + << " Supported gaits: " << SupportedGaitList() << "\n\n"; } void Stop() override {} @@ -61,7 +61,7 @@ public: private: std::string SupportedGaitList() const { - if (!loaded_gaits_ || loaded_gaits_->empty()) return "(无)"; + if (!loaded_gaits_ || loaded_gaits_->empty()) return "(none)"; std::string s; for (int gait : *loaded_gaits_) { char buf[16]; @@ -83,39 +83,39 @@ private: switch (int(target)) { case kMotionStanding: - name = "站立"; + name = "stand up"; next = uint8_t(RobotMotionState::StandingUp); accepted = (cur == RobotMotionState::WaitingForStand || cur == RobotMotionState::LieDown); break; case kMotionJointDamping: - name = "关节阻尼"; + name = "joint damping"; next = uint8_t(RobotMotionState::JointDamping); accepted = true; break; case kMotionBootDamping: - name = "开机阻尼(按关节阻尼处理)"; + name = "boot damping (handled as joint damping)"; next = uint8_t(RobotMotionState::JointDamping); accepted = true; break; case kMotionLieDown: - name = "趴下"; + name = "lie down"; next = uint8_t(RobotMotionState::LieDown); accepted = (cur == RobotMotionState::StandingUp || cur == RobotMotionState::RLControlMode); break; case kMotionRlControl: - name = "RL控制"; + name = "RL control"; next = uint8_t(RobotMotionState::RLControlMode); accepted = (cur == RobotMotionState::StandingUp); break; case kMotionIdle: // 状态机没有「直接回空闲」的路径(RL / 站立只能经阻尼或趴下退出),不作为请求 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; return false; default: - std::cout << "[MODE] 未知目标状态 " << int(target) << "(来自 " << src << ")" << std::endl; + std::cout << "[MODE] unknown target state " << int(target) << " (from " << src << ")" << std::endl; return false; } @@ -127,9 +127,9 @@ private: if (accepted) { 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) { - 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; return accepted; @@ -145,14 +145,14 @@ private: if (gait == 0 || gait == last_gait_request_) return; // 只有"启动时真的加载了模型"的步态才接受;否则状态会上报未切换,而不是假成功 if (!loaded_gaits_ || loaded_gaits_->count(int(gait)) == 0) { - std::cout << "[GAIT] 拒绝 " << GaitName(gait) << "(来自 " << src - << "):该步态没有对应的 RL 模型(启动时未加载,见 gait/policy_map)" << std::endl; + std::cout << "[GAIT] reject " << GaitName(gait) << " (from " << src + << "): this gait has no corresponding RL model (not loaded at startup, see gait/policy_map)" << std::endl; last_gait_request_ = gait; return; } usr_cmd_->gait = 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; } @@ -160,7 +160,7 @@ private: const std::string src = "invicta_remote"; if (!has_command_) { has_command_ = true; - std::cout << "[MODE] 收到第一条运动指令(" << topic_ << ")" << std::endl; + std::cout << "[MODE] received first motion command (" << topic_ << ")" << std::endl; } // 轴指令:协议里是比例 [-1,1],策略直接用这三个分量 @@ -171,7 +171,7 @@ private: if (msg->usage_mode != usr_cmd_->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); diff --git a/src/invicta_deploy/main.cpp b/src/invicta_deploy/main.cpp index 5941d57..c2b9e70 100644 --- a/src/invicta_deploy/main.cpp +++ b/src/invicta_deploy/main.cpp @@ -29,7 +29,7 @@ int main(int argc, char** argv) { std::string err; auto& cfg = invicta::Config(); 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; } std::cout << cfg.Summary() << std::flush; diff --git a/src/invicta_deploy/run_policy/invicta_policy_runner.hpp b/src/invicta_deploy/run_policy/invicta_policy_runner.hpp index 023f0bd..a0dcab9 100644 --- a/src/invicta_deploy/run_policy/invicta_policy_runner.hpp +++ b/src/invicta_deploy/run_policy/invicta_policy_runner.hpp @@ -37,7 +37,7 @@ public: action_dim_ = cfg_.action_dim; obs_dim_ = cfg_.obs_dim; 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); diff --git a/src/invicta_deploy/state_machine/quadruped_wheel/invicta_state_machine.hpp b/src/invicta_deploy/state_machine/quadruped_wheel/invicta_state_machine.hpp index 2006e6e..7233b16 100644 --- a/src/invicta_deploy/state_machine/quadruped_wheel/invicta_state_machine.hpp +++ b/src/invicta_deploy/state_machine/quadruped_wheel/invicta_state_machine.hpp @@ -88,7 +88,7 @@ public: current_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 判前提 + 状态上报)---- state_reporter_ = std::make_shared( diff --git a/src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp b/src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp index 596d973..e8e794b 100644 --- a/src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp +++ b/src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp @@ -34,13 +34,13 @@ public: virtual void OnEnter() { 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; msfb_.UpdateCurrentState(RobotMotionState::JointDamping); } 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_); } diff --git a/src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp b/src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp index 00a5948..420ff04 100644 --- a/src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp +++ b/src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp @@ -44,7 +44,7 @@ public: joint_cmd_(cfg.legs[l].wheel_index, 2) = float(cfg.damping_kd_wheel); 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); std::cout << buf; } diff --git a/src/invicta_deploy/state_machine/quadruped_wheel/rl_control_state.hpp b/src/invicta_deploy/state_machine/quadruped_wheel/rl_control_state.hpp index f2756ab..cef5fb3 100644 --- a/src/invicta_deploy/state_machine/quadruped_wheel/rl_control_state.hpp +++ b/src/invicta_deploy/state_machine/quadruped_wheel/rl_control_state.hpp @@ -88,8 +88,8 @@ private: auto next = PolicyForGait(gait, &switched); if (switched) { next->OnEnter(); // 清 last_action / run_cnt,避免跨模型残留 - std::cout << "[GAIT] 切换模型 -> " << invicta_msgs::GaitName(gait) - << (gait_runners_.count(int(gait)) ? "(已注册模型)" : "(默认模型)") << std::endl; + std::cout << "[GAIT] switch model -> " << invicta_msgs::GaitName(gait) + << (gait_runners_.count(int(gait)) ? " (registered model)" : " (default model)") << std::endl; } active_policy_ = next; active_gait_ = int(gait); @@ -101,7 +101,7 @@ private: MatXf res = ra.ConvertToMat(); if (!res.allFinite()) { 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 { ri_ptr_->SetJointCommand(res); } @@ -123,13 +123,13 @@ public: try { gait_paths_[std::stoi(kv.first)] = kv.second; } 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("invicta_policy", policy_path); 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_); @@ -138,14 +138,14 @@ public: for (const auto& kv : gait_paths_) { if (kv.first == default_gait_) continue; // 默认步态已在上面处理 if (kv.second.empty()) { - std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first)) - << " 未提供模型路径,该步态不可用" << std::endl; + std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first)) + << " has no model path, gait unavailable" << std::endl; continue; } if (kv.second == policy_path) { gait_runners_[kv.first] = default_policy_; 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; continue; } @@ -153,11 +153,11 @@ public: gait_runners_[kv.first] = std::make_shared("gait_" + std::to_string(kv.first), kv.second); 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; } catch (const std::exception& e) { - std::cout << "[GAIT] 步态 " << invicta_msgs::GaitName(uint32_t(kv.first)) - << " 模型加载失败(该步态将不可用):" << e.what() << std::endl; + std::cout << "[GAIT] gait " << invicta_msgs::GaitName(uint32_t(kv.first)) + << " failed to load model (gait unavailable): " << e.what() << std::endl; } } policy_output_bad_ = false; diff --git a/src/invicta_deploy/state_machine/quadruped_wheel/standup_state.hpp b/src/invicta_deploy/state_machine/quadruped_wheel/standup_state.hpp index 286142c..6c17ed3 100644 --- a/src/invicta_deploy/state_machine/quadruped_wheel/standup_state.hpp +++ b/src/invicta_deploy/state_machine/quadruped_wheel/standup_state.hpp @@ -58,7 +58,7 @@ public: joint_cmd_(cfg.legs[l].wheel_index, 0) = 0.f; // 轮 kp 恒 0 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, stand_swing_(int(l)) * 180. / M_PI); std::cout << buf; @@ -134,12 +134,12 @@ public: bool ReadyForRl() { const auto& cfg = invicta::Config(); if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) { - LogNotReady("安全标志非 0"); + LogNotReady("safe flag is not 0"); return false; } for (int i = 0; i < int(current_joint_pos_.size()); ++i) { if (std::isnan(current_joint_pos_(i)) || std::isnan(current_joint_vel_(i))) { - LogNotReady("关节数据含 NaN"); + LogNotReady("joint data contains NaN"); return false; } } @@ -147,11 +147,11 @@ public: for (size_t l = 0; l < cfg.legs.size(); ++l) { const int si = cfg.legs[l].swing_index; 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; } 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; } } @@ -161,12 +161,12 @@ public: const Vec3f acc = ri_ptr_->GetImuAcc(); for (int i = 0; i < 3; ++i) { if (std::isnan(rpy(i)) || std::isnan(acc(i))) { - LogNotReady("IMU 数据含 NaN"); + LogNotReady("IMU data contains NaN"); return false; } } if (acc.norm() < 0.1f * gravity || acc.norm() > 3.0f * gravity) { - LogNotReady("IMU 加速度异常"); + LogNotReady("IMU acceleration abnormal"); return false; } } @@ -176,7 +176,7 @@ public: void LogNotReady(const std::string& why) { if (run_time_ - last_ready_log_ < 1.0) return; 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() { @@ -200,7 +200,7 @@ public: if (elapsed >= ready_time + cfg.rl_enter_delay && ReadyForRl()) { if (!rl_enter_logged_) { 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; } diff --git a/src/invicta_hardware/include/invicta_hardware/hardware_config.h b/src/invicta_hardware/include/invicta_hardware/hardware_config.h index 3636d70..eb423ef 100644 --- a/src/invicta_hardware/include/invicta_hardware/hardware_config.h +++ b/src/invicta_hardware/include/invicta_hardware/hardware_config.h @@ -148,13 +148,13 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) { try { tbl = toml::parse_file(path); } 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"); - if (!robot) return fail("缺少 [robot] 段"); + if (!robot) return fail("missing [robot] section"); 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()); std::vector 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); if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_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{}); 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.wheel_index = IndexOf(leg.wheel_joint); 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* wh = detail::Section(*lt, "wheel"); @@ -206,7 +206,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) { 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")) { 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 MotorSpec& m = j.is_wheel ? motor_wheel : motor_joint; 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))) { - 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))) { - 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 (auto v = (*hw)["require_can"].value()) require_can = *v; } - if (publish_rate_hz <= 0.) return fail("[hardware].publish_rate_hz 必须 > 0"); - if (diagnostics_rate_hz <= 0.) return fail("[hardware].diagnostics_rate_hz 必须 > 0"); - if (command_timeout_s < 0.) return fail("[hardware].command_timeout_s 不能为负"); + 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 must be > 0"); + if (command_timeout_s < 0.) return fail("[hardware].command_timeout_s must not be negative"); if (error) error->clear(); return true; @@ -283,7 +283,7 @@ inline bool HardwareConfig::Load(const std::string& path, std::string* error) { inline std::string HardwareConfig::Summary() const { char buf[512]; 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()); s += buf; for (const auto& leg : legs) { @@ -292,11 +292,11 @@ inline std::string HardwareConfig::Summary() const { leg.wheel_bus.c_str(), leg.wheel_canid); 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_wheel.model.c_str(), motor_wheel.p_max, motor_wheel.v_max, motor_wheel.t_max); 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.data_bitrate / 1e6, can.enable_frames, can.enable_interval_s); s += buf; diff --git a/src/invicta_hardware/include/invicta_hardware/joint_module_protocol.h b/src/invicta_hardware/include/invicta_hardware/joint_module_protocol.h index 1520eaf..a006732 100644 --- a/src/invicta_hardware/include/invicta_hardware/joint_module_protocol.h +++ b/src/invicta_hardware/include/invicta_hardware/joint_module_protocol.h @@ -84,14 +84,14 @@ struct MotorFeedback { /// 状态码非零故障位的中文名,用于日志 inline const char* FaultName(uint8_t status) { switch ((status >> 1) & 0x3F) { - case 0x00: return "无"; - case 0x01: return "过压"; - case 0x02: return "过流"; - case 0x04: return "过温"; - case 0x08: return "过速"; - case 0x10: return "编码器"; - case 0x20: return "欠压/预驱"; - default: return "组合故障"; + case 0x00: return "none"; + case 0x01: return "overvoltage"; + case 0x02: return "overcurrent"; + case 0x04: return "overtemperature"; + case 0x08: return "overspeed"; + case 0x10: return "encoder"; + case 0x20: return "undervoltage/pre-driver"; + default: return "combined fault"; } } diff --git a/src/invicta_hardware/src/invicta_hardware_node.cpp b/src/invicta_hardware/src/invicta_hardware_node.cpp index c4766c8..c47dc69 100644 --- a/src/invicta_hardware/src/invicta_hardware_node.cpp +++ b/src/invicta_hardware/src/invicta_hardware_node.cpp @@ -59,7 +59,7 @@ HardwareBridge::HardwareBridge(const ros::NodeHandle& pnh) : nh_() { 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_, diagnostics_topic_.c_str(), diagnostics_rate_hz_); } @@ -69,7 +69,7 @@ bool HardwareBridge::OpenBuses() { buses_.clear(); 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_; } @@ -79,11 +79,11 @@ bool HardwareBridge::OpenBuses() { std::string err; if (!bus.can.Open(name, &err)) { - ROS_ERROR("[invicta_hardware] 打开 %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] failed to open %s: %s", name.c_str(), err.c_str()); + 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); 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; } @@ -93,7 +93,7 @@ bool HardwareBridge::OpenBuses() { 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) { - 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); if (require_can_) return false; continue; @@ -105,7 +105,7 @@ bool HardwareBridge::OpenBuses() { } 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)); } @@ -135,11 +135,11 @@ void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) { joint_names_ = cfg.joint_names; // 顺序必须与 invicta_deploy 一致(同一份配置,天然一致) 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; } 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; } } @@ -182,18 +182,18 @@ void HardwareBridge::HandleControlWord(uint16_t word, const std::vector case invicta_msgs::kDisable: SendCodeAll(kCodeDisable, 1); enabled_ = false; - ROS_INFO("[invicta_hardware] control_word -> kDisable:已发失能(0xFD),线圈断开"); + ROS_INFO("[invicta_hardware] control_word -> kDisable: disable sent (0xFD), coils open"); break; case invicta_msgs::kEnable: SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s); 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; case invicta_msgs::kErrorReset: // 协议没有独立的清错指令(手册 §6:故障后靠重新使能或重启恢复) SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s); 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; case invicta_msgs::kMotorControl: if (!enabled_) { @@ -201,13 +201,13 @@ void HardwareBridge::HandleControlWord(uint16_t word, const std::vector // 协议要求先使能再运行,这里补一次使能,而不是永远不发帧。 SendCodeAll(kCodeEnable, Config().can.enable_frames, Config().can.enable_interval_s); 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; case invicta_msgs::kGetStatus: // 每条指令都会回 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; case invicta_msgs::kSetZero: { // 把当前机械位置写成模组零位(0xEF)。只作用于指令 name 选中的关节子集, @@ -221,17 +221,17 @@ void HardwareBridge::HandleControlWord(uint16_t word, const std::vector if (mask[i]) sel += (sel.empty() ? "" : ",") + joint_names_[i]; } 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; } SendCodeAll(kCodeZero, 1, 0.0, &mask); - ROS_WARN("[invicta_hardware] control_word -> kSetZero:已发置零(0xEF)到 [%s];" - "此后这些摆臂该位姿的反馈角度约为 0,请把对应 offset_deg 设为 0(或据此重标)", + ROS_WARN("[invicta_hardware] control_word -> kSetZero: set zero sent (0xEF) to [%s]; " + "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()); break; } default: - ROS_WARN("[invicta_hardware] 未知 control_word=%u,忽略", word); + ROS_WARN("[invicta_hardware] unknown control_word=%u, ignore", word); break; } } @@ -263,7 +263,7 @@ void HardwareBridge::SendCodeAll(uint8_t code, int frames, double interval_s, co if (!any) continue; std::string 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()); } } @@ -299,7 +299,7 @@ bool HardwareBridge::SendTimeoutFrame() { std::string 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; @@ -318,12 +318,12 @@ bool HardwareBridge::ReadHardware() { const int canid = int(frame.id); const int joint = (canid > 0 && canid <= bus.max_canid) ? bus.canid_to_joint[size_t(canid)] : -1; 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)); continue; } 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)); continue; } @@ -344,7 +344,7 @@ bool HardwareBridge::ReadHardware() { has_feedback_[size_t(joint)] = 1; 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); } last_status_[size_t(joint)] = fb.status; @@ -362,7 +362,7 @@ bool HardwareBridge::WriteHardware() { if (!has_cmd_) { if (!warned_no_command_) { 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; } @@ -376,7 +376,7 @@ bool HardwareBridge::WriteHardware() { const std::vector selected = SelectionMask(cmd.name, &had_unknown); if (had_unknown && !warned_unknown_joint_) { 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()); } @@ -387,13 +387,13 @@ bool HardwareBridge::WriteHardware() { } if (cmd.position.size() != n || cmd.velocity.size() != n || cmd.effort.size() != n || cmd.kp.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; } for (size_t i = 0; i < n; ++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])) { - ROS_WARN_THROTTLE(1.0, "[invicta_hardware] 指令含非有限值,丢弃该帧"); + ROS_WARN_THROTTLE(1.0, "[invicta_hardware] command contains non-finite value, dropping frame"); return false; } } @@ -410,14 +410,14 @@ bool HardwareBridge::WriteHardware() { if (age > command_timeout_s_) { if (!timeout_active_) { 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_); } return SendTimeoutFrame(); } if (timeout_active_) { 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); double tau = cmd.effort[j]; 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 = std::min(std::max(tau, -tlim), tlim); } @@ -466,13 +466,13 @@ bool HardwareBridge::WriteHardware() { // 越界会被编码器静默饱和,这里显式夹紧并把配置错误报出来 double kp = cmd.kp[j]; 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 = std::min(std::max(kp, 0.0), range.kp_max); } double kd = cmd.kd[j]; 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 = std::min(std::max(kd, 0.0), range.kd_max); } @@ -484,7 +484,7 @@ bool HardwareBridge::WriteHardware() { std::string 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; @@ -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"; std::string 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; } std::cout << invicta_hardware::Config().Summary() << std::flush; invicta_hardware::HardwareBridge bridge(pnh); 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; } diff --git a/src/invicta_msgs/include/invicta_msgs/motion_defs.h b/src/invicta_msgs/include/invicta_msgs/motion_defs.h index 86dede4..acdce62 100644 --- a/src/invicta_msgs/include/invicta_msgs/motion_defs.h +++ b/src/invicta_msgs/include/invicta_msgs/motion_defs.h @@ -37,23 +37,23 @@ enum UsageMode : uint8_t { inline const char* MotionParamName(uint8_t v) { switch (v) { - case kMotionIdle: return "空闲"; - case kMotionStanding: return "站立"; - case kMotionJointDamping: return "关节阻尼"; - case kMotionBootDamping: return "开机阻尼"; - case kMotionLieDown: return "趴下"; - case kMotionRlControl: return "RL控制"; - default: return "未知"; + case kMotionIdle: return "Idle"; + case kMotionStanding: return "Standing"; + case kMotionJointDamping: return "Joint damping"; + case kMotionBootDamping: return "Boot damping"; + case kMotionLieDown: return "Lie down"; + case kMotionRlControl: return "RL control"; + default: return "Unknown"; } } inline const char* GaitName(uint32_t v) { switch (v) { - case kGaitBasic: return "基础(平地)"; - case kGaitStairs: return "楼梯"; - case kGaitFlatAgile: return "平地(敏捷)"; - case kGaitStairsAgile: return "楼梯(敏捷)"; - default: return "未知"; + case kGaitBasic: return "Basic (flat)"; + case kGaitStairs: return "Stairs"; + case kGaitFlatAgile: return "Flat (agile)"; + case kGaitStairsAgile: return "Stairs (agile)"; + default: return "Unknown"; } } diff --git a/src/invicta_remote/include/invicta_remote/heartbeat.h b/src/invicta_remote/include/invicta_remote/heartbeat.h index a1d16f1..55b1e31 100644 --- a/src/invicta_remote/include/invicta_remote/heartbeat.h +++ b/src/invicta_remote/include/invicta_remote/heartbeat.h @@ -52,7 +52,7 @@ public: c.port = port; c.last_seen = std::chrono::steady_clock::now(); 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 = std::chrono::duration_cast>(now - it->last_seen).count(); if (idle > timeout_sec_) { - std::cout << "[GAMEPAD] 客户端 " << it->ip << ":" << it->port << " 超过 " << timeout_sec_ - << " s 无报文,标记离线" << std::endl; + std::cout << "[GAMEPAD] client " << it->ip << ":" << it->port << " exceeded " << timeout_sec_ + << " s without packets, marked offline" << std::endl; it = clients_.erase(it); offline_logged = true; } else { diff --git a/src/invicta_remote/include/invicta_remote/remote_config.h b/src/invicta_remote/include/invicta_remote/remote_config.h index be750d3..35fe9e0 100644 --- a/src/invicta_remote/include/invicta_remote/remote_config.h +++ b/src/invicta_remote/include/invicta_remote/remote_config.h @@ -72,15 +72,15 @@ inline bool RemoteConfig::Load(const std::string& path, std::string* error) { try { tbl = toml::parse_file(path); } 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"); - if (!proto) return fail("缺少 [protocol] 段"); + if (!proto) return fail("missing [protocol] section"); detail::Get(proto, "udp_port", udp_port); detail::Get(proto, "heartbeat_timeout_s", heartbeat_timeout_s); 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"); 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 { char buf[256]; - snprintf(buf, sizeof(buf), "[config] UDP %d, 心跳超时 %.1f s, 未实现项回 0xE00A=%s\n", udp_port, - heartbeat_timeout_s, unsupported_as_success ? "否(回 0)" : "是"); + 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 ? "no (returns 0)" : "yes"); std::string s = buf; - snprintf(buf, sizeof(buf), "[config] 状态上报: %s 基础 %.0f Hz / 运控 %.0f Hz / 设备 %.0f Hz / 异常 %.0f Hz\n", - telemetry_enabled ? "开" : "关", telemetry_basic_hz, telemetry_motion_hz, telemetry_device_hz, + snprintf(buf, sizeof(buf), "[config] telemetry: %s basic %.0f Hz / motion %.0f Hz / device %.0f Hz / error %.0f Hz\n", + telemetry_enabled ? "on" : "off", telemetry_basic_hz, telemetry_motion_hz, telemetry_device_hz, telemetry_error_hz); s += buf; return s; diff --git a/src/invicta_remote/include/invicta_remote/remote_node.h b/src/invicta_remote/include/invicta_remote/remote_node.h index 31685cd..63f327e 100644 --- a/src/invicta_remote/include/invicta_remote/remote_node.h +++ b/src/invicta_remote/include/invicta_remote/remote_node.h @@ -86,13 +86,13 @@ public: udp_server_ = std::make_unique([this](const Frame& f) { return OnFrame(f); }, udp_port_); StartTelemetry(); - ROS_INFO_STREAM("[invicta_remote] 协议节点就绪:" << motion_command_topic_ << " <- " << motion_state_topic_); - ROS_INFO("[invicta_remote] 已实现:心跳 / 轴指令(2,21) / 运动状态(2,22) / 步态(2,23) / " - "使用模式(1101,5 仅常规) / 按键(1101,12)"); - ROS_INFO("[invicta_remote] 状态上报: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_ ? "需先发查询订阅" : "主动推送"); - ROS_INFO("[invicta_remote] 未实现:照明灯 休眠 充电 导航 速度指令(回 0xE00A)"); + 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: @@ -122,7 +122,7 @@ private: try { doc = json::parse(frame.body); } 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)); } @@ -356,7 +356,7 @@ private: 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,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()); } motor["Joint"] = std::vector(value, value + kM20Joints); @@ -427,7 +427,7 @@ private: 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 << " 查询状态上报 1002/" << command + std::cout << "[GAMEPAD] " << frame.ip << ":" << frame.port << " query telemetry 1002/" << command << std::endl; } switch (command) { @@ -501,7 +501,7 @@ private: 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 << "(状态上报已开始推送)" + std::cout << "[GAMEPAD] heartbeat from " << frame.ip << ":" << frame.port << " (telemetry push started)" << std::endl; } return Result::Ok(); @@ -562,8 +562,8 @@ private: std::lock_guard lock(state_mutex_); if (have_state_ && actual_motion_state_ != kMotionRlControl) { return Result::Err(kErrNotAllowedOperation, - std::string("Not allowed operation: 步态切换仅 RL 控制状态下有效(当前 ") + - MotionParamName(actual_motion_state_) + ")"); + std::string("Not allowed operation: gait switch only valid in RL control state (current ") + + MotionParamName(actual_motion_state_) + ")"); } } latest_.gait = uint32_t(gait); @@ -583,7 +583,7 @@ private: PublishMotionCommand(); 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)用按键,这里映射到同一套运动状态 @@ -601,7 +601,7 @@ private: latest_.target_state = it->second; PublishMotionCommand(); } else { - std::cout << "[GAMEPAD] 未知按键 " << name << std::endl; + std::cout << "[GAMEPAD] unknown key " << name << std::endl; } } } @@ -611,10 +611,10 @@ private: /// 未实现的功能:统一返回 0xE00A(可用参数改成回 0) Result Unsupported(const std::string& what) { 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(); } - std::cout << "[GAMEPAD] " << what << " 未实现,回 0xE00A" << std::endl; + std::cout << "[GAMEPAD] " << what << " not implemented, reply 0xE00A" << std::endl; 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, 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("自主充电 2/24"); }; + handlers_[{kTypeMotion, kCmdCharge}] = [this](json&, const Frame&) { return Unsupported("autonomous charging 2/24"); }; 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&) { - 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, 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&) { - return Unsupported("休眠查询 1101/7"); + return Unsupported("sleep query 1101/7"); }; handlers_[{kTypeDevice, kCmdKey}] = [this](json& i, const Frame& f) { return HandleKey(i, f); }; diff --git a/src/invicta_remote/include/invicta_remote/udp_server.h b/src/invicta_remote/include/invicta_remote/udp_server.h index 7725e24..84fd98b 100644 --- a/src/invicta_remote/include/invicta_remote/udp_server.h +++ b/src/invicta_remote/include/invicta_remote/udp_server.h @@ -154,7 +154,7 @@ private: uint8_t from_type = kFromTypeUser; bool format_ok = true; 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; } diff --git a/src/invicta_remote/src/invicta_remote_node.cpp b/src/invicta_remote/src/invicta_remote_node.cpp index 2083da1..c41304d 100644 --- a/src/invicta_remote/src/invicta_remote_node.cpp +++ b/src/invicta_remote/src/invicta_remote_node.cpp @@ -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"; std::string 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; } std::cout << invicta_remote::Config().Summary() << std::flush;