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:
@@ -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();
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user