From 0195ddb3f8ca9eb69129dbf44e650ad2d8a884e9 Mon Sep 17 00:00:00 2001 From: lyq Date: Tue, 6 Jan 2026 10:18:25 +0800 Subject: [PATCH] 260106 --- src/base/sweeper_interfaces/CMakeLists.txt | 1 + .../msg/VehicleIdentity.msg | 3 + src/base/vehicle_params/CMakeLists.txt | 55 +- .../include/vehicle_params}/httplib.h | 0 src/base/vehicle_params/package.xml | 22 +- .../src/vehicle_params_node.cpp | 125 +++ .../mqtt_report/src/mqtt_report.cpp | 766 +++++++++--------- 7 files changed, 555 insertions(+), 417 deletions(-) create mode 100644 src/base/sweeper_interfaces/msg/VehicleIdentity.msg rename src/{communication/mqtt_report/include => base/vehicle_params/include/vehicle_params}/httplib.h (100%) create mode 100644 src/base/vehicle_params/src/vehicle_params_node.cpp diff --git a/src/base/sweeper_interfaces/CMakeLists.txt b/src/base/sweeper_interfaces/CMakeLists.txt index 79c87e7..c966805 100644 --- a/src/base/sweeper_interfaces/CMakeLists.txt +++ b/src/base/sweeper_interfaces/CMakeLists.txt @@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Route.msg" "msg/Fu.msg" "msg/Task.msg" + "msg/VehicleIdentity.msg" DEPENDENCIES std_msgs ) diff --git a/src/base/sweeper_interfaces/msg/VehicleIdentity.msg b/src/base/sweeper_interfaces/msg/VehicleIdentity.msg new file mode 100644 index 0000000..34be16c --- /dev/null +++ b/src/base/sweeper_interfaces/msg/VehicleIdentity.msg @@ -0,0 +1,3 @@ +string imei +string vid +bool ready diff --git a/src/base/vehicle_params/CMakeLists.txt b/src/base/vehicle_params/CMakeLists.txt index ab74ff8..bf1ae33 100644 --- a/src/base/vehicle_params/CMakeLists.txt +++ b/src/base/vehicle_params/CMakeLists.txt @@ -1,25 +1,54 @@ cmake_minimum_required(VERSION 3.8) project(vehicle_params) +# ========================= +# 编译选项 +# ========================= if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies +# ========================= +# 依赖 +# ========================= find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) +find_package(sweeper_interfaces REQUIRED) # ⭐ 新增 -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() +# ========================= +# include 路径 +# ========================= +include_directories( + include +) +# ========================= +# 可执行文件 +# ========================= +add_executable(vehicle_params_node + src/vehicle_params_node.cpp +) + +# ========================= +# 链接依赖 +# ========================= +ament_target_dependencies(vehicle_params_node + rclcpp + sweeper_interfaces # ⭐ 新增 +) + +# ========================= +# 安装 +# ========================= +install(TARGETS vehicle_params_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +# ========================= +# 导出 +# ========================= ament_package() diff --git a/src/communication/mqtt_report/include/httplib.h b/src/base/vehicle_params/include/vehicle_params/httplib.h similarity index 100% rename from src/communication/mqtt_report/include/httplib.h rename to src/base/vehicle_params/include/vehicle_params/httplib.h diff --git a/src/base/vehicle_params/package.xml b/src/base/vehicle_params/package.xml index a7d09ca..fdce56f 100644 --- a/src/base/vehicle_params/package.xml +++ b/src/base/vehicle_params/package.xml @@ -1,17 +1,27 @@ - + + vehicle_params - 0.0.0 - TODO: Package description - nvidia - TODO: License declaration + 0.1.0 + + Central vehicle identity provider node. + Fetches IMEI / VID from B-side service and publishes them as ROS2 messages. + + + nvidia + Apache-2.0 + + ament_cmake + rclcpp - std_msgs + sweeper_interfaces + ament_lint_auto ament_lint_common diff --git a/src/base/vehicle_params/src/vehicle_params_node.cpp b/src/base/vehicle_params/src/vehicle_params_node.cpp new file mode 100644 index 0000000..2c134c7 --- /dev/null +++ b/src/base/vehicle_params/src/vehicle_params_node.cpp @@ -0,0 +1,125 @@ +#include +#include + +#include +#include +#include +#include + +#include "vehicle_params/httplib.h" +#include "sweeper_interfaces/msg/vehicle_identity.hpp" + +using sweeper_interfaces::msg::VehicleIdentity; + +class VehicleParamsNode : public rclcpp::Node +{ +public: + VehicleParamsNode() + : Node("vehicle_params_node") + { + RCLCPP_INFO(this->get_logger(), "vehicle_params_node starting..."); + + // ---------- neardi ---------- + this->declare_parameter("neardi.host", "192.168.5.1"); + this->declare_parameter("neardi.port", 8080); + + // ---------- publisher ---------- + identity_pub_ = this->create_publisher( + "/vehicle/identity", + rclcpp::QoS(1).transient_local().reliable()); + + // ---------- worker ---------- + worker_ = std::thread([this]() + { fetch_from_neardi_loop(); }); + } + + ~VehicleParamsNode() + { + running_.store(false); + if (worker_.joinable()) + worker_.join(); + } + +private: + std::atomic running_{true}; + std::thread worker_; + rclcpp::Publisher::SharedPtr identity_pub_; + + void fetch_from_neardi_loop() + { + std::string host; + int port = 0; + this->get_parameter("neardi.host", host); + this->get_parameter("neardi.port", port); + + httplib::Client cli(host, port); + cli.set_connection_timeout(3); + cli.set_read_timeout(3); + + while (rclcpp::ok() && running_.load()) + { + RCLCPP_INFO(this->get_logger(), + "Requesting vehicle identity from neardi (%s:%d)...", + host.c_str(), port); + + auto res = cli.Post("/api/v1/device/register"); + + if (!res || res->status != 200) + { + RCLCPP_WARN(this->get_logger(), "neardi request failed, retrying..."); + retry_sleep(); + continue; + } + + try + { + auto j = nlohmann::json::parse(res->body); + if (!j.value("ok", false)) + { + retry_sleep(); + continue; + } + + auto dev = j.at("device"); + std::string imei = dev.value("imei", ""); + std::string vid = dev.value("vid", ""); + + if (imei.empty() || vid.empty()) + { + retry_sleep(); + continue; + } + + VehicleIdentity msg; + msg.imei = imei; + msg.vid = vid; + msg.ready = true; + + identity_pub_->publish(msg); + + RCLCPP_INFO(this->get_logger(), + "Vehicle identity published: IMEI=%s, VID=%s", + imei.c_str(), vid.c_str()); + + return; // 成功一次即可 + } + catch (...) + { + retry_sleep(); + } + } + } + + void retry_sleep() + { + std::this_thread::sleep_for(std::chrono::seconds(2)); + } +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/communication/mqtt_report/src/mqtt_report.cpp b/src/communication/mqtt_report/src/mqtt_report.cpp index dc3478e..caa1723 100644 --- a/src/communication/mqtt_report/src/mqtt_report.cpp +++ b/src/communication/mqtt_report/src/mqtt_report.cpp @@ -1,342 +1,37 @@ +// mqtt_report_node.cpp + #include #include -#include #include +#include +#include +#include +#include +#include #include + #include "sweeper_interfaces/msg/can_frame.hpp" -#include "mqtt_report/mqtt_client.hpp" // 包含上面修改后的MQTTClientWrapper -#include "mqtt_report/get_config.h" -#include "mqtt_report/report_struct.h" +#include "sweeper_interfaces/msg/vehicle_identity.hpp" + #include "mqtt_report/fault_codes.h" -#include "httplib.h" +#include "mqtt_report/get_config.h" +#include "mqtt_report/mqtt_client.hpp" +#include "mqtt_report/report_struct.h" namespace sweeperMsg = sweeper_interfaces::msg; +using sweeper_interfaces::msg::VehicleIdentity; -Config config; // 清扫车配置文件 -GeneralMsg info_report; // 常规消息上报 -int mqtt_reconnect_interval = 5000; // 重连间隔(ms) +Config config; +int mqtt_reconnect_interval = 5000; // ms -// imei获取 -std::string g_imei; -std::mutex g_imei_mutex; -std::thread imei_thread_; - -bool has_imei() -{ - std::lock_guard lock(g_imei_mutex); - return !g_imei.empty(); -} - -std::string get_imei() -{ - std::lock_guard lock(g_imei_mutex); - return g_imei; -} - -void set_imei(const std::string &imei) -{ - std::lock_guard lock(g_imei_mutex); - g_imei = imei; -} - -bool fetch_imei_from_neardi(const std::string &host, int port) -{ - httplib::Client cli(host, port); - cli.set_connection_timeout(2); - cli.set_read_timeout(2); - - // 只发送最核心的身份信息 - nlohmann::json req; - req["vid"] = config.vid; - req["device_type"] = "orin"; - req["timestamp"] = getCurrentTimestampMs(); - - auto res = cli.Post( - "/api/v1/device/register", - req.dump(), - "application/json"); - - if (!res) - { - std::cerr << "[REGISTER] HTTP request failed\n"; - return false; - } - - if (res->status != 200) - { - std::cerr << "[REGISTER] HTTP status = " << res->status << "\n"; - return false; - } - - try - { - auto j = nlohmann::json::parse(res->body); - - if (!j.value("ok", false)) - { - std::cerr << "[REGISTER] rejected: " - << j.value("error", "unknown") << std::endl; - return false; - } - - if (j.contains("server") && j["server"].contains("imei")) - { - std::string imei = j["server"]["imei"]; - set_imei(imei); - std::cout << "[REGISTER] IMEI = " << imei << std::endl; - return true; - } - - std::cerr << "[REGISTER] no imei in response\n"; - } - catch (const std::exception &e) - { - std::cerr << "[REGISTER] JSON parse error: " << e.what() << "\n"; - } - - return false; -} - -// 解析can报文，做消息上报 -void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) -{ - switch (msg->id) - { - // BMS_Status 100 - case 0x100: - { - uint16_t voltage_raw = (msg->data[0] << 8) | msg->data[1]; // 总电压 - int16_t current_raw = (msg->data[2] << 8) | msg->data[3]; // 电流 - uint16_t capacity_raw = (msg->data[4] << 8) | msg->data[5]; // 剩余容量 - - float voltage = voltage_raw * 0.01f; // 10mV -> V 总电压 - float current = current_raw * 0.01f; // 10mA -> A 电流 - float capacity = capacity_raw * 0.01f; // 10mAh -> Ah 剩余容量 - - // 根据电流正负判断充放电状态 - info_report.chargeStatus = (current >= 0) ? true : false; - - std::cout << std::fixed << std::setprecision(2); - std::cout << "[0x100] 总电压: " << voltage << " V, " - << "电流: " << current << " A, " - << "剩余容量: " << capacity << " Ah, " - << "是否在充电: " << (info_report.chargeStatus == 1 ? "是" : "否") << std::endl; - break; - } - - // BMS_Status 101 - case 0x101: - { - // 提取原始数据 - uint16_t full_capacity_raw = (msg->data[0] << 8) | msg->data[1]; // 充满容量 - uint16_t cycle_count_raw = (msg->data[2] << 8) | msg->data[3]; // 循环次数 - uint16_t rsoc_raw = (msg->data[4] << 8) | msg->data[5]; // 剩余容量百分比 - - // 数据换算 - float full_capacity = full_capacity_raw * 0.01f; // 10mAh → Ah - uint16_t cycle_count = cycle_count_raw; // 单位：次 - float rsoc = rsoc_raw * 1.0f; // 以百分比% - - // 保存解析结果 - info_report.power = rsoc; - - // 输出打印 - std::cout << std::fixed << std::setprecision(2); - std::cout << "[0x101] 充满容量: " << full_capacity << " Ah, " - << "循环次数: " << cycle_count << " 次, " - << "剩余容量百分比(RSOC): " << rsoc << " %" << std::endl; - break; - } - - // MCU_Status_1 和 MCU_Fault（基础ID：0x0CF11E00，叠加设备ID） - case 0x0CF11E05: - { - // 解析电机实际转速（data[0]：低字节，data[1]：高字节） - uint16_t raw_speed = (static_cast(msg->data[1]) << 8) | msg->data[0]; - float actual_speed_rpm = static_cast(raw_speed); // 量程0-6000rpm，1rpm/bit - // 转换为km/h - float gear_ratio = 32.0; // 电机速比 - float wheel_radius = 0.3; // 轮胎直径(m) - info_report.speed = (actual_speed_rpm * 60 * 3.1416 * wheel_radius) / (1000 * gear_ratio); - // std::cout << "[0x0CF11E00] speed : " << info_report.speed << std::endl; - - // 故障码（data[6]：低字节，data[7]：高字节） - uint16_t mcu_fault_code = (static_cast(msg->data[7]) << 8) | msg->data[6]; - // 清空原有MCU故障码（1101-1116，对应ERR0-ERR15） - for (int code = 1101; code <= 1116; ++code) - { - vehicle_error_code.removeErrorCode(code); - } - // 遍历16个故障位（ERR0-ERR15） - for (int i = 0; i < 16; ++i) - { - bool is_fault = (mcu_fault_code >> i) & 0x01; // 提取第i位故障状态 - int error_code = 1101 + i; // 自定义故障码（1101=ERR0，1116=ERR15） - - if (is_fault && faultMap.count(error_code)) // 故障存在且在映射表中 - { - vehicle_error_code.addErrorCode(error_code); - updateFaultLevel(error_code, 1); // 协议未定义故障等级，默认1级 - } - else // 故障不存在 - { - vehicle_error_code.removeErrorCode(error_code); - } - } - - break; - } - - // MCU_Status_2（基础ID：0x0CF11F00，叠加设备ID） - case 0x0CF11F05: - { - // 解析控制器温度（data[2]，偏移量40℃，1℃/bit） - int raw_ctrl_temp = static_cast(msg->data[2]); - info_report.controllerTemp = raw_ctrl_temp - 40; // 实际温度=原始值-40 - - // 解析电机温度（data[3]，偏移量30℃，1℃/bit） - int raw_motor_temp = static_cast(msg->data[3]); - info_report.motorTemp = raw_motor_temp - 30; // 实际温度=原始值-30 - - // 解析控制器状态（data[4]，BIT1-BIT0：命令状态；BIT3-BIT2：反馈状态） - uint8_t ctrl_status = msg->data[4]; - // 反馈状态（BIT3-BIT2）：0=空挡，1=前进，2=后退，3=保留 - int feedback_gear = (ctrl_status >> 2) & 0x03; - info_report.gear = feedback_gear; - - // std::cout << "[0x0CF11F00] controllerTemp : " << info_report.controllerTemp << std::endl; - // std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl; - // std::cout << "[0x0CF11F00] gear : " << info_report.gear << std::endl; - - break; - } - - // EPS_Status and EPS_Fault - case 0x401: - { - // 第3、4字节拼接为原始角度数据 - uint16_t raw_value = static_cast((msg->data[3] << 8) | msg->data[4]); - info_report.steeringAngle = (raw_value - 1024) / 7.0f; - // std::cout << "[0x401] steeringAngle : " << info_report.steeringAngle << std::endl; - - // 清空所有 EPS 错误码 - for (int code = 1201; code <= 1218; ++code) - { - vehicle_error_code.removeErrorCode(code); - } - - // 添加当前报文中的错误码(最多2个) - uint8_t data_bytes[2] = {msg->data[2], msg->data[6]}; - for (uint8_t byte : data_bytes) - { - for (int i = 0; i < 18; ++i) - { - if (byte == epsErrorCode[i]) - { - vehicle_error_code.addErrorCode(1201 + i); - break; // 每个 byte 最多匹配一个错误码 - } - } - } - break; - } - - // VCU_Fault_1和Fault_2报文解析 (ID: 0x216) - case 0x216: - { - // 提取故障1 (32位): msg->data[0] ~ msg->data[3] - uint32_t fault1_bits = (static_cast(msg->data[0]) << 0) | - (static_cast(msg->data[1]) << 8) | - (static_cast(msg->data[2]) << 16) | - (static_cast(msg->data[3]) << 24); - - // 提取故障2 (32位): msg->data[4] ~ msg->data[7] - uint32_t fault2_bits = (static_cast(msg->data[4]) << 0) | - (static_cast(msg->data[5]) << 8) | - (static_cast(msg->data[6]) << 16) | - (static_cast(msg->data[7]) << 24); - - // 清空VCU故障码（1701 ~ 1764) - for (int code = 1701; code <= 1764; ++code) - { - vehicle_error_code.removeErrorCode(code); - } - - // 处理故障1的bit0 ~ bit31 → 1701 ~ 1732 - for (uint8_t bit = 0; bit <= 31; ++bit) - { - if ((fault1_bits >> bit) & 0x01) - { - vehicle_error_code.addErrorCode(1701 + bit); - } - } - - // 处理故障2的bit0 ~ bit31 → 1733 ~ 1764 - for (uint8_t bit = 0; bit <= 31; ++bit) - { - if ((fault2_bits >> bit) & 0x01) - { - vehicle_error_code.addErrorCode(1733 + bit); - } - } - - break; - } - - // VCU_Fault_3和Fault_4报文解析 (ID: 0x217) - case 0x217: - { - // 提取故障3 (32位): msg->data[0] ~ msg->data[3] - uint32_t fault3_bits = (static_cast(msg->data[0]) << 0) | - (static_cast(msg->data[1]) << 8) | - (static_cast(msg->data[2]) << 16) | - (static_cast(msg->data[3]) << 24); - - // 提取故障4 (32位): msg->data[4] ~ msg->data[7] - uint32_t fault4_bits = (static_cast(msg->data[4]) << 0) | - (static_cast(msg->data[5]) << 8) | - (static_cast(msg->data[6]) << 16) | - (static_cast(msg->data[7]) << 24); - - // 清空VCU故障码（1765 ~ 1814） - for (int code = 1765; code <= 1814; ++code) - { - vehicle_error_code.removeErrorCode(code); - } - - // 处理故障3的bit0 ~ bit31 → 1765 ~ 1796 - for (uint8_t bit = 0; bit <= 31; ++bit) - { - if ((fault3_bits >> bit) & 0x01) - { - vehicle_error_code.addErrorCode(1765 + bit); - } - } - - // 处理故障4的bit0 ~ bit18 → 1797 ~ 1814 - for (uint8_t bit = 0; bit <= 18; ++bit) - { - if ((fault4_bits >> bit) & 0x01) - { - vehicle_error_code.addErrorCode(1797 + bit); - } - } - - break; - } - } -} - -std::string pack_general_info_to_json(const GeneralMsg &msg) +// ===================== JSON 打包 ===================== +static std::string pack_general_info_to_json( + const GeneralMsg &msg, + const std::string &imei) { ordered_json j; - - if (has_imei()) - j["imei"] = get_imei(); - else - j["imei"] = nullptr; - + j["imei"] = imei; j["power"] = msg.power; j["chargeStatus"] = msg.chargeStatus; j["gear"] = msg.gear; @@ -345,25 +40,34 @@ std::string pack_general_info_to_json(const GeneralMsg &msg) j["motorTemp"] = msg.motorTemp; j["controllerTemp"] = msg.controllerTemp; j["timestamp"] = msg.timestamp; - - return j.dump(4); // 输出有序 JSON 字符串 + return j.dump(-1); } +// ===================== Node ===================== class CanDataSubscriber : public rclcpp::Node { public: - CanDataSubscriber(const Config &config) + explicit CanDataSubscriber(const Config &cfg) : Node("can_data_subscriber"), - mqtt_client_(config.mqtt_ip + ":" + std::to_string(config.mqtt_port), + mqtt_client_(cfg.mqtt_ip + ":" + std::to_string(cfg.mqtt_port), generate_mqtt_client_id(), - "zxwl", "zxwl1234@", - mqtt_reconnect_interval), // 传入重连间隔 - info_topic_(config.info_topic), - fault_topic_(config.fault_topic) + "zxwl", + "zxwl1234@", + mqtt_reconnect_interval) { + // CAN subscription_ = this->create_subscription( - "can_data", 10, std::bind(&CanDataSubscriber::topic_callback, this, std::placeholders::_1)); + "can_data", + 10, + std::bind(&CanDataSubscriber::topic_callback, this, std::placeholders::_1)); + // Identity（关键） + identity_sub_ = this->create_subscription( + "/vehicle/identity", + rclcpp::QoS(1).transient_local().reliable(), + std::bind(&CanDataSubscriber::identity_callback, this, std::placeholders::_1)); + + // Timers info_timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&CanDataSubscriber::info_timer_callback, this)); @@ -372,139 +76,405 @@ public: std::chrono::milliseconds(500), std::bind(&CanDataSubscriber::fault_timer_callback, this)); - // 添加定期连接检查定时器（每10秒） connection_check_timer_ = this->create_wall_timer( std::chrono::seconds(10), std::bind(&CanDataSubscriber::check_connection_callback, this)); - - imei_thread_ = std::thread([this]() - { - while (rclcpp::ok()) - { - bool ok = fetch_imei_from_neardi("192.168.5.1", 8080); - - if (ok && !has_imei()) - { - // 首次成功 - } - - // 不管成功还是失败，都睡 - std::this_thread::sleep_for(std::chrono::seconds(5)); - } }); } - ~CanDataSubscriber() + ~CanDataSubscriber() override { - // 停止所有定时器，避免回调继续执行 - info_timer_->cancel(); - fault_timer_->cancel(); - connection_check_timer_->cancel(); - - if (imei_thread_.joinable()) - imei_thread_.join(); + if (info_timer_) + info_timer_->cancel(); + if (fault_timer_) + fault_timer_->cancel(); + if (connection_check_timer_) + connection_check_timer_->cancel(); } private: - void topic_callback(const sweeperMsg::CanFrame::SharedPtr msg) + // ===================== identity ===================== + bool snapshot_identity(std::string &imei, std::string &vid) { - Msg_Handler(msg); + std::lock_guard lock(identity_mutex_); + if (!vehicle_ready_) + return false; + + imei = vehicle_imei_; + vid = vehicle_vid_; + return true; } + void identity_callback(const VehicleIdentity::SharedPtr msg) + { + std::lock_guard lock(identity_mutex_); + vehicle_imei_ = msg->imei; + vehicle_vid_ = msg->vid; + vehicle_ready_ = msg->ready; + + RCLCPP_INFO(this->get_logger(), + "Vehicle identity received: IMEI=%s, VID=%s", + vehicle_imei_.c_str(), + vehicle_vid_.c_str()); + } + + // ===================== timers ===================== void info_timer_callback() { if (!rclcpp::ok()) - { return; - } - // 核心规则：没有 IMEI 就不上报 - if (!has_imei()) + std::string imei, vid; + if (!snapshot_identity(imei, vid)) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, - "IMEI not ready, skip info publish"); + "Vehicle identity not ready, skip MQTT publish"); return; } - info_report.timestamp = getCurrentTimestampMs(); - std::string json_msg = pack_general_info_to_json(info_report); + const std::string topic = "/zxwl/vehicle/" + vid + "/info"; - if (!mqtt_client_.publish(info_topic_, json_msg, 1)) + GeneralMsg snapshot; { - RCLCPP_WARN(this->get_logger(), "Failed to publish info message to MQTT"); + std::lock_guard lock(info_mutex_); + info_report_.timestamp = getCurrentTimestampMs(); + snapshot = info_report_; + } + + const std::string payload = + pack_general_info_to_json(snapshot, imei); + + if (!mqtt_client_.publish(topic, payload, 1)) + { + RCLCPP_WARN(this->get_logger(), + "Failed to publish info message to MQTT"); } } void fault_timer_callback() { if (!rclcpp::ok()) - { return; + + std::string imei, vid; + if (!snapshot_identity(imei, vid)) + return; + + const std::string topic = "/zxwl/vehicle/" + vid + "/fault"; + + std::vector codes; + { + std::lock_guard lock(fault_mutex_); + const auto &err_set = vehicle_error_code.getAllErrorCodes(); + codes.assign(err_set.begin(), err_set.end()); } - // 没有 IMEI，一律不上报故障 - if (!has_imei()) + for (int code : codes) { - return; - } + const std::string payload = + generateFaultJson(code, vid, getCurrentTimestampMs()); - for (int code : vehicle_error_code.getAllErrorCodes()) - { - std::string fault_json = generateFaultJson(code, config.vid, getCurrentTimestampMs()); - if (!mqtt_client_.publish(fault_topic_, fault_json, 0)) + if (!mqtt_client_.publish(topic, payload, 0)) { - RCLCPP_WARN(this->get_logger(), "Failed to publish fault code %d to MQTT", code); + RCLCPP_WARN(this->get_logger(), + "Failed to publish fault code %d to MQTT", + code); } } } - // 定期检查连接状态，主动重连 void check_connection_callback() { if (!mqtt_client_.isConnected()) { - RCLCPP_WARN(this->get_logger(), "MQTT connection lost."); + RCLCPP_WARN(this->get_logger(), + "MQTT connection lost."); } } + // ===================== CAN handler ===================== + void topic_callback(const sweeperMsg::CanFrame::SharedPtr msg) + { + Msg_Handler(msg); + } + + void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) + { + switch (msg->id) + { + case 0x100: // BMS_Status 100 + { + uint16_t voltage_raw = (msg->data[0] << 8) | msg->data[1]; + int16_t current_raw = (msg->data[2] << 8) | msg->data[3]; + uint16_t capacity_raw = (msg->data[4] << 8) | msg->data[5]; + + float voltage = voltage_raw * 0.01f; + float current = current_raw * 0.01f; + float capacity = capacity_raw * 0.01f; + + { + std::lock_guard lock(info_mutex_); + info_report_.chargeStatus = (current >= 0); + } + + std::cout << std::fixed << std::setprecision(2); + std::cout << "[0x100] 总电压: " << voltage << " V, " + << "电流: " << current << " A, " + << "剩余容量: " << capacity << " Ah, " + << "是否在充电: " << ((current >= 0) ? "是" : "否") << std::endl; + break; + } + + case 0x101: // BMS_Status 101 + { + uint16_t full_capacity_raw = (msg->data[0] << 8) | msg->data[1]; + uint16_t cycle_count_raw = (msg->data[2] << 8) | msg->data[3]; + uint16_t rsoc_raw = (msg->data[4] << 8) | msg->data[5]; + + float full_capacity = full_capacity_raw * 0.01f; + uint16_t cycle_count = cycle_count_raw; + float rsoc = rsoc_raw * 1.0f; + + { + std::lock_guard lock(info_mutex_); + info_report_.power = rsoc; + } + + std::cout << std::fixed << std::setprecision(2); + std::cout << "[0x101] 充满容量: " << full_capacity << " Ah, " + << "循环次数: " << cycle_count << " 次, " + << "剩余容量百分比(RSOC): " << rsoc << " %" << std::endl; + break; + } + + case 0x0CF11E05: // MCU_Status_1 + MCU_Fault + { + uint16_t raw_speed = + (static_cast(msg->data[1]) << 8) | msg->data[0]; + float actual_speed_rpm = static_cast(raw_speed); + + float gear_ratio = 32.0f; + float wheel_radius = 0.3f; + float speed_kmh = + (actual_speed_rpm * 60.0f * 3.1416f * wheel_radius) / + (1000.0f * gear_ratio); + + { + std::lock_guard lock(info_mutex_); + info_report_.speed = speed_kmh; + } + + uint16_t mcu_fault_code = + (static_cast(msg->data[7]) << 8) | msg->data[6]; + + { + std::lock_guard lock(fault_mutex_); + + // 先清 MCU 故障区 + for (int code = 1101; code <= 1116; ++code) + vehicle_error_code.removeErrorCode(code); + + for (int i = 0; i < 16; ++i) + { + bool is_fault = ((mcu_fault_code >> i) & 0x01) != 0; + int error_code = 1101 + i; + + if (is_fault && faultMap.count(error_code)) + { + vehicle_error_code.addErrorCode(error_code); + updateFaultLevel(error_code, 1); + } + else + { + vehicle_error_code.removeErrorCode(error_code); + } + } + } + + break; + } + + case 0x0CF11F05: // MCU_Status_2 + { + int raw_ctrl_temp = static_cast(msg->data[2]); + int raw_motor_temp = static_cast(msg->data[3]); + uint8_t ctrl_status = msg->data[4]; + + int controllerTemp = raw_ctrl_temp - 40; + int motorTemp = raw_motor_temp - 30; + int feedback_gear = (ctrl_status >> 2) & 0x03; + + { + std::lock_guard lock(info_mutex_); + info_report_.controllerTemp = controllerTemp; + info_report_.motorTemp = motorTemp; + info_report_.gear = feedback_gear; + } + break; + } + + case 0x401: // EPS_Status + EPS_Fault + { + uint16_t raw_value = + static_cast((msg->data[3] << 8) | msg->data[4]); + float steeringAngle = (raw_value - 1024) / 7.0f; + + { + std::lock_guard lock(info_mutex_); + info_report_.steeringAngle = steeringAngle; + } + + { + std::lock_guard lock(fault_mutex_); + + for (int code = 1201; code <= 1218; ++code) + vehicle_error_code.removeErrorCode(code); + + uint8_t data_bytes[2] = {msg->data[2], msg->data[6]}; + for (uint8_t byte : data_bytes) + { + for (int i = 0; i < 18; ++i) + { + if (byte == epsErrorCode[i]) + { + vehicle_error_code.addErrorCode(1201 + i); + break; + } + } + } + } + break; + } + + case 0x216: // VCU_Fault_1/2 + { + uint32_t fault1_bits = + (static_cast(msg->data[0]) << 0) | + (static_cast(msg->data[1]) << 8) | + (static_cast(msg->data[2]) << 16) | + (static_cast(msg->data[3]) << 24); + + uint32_t fault2_bits = + (static_cast(msg->data[4]) << 0) | + (static_cast(msg->data[5]) << 8) | + (static_cast(msg->data[6]) << 16) | + (static_cast(msg->data[7]) << 24); + + { + std::lock_guard lock(fault_mutex_); + + for (int code = 1701; code <= 1764; ++code) + vehicle_error_code.removeErrorCode(code); + + for (uint8_t bit = 0; bit <= 31; ++bit) + if ((fault1_bits >> bit) & 0x01) + vehicle_error_code.addErrorCode(1701 + bit); + + for (uint8_t bit = 0; bit <= 31; ++bit) + if ((fault2_bits >> bit) & 0x01) + vehicle_error_code.addErrorCode(1733 + bit); + } + + break; + } + + case 0x217: // VCU_Fault_3/4 + { + uint32_t fault3_bits = + (static_cast(msg->data[0]) << 0) | + (static_cast(msg->data[1]) << 8) | + (static_cast(msg->data[2]) << 16) | + (static_cast(msg->data[3]) << 24); + + uint32_t fault4_bits = + (static_cast(msg->data[4]) << 0) | + (static_cast(msg->data[5]) << 8) | + (static_cast(msg->data[6]) << 16) | + (static_cast(msg->data[7]) << 24); + + { + std::lock_guard lock(fault_mutex_); + + for (int code = 1765; code <= 1814; ++code) + vehicle_error_code.removeErrorCode(code); + + for (uint8_t bit = 0; bit <= 31; ++bit) + if ((fault3_bits >> bit) & 0x01) + vehicle_error_code.addErrorCode(1765 + bit); + + for (uint8_t bit = 0; bit <= 18; ++bit) + if ((fault4_bits >> bit) & 0x01) + vehicle_error_code.addErrorCode(1797 + bit); + } + + break; + } + + default: + break; + } + } + +private: + // ROS rclcpp::Subscription::SharedPtr subscription_; - MQTTClientWrapper mqtt_client_; - std::string info_topic_; - std::string fault_topic_; + rclcpp::Subscription::SharedPtr identity_sub_; + rclcpp::TimerBase::SharedPtr info_timer_; rclcpp::TimerBase::SharedPtr fault_timer_; - rclcpp::TimerBase::SharedPtr connection_check_timer_; // 新增：连接检查定时器 + rclcpp::TimerBase::SharedPtr connection_check_timer_; + + // MQTT + MQTTClientWrapper mqtt_client_; + + // Data + GeneralMsg info_report_; + std::mutex info_mutex_; + + std::mutex fault_mutex_; + + // Identity cache + std::mutex identity_mutex_; + std::string vehicle_imei_; + std::string vehicle_vid_; + bool vehicle_ready_ = false; }; +// ===================== main ===================== int main(int argc, char **argv) { rclcpp::init(argc, argv); if (!load_config(config)) { - RCLCPP_ERROR(rclcpp::get_logger("main"), "Failed to load MQTT config."); + RCLCPP_ERROR(rclcpp::get_logger("main"), + "Failed to load MQTT config."); return 1; } - // 确保配置中有重连间隔（默认5000ms） if (mqtt_reconnect_interval <= 0) - { mqtt_reconnect_interval = 5000; - } auto node = std::make_shared(config); - RCLCPP_INFO(node->get_logger(), "info_topic : %s", config.info_topic.c_str()); - RCLCPP_INFO(node->get_logger(), "fault_topic : %s", config.fault_topic.c_str()); - RCLCPP_INFO(node->get_logger(), "vid : %s", config.vid.c_str()); - RCLCPP_INFO(node->get_logger(), "mqtt_ip : %s", config.mqtt_ip.c_str()); - RCLCPP_INFO(node->get_logger(), "MQTT reconnection interval: %d ms", mqtt_reconnect_interval); - - RCLCPP_INFO(node->get_logger(), "MqttReport node started."); + RCLCPP_INFO(node->get_logger(), + "mqtt_ip: %s", + config.mqtt_ip.c_str()); + RCLCPP_INFO(node->get_logger(), + "info_topic: %s", + config.info_topic.c_str()); + RCLCPP_INFO(node->get_logger(), + "fault_topic: %s", + config.fault_topic.c_str()); + RCLCPP_INFO(node->get_logger(), + "MQTT reconnection interval: %d ms", + mqtt_reconnect_interval); + RCLCPP_INFO(node->get_logger(), + "MqttReport node started."); rclcpp::spin(node); rclcpp::shutdown(); return 0; -} \ No newline at end of file +}