修改远控逻辑
This commit is contained in:
parent
4e6b05eb3f
commit
098e67d798
@ -91,8 +91,7 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
try_subscribe_ctrl_topic();
|
||||
});
|
||||
|
||||
// 主动连一次，后面内部 loop 线程也会自动尝试重连
|
||||
mqtt_->connect();
|
||||
// MQTTClientWrapper内部已有reconnectLoop线程自动处理连接和重连，无需主动调用connect()
|
||||
|
||||
identity_sub_ = this->create_subscription<sweeper_interfaces::msg::VehicleIdentity>(
|
||||
"/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(),
|
||||
@ -119,7 +118,7 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
|
||||
LOG_INFO("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 指令则停控
|
||||
@ -142,43 +141,6 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
}
|
||||
});
|
||||
|
||||
// 周期发布到 /remote_mc_ctrl
|
||||
timer_ = this->create_wall_timer(20ms, // 50Hz
|
||||
[this]()
|
||||
{
|
||||
// 未授权远控：完全不发
|
||||
if (!remote_authorized_.load(std::memory_order_acquire)) return;
|
||||
|
||||
sweeperMsg::McCtrl msg;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(state_mtx_);
|
||||
|
||||
// 已授权但超时：安全停车态
|
||||
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);
|
||||
});
|
||||
|
||||
// CAN 反馈订阅，用于计算当前转向角
|
||||
can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
|
||||
"can_data", 10,
|
||||
@ -216,36 +178,77 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
mqtt_->subscribe(ctrl_topic_, 1);
|
||||
}
|
||||
|
||||
void publishMcCtrlMsg()
|
||||
{
|
||||
// 未授权时：完全不发
|
||||
if (!remote_authorized_.load(std::memory_order_acquire))
|
||||
{
|
||||
LOG_WARN("[REMOTE] NOT AUTHORIZED - 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;
|
||||
msg.rpm = desired_.rpm;
|
||||
msg.angle = desired_.angle;
|
||||
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);
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
LOG_INFO("[REMOTE] control recovered");
|
||||
}
|
||||
|
||||
last_msg_time_ = std::chrono::steady_clock::now();
|
||||
|
||||
LOG_DEBUG("[REMOTE] recv MQTT payload: %s", payload.c_str());
|
||||
|
||||
try
|
||||
{
|
||||
auto j = json::parse(payload);
|
||||
if (!j.contains("data")) return;
|
||||
if (!j.contains("data"))
|
||||
{
|
||||
LOG_WARN("[REMOTE] Invalid JSON: missing 'data' field, payload=%s", payload.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
const auto& data = j["data"];
|
||||
if (!data.contains("command") || !data.contains("value")) return;
|
||||
if (!data.contains("command") || !data.contains("value"))
|
||||
{
|
||||
LOG_WARN("[REMOTE] Invalid JSON: missing 'command' or 'value' field");
|
||||
return;
|
||||
}
|
||||
|
||||
const std::string cmd = data["command"].get<std::string>();
|
||||
const auto& value = data["value"];
|
||||
|
||||
LOG_INFO("[REMOTE] Received command: %s", cmd.c_str());
|
||||
|
||||
// =====================================================
|
||||
// 2. mode 指令：唯一的“授权 / 取消授权”入口
|
||||
// mode 指令：唯一的授权入口
|
||||
// =====================================================
|
||||
if (cmd == "mode")
|
||||
{
|
||||
@ -253,35 +256,42 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
|
||||
if (mode == 3)
|
||||
{
|
||||
// 进入远控授权态（锁存）
|
||||
remote_authorized_.store(true, std::memory_order_release);
|
||||
remote_alive_.store(true, std::memory_order_relaxed);
|
||||
|
||||
LOG_INFO("[REMOTE] authorized (mode=3)");
|
||||
}
|
||||
else if (mode == 0)
|
||||
{
|
||||
// 退出远控授权态（锁存）
|
||||
remote_authorized_.store(false, std::memory_order_release);
|
||||
remote_alive_.store(false, std::memory_order_relaxed);
|
||||
|
||||
// 明确清空目标状态
|
||||
desired_ = {};
|
||||
|
||||
LOG_INFO("[REMOTE] deauthorized (mode=0)");
|
||||
}
|
||||
|
||||
// mode 指令处理完直接返回
|
||||
publishMcCtrlMsg();
|
||||
return;
|
||||
}
|
||||
|
||||
// =====================================================
|
||||
// 3. 非 mode 指令：只有在“已授权”状态下才处理
|
||||
// 非 mode 指令：检查授权
|
||||
// =====================================================
|
||||
if (!remote_authorized_.load(std::memory_order_acquire)) return;
|
||||
if (!remote_authorized_.load(std::memory_order_acquire))
|
||||
{
|
||||
LOG_WARN("[REMOTE] Command '%s' received but NOT AUTHORIZED - will not process", cmd.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
// 刷新活性
|
||||
bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed);
|
||||
if (was_dead)
|
||||
{
|
||||
LOG_INFO("[REMOTE] ✓ control recovered");
|
||||
}
|
||||
|
||||
last_msg_time_ = std::chrono::steady_clock::now();
|
||||
|
||||
LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str());
|
||||
|
||||
// =====================================================
|
||||
// 4. drive：油门 / 刹车 / 转向
|
||||
// drive 指令
|
||||
// =====================================================
|
||||
if (cmd == "drive")
|
||||
{
|
||||
@ -295,12 +305,14 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG_WARN("[REMOTE] drive value is not string");
|
||||
return;
|
||||
}
|
||||
|
||||
LOG_DEBUG("[REMOTE] drive: throttle=%d brake=%d steer=%d", throttle, brake, steer_raw);
|
||||
|
||||
if (brake > 0)
|
||||
{
|
||||
// 刹车优先
|
||||
desired_.brake = 1;
|
||||
desired_.rpm = 0;
|
||||
desired_.angle = 0.0f;
|
||||
@ -308,28 +320,25 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
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;
|
||||
}
|
||||
}
|
||||
// =====================================================
|
||||
// 5. gear：挡位
|
||||
// gear 指令
|
||||
// =====================================================
|
||||
else if (cmd == "gear")
|
||||
{
|
||||
int gear = value.get<int>();
|
||||
LOG_DEBUG("[REMOTE] gear command: %d", gear);
|
||||
|
||||
switch (gear)
|
||||
{
|
||||
case 0:
|
||||
@ -347,16 +356,26 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
}
|
||||
}
|
||||
// =====================================================
|
||||
// 6. sweepCtrl：清扫开关
|
||||
// sweepCtrl 指令
|
||||
// =====================================================
|
||||
else if (cmd == "sweepCtrl")
|
||||
{
|
||||
desired_.sweep = (value.get<int>() != 0);
|
||||
int val = value.get<int>();
|
||||
desired_.sweep = (val != 0);
|
||||
LOG_DEBUG("[REMOTE] sweepCtrl: %d", val);
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
publishMcCtrlMsg();
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
LOG_WARN("[REMOTE] JSON parse error: %s", e.what());
|
||||
LOG_ERROR("[REMOTE] Exception in onMqttMessage: %s, payload=%s", e.what(), payload.c_str());
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user