From ae9b7c1704bccf1a465c25faf23278a4f4074578 Mon Sep 17 00:00:00 2001 From: lyq Date: Thu, 22 Jan 2026 15:54:01 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E8=BF=9C=E6=8E=A7=E9=80=BB?= =?UTF-8?q?=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../remote_ctrl/src/remote_ctrl_node.cpp | 210 +++++++++--------- 1 file changed, 108 insertions(+), 102 deletions(-) diff --git a/src/control/remote_ctrl/src/remote_ctrl_node.cpp b/src/control/remote_ctrl/src/remote_ctrl_node.cpp index e0e7ed3..35a1df3 100644 --- a/src/control/remote_ctrl/src/remote_ctrl_node.cpp +++ b/src/control/remote_ctrl/src/remote_ctrl_node.cpp @@ -180,52 +180,49 @@ class RemoteCtrlNode : public rclcpp::Node void publishMcCtrlMsg() { + // 调试日志：显示当前授权状态 + bool authorized = remote_authorized_.load(std::memory_order_acquire); + bool alive = remote_alive_.load(std::memory_order_relaxed); + LOG_DEBUG("[REMOTE] publishMcCtrlMsg: authorized=%d, alive=%d", authorized, alive); + // 未授权时：完全不发 - if (!remote_authorized_.load(std::memory_order_acquire)) + if (!authorized) { LOG_WARN("[REMOTE] NOT AUTHORIZED - will NOT publish message"); return; } + // 已授权但超时：完全不发 + if (!alive) + { + LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - will NOT publish message"); + return; + } + sweeperMsg::McCtrl msg; { std::lock_guard lock(state_mtx_); - // 已授权但超时：安全停车状态 - if (!remote_alive_.load(std::memory_order_relaxed)) - { - LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - sending safe-stop message"); - msg.gear = desired_.gear; - msg.brake = 1; - msg.rpm = 0; - msg.angle = 0.0f; - msg.angle_speed = 120; - msg.sweep = false; - } // 正常远控 - else - { - LOG_INFO("[REMOTE] REMOTE ALIVE=TRUE - sending normal control message"); - msg.gear = desired_.gear; - msg.brake = desired_.brake; - msg.rpm = desired_.rpm; - msg.angle = desired_.angle; - msg.angle_speed = desired_.angle_speed; - msg.sweep = desired_.sweep; - } + LOG_INFO("[REMOTE] REMOTE ALIVE=TRUE - sending normal control message"); + msg.gear = desired_.gear; + msg.brake = desired_.brake; + msg.rpm = desired_.rpm; + msg.angle = desired_.angle; + msg.angle_speed = desired_.angle_speed; + msg.sweep = desired_.sweep; } LOG_INFO("[REMOTE] Publishing McCtrl: gear=%d brake=%d rpm=%d angle=%.1f angle_speed=%u sweep=%d", msg.gear, msg.brake, msg.rpm, msg.angle, msg.angle_speed, msg.sweep); pub_->publish(msg); + LOG_DEBUG("[REMOTE] Message published successfully"); } void onMqttMessage(const std::string& payload) { - std::lock_guard lock(state_mtx_); - try { auto j = json::parse(payload); @@ -254,16 +251,19 @@ class RemoteCtrlNode : public rclcpp::Node { int mode = value.get(); - if (mode == 3) { - remote_authorized_.store(true, std::memory_order_release); - remote_alive_.store(true, std::memory_order_relaxed); - } - else if (mode == 0) - { - remote_authorized_.store(false, std::memory_order_release); - remote_alive_.store(false, std::memory_order_relaxed); - desired_ = {}; + std::lock_guard lock(state_mtx_); + if (mode == 3) + { + remote_authorized_.store(true, std::memory_order_release); + remote_alive_.store(true, std::memory_order_relaxed); + } + else if (mode == 0) + { + remote_authorized_.store(false, std::memory_order_release); + remote_alive_.store(false, std::memory_order_relaxed); + desired_ = {}; + } } publishMcCtrlMsg(); @@ -291,83 +291,89 @@ class RemoteCtrlNode : public rclcpp::Node LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str()); // ===================================================== - // drive 指令 + // 处理具体指令 // ===================================================== - if (cmd == "drive") { - int throttle = 0; - int brake = 1; - int steer_raw = 0; - - if (value.is_string()) + std::lock_guard lock(state_mtx_); + // ===================================================== + // drive 指令 + // ===================================================== + if (cmd == "drive") { - sscanf(value.get().c_str(), "%d,%d,%d", &throttle, &brake, &steer_raw); + int throttle = 0; + int brake = 1; + int steer_raw = 0; + + if (value.is_string()) + { + sscanf(value.get().c_str(), "%d,%d,%d", &throttle, &brake, &steer_raw); + } + else + { + LOG_WARN("[REMOTE] drive value is not string"); + return; + } + + LOG_DEBUG("[REMOTE] drive: throttle=%d brake=%d steer=%d", throttle, brake, steer_raw); + + if (brake > 0) + { + desired_.brake = 1; + desired_.rpm = 0; + desired_.angle = 0.0f; + } + else + { + desired_.brake = 0; + desired_.rpm = (static_cast(throttle) * 6000U) / 65535U; + + float target_angle = (static_cast(steer_raw) * 100.0f / 65535.0f) - 50.0f; + float delta = target_angle - current_feedback_angle_; + float angle_speed_deg = std::fabs(delta) / DELTA_T; + float motor_rpm = angle_speed_deg * DEG_PER_SEC_TO_RPM * GEAR_RATIO; + + desired_.angle_speed = std::clamp(static_cast(motor_rpm), 120, 1500); + desired_.angle = target_angle; + } + } + // ===================================================== + // gear 指令 + // ===================================================== + else if (cmd == "gear") + { + int gear = value.get(); + LOG_DEBUG("[REMOTE] gear command: %d", gear); + + switch (gear) + { + case 0: + desired_.gear = 0; // N + break; + case 1: + desired_.gear = 2; // D + break; + case 2: + desired_.gear = 1; // R + break; + default: + desired_.gear = 0; + break; + } + } + // ===================================================== + // sweepCtrl 指令 + // ===================================================== + else if (cmd == "sweepCtrl") + { + int val = value.get(); + desired_.sweep = (val != 0); + LOG_DEBUG("[REMOTE] sweepCtrl: %d", val); } else { - LOG_WARN("[REMOTE] drive value is not string"); + LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str()); return; } - - LOG_DEBUG("[REMOTE] drive: throttle=%d brake=%d steer=%d", throttle, brake, steer_raw); - - if (brake > 0) - { - desired_.brake = 1; - desired_.rpm = 0; - desired_.angle = 0.0f; - } - else - { - desired_.brake = 0; - desired_.rpm = (static_cast(throttle) * 6000U) / 65535U; - - float target_angle = (static_cast(steer_raw) * 100.0f / 65535.0f) - 50.0f; - float delta = target_angle - current_feedback_angle_; - float angle_speed_deg = std::fabs(delta) / DELTA_T; - float motor_rpm = angle_speed_deg * DEG_PER_SEC_TO_RPM * GEAR_RATIO; - - desired_.angle_speed = std::clamp(static_cast(motor_rpm), 120, 1500); - desired_.angle = target_angle; - } - } - // ===================================================== - // gear 指令 - // ===================================================== - else if (cmd == "gear") - { - int gear = value.get(); - LOG_DEBUG("[REMOTE] gear command: %d", gear); - - switch (gear) - { - case 0: - desired_.gear = 0; // N - break; - case 1: - desired_.gear = 2; // D - break; - case 2: - desired_.gear = 1; // R - break; - default: - desired_.gear = 0; - break; - } - } - // ===================================================== - // sweepCtrl 指令 - // ===================================================== - else if (cmd == "sweepCtrl") - { - int val = value.get(); - desired_.sweep = (val != 0); - LOG_DEBUG("[REMOTE] sweepCtrl: %d", val); - } - else - { - LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str()); - return; } publishMcCtrlMsg();