修改远控逻辑
This commit is contained in:
parent
71e63419b8
commit
ae9b7c1704
@ -180,52 +180,49 @@ class RemoteCtrlNode : public rclcpp::Node
|
|||||||
|
|
||||||
void publishMcCtrlMsg()
|
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");
|
LOG_WARN("[REMOTE] NOT AUTHORIZED - will NOT publish message");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 已授权但超时：完全不发
|
||||||
|
if (!alive)
|
||||||
|
{
|
||||||
|
LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - will NOT publish message");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
sweeperMsg::McCtrl msg;
|
sweeperMsg::McCtrl msg;
|
||||||
|
|
||||||
{
|
{
|
||||||
std::lock_guard<std::mutex> lock(state_mtx_);
|
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;
|
||||||
LOG_INFO("[REMOTE] REMOTE ALIVE=TRUE - sending normal control message");
|
msg.brake = desired_.brake;
|
||||||
msg.gear = desired_.gear;
|
msg.rpm = desired_.rpm;
|
||||||
msg.brake = desired_.brake;
|
msg.angle = desired_.angle;
|
||||||
msg.rpm = desired_.rpm;
|
msg.angle_speed = desired_.angle_speed;
|
||||||
msg.angle = desired_.angle;
|
msg.sweep = desired_.sweep;
|
||||||
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,
|
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);
|
msg.brake, msg.rpm, msg.angle, msg.angle_speed, msg.sweep);
|
||||||
|
|
||||||
pub_->publish(msg);
|
pub_->publish(msg);
|
||||||
|
LOG_DEBUG("[REMOTE] Message published successfully");
|
||||||
}
|
}
|
||||||
|
|
||||||
void onMqttMessage(const std::string& payload)
|
void onMqttMessage(const std::string& payload)
|
||||||
{
|
{
|
||||||
std::lock_guard<std::mutex> lock(state_mtx_);
|
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
auto j = json::parse(payload);
|
auto j = json::parse(payload);
|
||||||
@ -254,16 +251,19 @@ class RemoteCtrlNode : public rclcpp::Node
|
|||||||
{
|
{
|
||||||
int mode = value.get<int>();
|
int mode = value.get<int>();
|
||||||
|
|
||||||
if (mode == 3)
|
|
||||||
{
|
{
|
||||||
remote_authorized_.store(true, std::memory_order_release);
|
std::lock_guard<std::mutex> lock(state_mtx_);
|
||||||
remote_alive_.store(true, std::memory_order_relaxed);
|
if (mode == 3)
|
||||||
}
|
{
|
||||||
else if (mode == 0)
|
remote_authorized_.store(true, std::memory_order_release);
|
||||||
{
|
remote_alive_.store(true, std::memory_order_relaxed);
|
||||||
remote_authorized_.store(false, std::memory_order_release);
|
}
|
||||||
remote_alive_.store(false, std::memory_order_relaxed);
|
else if (mode == 0)
|
||||||
desired_ = {};
|
{
|
||||||
|
remote_authorized_.store(false, std::memory_order_release);
|
||||||
|
remote_alive_.store(false, std::memory_order_relaxed);
|
||||||
|
desired_ = {};
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
publishMcCtrlMsg();
|
publishMcCtrlMsg();
|
||||||
@ -291,83 +291,89 @@ class RemoteCtrlNode : public rclcpp::Node
|
|||||||
LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str());
|
LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str());
|
||||||
|
|
||||||
// =====================================================
|
// =====================================================
|
||||||
// drive 指令
|
// 处理具体指令
|
||||||
// =====================================================
|
// =====================================================
|
||||||
if (cmd == "drive")
|
|
||||||
{
|
{
|
||||||
int throttle = 0;
|
std::lock_guard<std::mutex> lock(state_mtx_);
|
||||||
int brake = 1;
|
// =====================================================
|
||||||
int steer_raw = 0;
|
// drive 指令
|
||||||
|
// =====================================================
|
||||||
if (value.is_string())
|
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
|
else
|
||||||
{
|
{
|
||||||
LOG_WARN("[REMOTE] drive value is not string");
|
LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str());
|
||||||
return;
|
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();
|
publishMcCtrlMsg();
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user