Auto commit at 2025-05-29 14:24:58

src/mc/src/mc.cpp

@@ -10,8 +10,16 @@
 
 CANDriver canctl;
 
-// 急停状态量
+struct CanHandlerContext
-std::atomic<bool> estop_active = false;
+{
+  rclcpp::Node::SharedPtr node;
+  std::shared_ptr<rclcpp::Publisher<mc::msg::CanFrame>> publisher;
+};
+
+std::atomic<bool> estop_active = false;       // 急停状态量
+std::atomic<bool> estop_msg_received = false; // 是否收到过急停帧
+std::atomic<bool> vcu_awake = false;          // 是否唤醒成功
+rclcpp::Time last_estop_msg_time;             // 上次收到急停帧的时间
 
 struct ControlCache
 {
@@ -79,19 +87,15 @@ mc::msg::McCtrl get_safe_control()
 void receiveHandler(const CANFrame &frame, void *userData)
 {
   // 获取 ROS 2 Publisher
-  auto publisher = *static_cast<std::shared_ptr<rclcpp::Publisher<mc::msg::CanFrame>> *>(userData);
+  auto context = static_cast<CanHandlerContext *>(userData);
+  auto publisher = context->publisher;
+  auto now = context->node->now();
 
   // 创建一个新的消息
   auto msg = mc::msg::CanFrame();
   msg.id = frame.id;
   msg.dlc = frame.dlc;
-  msg.data.resize(frame.dlc);
+  msg.data.assign(frame.data, frame.data + frame.dlc);
-
-  // 复制数据
-  for (int i = 0; i < frame.dlc; ++i)
-  {
-    msg.data[i] = frame.data[i];
-  }
 
   // 发布消息
   publisher->publish(msg);
@@ -116,6 +120,10 @@ void receiveHandler(const CANFrame &frame, void *userData)
   // 检查是否为急停报文
   if (frame.id == 0x18FA0121 && frame.dlc >= 3)
   {
+    last_estop_msg_time = now; // 更新时间
+    estop_msg_received.store(true);
+    vcu_awake.store(true); // 唤醒成功
+
     uint8_t estop_status = (frame.data[2] & 0x0c) >> 2; // 急停状态位
 
     // 急停被按下
@@ -167,8 +175,60 @@ void mcCtrlCallback(const mc::msg::McCtrl::SharedPtr msg)
   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), []()
+      {
+        if (!vcu_awake.load())
+        {
+          std::cout << "[VCU] 未唤醒，发送唤醒帧..." << std::endl;
+          VCUWakeUp(); // 发唤醒帧函数
+          } });
+
+  // 急停报文 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_estop_msg_time;
+
+        if (estop_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
+        {
+          std::cout << "[VCU] 急停报文超时，重置唤醒状态。" << std::endl;
+          estop_msg_received.store(false);
+          vcu_awake.store(false);
+        } });
+
   // MCU、VCU1 控制，50Hz
   static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
       std::chrono::milliseconds(20), []()
@@ -236,34 +296,6 @@ void setupTimers(rclcpp::Node::SharedPtr node)
         canctl.sendFrame(edge_brush_cmd.toFrame()); });
 }
 
-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);
-}
-
 int main(int argc, char **argv)
 {
   rclcpp::init(argc, argv); // 初始化 ROS 2
@@ -274,7 +306,7 @@ int main(int argc, char **argv)
   auto node = rclcpp::Node::make_shared("can_driver_node");
 
   // 创建一个 Publisher
-  auto publisher = node->create_publisher<mc::msg::CanFrame>("can_data", 10);
+  auto can_publisher = node->create_publisher<mc::msg::CanFrame>("can_data", 10);
 
   // 创建 Subscriber（收控制指令）
   auto subscriber = node->create_subscription<mc::msg::McCtrl>(
@@ -293,10 +325,13 @@ int main(int argc, char **argv)
     return -1;
   }
 
-  VCUWakeUp();
+  // 创建上下文
+  auto context = std::make_shared<CanHandlerContext>();
+  context->node = node;
+  context->publisher = can_publisher;
 
-  // 设置接收回调并传递 ROS 2 Publisher 作为 userData
+  // 设置接收回调并传递上下文
-  canctl.setReceiveCallback(receiveHandler, (void *)&publisher);
+  canctl.setReceiveCallback(receiveHandler, (void *)context.get());
 
   // 添加定时器设置
   setupTimers(node);