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 2026-01-22 15:54:01 +08:00
parent 71e63419b8
commit ae9b7c1704
1 changed files with 108 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -180,32 +180,31 @@ class RemoteCtrlNode : public rclcpp::Node
void publishMcCtrlMsg()
{
// 调试日志：显示当前授权状态
bool authorized = remote_authorized_.load(std::memory_order_acquire);
bool alive = remote_alive_.load(std::memory_order_relaxed);
LOG_DEBUG("[REMOTE] publishMcCtrlMsg: authorized=%d, alive=%d", authorized, alive);
// 未授权时：完全不发
if (!remote_authorized_.load(std::memory_order_acquire))
if (!authorized)
{
LOG_WARN("[REMOTE] NOT AUTHORIZED - will NOT publish message");
return;
}
// 已授权但超时：完全不发
if (!alive)
{
LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - will NOT publish message");
return;
}
sweeperMsg::McCtrl msg;
{
std::lock_guard<std::mutex> lock(state_mtx_);
// 已授权但超时：安全停车状态
if (!remote_alive_.load(std::memory_order_relaxed))
{
LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - sending safe-stop message");
msg.gear = desired_.gear;
msg.brake = 1;
msg.rpm = 0;
msg.angle = 0.0f;
msg.angle_speed = 120;
msg.sweep = false;
}
// 正常远控
else
{
LOG_INFO("[REMOTE] REMOTE ALIVE=TRUE - sending normal control message");
msg.gear = desired_.gear;
msg.brake = desired_.brake;
@ -214,18 +213,16 @@ class RemoteCtrlNode : public rclcpp::Node
msg.angle_speed = desired_.angle_speed;
msg.sweep = desired_.sweep;
}
}
LOG_INFO("[REMOTE] Publishing McCtrl: gear=%d brake=%d rpm=%d angle=%.1f angle_speed=%u sweep=%d", msg.gear,
msg.brake, msg.rpm, msg.angle, msg.angle_speed, msg.sweep);
pub_->publish(msg);
LOG_DEBUG("[REMOTE] Message published successfully");
}
void onMqttMessage(const std::string& payload)
{
std::lock_guard<std::mutex> lock(state_mtx_);
try
{
auto j = json::parse(payload);
@ -254,6 +251,8 @@ class RemoteCtrlNode : public rclcpp::Node
{
int mode = value.get<int>();
{
std::lock_guard<std::mutex> lock(state_mtx_);
if (mode == 3)
{
remote_authorized_.store(true, std::memory_order_release);
@ -265,6 +264,7 @@ class RemoteCtrlNode : public rclcpp::Node
remote_alive_.store(false, std::memory_order_relaxed);
desired_ = {};
}
}
publishMcCtrlMsg();
return;
@ -290,6 +290,11 @@ class RemoteCtrlNode : public rclcpp::Node
LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str());
// =====================================================
// 处理具体指令
// =====================================================
{
std::lock_guard<std::mutex> lock(state_mtx_);
// =====================================================
// drive 指令
// =====================================================
@ -369,6 +374,7 @@ class RemoteCtrlNode : public rclcpp::Node
LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str());
return;
}
}
publishMcCtrlMsg();
}