From 3ba59b157bab2e60e71efb2fd67d937b61efd63f Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Fri, 3 Apr 2026 09:44:26 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B8=85=E6=89=AB=E9=83=A8=E4=BB=B6=E5=88=86?= =?UTF-8?q?=E5=BC=80=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/autonomy/fu/src/fu_node.cpp | 18 ++++-- src/base/mc/src/control_cache.cpp | 9 +++ src/base/mc/src/timer_tasks.cpp | 63 +++++++++++-------- src/base/sweeper_interfaces/msg/McCtrl.msg | 11 +++- src/communication/sub/src/control_mapper.cpp | 9 +++ .../can_radio_ctrl/src/can_radio_ctrl.cpp | 21 ++++++- src/control/ctrl_arbiter/src/ctrl_arbiter.cpp | 8 +++ .../remote_ctrl/src/remote_ctrl_node.cpp | 15 +++++ 8 files changed, 119 insertions(+), 35 deletions(-) diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index 3a3e137..c7e784b 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -350,7 +350,8 @@ class fu_node : public rclcpp::Node // ======== 任务消息回调 ======== void task_callback(const sweeper_interfaces::msg::Task::SharedPtr msg) { - if (msg->task_id > 0) { + if (msg->task_id > 0) + { current_task_mode = msg->mode; LOG_INFO("任务模式更新为: %d (0=标准模式, 1=混合模式)", current_task_mode); } @@ -1059,8 +1060,7 @@ class fu_node : public rclcpp::Node bool rtk_signal_good = (g_rtk.reliability == 1 && time_since_last_rtk <= rtk_timeout); if (state_machine_->getCurrentState() == StateMachine::WAITING && - !state_machine_->obstacle_analysis.has_front_critical && - rtk_signal_good) + !state_machine_->obstacle_analysis.has_front_critical && rtk_signal_good) { state_machine_->setState(StateMachine::NORMAL); LOG_INFO("[状态恢复] 障碍物已消失且RTK信号良好，恢复正常行驶状态"); @@ -1104,8 +1104,16 @@ class fu_node : public rclcpp::Node void publishControlCommand(bool emergency_brake = false) { auto message = sweeper_interfaces::msg::McCtrl(); - message.sweep = true; - message.sweep_mode = current_task_mode; + + message.enable_main_brush = true; + message.enable_vacuum = true; + message.enable_dust_shake = false; + message.enable_main_brush_pole = true; + message.enable_flap_pole = true; + message.enable_side_brush = true; + message.sweep_mode = current_task_mode; // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水) + message.enable_water_pump = (message.sweep_mode == 1); + message.brake = emergency_brake ? 1 : 0; // 紧急停车时启用刹车 message.gear = 2; message.angle_speed = 800; diff --git a/src/base/mc/src/control_cache.cpp b/src/base/mc/src/control_cache.cpp index 1d24549..5a9e01c 100644 --- a/src/base/mc/src/control_cache.cpp +++ b/src/base/mc/src/control_cache.cpp @@ -35,6 +35,15 @@ sweeper_interfaces::msg::McCtrl get_safe_control() msg.sweep = false; msg.sweep_mode = 0; // 默认标准模式 + + // 各清扫部件独立控制默认值 + msg.enable_main_brush = true; + msg.enable_vacuum = true; + msg.enable_dust_shake = false; + msg.enable_main_brush_pole = true; + msg.enable_flap_pole = true; + msg.enable_side_brush = true; + msg.enable_water_pump = true; } return msg; } diff --git a/src/base/mc/src/timer_tasks.cpp b/src/base/mc/src/timer_tasks.cpp index 2fb3254..5c5f0ea 100644 --- a/src/base/mc/src/timer_tasks.cpp +++ b/src/base/mc/src/timer_tasks.cpp @@ -36,44 +36,55 @@ void vcuTimerTask(CANDriver& canctl) { auto msg = get_safe_control(); - // 根据清扫状态设置所有部件 - int8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止 - int8_t target_value2 = msg.sweep ? -100 : 100; // -100表示下沉，100表示抬升 + // 基础值定义 + int8_t start_value = 100; // 100表示启动 + int8_t stop_value = 0; // 0表示停止 + int8_t lower_value = -100; // -100表示下沉/下降 + int8_t lift_value = 100; // 100表示抬升 // 发送CAN使能指令 vcu_enable_cmd.setCANEnable(true); canctl.sendFrame(vcu_enable_cmd.toFrame()); - // mode: 0=标准模式(刷子), 1=混合模式(刷子+洒水) - bool enable_brush = true; // 两种模式都启用刷子 - // bool enable_water = msg.sweep && (msg.sweep_mode == 1); // 仅混合模式启用洒水 - bool enable_water = true; + // 决定各部件的控制状态 + bool main_brush_enabled = msg.enable_main_brush; + bool vacuum_enabled = msg.enable_vacuum; + bool dust_shake_enabled = msg.enable_dust_shake; + bool main_brush_pole_enabled = msg.enable_main_brush_pole; + bool flap_pole_enabled = msg.enable_flap_pole; + bool side_brush_enabled = msg.enable_side_brush; + bool water_pump_enabled = msg.enable_water_pump; // ===== 控制0x211报文 (电机M1-M7) ===== - vcu_motor1_cmd.setByte0(msg.sweep ? 1 : 0); // 电机M1方向 1表示正向运行，0表示停止 - vcu_motor1_cmd.setByte1(enable_brush ? target_value : 0); //电机M1油门 - vcu_motor1_cmd.setByte2(enable_brush ? target_value : 0);//吸尘电机 - vcu_motor1_cmd.setByte3(0);//振尘电机 - vcu_motor1_cmd.setByte4(enable_brush ? target_value2 : 0);//主刷推杆-100 100 - vcu_motor1_cmd.setByte5(enable_brush ? target_value2 : 0);//前挡皮推杆电机-100 100 - vcu_motor1_cmd.setByte6(enable_brush ? target_value : 0);//边刷电机 - vcu_motor1_cmd.setByte7(enable_brush ? target_value : 0);//边刷电机 + vcu_motor1_cmd.setByte0(main_brush_enabled ? 1 : 0); // 电机M1方向 1表示正向运行，0表示停止 + vcu_motor1_cmd.setByte1(main_brush_enabled ? start_value : stop_value); // 电机M1油门 + vcu_motor1_cmd.setByte2(vacuum_enabled ? start_value : stop_value); // 吸尘电机 + vcu_motor1_cmd.setByte3(dust_shake_enabled ? start_value : stop_value); // 振尘电机 + vcu_motor1_cmd.setByte4(main_brush_pole_enabled ? lower_value : lift_value); // 主刷推杆-100 100 + vcu_motor1_cmd.setByte5(flap_pole_enabled ? lower_value : lift_value); // 前挡皮推杆电机-100 100 + vcu_motor1_cmd.setByte6(side_brush_enabled ? start_value : stop_value); // 边刷电机 + vcu_motor1_cmd.setByte7(side_brush_enabled ? start_value : stop_value); // 边刷电机 canctl.sendFrame(vcu_motor1_cmd.toFrame()); // ===== 控制0x212报文 (电机M8和预留输出) ===== - vcu_motor2_cmd.setByte0(enable_brush ? target_value : 0); //水泵电机 -100 100 - vcu_motor2_cmd.setByte1(0);// - vcu_motor2_cmd.setByte2(0);// - vcu_motor2_cmd.setByte3(0);// - vcu_motor2_cmd.setByte4(0); // - vcu_motor2_cmd.setByte5(0); // - vcu_motor2_cmd.setByte6(0);// - vcu_motor2_cmd.setByte7(0);// + vcu_motor2_cmd.setByte0(water_pump_enabled ? start_value : stop_value); // 水泵电机 -100 100 + vcu_motor2_cmd.setByte1(0); // + vcu_motor2_cmd.setByte2(0); // + vcu_motor2_cmd.setByte3(0); // + vcu_motor2_cmd.setByte4(0); // + vcu_motor2_cmd.setByte5(0); // + vcu_motor2_cmd.setByte6(0); // + vcu_motor2_cmd.setByte7(0); // canctl.sendFrame(vcu_motor2_cmd.toFrame()); - LOG_INFO_THROTTLE(1000, "[VCU] Sweep? %s, Mode: %d (0=标准,1=混合), Water: %s | Pump: %d", - msg.sweep ? "Yes" : "No", msg.sweep_mode, - enable_water ? "On" : "Off", enable_water); + LOG_INFO_THROTTLE(1000, + "[VCU]" + "主刷: %s, 吸尘: %s, 振尘: %s, " + "主刷推杆: %s, 前挡皮推杆电机: %s, 边刷: %s, 水泵: %s", + main_brush_enabled ? "On" : "Off", vacuum_enabled ? "On" : "Off", + dust_shake_enabled ? "On" : "Off", main_brush_pole_enabled ? "Lowered" : "Lifted", + flap_pole_enabled ? "Lowered" : "Lifted", side_brush_enabled ? "On" : "Off", + water_pump_enabled ? "On" : "Off"); } // 定时器回调：BMS 查询任务 diff --git a/src/base/sweeper_interfaces/msg/McCtrl.msg b/src/base/sweeper_interfaces/msg/McCtrl.msg index 6bedf30..be7e9a7 100644 --- a/src/base/sweeper_interfaces/msg/McCtrl.msg +++ b/src/base/sweeper_interfaces/msg/McCtrl.msg @@ -10,5 +10,14 @@ float32 angle #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减 uint16 angle_speed #转向角速度 120-1500rpm #vcu部分 -bool sweep #清扫 true:清扫 false:不清扫 +bool sweep #一键清扫 true:清扫 false:不清扫 (优先级低于各独立开关) int32 sweep_mode #清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水) + +# 各清扫部件独立控制 (当sweep为true时，以下开关可以单独控制对应部件) +bool enable_main_brush # 主刷电机 +bool enable_vacuum # 吸尘电机 +bool enable_dust_shake # 振尘电机 +bool enable_main_brush_pole # 主刷推杆 (下沉/抬升) +bool enable_flap_pole # 前挡皮推杆电机 +bool enable_side_brush # 边刷电机 +bool enable_water_pump # 水泵电机 diff --git a/src/communication/sub/src/control_mapper.cpp b/src/communication/sub/src/control_mapper.cpp index fa17732..60bb944 100644 --- a/src/communication/sub/src/control_mapper.cpp +++ b/src/communication/sub/src/control_mapper.cpp @@ -55,6 +55,15 @@ void fillMcCtrlFromCarCtrl(const CarCtrl& in, sweeper_interfaces::msg::McCtrl& o out.angle_speed = 120; out.sweep = in.sweepCtrl; + // 设置各清扫部件默认值 + out.enable_main_brush = true; + out.enable_vacuum = true; + out.enable_dust_shake = false; + out.enable_main_brush_pole = true; + out.enable_flap_pole = true; + out.enable_side_brush = true; + out.enable_water_pump = true; + if (in.mode == 3) { // 允许远控：逻辑 msg.brake=0 diff --git a/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp b/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp index a4631ca..a17b8ab 100644 --- a/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp +++ b/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp @@ -108,6 +108,15 @@ class CanRadioCtrlNode : public rclcpp::Node // 解析清扫开关 msg.sweep = (radio_data.sweep != 0); // 通道B + // 设置各清扫部件默认值 + msg.enable_main_brush = msg.sweep; // + msg.enable_vacuum = msg.sweep; // + msg.enable_dust_shake = false; + msg.enable_main_brush_pole = msg.sweep; // + msg.enable_flap_pole = msg.sweep; // + msg.enable_side_brush = msg.sweep; // + msg.enable_water_pump = msg.sweep; // + // 解析转向 float steering_ratio = static_cast(radio_data.steering) / 450.0f; float target_angle = steering_ratio * eps_angle_max_; @@ -135,9 +144,15 @@ class CanRadioCtrlNode : public rclcpp::Node const char* gear_str = "未知挡位"; switch (msg.gear) { - case 0: gear_str = "空挡"; break; - case 1: gear_str = "后退挡"; break; - case 2: gear_str = "前进挡"; break; + case 0: + gear_str = "空挡"; + break; + case 1: + gear_str = "后退挡"; + break; + case 2: + gear_str = "前进挡"; + break; } const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫"; diff --git a/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp b/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp index c97eea1..21e7c8e 100644 --- a/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp +++ b/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp @@ -85,6 +85,14 @@ class ArbitrationNode : public rclcpp::Node safe_msg.angle_speed = 120; safe_msg.sweep = false; + // 安全状态下所有部件都关闭 + safe_msg.enable_main_brush = false; + safe_msg.enable_vacuum = false; + safe_msg.enable_dust_shake = false; + safe_msg.enable_main_brush_pole = false; + safe_msg.enable_flap_pole = false; + safe_msg.enable_side_brush = false; + safe_msg.enable_water_pump = false; publisher_->publish(safe_msg); LOG_WARN_THROTTLE(1000, "[ARBITER] All sources timeout, publishing FAILSAFE control"); diff --git a/src/control/remote_ctrl/src/remote_ctrl_node.cpp b/src/control/remote_ctrl/src/remote_ctrl_node.cpp index c87dff2..1b0b649 100644 --- a/src/control/remote_ctrl/src/remote_ctrl_node.cpp +++ b/src/control/remote_ctrl/src/remote_ctrl_node.cpp @@ -151,6 +151,14 @@ class RemoteCtrlNode : public rclcpp::Node safe_msg.angle = 0.0f; safe_msg.angle_speed = 120; safe_msg.sweep = false; + // 安全状态下所有部件都关闭 + safe_msg.enable_main_brush = false; + safe_msg.enable_vacuum = false; + safe_msg.enable_dust_shake = false; + safe_msg.enable_main_brush_pole = false; + safe_msg.enable_flap_pole = false; + safe_msg.enable_side_brush = false; + safe_msg.enable_water_pump = false; pub_->publish(safe_msg); } } @@ -237,6 +245,13 @@ class RemoteCtrlNode : public rclcpp::Node msg.angle = desired_.angle; msg.angle_speed = desired_.angle_speed; msg.sweep = desired_.sweep; + msg.enable_main_brush = desired_.sweep; + msg.enable_vacuum = desired_.sweep; + msg.enable_dust_shake = false; + msg.enable_main_brush_pole = desired_.sweep; + msg.enable_flap_pole = desired_.sweep; + msg.enable_side_brush = desired_.sweep; + msg.enable_water_pump = desired_.sweep; } LOG_DEBUG("[REMOTE] Publishing McCtrl: gear=%d brake=%d rpm=%d angle=%.1f angle_speed=%u sweep=%d", msg.gear,