Auto commit at 2025-06-05 08:43:54

src/ctrl_arbiter/src/ctrl_arbiter.cpp

@@ -74,7 +74,7 @@ private:
 
         // 所有控制源均失联，发布安全默认指令
         mc::msg::McCtrl safe_msg;
-        safe_msg.mcu_enabled = false;
+        safe_msg.mcu_enabled = true;
         safe_msg.brake = 1;
         safe_msg.gear = 0;
         safe_msg.rpm = 0;

src/mc/src/mc.cpp

@@ -10,8 +10,19 @@
 
 CANDriver canctl;
 
-// 急停状态量
-std::atomic<bool> estop_active = false;
+struct CanHandlerContext
+{
+  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;             // 上次收到急停帧的时间
+
+// 全局开关，控制是否打印 CAN 消息到终端
+bool g_can_print_enable = false;
 
 struct ControlCache
 {
@@ -56,7 +67,7 @@ mc::msg::McCtrl get_safe_control()
   if (!control_cache.get(msg))
   {
     // 超时或未接收到控制数据，进入安全状态
-    msg.mcu_enabled = false;
+    msg.mcu_enabled = true;
     msg.brake = 1;
     msg.gear = 0;
     msg.rpm = 0;
@@ -79,43 +90,44 @@ 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 node = context->node;
+  auto now = node->now();
 
-  // 创建一个新的消息
+  // 创建并发布 CAN 消息
   auto msg = mc::msg::CanFrame();
   msg.id = frame.id;
   msg.dlc = frame.dlc;
-  msg.data.resize(frame.dlc);
-
-  // 复制数据
-  for (int i = 0; i < frame.dlc; ++i)
-  {
-    msg.data[i] = frame.data[i];
-  }
-
-  // 发布消息
+  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; // 更新时间
+    estop_msg_received.store(true);
+    vcu_awake.store(true); // 唤醒成功
+
     uint8_t estop_status = (frame.data[2] & 0x0c) >> 2; // 急停状态位
 
     // 急停被按下
@@ -123,14 +135,14 @@ 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);
 
       // 立即更新控制缓存，强制进入安全状态
       mc::msg::McCtrl safe_msg;
-      safe_msg.mcu_enabled = false;
+      safe_msg.mcu_enabled = true;
       safe_msg.brake = 1;
       safe_msg.gear = 0;
       safe_msg.rpm = 0;
@@ -152,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);
     }
@@ -167,8 +179,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), [node]()
+      {
+        if (!vcu_awake.load())
+        {
+          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(
+      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))
+        {
+          RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] Emergency stop message timeout, resetting wake-up state.");
+          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,45 +300,17 @@ 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
 
-  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 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>(
@@ -283,31 +319,35 @@ int main(int argc, char **argv)
       mcCtrlCallback // 收到消息后的回调
   );
 
+  last_estop_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type());
+
   Config mc_config;
   load_config(mc_config);
 
   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;
   }
 
-  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);
 
-  // ROS 2 spin
+  // 在 ROS shutdown 时自动清理
+  rclcpp::on_shutdown([&]()
+                      { canctl.close(); });
+
+  // 事件循环
   rclcpp::spin(node);
 
-  // 关闭 CAN 接口
-  canctl.close();
-
   rclcpp::shutdown(); // 关闭 ROS 2
-
   return 0;
 }

src/mqtt_report/src/mqtt_report.cpp

@@ -10,6 +10,26 @@ Config config; // 清扫车配置文件
 
 GeneralMsg info_report; // 常规消息上报
 
+std::deque<float> waterLevelHistory;
+const size_t maxSamples = 10;
+
+void updateWaterLevel(float newLevel)
+{
+    waterLevelHistory.push_back(newLevel);
+    if (waterLevelHistory.size() > maxSamples)
+    {
+        waterLevelHistory.pop_front();
+    }
+
+    float sum = 0;
+    for (float v : waterLevelHistory)
+        sum += v;
+    float filteredLevel = sum / waterLevelHistory.size();
+
+    filteredLevel = std::clamp(filteredLevel, 0.0f, 180.0f);
+    info_report.waterLevel = static_cast<int>(std::round(filteredLevel / 1.8f));
+}
+
 // 解析can报文，做消息上报
 void Msg_Handler(const mc::msg::CanFrame::SharedPtr msg)
 {
@@ -25,7 +45,8 @@ void Msg_Handler(const mc::msg::CanFrame::SharedPtr msg)
     case 0x1A2:
     {
         const uint8_t gearByte = msg->data[0];
-        info_report.gear = (gearByte & 0x01) ? 3 : ((gearByte >> 1) & 0x03);
+
+        info_report.gear = (gearByte & 0x01) ? 3 : ((gearByte >> 2) & 0x03);
 
         const int tempRaw = (msg->data[5] << 8) | msg->data[6];
         info_report.motorTemp = static_cast<int>(tempRaw * 0.1f - 100.0f);
@@ -38,6 +59,8 @@ void Msg_Handler(const mc::msg::CanFrame::SharedPtr msg)
         float gear_ratio = 32.0;
         float wheel_radius = 0.15; // 轮胎半径，单位米
         float vehicle_speed_kmh = (2 * 3.1416 * wheel_radius * (motorspeed / gear_ratio) * 60.0) / 1000.0;
+        // 保留两位小数
+        vehicle_speed_kmh = std::round(vehicle_speed_kmh * 100.0f) / 100.0f;
         info_report.speed = vehicle_speed_kmh;
         break;
     }
@@ -49,7 +72,7 @@ void Msg_Handler(const mc::msg::CanFrame::SharedPtr msg)
         uint16_t WaterValue = (WaterValue_h & 0x3f) * 16 + (WaterValue_l & 0x0f);
         float WaterLevel = WaterValue * (-0.235) + 199.75;
         WaterLevel = std::clamp(WaterLevel, 0.0f, 180.0f);
-        info_report.waterLevel = static_cast<int>(std::round(WaterLevel / 1.8f)); // 液位百分比
+        updateWaterLevel(WaterLevel);
         break;
     }
 
@@ -256,12 +279,14 @@ int main(int argc, char **argv)
 
     if (!load_config(config))
     {
-        std::cerr << "Failed to load MQTT config." << std::endl;
+        RCLCPP_ERROR(rclcpp::get_logger("main"), "Failed to load MQTT config.");
         return 1;
     }
 
     auto node = std::make_shared<CanDataSubscriber>(config);
 
+    RCLCPP_INFO(node->get_logger(), "MqttReport node started.");
+
     rclcpp::spin(node);
     rclcpp::shutdown();
     return 0;

src/radio_ctrl/src/radio_ctrl.cpp

@@ -157,7 +157,7 @@ private:
             }
             else // 未使能状态，发送安全默认控制指令
             {
-                msg.mcu_enabled = false;
+                msg.mcu_enabled = true;
                 msg.brake = 1;
                 msg.gear = 0;
                 msg.rpm = 0;