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

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
{
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);
}

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

@ -1,74 +1,40 @@
#include "remote_ctrl/config.hpp"
#include <fstream>
#include <string>
#include <algorithm>
#include <iostream>
#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");
if (start == std::string::npos)
return "";
auto end = s.find_last_not_of(" \t\r\n");
return s.substr(start, end - start + 1);
}
}
bool config::load(const std::string &path, Config &cfg)
{
std::ifstream ifs(path);
if (!ifs.is_open())
{
std::cerr << "Failed to open config file: " << path << 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;
json j;
ifs >> j;
auto eq = line.find('=');
if (eq == std::string::npos)
continue;
const auto& mqtt = j.at("mqtt");
std::string key = trim(line.substr(0, eq));
std::string val = trim(line.substr(eq + 1));
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>();
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;
}
}
// 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;
}
}

253
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 <random>
#include <rclcpp/rclcpp.hpp>
#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 "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<uint32_t> 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,83 +51,83 @@ 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<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 客户端 =====
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<MQTTClientWrapper>(
uri.str(),
client_id,
cfg.username,
cfg.password);
mqtt_ = std::make_unique<MQTTClientWrapper>(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<sweeper_interfaces::msg::VehicleIdentity>(
"/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,
watchdog_ = this->create_wall_timer(100ms,
[this]()
{
// 未授权远控，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();
@ -144,13 +144,11 @@ public:
});
// 周期发布到 /remote_mc_ctrl
timer_ = this->create_wall_timer(
20ms, // 50Hz
timer_ = this->create_wall_timer(20ms, // 50Hz
[this]()
{
// 未授权远控：完全不发
if (!remote_authorized_.load(std::memory_order_acquire))
return;
if (!remote_authorized_.load(std::memory_order_acquire)) return;
sweeperMsg::McCtrl msg;
@ -189,8 +187,7 @@ public:
{
if (msg->id == 0x401 && msg->dlc >= 5)
{
uint16_t raw = (static_cast<uint16_t>(msg->data[3]) << 8) |
msg->data[4];
uint16_t raw = (static_cast<uint16_t>(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<std::mutex> 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<std::string>();
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<std::string>().c_str(),
"%d,%d,%d",
&throttle,
&brake,
&steer_raw);
sscanf(value.get<std::string>().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<uint32_t>(throttle) * 6000U) / 65535U;
desired_.rpm = (static_cast<uint32_t>(throttle) * 6000U) / 65535U;
// 转向映射：0~65535 -> -50~+50 度
float target_angle =
(static_cast<float>(steer_raw) * 100.0f / 65535.0f) - 50.0f;
float target_angle = (static_cast<float>(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<uint16_t>(
static_cast<uint16_t>(motor_rpm),
120,
1500);
desired_.angle_speed = std::clamp<uint16_t>(static_cast<uint16_t>(motor_rpm), 120, 1500);
desired_.angle = target_angle;
}
@ -363,15 +355,13 @@ private:
desired_.sweep = (value.get<int>() != 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_;
@ -387,19 +377,58 @@ private:
std::chrono::steady_clock::time_point last_msg_time_;
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);
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);
// 这里 reset 会触发 RemoteCtrlNode 析构 → 停止 MQTT 线程
node.reset();
rclcpp::shutdown();
return 0;
}