diff --git a/src/communication/mqtt_report/config/config(copy).json b/src/communication/mqtt_report/config/config(copy).json deleted file mode 100644 index 4641bac..0000000 --- a/src/communication/mqtt_report/config/config(copy).json +++ /dev/null @@ -1,18 +0,0 @@ -{ - "vid": "V060003", - "mqtt": { - "ip": "192.168.4.196", - "port": 11883, - "info_topic": "/zxwl/vehicle/{vid}/info", - "fault_topic": "/zxwl/vehicle/{vid}/fault" - } -}, -{ - "vid": "V060003", - "mqtt": { - "ip": "36.153.162.171", - "port": 19683, - "info_topic": "/zxwl/vehicle/{vid}/info", - "fault_topic": "/zxwl/vehicle/{vid}/fault" - } -} \ No newline at end of file diff --git a/src/communication/mqtt_report/config/config.json b/src/communication/mqtt_report/config/config.json deleted file mode 100644 index ea4e9f8..0000000 --- a/src/communication/mqtt_report/config/config.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "vid": "V060003", - "mqtt": { - "ip": "36.153.162.171", - "port": 19683, - "info_topic": "/zxwl/sweeper/{vid}/info", - "fault_topic": "/zxwl/sweeper/{vid}/fault" - } -} \ No newline at end of file diff --git a/src/control/remote_ctrl/CMakeLists.txt b/src/control/remote_ctrl/CMakeLists.txt index 834f801..fb8bc33 100644 --- a/src/control/remote_ctrl/CMakeLists.txt +++ b/src/control/remote_ctrl/CMakeLists.txt @@ -11,8 +11,6 @@ find_package(rclcpp REQUIRED) find_package(sweeper_interfaces REQUIRED) find_package(ament_index_cpp REQUIRED) - - # ======== MQTT libs ======== if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64") message(STATUS "[MQTT] using x86 static Paho") @@ -27,53 +25,28 @@ else() endif() # ======== executable ======== -add_executable(remote_ctrl_node - src/remote_ctrl_node.cpp - src/config.cpp -) +add_executable(remote_ctrl_node src/remote_ctrl_node.cpp src/config.cpp) target_compile_options(remote_ctrl_node PRIVATE -Wno-pedantic) -target_include_directories(remote_ctrl_node - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/include -) +target_include_directories(remote_ctrl_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) -target_include_directories(remote_ctrl_node - SYSTEM PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/include/paho_mqtt_3c -) +target_include_directories(remote_ctrl_node SYSTEM PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include/paho_mqtt_3c) -target_link_libraries(remote_ctrl_node +target_link_libraries( + remote_ctrl_node ${PAHO_LIB} pthread dl ssl - crypto -) + crypto) -ament_target_dependencies(remote_ctrl_node - rclcpp - sweeper_interfaces - ament_index_cpp -) - -# ===== install config ini ===== -install( - FILES config/config.ini - DESTINATION share/${PROJECT_NAME}/config -) +ament_target_dependencies(remote_ctrl_node rclcpp sweeper_interfaces ament_index_cpp) # ===== install executable ===== -install( - TARGETS remote_ctrl_node - DESTINATION lib/${PROJECT_NAME} -) +install(TARGETS remote_ctrl_node DESTINATION lib/${PROJECT_NAME}) # ===== export include ===== -install( - DIRECTORY include/ - DESTINATION include -) +install(DIRECTORY include/ DESTINATION include) ament_package() diff --git a/src/control/remote_ctrl/config/config.ini b/src/control/remote_ctrl/config/config.ini deleted file mode 100644 index e77061f..0000000 --- a/src/control/remote_ctrl/config/config.ini +++ /dev/null @@ -1,6 +0,0 @@ -[MQTT] -host = 36.153.162.171 -port = 19683 -username = zxwl -password = zxwl1234@ -vehicle_id = V060003 diff --git a/src/control/remote_ctrl/include/remote_ctrl/config.hpp b/src/control/remote_ctrl/include/remote_ctrl/config.hpp index 6f05183..1162045 100644 --- a/src/control/remote_ctrl/include/remote_ctrl/config.hpp +++ b/src/control/remote_ctrl/include/remote_ctrl/config.hpp @@ -3,14 +3,15 @@ struct Config { - std::string host = "127.0.0.1"; - int port = 1883; - std::string username; - std::string password; - std::string vehicle_id = "DEFAULT"; + std::string mqtt_ip; + int mqtt_port = 1883; + std::string mqtt_username; + std::string mqtt_password; + + std::string remote_topic_template; }; namespace config { - bool load(const std::string &path, Config &cfg); +bool load(const std::string& path, Config& cfg); } diff --git a/src/control/remote_ctrl/src/config.cpp b/src/control/remote_ctrl/src/config.cpp index 8c27442..e9fd926 100644 --- a/src/control/remote_ctrl/src/config.cpp +++ b/src/control/remote_ctrl/src/config.cpp @@ -1,74 +1,40 @@ #include "remote_ctrl/config.hpp" + #include -#include -#include +#include +#include -namespace +using json = nlohmann::json; + +bool config::load(const std::string& path, Config& cfg) { - std::string trim(const std::string &s) + try { - auto start = s.find_first_not_of(" \t\r\n"); - if (start == std::string::npos) - return ""; - auto end = s.find_last_not_of(" \t\r\n"); - return s.substr(start, end - start + 1); - } -} + std::ifstream ifs(path); + if (!ifs.is_open()) + { + std::cerr << "Failed to open config file: " << path << std::endl; + return false; + } -bool config::load(const std::string &path, Config &cfg) -{ - std::ifstream ifs(path); - if (!ifs.is_open()) + json j; + ifs >> j; + + const auto& mqtt = j.at("mqtt"); + + cfg.mqtt_ip = mqtt.at("external_net_address").get(); + cfg.mqtt_port = mqtt.at("external_net_port").get(); + cfg.mqtt_username = mqtt.at("username").get(); + cfg.mqtt_password = mqtt.at("password").get(); + + // remote_ctrl 用这个 + cfg.remote_topic_template = mqtt.at("remote_topic").get(); + + return true; + } + catch (const std::exception& e) + { + std::cerr << "Error parsing remote_ctrl config: " << e.what() << std::endl; return false; - - std::string line; - bool inMQTT = false; - - while (std::getline(ifs, line)) - { - line = trim(line); - - if (line.empty() || line[0] == '#') - continue; - - if (line == "[MQTT]") - { - inMQTT = true; - continue; - } - - if (!inMQTT) - continue; - - auto eq = line.find('='); - if (eq == std::string::npos) - continue; - - std::string key = trim(line.substr(0, eq)); - std::string val = trim(line.substr(eq + 1)); - - if (key == "host") - { - cfg.host = val; - } - else if (key == "port") - { - if (!val.empty()) - cfg.port = std::stoi(val); - } - else if (key == "username") - { - cfg.username = val; - } - else if (key == "password") - { - cfg.password = val; - } - else if (key == "vehicle_id") - { - cfg.vehicle_id = val; - } } - - return true; } diff --git a/src/control/remote_ctrl/src/remote_ctrl_node.cpp b/src/control/remote_ctrl/src/remote_ctrl_node.cpp index b92c1a8..516be44 100644 --- a/src/control/remote_ctrl/src/remote_ctrl_node.cpp +++ b/src/control/remote_ctrl/src/remote_ctrl_node.cpp @@ -1,29 +1,30 @@ -#include +#include + +#include +#include #include #include +#include #include -#include -#include // for std::clamp -#include // for fabs -#include - -#include "sweeper_interfaces/msg/mc_ctrl.hpp" -#include "sweeper_interfaces/msg/can_frame.hpp" -#include "remote_ctrl/mqtt_client.hpp" -#include "remote_ctrl/config.hpp" #include "nlohmann/json.hpp" +#include "remote_ctrl/config.hpp" +#include "remote_ctrl/mqtt_client.hpp" +#include "sweeper_interfaces/msg/can_frame.hpp" +#include "sweeper_interfaces/msg/mc_ctrl.hpp" +#include "sweeper_interfaces/msg/vehicle_identity.hpp" using json = nlohmann::json; namespace sweeperMsg = sweeper_interfaces::msg; using namespace std::chrono_literals; -// 一些转速/角速度常量（目前你只是保留，逻辑在 onMqttMessage 里） +// =========================================================== +// 常量 +// =========================================================== constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f; constexpr float GEAR_RATIO = 16.5f; constexpr float DELTA_T = 0.02f; -// MQTT client ID 前缀 #define MQTT_CLIENT_PREFIX "remote_ctrl" static std::string makeRandomClientId() @@ -33,14 +34,13 @@ static std::string makeRandomClientId() std::uniform_int_distribution dist(0, 0xFFFFFF); std::stringstream ss; - ss << MQTT_CLIENT_PREFIX - << "_" << getpid() - << "_" << std::hex << dist(gen); - + ss << MQTT_CLIENT_PREFIX << "_" << getpid() << "_" << std::hex << dist(gen); return ss.str(); } +// =========================================================== // 控制状态 +// =========================================================== struct RemoteCtrlState { int gear = 0; @@ -51,136 +51,134 @@ struct RemoteCtrlState bool sweep = false; }; +// =========================================================== +// 主节点 +// =========================================================== class RemoteCtrlNode : public rclcpp::Node { -public: - RemoteCtrlNode() - : Node("remote_ctrl_node") + public: + explicit RemoteCtrlNode(const Config& cfg) : Node("remote_ctrl_node"), cfg_(cfg) { // 发布话题：/remote_mc_ctrl pub_ = this->create_publisher("/remote_mc_ctrl", 10); - // 加载配置 - std::string cfg_path; - try - { - cfg_path = ament_index_cpp::get_package_share_directory("remote_ctrl") + "/config/config.ini"; - } - catch (...) - { - RCLCPP_ERROR(this->get_logger(), "找不到 config.ini"); - throw; - } - - Config cfg; - if (!config::load(cfg_path, cfg)) - { - RCLCPP_ERROR(this->get_logger(), "无法加载配置文件: %s", cfg_path.c_str()); - throw std::runtime_error("config.ini missing"); - } - // ===== 构造 MQTT 客户端 ===== std::stringstream uri; - uri << "tcp://" << cfg.host << ":" << cfg.port; + uri << "tcp://" << cfg_.mqtt_ip << ":" << cfg_.mqtt_port; + std::string client_id = makeRandomClientId(); - mqtt_ = std::make_unique( - uri.str(), - client_id, - cfg.username, - cfg.password); + mqtt_ = std::make_unique(uri.str(), client_id, cfg_.mqtt_username, cfg_.mqtt_password); // 消息回调 - mqtt_->setMessageCallback( - [this](const std::string & /*topic*/, const std::string &payload) - { - this->onMqttMessage(payload); - }); + mqtt_->setMessageCallback([this](const std::string& /*topic*/, const std::string& payload) + { this->onMqttMessage(payload); }); // 连接成功回调：做订阅 - std::string topic = "/zxwl/vehicle/" + cfg.vehicle_id + "/ctrl"; mqtt_->setConnectedCallback( - [this, topic]() + [this]() { - RCLCPP_INFO(this->get_logger(), - "[REMOTE] MQTT connected, subscribe: %s", - topic.c_str()); - mqtt_->subscribe(topic, 1); + // 重连后允许重新订阅 + subscribed_.store(false, std::memory_order_relaxed); + + if (!identity_ready_.load()) + { + RCLCPP_WARN(this->get_logger(), "[REMOTE] MQTT connected but identity not ready"); + return; + } + + try_subscribe_ctrl_topic(); }); // 主动连一次，后面内部 loop 线程也会自动尝试重连 mqtt_->connect(); - RCLCPP_INFO(this->get_logger(), - "RemoteCtrlNode started mqtt=%s topic=%s client_id=%s", - uri.str().c_str(), - topic.c_str(), + identity_sub_ = this->create_subscription( + "/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(), + [this](const sweeper_interfaces::msg::VehicleIdentity::SharedPtr msg) + { + vid_ = msg->vid; + identity_ready_.store(!vid_.empty()); + + RCLCPP_INFO(this->get_logger(), "[REMOTE] Identity ready: VID=%s", vid_.c_str()); + + ctrl_topic_ = cfg_.remote_topic_template; + + if (ctrl_topic_.find("{vid}") == std::string::npos) + { + RCLCPP_WARN(this->get_logger(), "[REMOTE] remote_topic_template missing '{vid}', template=%s", + cfg_.remote_topic_template.c_str()); + } + + auto pos = ctrl_topic_.find("{vid}"); + if (pos != std::string::npos) ctrl_topic_.replace(pos, 5, vid_); + + try_subscribe_ctrl_topic(); + }); + + RCLCPP_INFO(this->get_logger(), "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 指令则停控 - watchdog_ = this->create_wall_timer( - 100ms, - [this]() - { - // 未授权远控，watchdog 什么都不做 - if (!remote_authorized_.load(std::memory_order_acquire)) - return; + watchdog_ = this->create_wall_timer(100ms, + [this]() + { + // 未授权远控，watchdog 什么都不做 + if (!remote_authorized_.load(std::memory_order_acquire)) return; - auto now = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); - // 超时阈值：建议 800ms ~ 1500ms - if (now - last_msg_time_ > 800ms) - { - // 只在第一次进入“失活”时打印日志 - if (remote_alive_.exchange(false)) - { - RCLCPP_WARN(this->get_logger(), - "[REMOTE] control timeout, enter safe-stop"); - } - } - }); + // 超时阈值：建议 800ms ~ 1500ms + if (now - last_msg_time_ > 800ms) + { + // 只在第一次进入“失活”时打印日志 + if (remote_alive_.exchange(false)) + { + RCLCPP_WARN(this->get_logger(), + "[REMOTE] control timeout, enter safe-stop"); + } + } + }); // 周期发布到 /remote_mc_ctrl - timer_ = this->create_wall_timer( - 20ms, // 50Hz - [this]() - { - // 未授权远控：完全不发 - if (!remote_authorized_.load(std::memory_order_acquire)) - return; + timer_ = this->create_wall_timer(20ms, // 50Hz + [this]() + { + // 未授权远控：完全不发 + if (!remote_authorized_.load(std::memory_order_acquire)) return; - sweeperMsg::McCtrl msg; + sweeperMsg::McCtrl msg; - { - std::lock_guard lock(state_mtx_); + { + 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; - } - } + // 已授权但超时：安全停车态 + 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); - }); + pub_->publish(msg); + }); // CAN 反馈订阅，用于计算当前转向角 can_sub_ = this->create_subscription( @@ -189,8 +187,7 @@ public: { if (msg->id == 0x401 && msg->dlc >= 5) { - uint16_t raw = (static_cast(msg->data[3]) << 8) | - msg->data[4]; + uint16_t raw = (static_cast(msg->data[3]) << 8) | msg->data[4]; // 保持与 SBUS 侧一致 current_feedback_angle_ = (raw - 1024) / 7.0f; } @@ -206,8 +203,21 @@ public: } } -private: - void onMqttMessage(const std::string &payload) + private: + void try_subscribe_ctrl_topic() + { + if (!identity_ready_.load()) return; + if (ctrl_topic_.empty()) return; + + bool already = subscribed_.exchange(true); + + if (already) return; // 已经订阅过，退出 + + RCLCPP_INFO(this->get_logger(), "[REMOTE] subscribe ctrl topic: %s", ctrl_topic_.c_str()); + mqtt_->subscribe(ctrl_topic_, 1); + } + + void onMqttMessage(const std::string& payload) { std::lock_guard lock(state_mtx_); @@ -217,28 +227,23 @@ private: bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed); if (was_dead) { - RCLCPP_INFO(this->get_logger(), - "[REMOTE] control recovered"); + RCLCPP_INFO(this->get_logger(), "[REMOTE] control recovered"); } last_msg_time_ = std::chrono::steady_clock::now(); - RCLCPP_DEBUG(this->get_logger(), - "[REMOTE] recv MQTT payload: %s", - payload.c_str()); + RCLCPP_DEBUG(this->get_logger(), "[REMOTE] recv MQTT payload: %s", payload.c_str()); try { auto j = json::parse(payload); - if (!j.contains("data")) - return; + if (!j.contains("data")) return; - const auto &data = j["data"]; - if (!data.contains("command") || !data.contains("value")) - return; + const auto& data = j["data"]; + if (!data.contains("command") || !data.contains("value")) return; const std::string cmd = data["command"].get(); - const auto &value = data["value"]; + const auto& value = data["value"]; // ===================================================== // 2. mode 指令：唯一的“授权 / 取消授权”入口 @@ -253,8 +258,7 @@ private: remote_authorized_.store(true, std::memory_order_release); remote_alive_.store(true, std::memory_order_relaxed); - RCLCPP_INFO(this->get_logger(), - "[REMOTE] authorized (mode=3)"); + RCLCPP_INFO(this->get_logger(), "[REMOTE] authorized (mode=3)"); } else if (mode == 0) { @@ -265,8 +269,7 @@ private: // 明确清空目标状态 desired_ = {}; - RCLCPP_INFO(this->get_logger(), - "[REMOTE] deauthorized (mode=0)"); + RCLCPP_INFO(this->get_logger(), "[REMOTE] deauthorized (mode=0)"); } // mode 指令处理完直接返回 @@ -276,8 +279,7 @@ private: // ===================================================== // 3. 非 mode 指令：只有在“已授权”状态下才处理 // ===================================================== - if (!remote_authorized_.load(std::memory_order_acquire)) - return; + if (!remote_authorized_.load(std::memory_order_acquire)) return; // ===================================================== // 4. drive：油门 / 刹车 / 转向 @@ -290,11 +292,7 @@ private: if (value.is_string()) { - sscanf(value.get().c_str(), - "%d,%d,%d", - &throttle, - &brake, - &steer_raw); + sscanf(value.get().c_str(), "%d,%d,%d", &throttle, &brake, &steer_raw); } else { @@ -313,22 +311,16 @@ private: desired_.brake = 0; // 油门映射：0~65535 -> 0~6000 rpm - desired_.rpm = - (static_cast(throttle) * 6000U) / 65535U; + 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 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; + 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_speed = std::clamp(static_cast(motor_rpm), 120, 1500); desired_.angle = target_angle; } @@ -341,18 +333,18 @@ private: int gear = value.get(); 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; + 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; } } // ===================================================== @@ -363,15 +355,13 @@ private: desired_.sweep = (value.get() != 0); } } - catch (const std::exception &e) + catch (const std::exception& e) { - RCLCPP_WARN(this->get_logger(), - "[REMOTE] JSON parse error: %s", - e.what()); + RCLCPP_WARN(this->get_logger(), "[REMOTE] JSON parse error: %s", e.what()); } } -private: + private: RemoteCtrlState desired_; std::mutex state_mtx_; @@ -382,24 +372,63 @@ private: rclcpp::TimerBase::SharedPtr timer_; rclcpp::Subscription::SharedPtr can_sub_; - std::atomic remote_authorized_{false}; // 是否允许远控（锁存） - std::atomic remote_alive_{false}; // 是否活跃（心跳） + std::atomic remote_authorized_{false}; // 是否允许远控（锁存） + std::atomic remote_alive_{false}; // 是否活跃（心跳） std::chrono::steady_clock::time_point last_msg_time_; float current_feedback_angle_{0.0f}; + + // ===== identity ===== + Config cfg_; + std::atomic subscribed_{false}; + std::string vid_; + std::atomic identity_ready_{false}; + + rclcpp::Subscription::SharedPtr identity_sub_; + + // MQTT ctrl topic（由 identity 决定） + std::string ctrl_topic_; }; -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { + // =============================== + // 1. 默认配置路径 + 手动解析 --config + // =============================== + std::string config_path = "./config.json"; // 默认路径，按你现状不动 + + for (int i = 1; i < argc; ++i) + { + if (std::string(argv[i]) == "--config" && i + 1 < argc) + { + config_path = argv[i + 1]; + ++i; + } + } + rclcpp::init(argc, argv); - auto node = std::make_shared(); + // =============================== + // 2. 解析配置 + // =============================== + Config cfg; + if (!config::load(config_path, cfg)) + { + RCLCPP_ERROR(rclcpp::get_logger("main"), "Failed to load config file: %s", config_path.c_str()); + return 1; + } + + RCLCPP_INFO(rclcpp::get_logger("main"), "Config loaded: broker=%s:%d remote_topic_template=%s", cfg.mqtt_ip.c_str(), + cfg.mqtt_port, cfg.remote_topic_template.c_str()); + + // =============================== + // 3. 把配置传入 Node 构造函数 + // =============================== + auto node = std::make_shared(cfg); rclcpp::spin(node); - // 这里 reset 会触发 RemoteCtrlNode 析构 → 停止 MQTT 线程 node.reset(); - rclcpp::shutdown(); return 0; }