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:
@@ -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<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);
|
||||
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<bool>()) 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;
|
||||
|
||||
@@ -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";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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<uint8_t>
|
||||
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<uint8_t>
|
||||
// 协议要求先使能再运行,这里补一次使能,而不是永远不发帧。
|
||||
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<uint8_t>
|
||||
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<uint8_t> 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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user