Auto commit at 2025-06-14 13:02:31

src/ctrl_arbiter/src/ctrl_arbiter.cpp

@@ -48,7 +48,6 @@ public:
 
         timer_ = this->create_wall_timer(20ms, [this]()
                                          { this->arbitrateAndPublish(); });
-        RCLCPP_INFO(this->get_logger(), "ArbitrationNode started, waiting for control sources...");
     }
 
 private:
@@ -61,19 +60,16 @@ private:
         if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000)
         {
             publisher_->publish(radio_msg_);
-            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using RADIO control");
             return;
         }
         if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000)
         {
             publisher_->publish(remote_msg_);
-            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using REMOTE control");
             return;
         }
         if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000)
         {
             publisher_->publish(auto_msg_);
-            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using AUTO control");
             return;
         }
 
@@ -91,8 +87,6 @@ private:
         safe_msg.dust_shake = false;
 
         publisher_->publish(safe_msg);
-        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
-                             "[ARBITER] All sources timeout, publishing FAILSAFE control");
     }
 
     rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr publisher_;

src/mc/CMakeLists.txt

@@ -5,6 +5,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib)
+
 # Find dependencies
 find_package(sweeper_interfaces REQUIRED)
 find_package(ament_cmake REQUIRED)
@@ -19,6 +21,8 @@ add_executable(mc_node ${SRC_FILES})
 
 ament_target_dependencies(mc_node rclcpp std_msgs sweeper_interfaces)
 
+target_link_libraries(mc_node controlcan pthread)
+
 # Set include directories for the target
 target_include_directories(
   mc_node
@@ -31,6 +35,15 @@ target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17)
 install(TARGETS mc_node DESTINATION lib/${PROJECT_NAME})
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 
+set(LIB_DIR ${CMAKE_CURRENT_SOURCE_DIR}/lib)
+
+# 安装libcontrolcan动态库（.so）
+install(
+  FILES
+    ${LIB_DIR}/libcontrolcan.so
+  DESTINATION lib
+)
+
 # Handle testing if needed
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

src/mc/include/mc/can_driver.h

@@ -42,25 +42,13 @@ public:
     // 设置接收回调
     void setReceiveCallback(ReceiveCallback callback, void *userData = nullptr);
 
-    // 设置硬件过滤规则
-    bool setFilter(const std::vector<can_filter> &filters);
-
-    // 追加一个过滤器
-    bool addFilter(const can_filter &filter);
-
-    // 追加一组过滤器
-    bool addFilters(const std::vector<can_filter> &filters);
-
 private:
     void receiveThreadFunc();
-    bool applyFilters(); // 应用当前filters_
-
     int sockfd = -1;
     std::atomic<bool> running{false};
     std::thread receiveThread;
     ReceiveCallback callback;
     void *userData = nullptr;
-    std::vector<can_filter> filters_; // 当前所有过滤器
 };
 
 #endif // CAN_DRIVER_H

src/mc/include/mc/can_struct.h

@@ -57,19 +57,6 @@ union can_EPS_cmd_union
 
 #pragma pack(pop)
 
-union can_DBS_cmd_union
-{
-    struct __attribute__((packed))
-    {
-        uint8_t valid;       // Byte 0: VCU_DBS_Valid
-        uint8_t work_mode;   // Byte 1: VCU_DBS_Work_Mode
-        uint8_t pressure;    // Byte 2: VCU_DBS_HP_Pressure（0.1MPa）
-        uint8_t abs_active;  // Byte 3: ABS_Active
-        uint8_t reserved[4]; // Byte 4~7: 保留
-    } fields;
-    uint8_t bytes[8]; // 原始字节流
-};
-
 struct can_MCU_cmd
 {
     can_MCU_cmd_union data;
@@ -339,52 +326,9 @@ struct can_VCU_out2_cmd
     }
 };
 
-struct can_DBS_cmd
-{
-    can_DBS_cmd_union data;
-
-    static constexpr uint32_t CMD_ID = 0x154;
-    static constexpr bool EXT_FLAG = false;
-    static constexpr bool RTR_FLAG = false;
-
-    // 构造函数初始化固定字段
-    can_DBS_cmd()
-    {
-        memset(&data, 0, sizeof(data));
-        data.fields.work_mode = 0;
-        data.fields.abs_active = 1;
-    }
-
-    void setEnabled(bool en)
-    {
-        data.fields.work_mode = en ? 1 : 0;
-    }
-
-    // 设置压力（单位 MPa，0~8.0，有效范围 0~80）
-    can_DBS_cmd &setPressure(float mpa)
-    {
-        uint8_t val = static_cast<uint8_t>(std::clamp(mpa * 10.0f, 0.0f, 80.0f));
-        data.fields.pressure = val;
-        return *this;
-    }
-
-    // 转换为 CAN 帧
-    CANFrame toFrame() const
-    {
-        CANFrame frame;
-        frame.id = CMD_ID;
-        memcpy(frame.data, data.bytes, 8);
-        frame.dlc = 8;
-        frame.ext = EXT_FLAG;
-        frame.rtr = RTR_FLAG;
-        return frame;
-    }
-};
-
 extern can_MCU_cmd mcu_cmd;
 extern can_EPS_cmd eps_cmd;
 extern can_VCU_out1_cmd vcu1_cmd;
 extern can_VCU_out2_cmd vcu2_cmd;
-extern can_DBS_cmd dbs_cmd;
 
 #endif

src/mc/include/mc/can_utils.hpp

@@ -1,14 +0,0 @@
-#pragma once
-#include "rclcpp/rclcpp.hpp"
-#include "sweeper_interfaces/msg/can_frame.hpp"
-#include "mc/can_struct.h"
-
-extern bool g_can_print_enable;
-
-struct CanHandlerContext
-{
-    rclcpp::Node::SharedPtr node;
-    std::shared_ptr<rclcpp::Publisher<sweeper_interfaces::msg::CanFrame>> publisher;
-};
-
-void receiveHandler(const CANFrame &frame, void *userData);

src/mc/include/mc/control_cache.hpp

@@ -1,20 +0,0 @@
-#pragma once
-#include "sweeper_interfaces/msg/mc_ctrl.hpp"
-#include <mutex>
-#include <chrono>
-
-class ControlCache
-{
-public:
-    void update(const sweeper_interfaces::msg::McCtrl &msg);
-    bool get(sweeper_interfaces::msg::McCtrl &msg);
-
-private:
-    std::mutex mutex_;
-    sweeper_interfaces::msg::McCtrl latest_msg_;
-    std::chrono::steady_clock::time_point last_update_time_;
-    bool has_data_ = false;
-};
-
-extern ControlCache control_cache;
-sweeper_interfaces::msg::McCtrl get_safe_control(); // 获取带超时判断的控制指令

src/mc/include/mc/controlcan.h

@@ -0,0 +1,104 @@
+#ifndef CONTROLCAN_H
+#define CONTROLCAN_H
+
+////文件版本：v2.02 20190609
+//接口卡类型定义
+
+#define VCI_USBCAN1		3
+#define VCI_USBCAN2		4
+#define VCI_USBCAN2A		4
+
+#define VCI_USBCAN_E_U 		20
+#define VCI_USBCAN_2E_U 	21
+
+//函数调用返回状态值
+#define	STATUS_OK					1
+#define STATUS_ERR					0
+	
+#define USHORT unsigned short int
+#define BYTE unsigned char
+#define CHAR char
+#define UCHAR unsigned char
+#define UINT unsigned int
+#define DWORD unsigned int
+#define PVOID void*
+#define ULONG unsigned int
+#define INT int
+#define UINT32 UINT
+#define LPVOID void*
+#define BOOL BYTE
+#define TRUE 1
+#define FALSE 0
+
+//1.ZLGCAN系列接口卡信息的数据类型。
+typedef  struct  _VCI_BOARD_INFO{
+		USHORT	hw_Version;
+		USHORT	fw_Version;
+		USHORT	dr_Version;
+		USHORT	in_Version;
+		USHORT	irq_Num;
+		BYTE	can_Num;
+		CHAR	str_Serial_Num[20];
+		CHAR	str_hw_Type[40];
+		USHORT	Reserved[4];
+} VCI_BOARD_INFO,*PVCI_BOARD_INFO; 
+
+//2.定义CAN信息帧的数据类型。
+typedef  struct  _VCI_CAN_OBJ{
+	UINT	ID;
+	UINT	TimeStamp;
+	BYTE	TimeFlag;
+	BYTE	SendType;
+	BYTE	RemoteFlag;//是否是远程帧
+	BYTE	ExternFlag;//是否是扩展帧
+	BYTE	DataLen;
+	BYTE	Data[8];
+	BYTE	Reserved[3];
+}VCI_CAN_OBJ,*PVCI_CAN_OBJ;
+
+//3.定义初始化CAN的数据类型
+typedef struct _INIT_CONFIG{
+	DWORD	AccCode;
+	DWORD	AccMask;
+	DWORD	Reserved;
+	UCHAR	Filter;
+	UCHAR	Timing0;	
+	UCHAR	Timing1;	
+	UCHAR	Mode;
+}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
+
+///////// new add struct for filter /////////
+typedef struct _VCI_FILTER_RECORD{
+	DWORD ExtFrame;	//是否为扩展帧
+	DWORD Start;
+	DWORD End;
+}VCI_FILTER_RECORD,*PVCI_FILTER_RECORD;
+
+#ifdef __cplusplus
+#define EXTERN_C  extern "C"
+#else
+#define EXTERN_C
+#endif
+
+EXTERN_C DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
+EXTERN_C DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
+EXTERN_C DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
+
+EXTERN_C DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
+
+EXTERN_C DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
+
+EXTERN_C ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+EXTERN_C DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+EXTERN_C DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+EXTERN_C DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+EXTERN_C ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,UINT Len);
+EXTERN_C ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,UINT Len,INT WaitTime);
+
+EXTERN_C DWORD  VCI_UsbDeviceReset(DWORD DevType,DWORD DevIndex,DWORD Reserved);
+EXTERN_C DWORD  VCI_FindUsbDevice2(PVCI_BOARD_INFO pInfo);
+
+#endif
+

src/mc/include/mc/dumper.h

@@ -1,63 +0,0 @@
-// 专门用于垃圾倾倒的头文件
-#ifndef DUMPER_H
-#define DUMPER_H
-
-#include "can_driver.h"
-#include <mutex>
-#include <queue>
-#include <rclcpp/rclcpp.hpp>
-
-enum class DumperState
-{
-    IDLE,
-    RAISING,    // 斗升中
-    DUMPING,    // 放斗中
-    RETRACTING, // 收斗中
-    LOWERING    // 斗降中
-};
-
-enum class DumperAction
-{
-    RAISE,
-    DUMP,
-    RETRACT,
-    LOWER
-};
-
-class DumperController
-{
-public:
-    DumperController(CANDriver &driver);
-
-    void onStartup();                                         // 上电复位（只在内部用）
-    void tryStartup();                                        // 唤醒后自动执行复位（防重）
-    void updateDial(int new_dial, int current_state);         // 拨钮变化检测与处理
-    void resetAwake();                                        // 清除唤醒状态（VCU 掉线）
-    void setSweeping(bool sweeping) { sweeping_ = sweeping; } // 外部设置扫地状态，
-    int getSimpleState() const;
-
-private:
-    void handleDialChange(int from, int to);
-    void runActionSequence(const std::vector<DumperAction> &actions);
-    void doAction(DumperAction act);
-    void sendControlOff();
-    void logState() const;
-
-    bool raised_ = true;
-    bool dumped_ = true;
-    bool in_startup_reset_ = true;
-
-    bool awakened_ = false;
-    int last_dial_target_ = -1;
-
-    std::mutex queue_mutex_;
-    std::queue<DumperAction> action_queue_;
-    std::atomic<bool> busy_;
-    std::atomic<bool> has_pending_task_;
-    std::atomic<bool> sweeping_ = false; // 扫地任务标志
-    DumperState current_state_;
-    CANDriver &can_;
-    rclcpp::Logger logger_;
-};
-
-#endif

src/mc/include/mc/timer_tasks.hpp

@@ -1,7 +0,0 @@
-#pragma once
-#include "rclcpp/rclcpp.hpp"
-#include "mc/can_driver.h"
-#include "mc/dumper.h"
-
-// 注册所有定时器任务
-void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl);

src/mc/include/mc/vcu_wakeup.hpp

@@ -1,4 +0,0 @@
-#pragma once
-#include "mc/can_struct.h"
-
-void VCUWakeUp(CANDriver &driver); // 发送唤醒帧

src/mc/src/can_driver.cpp

@@ -1,174 +1,159 @@
 #include "mc/can_driver.h"
+#include "mc/controlcan.h"
 #include <cstring>
-#include <stdexcept>
-#include <iostream>
-#include <thread>
 #include <fcntl.h>
-#include <unistd.h>
-#include <sys/ioctl.h>
-#include <net/if.h>
+#include <iostream>
 #include <linux/can.h>
 #include <linux/can/raw.h>
+#include <net/if.h>
+#include <stdexcept>
+#include <sys/ioctl.h>
+#include <thread>
+#include <unistd.h>
+#include <mutex>
+
+std::mutex canMutex;
+
+DWORD deviceType = VCI_USBCAN2; // 或 VCI_USBCAN_2E_U 等
+DWORD deviceIndex = 0;          // 通常为 0（第一个设备）
+DWORD channelIndex = 0;         // 通道号：0 表示 CAN1，1 表示 CAN2
 
 CANDriver::CANDriver() = default;
 
-CANDriver::~CANDriver()
-{
-    close();
-}
+CANDriver::~CANDriver() { close(); }
 
 bool CANDriver::open(const std::string &interface)
 {
-    if (running)
-        return false;
+  if (running)
+    return false;
 
-    sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
-    if (sockfd < 0)
-    {
-        perror("socket");
-        return false;
-    }
+  // 参数可解析 interface 设定 channelIndex，例如 interface="can0" =>
+  // channelIndex=0
+  deviceType = VCI_USBCAN2;
+  deviceIndex = 0;
+  channelIndex = (interface == "can1") ? 1 : 0;
 
-    struct ifreq ifr;
-    std::strncpy(ifr.ifr_name, interface.c_str(), IFNAMSIZ - 1);
-    ifr.ifr_name[IFNAMSIZ - 1] = '\0';
+  if (VCI_OpenDevice(deviceType, deviceIndex, 0) != STATUS_OK)
+  {
+    std::cerr << "VCI_OpenDevice failed" << std::endl;
+    return false;
+  }
 
-    if (ioctl(sockfd, SIOCGIFINDEX, &ifr) < 0)
-    {
-        perror("ioctl");
-        ::close(sockfd);
-        return false;
-    }
+  VCI_INIT_CONFIG config{};
+  config.AccCode = 0;
+  config.AccMask = 0xFFFFFFFF;
+  config.Filter = 0;     // 接收所有
+  config.Timing0 = 0x00; // 500Kbps 示例（需查文档确认 Timing）
+  config.Timing1 = 0x1C;
+  config.Mode = 0; // 正常模式
 
-    struct sockaddr_can addr{};
-    addr.can_family = AF_CAN;
-    addr.can_ifindex = ifr.ifr_ifindex;
+  if (VCI_InitCAN(deviceType, deviceIndex, channelIndex, &config) !=
+      STATUS_OK)
+  {
+    std::cerr << "VCI_InitCAN failed" << std::endl;
+    VCI_CloseDevice(deviceType, deviceIndex);
+    return false;
+  }
 
-    if (bind(sockfd, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr)) < 0)
-    {
-        perror("bind");
-        ::close(sockfd);
-        return false;
-    }
+  if (VCI_StartCAN(deviceType, deviceIndex, channelIndex) != STATUS_OK)
+  {
+    std::cerr << "VCI_StartCAN failed" << std::endl;
+    VCI_CloseDevice(deviceType, deviceIndex);
+    return false;
+  }
 
-    // 设置为非阻塞
-    int flags = fcntl(sockfd, F_GETFL, 0);
-    fcntl(sockfd, F_SETFL, flags | O_NONBLOCK);
+  if (VCI_ClearBuffer(deviceType, deviceIndex, channelIndex) != 1)
+  {
+    std::cerr << "VCI_ClearBuffer failed" << std::endl;
+    VCI_CloseDevice(deviceType, deviceIndex);
+    return false;
+  }
 
-    running = true;
-    receiveThread = std::thread(&CANDriver::receiveThreadFunc, this);
-    return true;
+  running = true;
+  receiveThread = std::thread(&CANDriver::receiveThreadFunc, this);
+  return true;
 }
 
 void CANDriver::close()
 {
-    if (!running)
-        return;
+  if (!running)
+    return;
 
-    running = false;
-    if (receiveThread.joinable())
-        receiveThread.join();
+  running = false;
+  if (receiveThread.joinable())
+    receiveThread.join();
 
-    if (sockfd >= 0)
-    {
-        ::close(sockfd);
-        sockfd = -1;
-    }
+  // 先清空缓冲区，防止缓冲区残留数据导致重复接收
+  VCI_ClearBuffer(deviceType, deviceIndex, channelIndex);
+
+  VCI_ResetCAN(deviceType, deviceIndex, channelIndex);
+  VCI_CloseDevice(deviceType, deviceIndex);
 }
 
 bool CANDriver::sendFrame(const CANFrame &frame)
 {
-    if (!running)
-        return false;
+  if (!running)
+    return false;
 
-    struct can_frame raw_frame{};
-    raw_frame.can_id = frame.id;
-    if (frame.ext)
-        raw_frame.can_id |= CAN_EFF_FLAG;
-    if (frame.rtr)
-        raw_frame.can_id |= CAN_RTR_FLAG;
-    raw_frame.can_dlc = frame.dlc;
-    std::memcpy(raw_frame.data, frame.data, frame.dlc);
+  VCI_CAN_OBJ obj{};
+  obj.ID = frame.id;
+  obj.RemoteFlag = frame.rtr ? 1 : 0;
+  obj.ExternFlag = frame.ext ? 1 : 0;
+  obj.SendType = 0; // 普通发送
+  obj.DataLen = frame.dlc;
+  std::memcpy(obj.Data, frame.data, frame.dlc);
 
-    if (write(sockfd, &raw_frame, sizeof(raw_frame)) != sizeof(raw_frame))
-    {
-        perror("write");
-        return false;
-    }
+  std::lock_guard<std::mutex> lock(canMutex); // 加锁
+  if (VCI_Transmit(deviceType, deviceIndex, channelIndex, &obj, 1) != 1)
+  {
+    std::cerr << "VCI_Transmit failed" << std::endl;
+    return false;
+  }
 
-    return true;
+  return true;
 }
 
 void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData)
 {
-    this->callback = callback;
-    this->userData = userData;
-}
-
-bool CANDriver::setFilter(const std::vector<can_filter> &filters)
-{
-    if (!running)
-        return false;
-
-    filters_ = filters;
-    return applyFilters();
-}
-
-bool CANDriver::addFilter(const can_filter &filter)
-{
-    filters_.push_back(filter);
-    return applyFilters();
-}
-
-bool CANDriver::addFilters(const std::vector<can_filter> &filters)
-{
-    filters_.insert(filters_.end(), filters.begin(), filters.end());
-    return applyFilters();
-}
-
-bool CANDriver::applyFilters()
-{
-    if (!running)
-        return false;
-
-    if (filters_.empty())
-    {
-        setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, nullptr, 0);
-        return true;
-    }
-
-    if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER,
-                   filters_.data(), filters_.size() * sizeof(can_filter)) < 0)
-    {
-        perror("setsockopt");
-        return false;
-    }
-
-    return true;
+  this->callback = callback;
+  this->userData = userData;
 }
 
 void CANDriver::receiveThreadFunc()
 {
-    struct can_frame raw_frame;
-    while (running)
-    {
-        ssize_t nbytes = read(sockfd, &raw_frame, sizeof(raw_frame));
-        if (nbytes < 0)
-        {
-            if (errno != EAGAIN)
-                perror("read");
-            continue;
-        }
+  constexpr int maxFrames = 2500;
+  VCI_CAN_OBJ rec[maxFrames];
 
-        if (nbytes == sizeof(raw_frame) && callback)
-        {
-            CANFrame frame;
-            frame.id = raw_frame.can_id & CAN_EFF_MASK;
-            frame.ext = static_cast<bool>(raw_frame.can_id & CAN_EFF_FLAG);
-            frame.rtr = static_cast<bool>(raw_frame.can_id & CAN_RTR_FLAG);
-            frame.dlc = raw_frame.can_dlc;
-            std::memcpy(frame.data, raw_frame.data, raw_frame.can_dlc);
-            callback(frame, userData);
-        }
+  while (running)
+  {
+    ULONG len = 0;
+    {
+      std::lock_guard<std::mutex> lock(canMutex); // 加锁
+      len = VCI_Receive(deviceType, deviceIndex, channelIndex, rec, maxFrames, 10);
     }
+
+    if (len == 0)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+      continue;
+    }
+
+    for (ULONG i = 0; i < len && callback; ++i)
+    {
+      CANFrame frame;
+      frame.id = rec[i].ID;
+      frame.ext = rec[i].ExternFlag != 0;
+      frame.rtr = rec[i].RemoteFlag != 0;
+      frame.dlc = rec[i].DataLen;
+
+      if (frame.dlc > 8)
+      {
+        std::cerr << "Invalid CAN data length: " << (int)frame.dlc << std::endl;
+        frame.dlc = 8; // 或跳过此帧
+      }
+
+      std::memcpy(frame.data, rec[i].Data, frame.dlc);
+      callback(frame, userData);
+    }
+  }
 }

src/mc/src/can_struct.cpp

@@ -4,4 +4,3 @@ can_MCU_cmd mcu_cmd;
 can_EPS_cmd eps_cmd;
 can_VCU_out1_cmd vcu1_cmd;
 can_VCU_out2_cmd vcu2_cmd;
-can_DBS_cmd dbs_cmd;

src/mc/src/can_utils.cpp

@@ -1,39 +0,0 @@
-#include "mc/can_utils.hpp"
-#include <sstream>
-#include <iomanip>
-
-bool g_can_print_enable = false;
-extern std::atomic<bool> vcu_msg_received;
-extern std::atomic<bool> vcu_awake;
-extern rclcpp::Time last_vcu_msg_time;
-
-void receiveHandler(const CANFrame &frame, void *userData)
-{
-    auto *context = static_cast<CanHandlerContext *>(userData);
-    auto node = context->node;
-    auto pub = context->publisher;
-    auto now = node->now();
-
-    sweeper_interfaces::msg::CanFrame msg;
-    msg.id = frame.id;
-    msg.dlc = frame.dlc;
-    msg.data.assign(frame.data, frame.data + frame.dlc);
-    pub->publish(msg);
-
-    if (g_can_print_enable)
-    {
-        std::stringstream ss;
-        ss << "CAN ID: " << std::hex << std::uppercase
-           << std::setw((frame.id > 0x7FF) ? 8 : 5) << std::setfill(' ') << frame.id << " Data: ";
-        for (int i = 0; i < frame.dlc; ++i)
-            ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
-        RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
-    }
-
-    if (frame.id == 0x18FA0121)
-    {
-        last_vcu_msg_time = now;
-        vcu_msg_received.store(true);
-        vcu_awake.store(true);
-    }
-}

src/mc/src/control_cache.cpp

@@ -1,44 +0,0 @@
-#include "mc/control_cache.hpp"
-
-ControlCache control_cache;
-
-void ControlCache::update(const sweeper_interfaces::msg::McCtrl &msg)
-{
-    std::lock_guard<std::mutex> lock(mutex_);
-    latest_msg_ = msg;
-    last_update_time_ = std::chrono::steady_clock::now();
-    has_data_ = true;
-}
-
-bool ControlCache::get(sweeper_interfaces::msg::McCtrl &msg)
-{
-    std::lock_guard<std::mutex> lock(mutex_);
-    if (!has_data_)
-        return false;
-
-    auto now = std::chrono::steady_clock::now();
-    if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time_).count() > 100)
-        return false;
-
-    msg = latest_msg_;
-    return true;
-}
-
-sweeper_interfaces::msg::McCtrl get_safe_control()
-{
-    sweeper_interfaces::msg::McCtrl msg;
-    if (!control_cache.get(msg))
-    {
-        msg.brake = 1;
-        msg.gear = 0;
-        msg.rpm = 0;
-        msg.angle = 0;
-        msg.angle_speed = 120;
-        msg.edge_brush_lift = false;
-        msg.sweep_ctrl = false;
-        msg.spray = false;
-        msg.mud_flap = false;
-        msg.dust_shake = false;
-    }
-    return msg;
-}

src/mc/src/dumper.cpp

@@ -1,218 +0,0 @@
-#include "mc/dumper.h"
-#include <thread>
-#include <chrono>
-#include <iostream>
-
-DumperController::DumperController(CANDriver &driver)
-    : busy_(false),
-      has_pending_task_(false),
-      current_state_(DumperState::IDLE),
-      can_(driver),
-      logger_(rclcpp::get_logger("DumperController"))
-{
-}
-
-// 上电复位：自动收斗 + 降斗
-void DumperController::onStartup()
-{
-    RCLCPP_INFO(logger_, "[Init] Power-on reset: retract and lower...");
-    doAction(DumperAction::RETRACT);
-    doAction(DumperAction::LOWER);
-    in_startup_reset_ = false;
-    sendControlOff();
-}
-
-void DumperController::tryStartup()
-{
-    if (awakened_)
-        return;
-
-    RCLCPP_INFO(logger_, "[Init] Dumper first wake-up: resetting...");
-    onStartup();
-    awakened_ = true;
-}
-
-void DumperController::resetAwake()
-{
-    awakened_ = false;
-    last_dial_target_ = -1;
-    RCLCPP_INFO(logger_, "[INFO] Dumper awake state reset.");
-}
-
-void DumperController::updateDial(int new_dial, int current_state)
-{
-    if (in_startup_reset_)
-    {
-        RCLCPP_WARN(logger_, "[Dumper] In startup reset, ignoring remote control dump command.");
-        return;
-    }
-
-    if (new_dial < 0 || new_dial > 2)
-        return;
-
-    if (last_dial_target_ == new_dial)
-        return;
-
-    RCLCPP_INFO(logger_, "[Dial] Changed from %d to %d", current_state, new_dial);
-    handleDialChange(current_state, new_dial);
-    last_dial_target_ = new_dial;
-}
-
-// 拨钮变更处理（如从最下拨到最上，动作会排队）
-void DumperController::handleDialChange(int from, int to)
-{
-    std::vector<DumperAction> actions;
-
-    if (from == 0 && to == 1)
-        actions = {DumperAction::RAISE};
-    else if (from == 1 && to == 2)
-        actions = {DumperAction::DUMP};
-    else if (from == 2 && to == 1)
-        actions = {DumperAction::RETRACT};
-    else if (from == 1 && to == 0)
-        actions = {DumperAction::LOWER};
-    else if (from == 0 && to == 2)
-        actions = {DumperAction::RAISE, DumperAction::DUMP};
-    else if (from == 2 && to == 0)
-        actions = {DumperAction::RETRACT, DumperAction::LOWER};
-    else
-        return;
-
-    runActionSequence(actions);
-}
-
-// 执行动作序列，如果当前正在执行，则加入队列等待
-void DumperController::runActionSequence(const std::vector<DumperAction> &actions)
-{
-    if (busy_)
-    {
-        std::lock_guard<std::mutex> lock(queue_mutex_);
-        for (const auto &a : actions)
-            action_queue_.push(a);
-        has_pending_task_ = true;
-        return;
-    }
-
-    busy_ = true;
-    has_pending_task_ = false;
-
-    std::thread([this, actions]
-                {
-                    for (auto action : actions)
-                    {
-                        doAction(action);
-                    }
-
-                    // 执行排队的动作
-                    while (true)
-                    {
-                        DumperAction next;
-                        {
-                            std::lock_guard<std::mutex> lock(queue_mutex_);
-                            if (action_queue_.empty())
-                                break;
-                            next = action_queue_.front();
-                            action_queue_.pop();
-                        }
-                        doAction(next);
-                    }
-
-                    sendControlOff();
-                    busy_ = false; })
-        .detach();
-}
-
-// 每个动作的具体 CAN 指令 + 延时
-void DumperController::doAction(DumperAction act)
-{
-    if (sweeping_)
-    {
-        RCLCPP_WARN(logger_, "[Warning] Sweeping active, dumper action blocked.");
-        return;
-    }
-
-    constexpr int delay_ms = 15000; // 可统一调整延时时间（单位：毫秒）
-    using namespace std::chrono_literals;
-
-    CANFrame frame1;
-    CANFrame frame2;
-
-    switch (act)
-    {
-    case DumperAction::RAISE:
-        if (raised_)
-            return;
-        RCLCPP_INFO(logger_, "[Action] Raising...");
-        frame1 = {0x18FA1117, {0x00, 0x40, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
-        can_.sendFrame(frame1);
-        current_state_ = DumperState::RAISING;
-        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
-        raised_ = true;
-        dumped_ = false;
-        break;
-
-    case DumperAction::DUMP:
-        if (dumped_)
-            return;
-        RCLCPP_INFO(logger_, "[Action] Dumping...");
-        frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x10, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
-        can_.sendFrame(frame1);
-        current_state_ = DumperState::DUMPING;
-        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
-        dumped_ = true;
-        break;
-
-    case DumperAction::RETRACT:
-        if (!dumped_)
-            return;
-        RCLCPP_INFO(logger_, "[Action] Retracting...");
-        frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
-        frame2 = {0x18F21117, {0x00, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 8, true, false};
-        can_.sendFrame(frame1);
-        can_.sendFrame(frame2);
-        current_state_ = DumperState::RETRACTING;
-        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
-        dumped_ = false;
-        break;
-
-    case DumperAction::LOWER:
-        if (!raised_)
-            return;
-        RCLCPP_INFO(logger_, "[Action] Lowering...");
-        frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
-        frame2 = {0x18F21117, {0x00, 0x00, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00}, 8, true, false};
-        can_.sendFrame(frame1);
-        can_.sendFrame(frame2);
-        current_state_ = DumperState::LOWERING;
-        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
-        raised_ = false;
-        break;
-    }
-    logState();
-}
-
-// 所有动作完成后关闭控制
-void DumperController::sendControlOff()
-{
-    CANFrame off1 = {0x18FA1117, {0, 0, 0, 0, 0, 0, 0, 0}, 8, true, false};
-    CANFrame off2 = {0x18F21117, {0, 0, 0, 0, 0, 0, 0, 0}, 8, true, false};
-    can_.sendFrame(off1);
-    can_.sendFrame(off2);
-    RCLCPP_INFO(logger_, "[INFO] Control OFF sent.");
-}
-
-void DumperController::logState() const
-{
-    const char *state_str[] = {"IDLE", "RAISING", "DUMPING", "RETRACTING", "LOWERING"};
-    RCLCPP_INFO(logger_, "[STATE] Dumper state: %s", state_str[static_cast<int>(current_state_)]);
-}
-
-int DumperController::getSimpleState() const
-{
-    if (!raised_)
-        return 0; // 斗未升（已收）
-    else if (!dumped_)
-        return 1; // 斗升但未放斗
-    else
-        return 2; // 斗升且已放斗
-}

src/mc/src/mc.cpp

@@ -1,36 +1,264 @@
-#include "rclcpp/rclcpp.hpp"
-#include "mc/get_config.h"
 #include "mc/can_driver.h"
-#include "mc/dumper.h"
-#include "mc/can_utils.hpp"
-#include "mc/control_cache.hpp"
-#include "mc/timer_tasks.hpp"
-#include "mc/vcu_wakeup.hpp"
+#include "mc/can_struct.h"
+#include "mc/get_config.h"
+#include "rclcpp/rclcpp.hpp"
 #include "sweeper_interfaces/msg/can_frame.hpp"
 #include "sweeper_interfaces/msg/mc_ctrl.hpp"
+#include <cstdio>
+#include <iomanip>
+#include <iostream>
 
 namespace sweeperMsg = sweeper_interfaces::msg;
 
 CANDriver canctl;
-DumperController dumper(canctl);
-std::atomic<bool> vcu_msg_received = false;
-std::atomic<bool> vcu_awake = false;
-rclcpp::Time last_vcu_msg_time;
+
+struct CanHandlerContext
+{
+  rclcpp::Node::SharedPtr node;
+  std::shared_ptr<rclcpp::Publisher<sweeperMsg::CanFrame>> publisher;
+};
+
+std::atomic<bool> vcu_msg_received = false; // 是否收到过vcu数据帧
+std::atomic<bool> vcu_awake = false;        // 是否唤醒成功
+rclcpp::Time last_vcu_msg_time;             // 上次收到vcu数据帧的时间
+
+// 全局开关，控制是否打印 CAN 消息到终端
+bool g_can_print_enable = true;
+
+struct ControlCache
+{
+  std::mutex mutex; // 防止多线程冲突
+  sweeperMsg::McCtrl latest_msg;
+  std::chrono::steady_clock::time_point last_update_time;
+  bool has_data = false;
+
+  void update(const sweeperMsg::McCtrl &msg)
+  {
+    std::lock_guard<std::mutex> lock(mutex);
+    latest_msg = msg;
+    last_update_time = std::chrono::steady_clock::now();
+    has_data = true;
+  }
+
+  bool get(sweeperMsg::McCtrl &msg)
+  {
+    std::lock_guard<std::mutex> lock(mutex);
+    if (!has_data)
+      return false;
+
+    auto now = std::chrono::steady_clock::now();
+    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
+        now - last_update_time);
+
+    if (duration.count() > 100) // ms超时
+    {
+      return false; // 数据过期
+    }
+
+    msg = latest_msg;
+    return true;
+  }
+};
+
+ControlCache control_cache;
+
+// 发布检测
+sweeperMsg::McCtrl get_safe_control()
+{
+  sweeperMsg::McCtrl msg;
+  if (!control_cache.get(msg))
+  {
+    // 超时或未接收到控制数据，进入安全状态
+    msg.brake = 1;
+    msg.gear = 0;
+    msg.rpm = 0;
+    msg.angle = 0;
+    msg.angle_speed = 120;
+    msg.edge_brush_lift = false;
+    msg.sweep_ctrl = false;
+    msg.spray = false;
+    msg.mud_flap = false;
+    msg.dust_shake = false;
+  }
+  return msg;
+}
+
+// CAN消息接收回调
+void receiveHandler(const CANFrame &frame, void *userData)
+{
+  // 获取 ROS 2 Publisher
+  auto context = static_cast<CanHandlerContext *>(userData);
+  auto publisher = context->publisher;
+  auto node = context->node;
+  auto now = node->now();
+
+  // 创建并发布 CAN 消息
+  auto msg = sweeperMsg::CanFrame();
+  msg.id = frame.id;
+  msg.dlc = frame.dlc;
+  size_t len = frame.dlc <= 8 ? frame.dlc : 8;
+  msg.data.assign(frame.data, frame.data + len);
+
+  publisher->publish(msg);
+
+  // 根据开关决定是否打印 CAN 消息
+  if (g_can_print_enable && len > 0)
+  {
+    std::stringstream ss;
+    ss << "CAN ID: ";
+    if (frame.id > 0x7FF)
+      ss << std::hex << std::uppercase << frame.id;
+    else
+      ss << std::hex << std::uppercase << std::setw(5) << std::setfill(' ')
+         << frame.id;
+
+    ss << " Data: ";
+    for (int i = 0; i < frame.dlc; ++i)
+    {
+      ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i]
+         << " ";
+    }
+
+    RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
+  }
+
+  // VCU报文检查
+  if (frame.id == 0x18FA0121)
+  {
+    last_vcu_msg_time = now; // 更新时间
+    vcu_msg_received.store(true);
+    vcu_awake.store(true); // 唤醒成功
+  }
+}
 
 void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
 {
-  dumper.setSweeping(msg->sweep_ctrl);
   control_cache.update(*msg);
 }
 
+void VCUWakeUp()
+{
+  CANFrame frame;
+
+  // 公共设置
+  frame.id = 0x18FF8017;
+  frame.dlc = 8;
+  frame.ext = true;  // 使用扩展帧
+  frame.rtr = false; // 不是远程帧
+
+  // 第1帧：握手帧
+  frame.data[0] = 0x55;
+  frame.data[1] = 0x21;
+  for (int i = 2; i < 8; ++i)
+    frame.data[i] = 0xFF;
+  canctl.sendFrame(frame);
+
+  // 延时，例如 50ms
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+  // 第2帧：启动帧
+  frame.data[0] = 0xAA;
+  frame.data[1] = 0x21;
+  for (int i = 2; i < 8; ++i)
+    frame.data[i] = 0xFF;
+  canctl.sendFrame(frame);
+}
+
+void setupTimers(rclcpp::Node::SharedPtr node)
+{
+  // VCU 唤醒帧发送定时器，1Hz
+  static rclcpp::TimerBase::SharedPtr timer_wakeup =
+      node->create_wall_timer(std::chrono::seconds(1), [node]()
+                              {
+        if (!vcu_awake.load()) {
+          RCLCPP_INFO(node->get_logger(),
+                      "[TIMER][VCU] Not awake, sending wake-up frame...");
+          VCUWakeUp();
+        } });
+
+  // vcu报文 watchdog 检查，200ms
+  static rclcpp::TimerBase::SharedPtr timer_estop_watchdog =
+      node->create_wall_timer(std::chrono::milliseconds(200), [node]()
+                              {
+        auto now = node->now();
+        auto elapsed = now - last_vcu_msg_time;
+
+        if (vcu_msg_received.load() &&
+            elapsed > rclcpp::Duration::from_seconds(0.5)) {
+          RCLCPP_WARN(node->get_logger(),
+                      "[TIMER][VCU] message timeout, resetting wake-up state.");
+          vcu_msg_received.store(false);
+          vcu_awake.store(false);
+        } });
+
+  // MCU控制，50Hz
+  static rclcpp::TimerBase::SharedPtr timer_mcu =
+      node->create_wall_timer(std::chrono::milliseconds(20), []()
+                              {
+        sweeperMsg::McCtrl msg = get_safe_control();
+
+        mcu_cmd.setEnabled(true);
+        mcu_cmd.setGear(msg.gear);
+        mcu_cmd.setRPM(msg.rpm);
+        mcu_cmd.setBrake(msg.brake);
+
+        canctl.sendFrame(mcu_cmd.toFrame()); });
+
+  // EPS 控制，20Hz
+  static rclcpp::TimerBase::SharedPtr timer_eps =
+      node->create_wall_timer(std::chrono::milliseconds(50), []()
+                              {
+        sweeperMsg::McCtrl msg = get_safe_control();
+
+        eps_cmd.setCenterCmd(0);
+        eps_cmd.setAngle(msg.angle);
+        eps_cmd.setAngularSpeed(msg.angle_speed);
+        eps_cmd.pack();
+
+        canctl.sendFrame(eps_cmd.toFrame()); });
+
+  // VCU 控制，10Hz
+  static rclcpp::TimerBase::SharedPtr timer_vcu =
+      node->create_wall_timer(std::chrono::milliseconds(100), []()
+                              {
+        sweeperMsg::McCtrl msg = get_safe_control();
+
+        vcu1_cmd.setLeftLight(msg.left_light);
+        vcu1_cmd.setDustShake(msg.dust_shake);
+        vcu1_cmd.setHeadLight(msg.headlight);
+
+        vcu2_cmd.setBrakeLight(msg.brake_light);
+        vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
+        vcu2_cmd.setHeadlight(msg.headlight);
+        vcu2_cmd.setMudFlap(msg.mud_flap);
+        vcu2_cmd.setRightLight(msg.right_light);
+        vcu2_cmd.setSpray(msg.spray);
+        vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
+
+        canctl.sendFrame(vcu1_cmd.toFrame());
+        canctl.sendFrame(vcu2_cmd.toFrame()); });
+}
+
 int main(int argc, char **argv)
 {
-  rclcpp::init(argc, argv);
+  rclcpp::init(argc, argv); // 初始化 ROS 2
+
+  // 创建一个 ROS 2 节点
   auto node = rclcpp::Node::make_shared("can_driver_node");
+
   RCLCPP_INFO(node->get_logger(), "Starting mc package...");
 
-  auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
-  auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback);
+  // 创建一个 Publisher
+  auto can_publisher =
+      node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
+
+  // 创建 Subscriber（收控制指令）
+  auto subscriber = node->create_subscription<sweeperMsg::McCtrl>(
+      "mc_ctrl",     // 订阅的 topic 名字
+      10,            // 队列长度
+      mcCtrlCallback // 收到消息后的回调
+  );
+
   last_vcu_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type());
 
   Config mc_config;
@@ -38,20 +266,29 @@ int main(int argc, char **argv)
 
   if (!canctl.open(mc_config.can_dev))
   {
-    RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str());
+    RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s",
+                 mc_config.can_dev.c_str());
     return -1;
   }
 
+  // 创建上下文
   auto context = std::make_shared<CanHandlerContext>();
   context->node = node;
-  context->publisher = pub;
-  canctl.setReceiveCallback(receiveHandler, context.get());
+  context->publisher = can_publisher;
 
-  setupTimers(node, dumper, canctl);
+  // 设置接收回调并传递上下文
+  canctl.setReceiveCallback(receiveHandler, (void *)context.get());
 
+  // 添加定时器设置
+  setupTimers(node);
+
+  // 在 ROS shutdown 时自动清理
   rclcpp::on_shutdown([&]()
                       { canctl.close(); });
+
+  // 事件循环
   rclcpp::spin(node);
-  rclcpp::shutdown();
+
+  rclcpp::shutdown(); // 关闭 ROS 2
   return 0;
 }

src/mc/src/timer_tasks.cpp

@@ -1,135 +0,0 @@
-#include "mc/timer_tasks.hpp"
-#include "mc/control_cache.hpp"
-#include "mc/vcu_wakeup.hpp"
-#include "mc/can_struct.h"
-
-extern std::atomic<bool> vcu_awake;
-extern std::atomic<bool> vcu_msg_received;
-extern rclcpp::Time last_vcu_msg_time;
-
-// 定时器回调：VCU 唤醒
-void wakeupTimerTask(const rclcpp::Node::SharedPtr &node, CANDriver &canctl)
-{
-  if (!vcu_awake.load())
-  {
-    RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
-    VCUWakeUp(canctl);
-  }
-}
-
-// 定时器回调：VCU watchdog + Dumper Dial处理
-void vcuWatchdogTask(const rclcpp::Node::SharedPtr &node, DumperController &dumper)
-{
-  auto now = node->now();
-  if (vcu_msg_received.load() && (now - last_vcu_msg_time > rclcpp::Duration::from_seconds(0.5)))
-  {
-    RCLCPP_WARN(node->get_logger(), "[VCU] Timeout, resetting state.");
-    vcu_awake.store(false);
-    vcu_msg_received.store(false);
-    dumper.resetAwake();
-  }
-
-  if (vcu_awake.load())
-  {
-    dumper.tryStartup();
-    auto msg = get_safe_control();
-    dumper.updateDial(static_cast<int>(msg.dump), dumper.getSimpleState());
-  }
-}
-
-// 定时器回调：DBS 控制
-void dbsTimerTask(CANDriver &canctl)
-{
-  auto msg = get_safe_control();
-  dbs_cmd.setEnabled(msg.ehb_anable);
-  dbs_cmd.setPressure(msg.ehb_brake_pressure);
-  canctl.sendFrame(dbs_cmd.toFrame());
-}
-
-// 定时器回调：MCU 控制
-void mcuTimerTask(CANDriver &canctl)
-{
-  auto msg = get_safe_control();
-  mcu_cmd.setEnabled(true);
-  mcu_cmd.setGear(msg.gear);
-  mcu_cmd.setRPM(msg.rpm);
-  mcu_cmd.setBrake(msg.brake);
-  canctl.sendFrame(mcu_cmd.toFrame());
-}
-
-// 定时器回调：EPS 控制
-void epsTimerTask(CANDriver &canctl)
-{
-  auto msg = get_safe_control();
-  eps_cmd.setCenterCmd(0);
-  eps_cmd.setAngle(msg.angle);
-  eps_cmd.setAngularSpeed(msg.angle_speed);
-  eps_cmd.pack();
-  canctl.sendFrame(eps_cmd.toFrame());
-}
-
-// 定时器回调：VCU 灯控、扫地、除尘等功能
-void vcuTimerTask(CANDriver &canctl, DumperController &dumper)
-{
-  static bool last_sweep_ctrl = false; // 上一次扫地状态（用于比较是否发生变化）
-
-  auto msg = get_safe_control();
-
-  vcu1_cmd.setLeftLight(msg.left_light);
-  vcu1_cmd.setDustShake(msg.dust_shake);
-  vcu1_cmd.setHeadLight(msg.headlight);
-
-  vcu2_cmd.setBrakeLight(msg.brake_light);
-  vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
-  vcu2_cmd.setHeadlight(msg.headlight);
-  vcu2_cmd.setMudFlap(msg.mud_flap);
-  vcu2_cmd.setRightLight(msg.right_light);
-  vcu2_cmd.setSpray(msg.spray);
-  vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
-
-  bool sweep_changed = (msg.sweep_ctrl != last_sweep_ctrl);
-  bool can_start_sweep = (msg.sweep_ctrl && dumper.getSimpleState() == 0);
-
-  if (sweep_changed || can_start_sweep)
-  {
-    // 状态变更（开或关） 或者 满足开始扫地条件，发出指令
-    canctl.sendFrame(vcu1_cmd.toFrame());
-    canctl.sendFrame(vcu2_cmd.toFrame());
-
-    last_sweep_ctrl = msg.sweep_ctrl; // 更新上一次状态
-  }
-}
-
-// 注册所有定时器任务
-void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl)
-{
-  static auto timer_wakeup = node->create_wall_timer(
-      std::chrono::seconds(1),
-      [node, &canctl]()
-      { wakeupTimerTask(node, canctl); });
-
-  static auto timer_watchdog = node->create_wall_timer(
-      std::chrono::milliseconds(200),
-      [node, &dumper]()
-      { vcuWatchdogTask(node, dumper); });
-
-  static auto timer_dbs = node->create_wall_timer(
-      std::chrono::milliseconds(10),
-      [&canctl]()
-      { dbsTimerTask(canctl); });
-
-  static auto timer_mcu = node->create_wall_timer(
-      std::chrono::milliseconds(20),
-      [&canctl]()
-      { mcuTimerTask(canctl); });
-
-  static auto timer_eps = node->create_wall_timer(
-      std::chrono::milliseconds(50),
-      [&canctl]()
-      { epsTimerTask(canctl); });
-
-  static auto timer_vcu = node->create_wall_timer(
-      std::chrono::milliseconds(100),
-      [&canctl, &dumper]()
-      { vcuTimerTask(canctl, dumper); });
-}

src/mc/src/vcu_wakeup.cpp

@@ -1,21 +0,0 @@
-#include "mc/vcu_wakeup.hpp"
-#include <thread>
-
-void VCUWakeUp(CANDriver &driver)
-{
-    CANFrame frame;
-    frame.id = 0x18FF8017;
-    frame.dlc = 8;
-    frame.ext = true;
-    frame.rtr = false;
-
-    frame.data[0] = 0x55;
-    frame.data[1] = 0x21;
-    std::fill(frame.data + 2, frame.data + 8, 0xFF);
-    driver.sendFrame(frame);
-
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
-
-    frame.data[0] = 0xAA;
-    driver.sendFrame(frame);
-}

src/radio_ctrl/src/radio_ctrl.cpp

@@ -11,6 +11,7 @@ namespace sweeperMsg = sweeper_interfaces::msg;
 constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f;
 constexpr float GEAR_RATIO = 7.0f;
 constexpr float DELTA_T = 0.02f; // 20ms
+const uint16_t speed[3] = {192, 992, 1792};
 
 class SBUSNode : public rclcpp::Node
 {
@@ -62,12 +63,11 @@ private:
     {
         static int MCU_RPM_MAX = config.mcu_rpm_max;
         static float EPS_ANGLE_MAX = config.eps_angle_max;
-        static bool initialized = false; // 新增初始化标志
 
         bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性
 
         auto msg = sweeperMsg::McCtrl(); // 控制消息对象
-        uint16_t ch_data[10];            // 各通道遥控数据
+        uint16_t ch_data[8];             // 各通道遥控数据
 
         if (data_safe) // 数据安全，进行数据解析并发布
         {
@@ -79,89 +79,42 @@ private:
             }
             // printf("\n");
 
-            uint16_t ctrl = ch_data[4];       // 手刹
-            uint16_t gear = ch_data[5];       // 挡位
-            uint16_t sweep = ch_data[6];      // 清扫
-            uint16_t dump = ch_data[7];       // 垃圾倾倒
-            int16_t speed = ch_data[1] - 992; //[-800,800]
-
-            if (!initialized)
-            {
-                if (sweep == 1792 && dump == 1792)
-                {
-                    initialized = true;
-                    RCLCPP_INFO(this->get_logger(), "[RADIO_CTRL] Initialize Success.");
-                }
-                else
-                {
-                    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000,
-                                         "[RADIO_CTRL] Please set sweep and dump to bottom to initialize.");
-                    return;
-                }
-            }
-
-            switch (dump)
-            {
-            case 1792:
-                msg.dump = 0;
-                break;
-            case 992:
-                msg.dump = 1;
-                break;
-            case 192:
-                msg.dump = 2;
-                break;
-            default:
-                break;
-            }
-
-            static uint16_t last_ctrl = 992; // 初始手刹值
-            static int current_brake = 1;    // 默认上电是拉手刹（1）
-
-            // 判断是否是上拨触发（192）
-            if (last_ctrl == 992 && ctrl == 192)
-            {
-                // 上拨 → 松手刹
-                current_brake = 0;
-            }
-            // 判断是否是下拨触发（1792）
-            else if (last_ctrl == 992 && ctrl == 1792)
-            {
-                // 下拨 → 拉手刹
-                current_brake = 1;
-            }
-
-            last_ctrl = ctrl;
-            msg.brake = current_brake;
-
-            if (msg.brake == 0) // 手刹释放
+            if (ch_data[6] == 192) // 是否使能车辆控制
             {
+                msg.brake = 0;
                 // 挡位选择
-                if (gear == 192)
+                if (ch_data[7] == 192)
                 {
                     msg.gear = 0; // D挡
                 }
-                else if (gear == 1792)
+                else if (ch_data[7] == 1792)
                 {
                     msg.gear = 1; // R挡
                 }
-
-                // 油门 / 刹车逻辑
-                if (speed <= 0)
-                {
-                    msg.ehb_anable = true;
-                    msg.ehb_brake_pressure = -0.01f * std::clamp(static_cast<int>(speed), -800, 0);
-                    msg.rpm = 0;
-                }
                 else
                 {
-                    msg.ehb_anable = false;
-                    msg.ehb_brake_pressure = 0;
-                    msg.rpm = static_cast<uint8_t>(std::clamp(MCU_RPM_MAX * speed / 800, 0, MCU_RPM_MAX));
+                    msg.gear = 0;
+                    msg.brake = 1;
+                    msg.rpm = 0;
+                }
+
+                if (ch_data[7] != 992)
+                {
+                    // 油门 / 刹车逻辑
+                    if (ch_data[1] <= speed[1])
+                    {
+                        msg.brake = 1;
+                        msg.rpm = 0;
+                    }
+                    else
+                    {
+                        msg.brake = 0;
+                        msg.rpm = static_cast<uint8_t>(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1]));
+                    }
                 }
 
                 // 一键清扫
-                if (sweep == 1792)
+                if (ch_data[5] == 1792)
                 {
                     msg.edge_brush_lift = false;
                     msg.sweep_ctrl = false;
@@ -169,17 +122,17 @@ private:
                     msg.mud_flap = false;
                     msg.dust_shake = false;
                 }
-                else if (sweep == 192)
+                else if (ch_data[5] == 192)
                 {
                     msg.edge_brush_lift = true;
                     msg.sweep_ctrl = true;
                     msg.spray = true;
                     msg.mud_flap = true;
-                    msg.dust_shake = true;
+                    msg.dust_shake = false;
                 }
 
                 // 转向逻辑
-                float target_angle = (static_cast<float>(ch_data[3]) - 992) / 800 * EPS_ANGLE_MAX;
+                float target_angle = (static_cast<float>(ch_data[3] - 992)) / 800 * EPS_ANGLE_MAX;
 
                 // 角速度（度/秒）
                 float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T;
@@ -196,7 +149,7 @@ private:
                 msg.angle = target_angle;
                 msg.angle_speed = can_speed;
             }
-            else // 未释放，发送安全默认控制指令
+            else // 未使能状态，发送安全默认控制指令
             {
                 msg.brake = 1;
                 msg.gear = 0;
@@ -214,23 +167,22 @@ private:
             pub_->publish(msg);
 
             RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
-                                                       << "\n  brake: " << static_cast<int>(msg.brake)                             // uint8
-                                                       << "\n  gear: " << static_cast<int>(msg.gear)                               // uint8
-                                                       << "\n  rpm: " << static_cast<int>(msg.rpm)                                 // uint8
-                                                       << "\n  ehb_enable: " << static_cast<int>(msg.ehb_anable)                   // bool
-                                                       << "\n  ehb_brake_pressure: " << static_cast<float>(msg.ehb_brake_pressure) // float32
-                                                       << "\n  angle: " << msg.angle                                               // float32
-                                                       << "\n  angle_speed: " << msg.angle_speed                                   // uint16
-                                                       << "\n  edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift)         // bool
-                                                       << "\n  sweep_ctrl: " << static_cast<int>(msg.sweep_ctrl)                   // bool
-                                                       << "\n  spray: " << static_cast<int>(msg.spray)                             // bool
-                                                       << "\n  mud_flap: " << static_cast<int>(msg.mud_flap)                       // bool
-                                                       << "\n  dust_shake: " << static_cast<int>(msg.dust_shake)                   // bool
-                                                       << "\n  left_light: " << static_cast<int>(msg.left_light)                   // bool
-                                                       << "\n  right_light: " << static_cast<int>(msg.right_light)                 // bool
-                                                       << "\n  brake_light: " << static_cast<int>(msg.brake_light)                 // bool
-                                                       << "\n  headlight: " << static_cast<int>(msg.headlight)                     // bool
-                                                       << "\n  dump: " << static_cast<int>(msg.dump)                               // uint8
+                                                       << "\n  brake: " << static_cast<int>(msg.brake)                           // uint8
+                                                       << "\n  gear: " << static_cast<int>(msg.gear)                             // uint8
+                                                       << "\n  rpm: " << static_cast<int>(msg.rpm)                               // uint8
+                                                       << "\n  ehb_enable: " << static_cast<int>(msg.ehb_anable)                 // bool
+                                                       << "\n  ehb_brake_pressure: " << static_cast<int>(msg.ehb_brake_pressure) // uint8
+                                                       << "\n  angle: " << msg.angle                                             // float32
+                                                       << "\n  angle_speed: " << msg.angle_speed                                 // uint16
+                                                       << "\n  edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift)       // bool
+                                                       << "\n  sweep_ctrl: " << static_cast<int>(msg.sweep_ctrl)                 // bool
+                                                       << "\n  spray: " << static_cast<int>(msg.spray)                           // bool
+                                                       << "\n  mud_flap: " << static_cast<int>(msg.mud_flap)                     // bool
+                                                       << "\n  dust_shake: " << static_cast<int>(msg.dust_shake)                 // bool
+                                                       << "\n  left_light: " << static_cast<int>(msg.left_light)                 // bool
+                                                       << "\n  right_light: " << static_cast<int>(msg.right_light)               // bool
+                                                       << "\n  brake_light: " << static_cast<int>(msg.brake_light)               // bool
+                                                       << "\n  headlight: " << static_cast<int>(msg.headlight)                   // bool
             );
         }
         else

src/sweeper_interfaces/msg/McCtrl.msg

@@ -7,7 +7,7 @@ uint8 rpm	            #转速占空比 0-100
 
 #ehb部分
 bool ehb_anable                 #ehb使能
-float32 ehb_brake_pressure      #ehb制动压力 0-8MPa
+uint8 ehb_brake_pressure        #ehb制动压力 0-8MPa
 
 #eps部分
 float32 angle	    #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减
@@ -24,6 +24,3 @@ bool left_light     #左转向灯
 bool right_light    #右转向灯
 bool brake_light    #刹车灯
 bool headlight      #大灯
-
-#倒灰
-uint8 dump          # 0 全部收回 1 斗升 2 倒斗