From ec1acd4f53fd8578ce21bebcfb2fb96092ea136c Mon Sep 17 00:00:00 2001
From: cxh <zxwl@56.com>
Date: Thu, 29 May 2025 14:58:54 +0800
Subject: [PATCH] Auto commit at 2025-05-29 14:58:54

---
 src/mc/src/mc.cpp | 67 +++++++++++++++++++++++++----------------------
 1 file changed, 35 insertions(+), 32 deletions(-)

diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp
index 5e850c2..41a1dd3 100644
--- a/src/mc/src/mc.cpp
+++ b/src/mc/src/mc.cpp
@@ -21,6 +21,9 @@ std::atomic<bool> estop_msg_received = false; // 是否收到过急停帧
 std::atomic<bool> vcu_awake = false;          // 是否唤醒成功
 rclcpp::Time last_estop_msg_time;             // 上次收到急停帧的时间
 
+// 全局开关，控制是否打印 CAN 消息到终端
+bool g_can_print_enable = true;
+
 struct ControlCache
 {
   std::mutex mutex; // 防止多线程冲突
@@ -89,35 +92,36 @@ void receiveHandler(const CANFrame &frame, void *userData)
   // 获取 ROS 2 Publisher
   auto context = static_cast<CanHandlerContext *>(userData);
   auto publisher = context->publisher;
-  auto now = context->node->now();
+  auto node = context->node;
+  auto now = node->now();
 
-  // 创建一个新的消息
+  // 创建并发布 CAN 消息
   auto msg = mc::msg::CanFrame();
   msg.id = frame.id;
   msg.dlc = frame.dlc;
   msg.data.assign(frame.data, frame.data + frame.dlc);
-
-  // 发布消息
   publisher->publish(msg);
 
-  // 终端打印
-  // std::cout << "CAN ID: ";
-  // if (frame.id > 0x7ff)
-  // {
-  //   std::cout << std::hex << std::uppercase << frame.id;
-  // }
-  // else
-  // {
-  //   std::cout << std::hex << std::uppercase << "     " << frame.id;
-  // }
-  // std::cout << " Data: ";
-  // for (int i = 0; i < frame.dlc; ++i)
-  // {
-  //   std::cout << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
-  // }
-  // std::cout << std::dec << "\n";
+  // 根据开关决定是否打印 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());
+  }
+
+  // 急停报文检查
   if (frame.id == 0x18FA0121 && frame.dlc >= 3)
   {
     last_estop_msg_time = now; // 更新时间
@@ -131,7 +135,7 @@ void receiveHandler(const CANFrame &frame, void *userData)
     {
       if (!estop_active.load())
       {
-        std::cout << "!!! E-STOP TRIGGERED, entering safe mode" << std::endl;
+        RCLCPP_WARN(node->get_logger(), "E-STOP TRIGGERED, entering safe mode");
       }
 
       estop_active.store(true);
@@ -160,7 +164,7 @@ void receiveHandler(const CANFrame &frame, void *userData)
     {
       if (estop_active.load())
       {
-        std::cout << "E-STOP released, resuming control" << std::endl;
+        RCLCPP_INFO(node->get_logger(), "E-STOP released, resuming control");
       }
       estop_active.store(false);
     }
@@ -207,13 +211,13 @@ void setupTimers(rclcpp::Node::SharedPtr node)
 {
   // VCU 唤醒帧发送定时器，1Hz
   static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer(
-      std::chrono::seconds(1), []()
+      std::chrono::seconds(1), [node]()
       {
         if (!vcu_awake.load())
         {
-          std::cout << "[VCU] 未唤醒，发送唤醒帧..." << std::endl;
-          VCUWakeUp(); // 发唤醒帧函数
-          } });
+          RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
+          VCUWakeUp();
+        } });
 
   // 急停报文 watchdog 检查，200ms
   static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer(
@@ -224,7 +228,7 @@ void setupTimers(rclcpp::Node::SharedPtr node)
 
         if (estop_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
         {
-          std::cout << "[VCU] 急停报文超时，重置唤醒状态。" << std::endl;
+          RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] Emergency stop message timeout, resetting wake-up state.");
           estop_msg_received.store(false);
           vcu_awake.store(false);
         } });
@@ -300,11 +304,11 @@ int main(int argc, char **argv)
 {
   rclcpp::init(argc, argv); // 初始化 ROS 2
 
-  printf("Hello world mc package\n");
-
   // 创建一个 ROS 2 节点
   auto node = rclcpp::Node::make_shared("can_driver_node");
 
+  RCLCPP_INFO(node->get_logger(), "Starting mc package...");
+
   // 创建一个 Publisher
   auto can_publisher = node->create_publisher<mc::msg::CanFrame>("can_data", 10);
 
@@ -322,8 +326,7 @@ int main(int argc, char **argv)
 
   if (!canctl.open(mc_config.can_dev))
   {
-    std::cerr << "Failed to open CAN interface :";
-    std::cerr << mc_config.can_dev << std::endl;
+    RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str());
     return -1;
   }