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

修改远控节点获取vid及远控话题的方式

Browse Source
This commit is contained in:
lyq 2026-01-19 10:17:00 +08:00
parent caeb6cbc2b
commit cec7a19971
7 changed files with 246 additions and 310 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/communication/mqtt_report/config/config(copy).json
View File

@ -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"
}
}

9
src/communication/mqtt_report/config/config.json
View File

@ -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"
}
}

45
src/control/remote_ctrl/CMakeLists.txt
View File

@ -11,8 +11,6 @@ find_package(rclcpp REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(ament_index_cpp REQUIRED) find_package(ament_index_cpp REQUIRED)
# ======== MQTT libs ======== # ======== MQTT libs ========
if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64") if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
message(STATUS "[MQTT] using x86 static Paho") message(STATUS "[MQTT] using x86 static Paho")
@ -27,53 +25,28 @@ else()
endif() endif()
# ======== executable ======== # ======== executable ========
add_executable(remote_ctrl_node add_executable(remote_ctrl_node src/remote_ctrl_node.cpp src/config.cpp)
src/remote_ctrl_node.cpp
src/config.cpp
)
target_compile_options(remote_ctrl_node PRIVATE -Wno-pedantic) target_compile_options(remote_ctrl_node PRIVATE -Wno-pedantic)
target_include_directories(remote_ctrl_node target_include_directories(remote_ctrl_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
)
target_include_directories(remote_ctrl_node target_include_directories(remote_ctrl_node SYSTEM PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include/paho_mqtt_3c)
SYSTEM PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include/paho_mqtt_3c
)
target_link_libraries(remote_ctrl_node target_link_libraries(
remote_ctrl_node
${PAHO_LIB} ${PAHO_LIB}
pthread pthread
dl dl
ssl ssl
crypto crypto)
)
ament_target_dependencies(remote_ctrl_node ament_target_dependencies(remote_ctrl_node rclcpp sweeper_interfaces ament_index_cpp)
rclcpp
sweeper_interfaces
ament_index_cpp
)
# ===== install config ini =====
install(
FILES config/config.ini
DESTINATION share/${PROJECT_NAME}/config
)
# ===== install executable ===== # ===== install executable =====
install( install(TARGETS remote_ctrl_node DESTINATION lib/${PROJECT_NAME})
TARGETS remote_ctrl_node
DESTINATION lib/${PROJECT_NAME}
)
# ===== export include ===== # ===== export include =====
install( install(DIRECTORY include/ DESTINATION include)
DIRECTORY include/
DESTINATION include
)
ament_package() ament_package()

6
src/control/remote_ctrl/config/config.ini
View File

@ -1,6 +0,0 @@
[MQTT]
host = 36.153.162.171
port = 19683
username = zxwl
password = zxwl1234@
vehicle_id = V060003

13
src/control/remote_ctrl/include/remote_ctrl/config.hpp
View File

@ -3,14 +3,15 @@
struct Config struct Config
{ {
std::string host = "127.0.0.1"; std::string mqtt_ip;
int port = 1883; int mqtt_port = 1883;
std::string username; std::string mqtt_username;
std::string password; std::string mqtt_password;
std::string vehicle_id = "DEFAULT";
std::string remote_topic_template;
}; };
namespace config namespace config
{ {
bool load(const std::string &path, Config &cfg); bool load(const std::string& path, Config& cfg);
} }

96
src/control/remote_ctrl/src/config.cpp
View File

@ -1,74 +1,40 @@
#include "remote_ctrl/config.hpp" #include "remote_ctrl/config.hpp"
#include <fstream> #include <fstream>
#include <string> #include <iostream>
#include <algorithm> #include <nlohmann/json.hpp>
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"); std::ifstream ifs(path);
if (start == std::string::npos) if (!ifs.is_open())
return ""; {
auto end = s.find_last_not_of(" \t\r\n"); std::cerr << "Failed to open config file: " << path << std::endl;
return s.substr(start, end - start + 1); return false;
} }
}
bool config::load(const std::string &path, Config &cfg) json j;
{ ifs >> j;
std::ifstream ifs(path);
if (!ifs.is_open()) const auto& mqtt = j.at("mqtt");
cfg.mqtt_ip = mqtt.at("external_net_address").get<std::string>();
cfg.mqtt_port = mqtt.at("external_net_port").get<int>();
cfg.mqtt_username = mqtt.at("username").get<std::string>();
cfg.mqtt_password = mqtt.at("password").get<std::string>();
// remote_ctrl 用这个
cfg.remote_topic_template = mqtt.at("remote_topic").get<std::string>();
return true;
}
catch (const std::exception& e)
{
std::cerr << "Error parsing remote_ctrl config: " << e.what() << std::endl;
return false; 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;
} }

369
src/control/remote_ctrl/src/remote_ctrl_node.cpp
View File

@ -1,29 +1,30 @@
#include <rclcpp/rclcpp.hpp> #include <unistd.h>
#include <algorithm>
#include <cmath>
#include <mutex> #include <mutex>
#include <random> #include <random>
#include <rclcpp/rclcpp.hpp>
#include <sstream> #include <sstream>
#include <unistd.h>
#include <algorithm> // for std::clamp
#include <cmath> // for fabs
#include <ament_index_cpp/get_package_share_directory.hpp>
#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 "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; using json = nlohmann::json;
namespace sweeperMsg = sweeper_interfaces::msg; namespace sweeperMsg = sweeper_interfaces::msg;
using namespace std::chrono_literals; using namespace std::chrono_literals;
// 一些转速/角速度常量（目前你只是保留，逻辑在 onMqttMessage 里） // ===========================================================
// 常量
// ===========================================================
constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f; constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f;
constexpr float GEAR_RATIO = 16.5f; constexpr float GEAR_RATIO = 16.5f;
constexpr float DELTA_T = 0.02f; constexpr float DELTA_T = 0.02f;
// MQTT client ID 前缀
#define MQTT_CLIENT_PREFIX "remote_ctrl" #define MQTT_CLIENT_PREFIX "remote_ctrl"
static std::string makeRandomClientId() static std::string makeRandomClientId()
@ -33,14 +34,13 @@ static std::string makeRandomClientId()
std::uniform_int_distribution<uint32_t> dist(0, 0xFFFFFF); std::uniform_int_distribution<uint32_t> dist(0, 0xFFFFFF);
std::stringstream ss; std::stringstream ss;
ss << MQTT_CLIENT_PREFIX ss << MQTT_CLIENT_PREFIX << "_" << getpid() << "_" << std::hex << dist(gen);
<< "_" << getpid()
<< "_" << std::hex << dist(gen);
return ss.str(); return ss.str();
} }
// ===========================================================
// 控制状态 // 控制状态
// ===========================================================
struct RemoteCtrlState struct RemoteCtrlState
{ {
int gear = 0; int gear = 0;
@ -51,136 +51,134 @@ struct RemoteCtrlState
bool sweep = false; bool sweep = false;
}; };
// ===========================================================
// 主节点
// ===========================================================
class RemoteCtrlNode : public rclcpp::Node class RemoteCtrlNode : public rclcpp::Node
{ {
public: public:
RemoteCtrlNode() explicit RemoteCtrlNode(const Config& cfg) : Node("remote_ctrl_node"), cfg_(cfg)
: Node("remote_ctrl_node")
{ {
// 发布话题：/remote_mc_ctrl // 发布话题：/remote_mc_ctrl
pub_ = this->create_publisher<sweeperMsg::McCtrl>("/remote_mc_ctrl", 10); pub_ = this->create_publisher<sweeperMsg::McCtrl>("/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 客户端 ===== // ===== 构造 MQTT 客户端 =====
std::stringstream uri; std::stringstream uri;
uri << "tcp://" << cfg.host << ":" << cfg.port; uri << "tcp://" << cfg_.mqtt_ip << ":" << cfg_.mqtt_port;
std::string client_id = makeRandomClientId(); std::string client_id = makeRandomClientId();
mqtt_ = std::make_unique<MQTTClientWrapper>( mqtt_ = std::make_unique<MQTTClientWrapper>(uri.str(), client_id, cfg_.mqtt_username, cfg_.mqtt_password);
uri.str(),
client_id,
cfg.username,
cfg.password);
// 消息回调 // 消息回调
mqtt_->setMessageCallback( mqtt_->setMessageCallback([this](const std::string& /*topic*/, const std::string& payload)
[this](const std::string & /*topic*/, const std::string &payload) { this->onMqttMessage(payload); });
{
this->onMqttMessage(payload);
});
// 连接成功回调：做订阅 // 连接成功回调：做订阅
std::string topic = "/zxwl/vehicle/" + cfg.vehicle_id + "/ctrl";
mqtt_->setConnectedCallback( mqtt_->setConnectedCallback(
[this, topic]() [this]()
{ {
RCLCPP_INFO(this->get_logger(), // 重连后允许重新订阅
"[REMOTE] MQTT connected, subscribe: %s", subscribed_.store(false, std::memory_order_relaxed);
topic.c_str());
mqtt_->subscribe(topic, 1); if (!identity_ready_.load())
{
RCLCPP_WARN(this->get_logger(), "[REMOTE] MQTT connected but identity not ready");
return;
}
try_subscribe_ctrl_topic();
}); });
// 主动连一次，后面内部 loop 线程也会自动尝试重连 // 主动连一次，后面内部 loop 线程也会自动尝试重连
mqtt_->connect(); mqtt_->connect();
RCLCPP_INFO(this->get_logger(), identity_sub_ = this->create_subscription<sweeper_interfaces::msg::VehicleIdentity>(
"RemoteCtrlNode started mqtt=%s topic=%s client_id=%s", "/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(),
uri.str().c_str(), [this](const sweeper_interfaces::msg::VehicleIdentity::SharedPtr msg)
topic.c_str(), {
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()); client_id.c_str());
// ===== 看门狗 & 定时发布 ===== // ===== 看门狗 & 定时发布 =====
last_msg_time_ = std::chrono::steady_clock::now(); last_msg_time_ = std::chrono::steady_clock::now();
// 看门狗：800ms 内没收到新 remote 指令则停控 // 看门狗：800ms 内没收到新 remote 指令则停控
watchdog_ = this->create_wall_timer( watchdog_ = this->create_wall_timer(100ms,
100ms, [this]()
[this]() {
{ // 未授权远控，watchdog 什么都不做
// 未授权远控，watchdog 什么都不做 if (!remote_authorized_.load(std::memory_order_acquire)) return;
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 // 超时阈值：建议 800ms ~ 1500ms
if (now - last_msg_time_ > 800ms) if (now - last_msg_time_ > 800ms)
{ {
// 只在第一次进入“失活”时打印日志 // 只在第一次进入“失活”时打印日志
if (remote_alive_.exchange(false)) if (remote_alive_.exchange(false))
{ {
RCLCPP_WARN(this->get_logger(), RCLCPP_WARN(this->get_logger(),
"[REMOTE] control timeout, enter safe-stop"); "[REMOTE] control timeout, enter safe-stop");
} }
} }
}); });
// 周期发布到 /remote_mc_ctrl // 周期发布到 /remote_mc_ctrl
timer_ = this->create_wall_timer( timer_ = this->create_wall_timer(20ms, // 50Hz
20ms, // 50Hz [this]()
[this]() {
{ // 未授权远控：完全不发
// 未授权远控：完全不发 if (!remote_authorized_.load(std::memory_order_acquire)) return;
if (!remote_authorized_.load(std::memory_order_acquire))
return;
sweeperMsg::McCtrl msg; sweeperMsg::McCtrl msg;
{ {
std::lock_guard<std::mutex> lock(state_mtx_); std::lock_guard<std::mutex> lock(state_mtx_);
// 已授权但超时：安全停车态 // 已授权但超时：安全停车态
if (!remote_alive_.load(std::memory_order_relaxed)) if (!remote_alive_.load(std::memory_order_relaxed))
{ {
msg.gear = desired_.gear; // 通常保持挡位 msg.gear = desired_.gear; // 通常保持挡位
msg.brake = 1; msg.brake = 1;
msg.rpm = 0; msg.rpm = 0;
msg.angle = 0.0f; msg.angle = 0.0f;
msg.angle_speed = 120; msg.angle_speed = 120;
msg.sweep = false; msg.sweep = false;
} }
// 正常远控 // 正常远控
else else
{ {
msg.gear = desired_.gear; msg.gear = desired_.gear;
msg.brake = desired_.brake; msg.brake = desired_.brake;
msg.rpm = desired_.rpm; msg.rpm = desired_.rpm;
msg.angle = desired_.angle; msg.angle = desired_.angle;
msg.angle_speed = desired_.angle_speed; msg.angle_speed = desired_.angle_speed;
msg.sweep = desired_.sweep; msg.sweep = desired_.sweep;
} }
} }
pub_->publish(msg); pub_->publish(msg);
}); });
// CAN 反馈订阅，用于计算当前转向角 // CAN 反馈订阅，用于计算当前转向角
can_sub_ = this->create_subscription<sweeperMsg::CanFrame>( can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
@ -189,8 +187,7 @@ public:
{ {
if (msg->id == 0x401 && msg->dlc >= 5) if (msg->id == 0x401 && msg->dlc >= 5)
{ {
uint16_t raw = (static_cast<uint16_t>(msg->data[3]) << 8) | uint16_t raw = (static_cast<uint16_t>(msg->data[3]) << 8) | msg->data[4];
msg->data[4];
// 保持与 SBUS 侧一致 // 保持与 SBUS 侧一致
current_feedback_angle_ = (raw - 1024) / 7.0f; current_feedback_angle_ = (raw - 1024) / 7.0f;
} }
@ -206,8 +203,21 @@ public:
} }
} }
private: private:
void onMqttMessage(const std::string &payload) 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<std::mutex> lock(state_mtx_); std::lock_guard<std::mutex> lock(state_mtx_);
@ -217,28 +227,23 @@ private:
bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed); bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed);
if (was_dead) if (was_dead)
{ {
RCLCPP_INFO(this->get_logger(), RCLCPP_INFO(this->get_logger(), "[REMOTE] control recovered");
"[REMOTE] control recovered");
} }
last_msg_time_ = std::chrono::steady_clock::now(); last_msg_time_ = std::chrono::steady_clock::now();
RCLCPP_DEBUG(this->get_logger(), RCLCPP_DEBUG(this->get_logger(), "[REMOTE] recv MQTT payload: %s", payload.c_str());
"[REMOTE] recv MQTT payload: %s",
payload.c_str());
try try
{ {
auto j = json::parse(payload); auto j = json::parse(payload);
if (!j.contains("data")) if (!j.contains("data")) return;
return;
const auto &data = j["data"]; const auto& data = j["data"];
if (!data.contains("command") || !data.contains("value")) if (!data.contains("command") || !data.contains("value")) return;
return;
const std::string cmd = data["command"].get<std::string>(); const std::string cmd = data["command"].get<std::string>();
const auto &value = data["value"]; const auto& value = data["value"];
// ===================================================== // =====================================================
// 2. mode 指令：唯一的“授权 / 取消授权”入口 // 2. mode 指令：唯一的“授权 / 取消授权”入口
@ -253,8 +258,7 @@ private:
remote_authorized_.store(true, std::memory_order_release); remote_authorized_.store(true, std::memory_order_release);
remote_alive_.store(true, std::memory_order_relaxed); remote_alive_.store(true, std::memory_order_relaxed);
RCLCPP_INFO(this->get_logger(), RCLCPP_INFO(this->get_logger(), "[REMOTE] authorized (mode=3)");
"[REMOTE] authorized (mode=3)");
} }
else if (mode == 0) else if (mode == 0)
{ {
@ -265,8 +269,7 @@ private:
// 明确清空目标状态 // 明确清空目标状态
desired_ = {}; desired_ = {};
RCLCPP_INFO(this->get_logger(), RCLCPP_INFO(this->get_logger(), "[REMOTE] deauthorized (mode=0)");
"[REMOTE] deauthorized (mode=0)");
} }
// mode 指令处理完直接返回 // mode 指令处理完直接返回
@ -276,8 +279,7 @@ private:
// ===================================================== // =====================================================
// 3. 非 mode 指令：只有在“已授权”状态下才处理 // 3. 非 mode 指令：只有在“已授权”状态下才处理
// ===================================================== // =====================================================
if (!remote_authorized_.load(std::memory_order_acquire)) if (!remote_authorized_.load(std::memory_order_acquire)) return;
return;
// ===================================================== // =====================================================
// 4. drive：油门 / 刹车 / 转向 // 4. drive：油门 / 刹车 / 转向
@ -290,11 +292,7 @@ private:
if (value.is_string()) if (value.is_string())
{ {
sscanf(value.get<std::string>().c_str(), sscanf(value.get<std::string>().c_str(), "%d,%d,%d", &throttle, &brake, &steer_raw);
"%d,%d,%d",
&throttle,
&brake,
&steer_raw);
} }
else else
{ {
@ -313,22 +311,16 @@ private:
desired_.brake = 0; desired_.brake = 0;
// 油门映射：0~65535 -> 0~6000 rpm // 油门映射：0~65535 -> 0~6000 rpm
desired_.rpm = desired_.rpm = (static_cast<uint32_t>(throttle) * 6000U) / 65535U;
(static_cast<uint32_t>(throttle) * 6000U) / 65535U;
// 转向映射：0~65535 -> -50~+50 度 // 转向映射：0~65535 -> -50~+50 度
float target_angle = float target_angle = (static_cast<float>(steer_raw) * 100.0f / 65535.0f) - 50.0f;
(static_cast<float>(steer_raw) * 100.0f / 65535.0f) - 50.0f;
float delta = target_angle - current_feedback_angle_; float delta = target_angle - current_feedback_angle_;
float angle_speed_deg = std::fabs(delta) / DELTA_T; float angle_speed_deg = std::fabs(delta) / DELTA_T;
float motor_rpm = float motor_rpm = angle_speed_deg * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
angle_speed_deg * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
desired_.angle_speed = std::clamp<uint16_t>( desired_.angle_speed = std::clamp<uint16_t>(static_cast<uint16_t>(motor_rpm), 120, 1500);
static_cast<uint16_t>(motor_rpm),
120,
1500);
desired_.angle = target_angle; desired_.angle = target_angle;
} }
@ -341,18 +333,18 @@ private:
int gear = value.get<int>(); int gear = value.get<int>();
switch (gear) switch (gear)
{ {
case 0: case 0:
desired_.gear = 0; // N desired_.gear = 0; // N
break; break;
case 1: case 1:
desired_.gear = 2; // D desired_.gear = 2; // D
break; break;
case 2: case 2:
desired_.gear = 1; // R desired_.gear = 1; // R
break; break;
default: default:
desired_.gear = 0; desired_.gear = 0;
break; break;
} }
} }
// ===================================================== // =====================================================
@ -363,15 +355,13 @@ private:
desired_.sweep = (value.get<int>() != 0); desired_.sweep = (value.get<int>() != 0);
} }
} }
catch (const std::exception &e) catch (const std::exception& e)
{ {
RCLCPP_WARN(this->get_logger(), RCLCPP_WARN(this->get_logger(), "[REMOTE] JSON parse error: %s", e.what());
"[REMOTE] JSON parse error: %s",
e.what());
} }
} }
private: private:
RemoteCtrlState desired_; RemoteCtrlState desired_;
std::mutex state_mtx_; std::mutex state_mtx_;
@ -382,24 +372,63 @@ private:
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr can_sub_; rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr can_sub_;
std::atomic<bool> remote_authorized_{false}; // 是否允许远控（锁存） std::atomic<bool> remote_authorized_{false}; // 是否允许远控（锁存）
std::atomic<bool> remote_alive_{false}; // 是否活跃（心跳） std::atomic<bool> remote_alive_{false}; // 是否活跃（心跳）
std::chrono::steady_clock::time_point last_msg_time_; std::chrono::steady_clock::time_point last_msg_time_;
float current_feedback_angle_{0.0f}; float current_feedback_angle_{0.0f};
// ===== identity =====
Config cfg_;
std::atomic<bool> subscribed_{false};
std::string vid_;
std::atomic<bool> identity_ready_{false};
rclcpp::Subscription<sweeper_interfaces::msg::VehicleIdentity>::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); rclcpp::init(argc, argv);
auto node = std::make_shared<RemoteCtrlNode>(); // ===============================
// 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<RemoteCtrlNode>(cfg);
rclcpp::spin(node); rclcpp::spin(node);
// 这里 reset 会触发 RemoteCtrlNode 析构 → 停止 MQTT 线程
node.reset(); node.reset();
rclcpp::shutdown(); rclcpp::shutdown();
return 0; return 0;
} }