lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

260106

Browse Source
This commit is contained in:
lyq 2026-01-06 10:18:25 +08:00
parent f445d3718b
commit 0195ddb3f8
7 changed files with 555 additions and 417 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/base/sweeper_interfaces/CMakeLists.txt
View File

@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Route.msg"
"msg/Fu.msg"
"msg/Task.msg"
"msg/VehicleIdentity.msg"
DEPENDENCIES std_msgs
)

3
src/base/sweeper_interfaces/msg/VehicleIdentity.msg Normal file
View File

@ -0,0 +1,3 @@
string imei
string vid
bool ready

55
src/base/vehicle_params/CMakeLists.txt
View File

@ -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()

0
src/communication/mqtt_report/include/httplib.h → src/base/vehicle_params/include/vehicle_params/httplib.h
View File

22
src/base/vehicle_params/package.xml
View File

@ -1,17 +1,27 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd"
schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>vehicle_params</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="zxwl@44.com">nvidia</maintainer>
<license>TODO: License declaration</license>
<version>0.1.0</version>
<description>
Central vehicle identity provider node.
Fetches IMEI / VID from B-side service and publishes them as ROS2 messages.
</description>
<maintainer email="zxwl@44.com">nvidia</maintainer>
<license>Apache-2.0</license>
<!-- build system -->
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- runtime deps -->
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>sweeper_interfaces</depend> <!-- ⭐ 新增 -->
<!-- tests -->
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

125
src/base/vehicle_params/src/vehicle_params_node.cpp Normal file
View File

@ -0,0 +1,125 @@
#include <rclcpp/rclcpp.hpp>
#include <nlohmann/json.hpp>
#include <atomic>
#include <chrono>
#include <string>
#include <thread>
#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<std::string>("neardi.host", "192.168.5.1");
this->declare_parameter<int>("neardi.port", 8080);
// ---------- publisher ----------
identity_pub_ = this->create_publisher<VehicleIdentity>(
"/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<bool> running_{true};
std::thread worker_;
rclcpp::Publisher<VehicleIdentity>::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<VehicleParamsNode>());
rclcpp::shutdown();
return 0;
}

756
src/communication/mqtt_report/src/mqtt_report.cpp
View File

@ -1,342 +1,37 @@
// mqtt_report_node.cpp
#include <algorithm>
#include <bitset>
#include <iostream>
#include <chrono>
#include <iomanip>
#include <iostream>
#include <mutex>
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#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<std::mutex> lock(g_imei_mutex);
return !g_imei.empty();
}
std::string get_imei()
{
std::lock_guard<std::mutex> lock(g_imei_mutex);
return g_imei;
}
void set_imei(const std::string &imei)
{
std::lock_guard<std::mutex> 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<uint16_t>(msg->data[1]) << 8) | msg->data[0];
float actual_speed_rpm = static_cast<float>(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<uint16_t>(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<int>(msg->data[2]);
info_report.controllerTemp = raw_ctrl_temp - 40; // 实际温度=原始值-40
// 解析电机温度（data[3]，偏移量30℃，1℃/bit）
int raw_motor_temp = static_cast<int>(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<uint16_t>((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<uint32_t>(msg->data[0]) << 0) |
(static_cast<uint32_t>(msg->data[1]) << 8) |
(static_cast<uint32_t>(msg->data[2]) << 16) |
(static_cast<uint32_t>(msg->data[3]) << 24);
// 提取故障2 (32位): msg->data[4] ~ msg->data[7]
uint32_t fault2_bits = (static_cast<uint32_t>(msg->data[4]) << 0) |
(static_cast<uint32_t>(msg->data[5]) << 8) |
(static_cast<uint32_t>(msg->data[6]) << 16) |
(static_cast<uint32_t>(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<uint32_t>(msg->data[0]) << 0) |
(static_cast<uint32_t>(msg->data[1]) << 8) |
(static_cast<uint32_t>(msg->data[2]) << 16) |
(static_cast<uint32_t>(msg->data[3]) << 24);
// 提取故障4 (32位): msg->data[4] ~ msg->data[7]
uint32_t fault4_bits = (static_cast<uint32_t>(msg->data[4]) << 0) |
(static_cast<uint32_t>(msg->data[5]) << 8) |
(static_cast<uint32_t>(msg->data[6]) << 16) |
(static_cast<uint32_t>(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<sweeperMsg::CanFrame>(
"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<VehicleIdentity>(
"/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,137 +76,403 @@ 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
{
// 停止所有定时器，避免回调继续执行
if (info_timer_)
info_timer_->cancel();
if (fault_timer_)
fault_timer_->cancel();
if (connection_check_timer_)
connection_check_timer_->cancel();
if (imei_thread_.joinable())
imei_thread_.join();
}
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<std::mutex> 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<std::mutex> 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<std::mutex> 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;
}
// 没有 IMEI，一律不上报故障
if (!has_imei())
{
std::string imei, vid;
if (!snapshot_identity(imei, vid))
return;
const std::string topic = "/zxwl/vehicle/" + vid + "/fault";
std::vector<int> codes;
{
std::lock_guard<std::mutex> lock(fault_mutex_);
const auto &err_set = vehicle_error_code.getAllErrorCodes();
codes.assign(err_set.begin(), err_set.end());
}
for (int code : vehicle_error_code.getAllErrorCodes())
for (int code : codes)
{
std::string fault_json = generateFaultJson(code, config.vid, getCurrentTimestampMs());
if (!mqtt_client_.publish(fault_topic_, fault_json, 0))
const std::string payload =
generateFaultJson(code, vid, getCurrentTimestampMs());
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<std::mutex> 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<std::mutex> 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<uint16_t>(msg->data[1]) << 8) | msg->data[0];
float actual_speed_rpm = static_cast<float>(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<std::mutex> lock(info_mutex_);
info_report_.speed = speed_kmh;
}
uint16_t mcu_fault_code =
(static_cast<uint16_t>(msg->data[7]) << 8) | msg->data[6];
{
std::lock_guard<std::mutex> 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<int>(msg->data[2]);
int raw_motor_temp = static_cast<int>(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<std::mutex> 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<uint16_t>((msg->data[3] << 8) | msg->data[4]);
float steeringAngle = (raw_value - 1024) / 7.0f;
{
std::lock_guard<std::mutex> lock(info_mutex_);
info_report_.steeringAngle = steeringAngle;
}
{
std::lock_guard<std::mutex> 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<uint32_t>(msg->data[0]) << 0) |
(static_cast<uint32_t>(msg->data[1]) << 8) |
(static_cast<uint32_t>(msg->data[2]) << 16) |
(static_cast<uint32_t>(msg->data[3]) << 24);
uint32_t fault2_bits =
(static_cast<uint32_t>(msg->data[4]) << 0) |
(static_cast<uint32_t>(msg->data[5]) << 8) |
(static_cast<uint32_t>(msg->data[6]) << 16) |
(static_cast<uint32_t>(msg->data[7]) << 24);
{
std::lock_guard<std::mutex> 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<uint32_t>(msg->data[0]) << 0) |
(static_cast<uint32_t>(msg->data[1]) << 8) |
(static_cast<uint32_t>(msg->data[2]) << 16) |
(static_cast<uint32_t>(msg->data[3]) << 24);
uint32_t fault4_bits =
(static_cast<uint32_t>(msg->data[4]) << 0) |
(static_cast<uint32_t>(msg->data[5]) << 8) |
(static_cast<uint32_t>(msg->data[6]) << 16) |
(static_cast<uint32_t>(msg->data[7]) << 24);
{
std::lock_guard<std::mutex> 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<sweeperMsg::CanFrame>::SharedPtr subscription_;
MQTTClientWrapper mqtt_client_;
std::string info_topic_;
std::string fault_topic_;
rclcpp::Subscription<VehicleIdentity>::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<CanDataSubscriber>(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();