Auto commit at 2025-06-18 15:49:41

src/mc/include/mc/can_utils.hpp

@@ -0,0 +1,14 @@
+#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

@@ -0,0 +1,20 @@
+#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/dumper.h

@@ -28,11 +28,11 @@ class DumperController
 public:
     DumperController(CANDriver &driver);
 
-    void onStartup();                                 // 上电复位（只在内部用）
+    void onStartup();                                         // 上电复位（只在内部用）
-    void tryStartup();                                // 唤醒后自动执行复位（防重）
+    void tryStartup();                                        // 唤醒后自动执行复位（防重）
-    void updateDial(int new_dial, int current_state); // 拨钮变化检测与处理
+    void updateDial(int new_dial, int current_state);         // 拨钮变化检测与处理
-    void resetAwake();                                // 清除唤醒状态（VCU 掉线）
+    void resetAwake();                                        // 清除唤醒状态（VCU 掉线）
-
+    void setSweeping(bool sweeping) { sweeping_ = sweeping; } // 外部设置扫地状态，
     int getSimpleState() const;
 
 private:
@@ -53,6 +53,7 @@ private:
     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_;
 };

src/mc/include/mc/timer_tasks.hpp

@@ -0,0 +1,6 @@
+#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

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

src/mc/src/can_utils.cpp

@@ -0,0 +1,39 @@
+#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

@@ -0,0 +1,44 @@
+#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

@@ -124,7 +124,13 @@ void DumperController::runActionSequence(const std::vector<DumperAction> &action
 // 每个动作的具体 CAN 指令 + 延时
 void DumperController::doAction(DumperAction act)
 {
-    constexpr int delay_ms = 10000; // 可统一调整延时时间（单位：毫秒）
+    if (sweeping_)
+    {
+        std::cout << "[Warning] Sweeping active, dumper action blocked.\n";
+        return;
+    }
+
+    constexpr int delay_ms = 15000; // 可统一调整延时时间（单位：毫秒）
     using namespace std::chrono_literals;
 
     CANFrame frame1;

src/mc/src/mc.cpp

@@ -1,283 +1,36 @@
-#include <cstdio>
-#include <iostream>
-#include <iomanip>
-#include "mc/can_driver.h"
-#include "mc/can_struct.h"
-#include "mc/get_config.h"
-#include "mc/dumper.h"
 #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 "sweeper_interfaces/msg/can_frame.hpp"
 #include "sweeper_interfaces/msg/mc_ctrl.hpp"
 
-// 全局开关，控制是否打印 CAN 消息到终端
-bool g_can_print_enable = false;
-
 namespace sweeperMsg = sweeper_interfaces::msg;
 
-CANDriver canctl;                // can驱动实例
+CANDriver canctl;
-DumperController dumper(canctl); // 斗控制器实例
+DumperController dumper(canctl);
-
+std::atomic<bool> vcu_msg_received = false;
-struct CanHandlerContext
+std::atomic<bool> vcu_awake = false;
-{
+rclcpp::Time last_vcu_msg_time;
-  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数据帧的时间
-
-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;
-  msg.data.assign(frame.data, frame.data + frame.dlc);
-  publisher->publish(msg);
-
-  // 根据开关决定是否打印 CAN 消息
-  if (g_can_print_enable)
-  {
-    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);
-            dumper.resetAwake();
-          }
-
-          if (vcu_awake.load())
-          {
-            dumper.tryStartup();
-
-            sweeperMsg::McCtrl msg = get_safe_control();
-
-            int to=static_cast<int>(msg.dump);
-            int from =dumper.getSimpleState();
-
-            dumper.updateDial(to, from);
-          } });
-
-  //  DBS控制，100Hz
-  static rclcpp::TimerBase::SharedPtr timer_dbs = node->create_wall_timer(
-      std::chrono::milliseconds(10), []()
-      {
-        sweeperMsg::McCtrl msg = get_safe_control();
-
-        dbs_cmd.setEnabled(msg.ehb_anable);
-        dbs_cmd.setPressure(msg.ehb_brake_pressure);
-        canctl.sendFrame(dbs_cmd.toFrame()); });
-
-  // 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); // 初始化 ROS 2
+  rclcpp::init(argc, argv);
-
-  // 创建一个 ROS 2 节点
   auto node = rclcpp::Node::make_shared("can_driver_node");
-
   RCLCPP_INFO(node->get_logger(), "Starting mc package...");
 
-  // 创建一个 Publisher
+  auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
-  auto can_publisher = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
+  auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback);
-
-  // 创建 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;
@@ -289,24 +42,16 @@ int main(int argc, char **argv)
     return -1;
   }
 
-  // 创建上下文
   auto context = std::make_shared<CanHandlerContext>();
   context->node = node;
-  context->publisher = can_publisher;
+  context->publisher = pub;
+  canctl.setReceiveCallback(receiveHandler, context.get());
 
-  // 设置接收回调并传递上下文
+  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

@@ -0,0 +1,89 @@
+#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;
+
+void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl)
+{
+    static auto timer_wakeup = node->create_wall_timer(
+        std::chrono::seconds(1), [node, &canctl]()
+        {
+        if (!vcu_awake.load())
+        {
+          RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
+          VCUWakeUp(canctl);
+        } });
+
+    static auto timer_watchdog = node->create_wall_timer(
+        std::chrono::milliseconds(200), [node, &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());
+        } });
+
+    static auto timer_dbs = node->create_wall_timer(
+        std::chrono::milliseconds(10), [&canctl]()
+        {
+        auto msg = get_safe_control();
+        dbs_cmd.setEnabled(msg.ehb_anable);
+        dbs_cmd.setPressure(msg.ehb_brake_pressure);
+        canctl.sendFrame(dbs_cmd.toFrame()); });
+
+    static auto timer_mcu = node->create_wall_timer(
+        std::chrono::milliseconds(20), [&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()); });
+
+    static auto timer_eps = node->create_wall_timer(
+        std::chrono::milliseconds(50), [&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()); });
+
+    static auto timer_vcu = node->create_wall_timer(
+        std::chrono::milliseconds(100), [&canctl, &dumper]()
+        {
+        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);
+
+        if (msg.sweep_ctrl && dumper.getSimpleState() == 0)
+        {
+          canctl.sendFrame(vcu1_cmd.toFrame());
+          canctl.sendFrame(vcu2_cmd.toFrame());
+        } });
+}

src/mc/src/vcu_wakeup.cpp

@@ -0,0 +1,21 @@
+#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);
+}