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

修改远控看门狗逻辑

Browse Source
This commit is contained in:
lyq 2025-12-30 11:25:32 +08:00
parent 3ce0c0b16e
commit 673a5e4adc
3 changed files with 231 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,6 +1,6 @@
[MQTT]
host = 192.168.4.150
port = 1883
host = 36.153.162.171
port = 61883
username = zxwl
password = zxwl1234@
vehicle_id = V090001
vehicle_id = V090001

72
src/remote_ctrl/include/remote_ctrl/mqtt_client.hpp
View File

@ -81,6 +81,7 @@ public:
msg_cb_ = std::move(cb);
}
// 注意：connected_cb_ 在 MQTT 网络线程中调用，禁止阻塞
void setConnectedCallback(ConnectedCallback cb)
{
std::lock_guard<std::mutex> lk(mtx_);
@ -90,7 +91,10 @@ public:
bool subscribe(const std::string &topic, int qos = 0)
{
std::lock_guard<std::mutex> lk(mtx_);
if (!client_ || !connected_)
if (!client_ ||
!connected_.load(std::memory_order_acquire) ||
!ready_.load(std::memory_order_acquire))
return false;
int rc = MQTTClient_subscribe(client_, topic.c_str(), qos);
@ -109,7 +113,10 @@ public:
int qos = 0)
{
std::lock_guard<std::mutex> lk(mtx_);
if (!client_ || !connected_)
if (!client_ ||
!connected_.load(std::memory_order_acquire) ||
!ready_.load(std::memory_order_acquire))
return false;
MQTTClient_message msg = MQTTClient_message_initializer;
@ -122,7 +129,8 @@ public:
if (rc != MQTTCLIENT_SUCCESS)
{
connected_ = false;
connected_.store(false, std::memory_order_release);
ready_.store(false, std::memory_order_release);
return false;
}
@ -153,20 +161,27 @@ private:
int rc = MQTTClient_connect(client_, &opts);
if (rc == MQTTCLIENT_SUCCESS)
{
connected_ = true;
if (connected_cb_)
connected_cb_();
connected_.store(true, std::memory_order_release);
ready_.store(false, std::memory_order_release);
// ⭐ 标记：需要在 loop 线程中触发 connected_cb_
need_callback_.store(true, std::memory_order_release);
return true;
}
connected_ = false;
connected_.store(false, std::memory_order_release);
ready_.store(false, std::memory_order_release);
return false;
}
static void connLostCB(void *ctx, char *cause)
{
auto self = static_cast<MQTTClientWrapper *>(ctx);
self->connected_ = false;
self->connected_.store(false, std::memory_order_release);
self->ready_.store(false, std::memory_order_release);
std::cerr << "[MQTT] lost connection: "
<< (cause ? cause : "unknown") << std::endl;
}
@ -193,19 +208,41 @@ private:
// 异步线程：负责 reconnect + yield
void loopFunc()
{
auto last_connect_try = std::chrono::steady_clock::now();
while (!stop_flag_)
{
if (!connected_)
// 1️⃣ 未连接：按重连间隔尝试连接
if (!connected_.load(std::memory_order_acquire))
{
doConnect();
auto now = std::chrono::steady_clock::now();
if (now - last_connect_try >= std::chrono::milliseconds(reconnect_ms_))
{
last_connect_try = now;
doConnect();
}
}
else
{
// 2️⃣ 已连接：处理网络事件
MQTTClient_yield();
// 3️⃣ 第一次 yield 后，才算真正 ready
if (!ready_.load(std::memory_order_acquire))
{
ready_.store(true, std::memory_order_release);
// ⭐ 在 ready 状态触发 connected callback
if (need_callback_.exchange(false))
{
if (connected_cb_)
connected_cb_();
}
}
}
// 关键：处理 MQTT 网络事件
MQTTClient_yield();
std::this_thread::sleep_for(
std::chrono::milliseconds(reconnect_ms_));
// ⭐ 正常网络轮询节奏
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
}
@ -223,4 +260,7 @@ private:
MessageCallback msg_cb_;
ConnectedCallback connected_cb_;
std::atomic<bool> ready_{false}; // 是否真正进入可用状态
std::atomic<bool> need_callback_{false};
};

255
src/remote_ctrl/src/remote_ctrl_node.cpp
View File

@ -118,22 +118,28 @@ public:
client_id.c_str());
// ===== 看门狗 & 定时发布 =====
remote_enabled_.store(false);
last_msg_time_ = std::chrono::steady_clock::now();
// 看门狗：300ms 内没收到新 remote 指令则停控
// 看门狗：800ms 内没收到新 remote 指令则停控
watchdog_ = this->create_wall_timer(
100ms,
[this]()
{
// 未授权远控，watchdog 什么都不做
if (!remote_authorized_.load(std::memory_order_acquire))
return;
auto now = std::chrono::steady_clock::now();
if (remote_enabled_.load() &&
(now - last_msg_time_ > 300ms))
// 超时阈值：建议 800ms ~ 1500ms
if (now - last_msg_time_ > 800ms)
{
RCLCPP_WARN(this->get_logger(),
"[REMOTE] timeout, disable remote");
remote_enabled_.store(false);
desired_ = {};
// 只在第一次进入“失活”时打印日志
if (remote_alive_.exchange(false))
{
RCLCPP_WARN(this->get_logger(),
"[REMOTE] control timeout, enter safe-stop");
}
}
});
@ -142,18 +148,35 @@ public:
20ms, // 50Hz
[this]()
{
if (!remote_enabled_.load())
// 未授权远控：完全不发
if (!remote_authorized_.load(std::memory_order_acquire))
return;
sweeperMsg::McCtrl msg;
{
std::lock_guard<std::mutex> lock(state_mtx_);
msg.gear = desired_.gear;
msg.rpm = desired_.rpm;
msg.brake = desired_.brake;
msg.angle = desired_.angle;
msg.sweep = desired_.sweep;
msg.angle_speed = desired_.angle_speed;
// 已授权但超时：安全停车态
if (!remote_alive_.load(std::memory_order_relaxed))
{
msg.gear = desired_.gear; // 通常保持挡位
msg.brake = 1;
msg.rpm = 0;
msg.angle = 0.0f;
msg.angle_speed = 120;
msg.sweep = false;
}
// 正常远控
else
{
msg.gear = desired_.gear;
msg.brake = desired_.brake;
msg.rpm = desired_.rpm;
msg.angle = desired_.angle;
msg.angle_speed = desired_.angle_speed;
msg.sweep = desired_.sweep;
}
}
pub_->publish(msg);
@ -187,11 +210,22 @@ private:
void onMqttMessage(const std::string &payload)
{
std::lock_guard<std::mutex> lock(state_mtx_);
// =====================================================
// 1. MQTT 消息到达：刷新活性 & 判断是否从超时中恢复
// =====================================================
bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed);
if (was_dead)
{
RCLCPP_INFO(this->get_logger(),
"[REMOTE] control recovered");
}
last_msg_time_ = std::chrono::steady_clock::now();
RCLCPP_INFO(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
{
@ -199,93 +233,145 @@ private:
if (!j.contains("data"))
return;
std::string cmd = j["data"]["command"].get<std::string>();
auto value = j["data"]["value"];
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"];
// =====================================================
// 2. mode 指令：唯一的“授权 / 取消授权”入口
// =====================================================
if (cmd == "mode")
{
int mode = value.get<int>();
if (mode == 3)
{
remote_enabled_.store(true);
RCLCPP_INFO(this->get_logger(), "[REMOTE] enabled");
// 进入远控授权态（锁存）
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)");
}
else if (mode == 0)
{
remote_enabled_.store(false);
// 退出远控授权态（锁存）
remote_authorized_.store(false, std::memory_order_release);
remote_alive_.store(false, std::memory_order_relaxed);
// 明确清空目标状态
desired_ = {};
RCLCPP_INFO(this->get_logger(), "[REMOTE] disabled");
RCLCPP_INFO(this->get_logger(),
"[REMOTE] deauthorized (mode=0)");
}
// mode 指令处理完直接返回
return;
}
// =====================================================
// 3. 非 mode 指令：只有在“已授权”状态下才处理
// =====================================================
if (!remote_authorized_.load(std::memory_order_acquire))
return;
// =====================================================
// 4. drive：油门 / 刹车 / 转向
// =====================================================
if (cmd == "drive")
{
int throttle = 0;
int brake = 1;
int steer_raw = 0;
if (value.is_string())
{
sscanf(value.get<std::string>().c_str(),
"%d,%d,%d",
&throttle,
&brake,
&steer_raw);
}
else
{
return;
}
if (brake > 0)
{
// 刹车优先
desired_.brake = 1;
desired_.rpm = 0;
desired_.angle = 0.0f;
}
else
{
desired_.brake = 0;
// 油门映射：0~65535 -> 0~6000 rpm
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 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;
desired_.angle_speed = std::clamp<uint16_t>(
static_cast<uint16_t>(motor_rpm),
120,
1500);
desired_.angle = target_angle;
}
}
else if (remote_enabled_.load())
// =====================================================
// 5. gear：挡位
// =====================================================
else if (cmd == "gear")
{
if (cmd == "drive")
int gear = value.get<int>();
switch (gear)
{
int t = 0, b = 1, s = 0;
sscanf(value.get<std::string>().c_str(),
"%d,%d,%d", &t, &b, &s);
if (b > 0)
{
desired_.brake = 1;
desired_.rpm = 0;
desired_.angle = 0;
}
else
{
desired_.brake = 0;
desired_.rpm = (uint32_t(t) * 6000) / 65535;
// s: 0~65535 -> -50~+50
float steer = (float(s) * 100.0f / 65535.0f) - 50.0f;
float target_angle = steer;
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;
uint16_t can_speed = std::clamp<uint16_t>(
static_cast<uint16_t>(motor_rpm),
120,
1500);
desired_.angle = target_angle;
desired_.angle_speed = can_speed;
}
}
else if (cmd == "gear")
{
int gear = value.get<int>();
switch (gear)
{
case 0:
desired_.gear = 0; // N
break;
case 1:
desired_.gear = 2; // D
break;
case 2:
desired_.gear = 1; // R
break;
default:
desired_.gear = 0;
break;
}
}
else if (cmd == "sweepCtrl")
{
desired_.sweep = (value.get<int>() != 0);
case 0:
desired_.gear = 0; // N
break;
case 1:
desired_.gear = 2; // D
break;
case 2:
desired_.gear = 1; // R
break;
default:
desired_.gear = 0;
break;
}
}
// =====================================================
// 6. sweepCtrl：清扫开关
// =====================================================
else if (cmd == "sweepCtrl")
{
desired_.sweep = (value.get<int>() != 0);
}
}
catch (...)
catch (const std::exception &e)
{
RCLCPP_WARN(this->get_logger(), "[REMOTE] invalid JSON");
RCLCPP_WARN(this->get_logger(),
"[REMOTE] JSON parse error: %s",
e.what());
}
}
private:
std::atomic<bool> remote_enabled_{false};
RemoteCtrlState desired_;
std::mutex state_mtx_;
@ -296,7 +382,10 @@ private:
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr can_sub_;
std::atomic<bool> remote_authorized_{false}; // 是否允许远控（锁存）
std::atomic<bool> remote_alive_{false}; // 是否活跃（心跳）
std::chrono::steady_clock::time_point last_msg_time_;
float current_feedback_angle_{0.0f};
};