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

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

@ -180,52 +180,49 @@ 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;
msg.rpm = desired_.rpm;
msg.angle = desired_.angle;
msg.angle_speed = desired_.angle_speed;
msg.sweep = desired_.sweep;
}
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);
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,16 +251,19 @@ class RemoteCtrlNode : public rclcpp::Node
{
int mode = value.get<int>();
if (mode == 3)
{
remote_authorized_.store(true, std::memory_order_release);
remote_alive_.store(true, std::memory_order_relaxed);
}
else if (mode == 0)
{
remote_authorized_.store(false, std::memory_order_release);
remote_alive_.store(false, std::memory_order_relaxed);
desired_ = {};
std::lock_guard<std::mutex> lock(state_mtx_);
if (mode == 3)
{
remote_authorized_.store(true, std::memory_order_release);
remote_alive_.store(true, std::memory_order_relaxed);
}
else if (mode == 0)
{
remote_authorized_.store(false, std::memory_order_release);
remote_alive_.store(false, std::memory_order_relaxed);
desired_ = {};
}
}
publishMcCtrlMsg();
@ -291,83 +291,89 @@ class RemoteCtrlNode : public rclcpp::Node
LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str());
// =====================================================
// drive 指令
// 处理具体指令
// =====================================================
if (cmd == "drive")
{
int throttle = 0;
int brake = 1;
int steer_raw = 0;
if (value.is_string())
std::lock_guard<std::mutex> lock(state_mtx_);
// =====================================================
// drive 指令
// =====================================================
if (cmd == "drive")
{
sscanf(value.get<std::string>().c_str(), "%d,%d,%d", &throttle, &brake, &steer_raw);
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
{
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;
}
else
{
desired_.brake = 0;
desired_.rpm = (static_cast<uint32_t>(throttle) * 6000U) / 65535U;
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;
}
}
// =====================================================
// gear 指令
// =====================================================
else if (cmd == "gear")
{
int gear = value.get<int>();
LOG_DEBUG("[REMOTE] gear command: %d", gear);
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;
}
}
// =====================================================
// sweepCtrl 指令
// =====================================================
else if (cmd == "sweepCtrl")
{
int val = value.get<int>();
desired_.sweep = (val != 0);
LOG_DEBUG("[REMOTE] sweepCtrl: %d", val);
}
else
{
LOG_WARN("[REMOTE] drive value is not string");
LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str());
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;
}
else
{
desired_.brake = 0;
desired_.rpm = (static_cast<uint32_t>(throttle) * 6000U) / 65535U;
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;
}
}
// =====================================================
// gear 指令
// =====================================================
else if (cmd == "gear")
{
int gear = value.get<int>();
LOG_DEBUG("[REMOTE] gear command: %d", gear);
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;
}
}
// =====================================================
// sweepCtrl 指令
// =====================================================
else if (cmd == "sweepCtrl")
{
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();