diff --git a/src/control/remote_ctrl/src/remote_ctrl_node.cpp b/src/control/remote_ctrl/src/remote_ctrl_node.cpp index 55a522b..e0e7ed3 100644 --- a/src/control/remote_ctrl/src/remote_ctrl_node.cpp +++ b/src/control/remote_ctrl/src/remote_ctrl_node.cpp @@ -91,8 +91,7 @@ class RemoteCtrlNode : public rclcpp::Node try_subscribe_ctrl_topic(); }); - // 主动连一次，后面内部 loop 线程也会自动尝试重连 - mqtt_->connect(); + // MQTTClientWrapper内部已有reconnectLoop线程自动处理连接和重连，无需主动调用connect() identity_sub_ = this->create_subscription( "/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(), @@ -119,7 +118,7 @@ class RemoteCtrlNode : public rclcpp::Node LOG_INFO("RemoteCtrlNode started mqtt=%s client_id=%s", uri.str().c_str(), client_id.c_str()); - // ===== 看门狗 & 定时发布 ===== + // ===== 看门狗 & 消息发布 ===== last_msg_time_ = std::chrono::steady_clock::now(); // 看门狗：800ms 内没收到新 remote 指令则停控 @@ -142,43 +141,6 @@ class RemoteCtrlNode : public rclcpp::Node } }); - // 周期发布到 /remote_mc_ctrl - timer_ = this->create_wall_timer(20ms, // 50Hz - [this]() - { - // 未授权远控：完全不发 - if (!remote_authorized_.load(std::memory_order_acquire)) return; - - sweeperMsg::McCtrl msg; - - { - std::lock_guard lock(state_mtx_); - - // 已授权但超时：安全停车态 - if (!remote_alive_.load(std::memory_order_relaxed)) - { - msg.gear = desired_.gear; // 通常保持挡位 - msg.brake = 1; - msg.rpm = 0; - msg.angle = 0.0f; - msg.angle_speed = 120; - msg.sweep = false; - } - // 正常远控 - else - { - 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; - } - } - - pub_->publish(msg); - }); - // CAN 反馈订阅，用于计算当前转向角 can_sub_ = this->create_subscription( "can_data", 10, @@ -216,36 +178,77 @@ class RemoteCtrlNode : public rclcpp::Node mqtt_->subscribe(ctrl_topic_, 1); } + void publishMcCtrlMsg() + { + // 未授权时：完全不发 + if (!remote_authorized_.load(std::memory_order_acquire)) + { + LOG_WARN("[REMOTE] NOT AUTHORIZED - 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] 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); + } + void onMqttMessage(const std::string& payload) { std::lock_guard lock(state_mtx_); - // ===================================================== - // 1. MQTT 消息到达：刷新活性 & 判断是否从超时中恢复 - // ===================================================== - bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed); - if (was_dead) - { - LOG_INFO("[REMOTE] control recovered"); - } - - last_msg_time_ = std::chrono::steady_clock::now(); - - LOG_DEBUG("[REMOTE] recv MQTT payload: %s", payload.c_str()); - try { auto j = json::parse(payload); - if (!j.contains("data")) return; + if (!j.contains("data")) + { + LOG_WARN("[REMOTE] Invalid JSON: missing 'data' field, payload=%s", payload.c_str()); + return; + } const auto& data = j["data"]; - if (!data.contains("command") || !data.contains("value")) return; + if (!data.contains("command") || !data.contains("value")) + { + LOG_WARN("[REMOTE] Invalid JSON: missing 'command' or 'value' field"); + return; + } const std::string cmd = data["command"].get(); const auto& value = data["value"]; + LOG_INFO("[REMOTE] Received command: %s", cmd.c_str()); + // ===================================================== - // 2. mode 指令：唯一的“授权 / 取消授权”入口 + // mode 指令：唯一的授权入口 // ===================================================== if (cmd == "mode") { @@ -253,35 +256,42 @@ class RemoteCtrlNode : public rclcpp::Node if (mode == 3) { - // 进入远控授权态（锁存） remote_authorized_.store(true, std::memory_order_release); remote_alive_.store(true, std::memory_order_relaxed); - - LOG_INFO("[REMOTE] authorized (mode=3)"); } else if (mode == 0) { - // 退出远控授权态（锁存） remote_authorized_.store(false, std::memory_order_release); remote_alive_.store(false, std::memory_order_relaxed); - - // 明确清空目标状态 desired_ = {}; - - LOG_INFO("[REMOTE] deauthorized (mode=0)"); } - // mode 指令处理完直接返回 + publishMcCtrlMsg(); return; } // ===================================================== - // 3. 非 mode 指令：只有在“已授权”状态下才处理 + // 非 mode 指令：检查授权 // ===================================================== - if (!remote_authorized_.load(std::memory_order_acquire)) return; + if (!remote_authorized_.load(std::memory_order_acquire)) + { + LOG_WARN("[REMOTE] Command '%s' received but NOT AUTHORIZED - will not process", cmd.c_str()); + return; + } + + // 刷新活性 + bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed); + if (was_dead) + { + LOG_INFO("[REMOTE] ✓ control recovered"); + } + + last_msg_time_ = std::chrono::steady_clock::now(); + + LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str()); // ===================================================== - // 4. drive：油门 / 刹车 / 转向 + // drive 指令 // ===================================================== if (cmd == "drive") { @@ -295,12 +305,14 @@ class RemoteCtrlNode : public rclcpp::Node } 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; @@ -308,28 +320,25 @@ class RemoteCtrlNode : public rclcpp::Node else { desired_.brake = 0; - - // 油门映射：0~65535 -> 0~6000 rpm desired_.rpm = (static_cast(throttle) * 6000U) / 65535U; - // 转向映射：0~65535 -> -50~+50 度 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; } } // ===================================================== - // 5. gear：挡位 + // gear 指令 // ===================================================== else if (cmd == "gear") { int gear = value.get(); + LOG_DEBUG("[REMOTE] gear command: %d", gear); + switch (gear) { case 0: @@ -347,16 +356,26 @@ class RemoteCtrlNode : public rclcpp::Node } } // ===================================================== - // 6. sweepCtrl：清扫开关 + // sweepCtrl 指令 // ===================================================== else if (cmd == "sweepCtrl") { - desired_.sweep = (value.get() != 0); + 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(); } catch (const std::exception& e) { - LOG_WARN("[REMOTE] JSON parse error: %s", e.what()); + LOG_ERROR("[REMOTE] Exception in onMqttMessage: %s, payload=%s", e.what(), payload.c_str()); + return; } }