清扫部件分开控制

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 &&
+                !state_machine_->obstacle_analysis.has_front_critical && rtk_signal_good)
-                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;

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;
 }

src/base/mc/src/timer_tasks.cpp

@@ -36,32 +36,38 @@ void vcuTimerTask(CANDriver& canctl)
 {
     auto msg = get_safe_control();
 
-    // 根据清扫状态设置所有部件
+    // 基础值定义
-    int8_t target_value = msg.sweep ? 100 : 0;      // 100表示启动，0表示停止
+    int8_t start_value = 100;   // 100表示启动
-    int8_t target_value2 = msg.sweep ? -100 : 100;  // -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 main_brush_enabled = msg.enable_main_brush;
-    // bool enable_water = msg.sweep && (msg.sweep_mode == 1);  // 仅混合模式启用洒水
+    bool vacuum_enabled = msg.enable_vacuum;
-    bool enable_water = true;
+    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.setByte0(main_brush_enabled ? 1 : 0);                          // 电机M1方向 1表示正向运行，0表示停止
-    vcu_motor1_cmd.setByte1(enable_brush ? target_value : 0); //电机M1油门
+    vcu_motor1_cmd.setByte1(main_brush_enabled ? start_value : stop_value);       // 电机M1油门
-    vcu_motor1_cmd.setByte2(enable_brush ? target_value : 0);//吸尘电机
+    vcu_motor1_cmd.setByte2(vacuum_enabled ? start_value : stop_value);           // 吸尘电机
-    vcu_motor1_cmd.setByte3(0);//振尘电机
+    vcu_motor1_cmd.setByte3(dust_shake_enabled ? start_value : stop_value);       // 振尘电机
-    vcu_motor1_cmd.setByte4(enable_brush ? target_value2 : 0);//主刷推杆-100 100
+    vcu_motor1_cmd.setByte4(main_brush_pole_enabled ? lower_value : lift_value);  // 主刷推杆-100 100
-    vcu_motor1_cmd.setByte5(enable_brush ? target_value2 : 0);//前挡皮推杆电机-100 100
+    vcu_motor1_cmd.setByte5(flap_pole_enabled ? lower_value : lift_value);        // 前挡皮推杆电机-100 100
-    vcu_motor1_cmd.setByte6(enable_brush ? target_value : 0);//边刷电机
+    vcu_motor1_cmd.setByte6(side_brush_enabled ? start_value : stop_value);       // 边刷电机
-    vcu_motor1_cmd.setByte7(enable_brush ? target_value : 0);//边刷电机
+    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.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);                                              //
@@ -71,9 +77,14 @@ void vcuTimerTask(CANDriver& canctl)
     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",
+    LOG_INFO_THROTTLE(1000,
-                      msg.sweep ? "Yes" : "No", msg.sweep_mode,
+                      "[VCU]"
-                      enable_water ? "On" : "Off", enable_water);
+                      "主刷: %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 查询任务

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      # 水泵电机

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

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<float>(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 0:
-                    case 1: gear_str = "后退挡"; break;
+                        gear_str = "空挡";
-                    case 2: gear_str = "前进挡"; break;
+                        break;
+                    case 1:
+                        gear_str = "后退挡";
+                        break;
+                    case 2:
+                        gear_str = "前进挡";
+                        break;
                 }
                 const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫";
 

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");

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,