lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

清扫部件分开控制

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-03 09:44:26 +08:00
parent b00863bc68
commit 3ba59b157b
8 changed files with 119 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/autonomy/fu/src/fu_node.cpp
View File

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

9
src/base/mc/src/control_cache.cpp
View File

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

59
src/base/mc/src/timer_tasks.cpp
View File

@ -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.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);//
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 查询任务

11
src/base/sweeper_interfaces/msg/McCtrl.msg
View File

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

9
src/communication/sub/src/control_mapper.cpp
View File

@ -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

21
src/control/can_radio_ctrl/src/can_radio_ctrl.cpp
View File

@ -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 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 ? "正在清扫" : "未清扫";

8
src/control/ctrl_arbiter/src/ctrl_arbiter.cpp
View File

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

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

@ -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,