Initial commit

sensors/can_driver/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(can_driver)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  can_msgs
+  message_generation
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+set(LOCAL_LIBS ${CMAKE_CURRENT_SOURCE_DIR}/libcontrolcan.so)
+
+catkin_package(
+  CATKIN_DEPENDS roscpp std_msgs message_runtime
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  include
+)
+
+add_executable(can_ros_node
+  src/can_ros_node.cpp
+  src/can_driver.cpp
+)
+
+target_link_libraries(can_ros_node
+  ${catkin_LIBRARIES}
+  ${LOCAL_LIBS}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)

sensors/can_driver/include/can_ros_package/can_ros_node.h

@@ -0,0 +1,59 @@
+#ifndef CAN_ROS_PACKAGE_CAN_ROS_NODE_H
+#define CAN_ROS_PACKAGE_CAN_ROS_NODE_H
+
+#include <ros/ros.h>
+#include <vector>
+#include <thread>
+#include <mutex>
+#include <atomic>
+#include <condition_variable>
+#include "controlcan.h"
+#include "can_msgs/Frame.h"
+
+namespace can_ros_package {
+
+class CanDriver {
+public:
+    CanDriver();
+    ~CanDriver();
+    
+    bool init();
+    bool initCanChannel(int channel);
+    bool sendToCan1(VCI_CAN_OBJ& msg);
+    bool sendToCan2(VCI_CAN_OBJ& msg);
+    std::vector<VCI_CAN_OBJ> getReceivedMessages();
+    void stop();
+    
+    bool isCan1Connected();
+    bool isCan2Connected();
+    
+private:
+    void receiveLoop();
+    void reconnect(int channel);
+    bool tryReconnectChannel(int channel);
+    
+    
+    std::atomic<bool> is_running_;
+    std::atomic<bool> can1_connected_;
+    std::atomic<bool> can2_connected_;
+    DWORD device_type_;
+    DWORD device_ind_;
+    VCI_BOARD_INFO board_info_;
+    
+    std::thread receive_thread_;
+    std::mutex receive_mutex_;
+    std::mutex send_mutex_;
+    std::mutex reconnect_mutex_;
+    std::vector<VCI_CAN_OBJ> can_msgs_;
+    
+    // 接收缓冲区状态管理
+    std::condition_variable receive_cv_;
+    bool new_data_available_;
+    
+    const int reconnect_interval_ms_ = 600;
+    const int max_reconnect_attempts_ = 5;  // 最大重连尝试次数
+};
+
+} // namespace can_ros_package
+
+#endif // CAN_ROS_PACKAGE_CAN_ROS_NODE_H

sensors/can_driver/include/can_ros_package/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 // CONTROLCAN_H
+

sensors/can_driver/launch/can_driver.launch

@@ -0,0 +1,22 @@
+<launch>
+  <!-- CAN驱动节点 -->
+  <node pkg="can_driver" type="can_ros_node" name="can_ros_node" output="screen">
+    <!-- CAN设备参数 -->
+    <param name="device_type" value="4" />
+    <param name="device_index" value="0" />
+    <param name="can_index_1" value="0" />
+    <param name="can_index_2" value="1" />
+    
+    <!-- 波特率设置，单位为kbps  波特率修改在can_driver.cpp initCanChannel 函数内修改 -->
+    <!-- <param name="baud_rate_1" value="1000" />
+    <param name="baud_rate_2" value="250" /> -->
+    
+    <!-- ROS主题配置 -->
+    <param name="vcu_topic" value="can_frame_topic" />
+    <param name="bms_topic" value="/sent_messages_bms" />
+    <param name="output_topic" value="/receive_canMessages" />
+    
+    <!-- 接收缓冲区大小 -->
+    <param name="receive_buffer_size" value="1000" />
+  </node>
+</launch>

sensors/can_driver/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>can_driver</name>
+  <version>0.0.1</version>
+  <description>ROS1 package for CAN bus communication</description>
+  <maintainer email="your@email.com">Your Name</maintainer>
+  
+  <license>BSD</license>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>can_msgs</build_depend>
+  
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>can_msgs</exec_depend>
+
+  <export>
+    <build_type>catkin</build_type>
+  </export>
+</package>
+

sensors/can_driver/src/can_driver.cpp

@@ -0,0 +1,314 @@
+#include "can_ros_package/can_ros_node.h"
+#include <iostream>
+#include <thread>
+#include <chrono>
+
+namespace can_ros_package {
+
+CanDriver::CanDriver() 
+    : is_running_(false),
+      device_type_(VCI_USBCAN2), 
+      device_ind_(0),
+      can1_connected_(false),
+      can2_connected_(false),
+      reconnect_interval_ms_(600),
+      max_reconnect_attempts_(5),
+      new_data_available_(false) {
+    // 初始化互斥锁和原子变量
+}
+
+CanDriver::~CanDriver() {
+    stop();
+}
+
+bool CanDriver::init() {
+    // 打开设备
+    if (VCI_OpenDevice(device_type_, device_ind_, 0) != STATUS_OK) {
+        std::cerr << "Failed to open CAN device" << std::endl;
+        return false;
+    }
+    
+    // 读取设备信息
+    if (VCI_ReadBoardInfo(device_type_, device_ind_, &board_info_) != STATUS_OK) {
+        std::cerr << "Failed to read board info" << std::endl;
+        VCI_CloseDevice(device_type_, device_ind_);
+        return false;
+    }
+    
+    // 分别初始化两路CAN通道
+    bool can1_init = initCanChannel(0);
+    bool can2_init = initCanChannel(1);
+    
+    can1_connected_ = can1_init;
+    can2_connected_ = can2_init;
+    
+    if (!can1_init && !can2_init) {
+        VCI_CloseDevice(device_type_, device_ind_);
+        return false;
+    }
+    
+    is_running_ = true;
+    new_data_available_ = false;
+    
+    // 启动接收线程
+    receive_thread_ = std::thread(&CanDriver::receiveLoop, this);
+    
+    return true;
+}
+
+// 波特率	Timing0 (BTR0)	Timing1 (BTR1)	说明
+// 1 Mbps	0x00	0x14	高速通信
+// 500 Kbps	0x00	0x1C	最常用配置
+// 250 Kbps	0x01	0x1C	中等速度通信
+// 125 Kbps	0x03	0x1C	低速通信，你的原始配置
+// 50 Kbps	0x09	0x1C	长距离或低干扰环境
+// 10 Kbps	0x13	0x22	非常低速，特殊场景
+bool CanDriver::initCanChannel(int channel) {
+    VCI_INIT_CONFIG config;
+    config.AccCode = 0;
+    config.AccMask = 0xFFFFFFFF;
+    config.Filter = 1;  // 接收所有帧
+    
+    if(channel == 0){
+        // 设置1000Kbps波特率
+        config.Timing0 = 0x00;
+        config.Timing1 = 0x14;
+    } else if(channel == 1) {
+        // 设置250Kbps波特率
+        config.Timing0 = 0x01;
+        config.Timing1 = 0x1C;
+    }
+    
+    config.Mode = 0;  // 正常模式
+    
+    if (VCI_InitCAN(device_type_, device_ind_, channel, &config) != STATUS_OK) {
+        std::cerr << "Failed to initialize CAN channel " << channel << std::endl;
+        return false;
+    }
+    
+    // 启动前短暂延迟
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    
+    if (VCI_StartCAN(device_type_, device_ind_, channel) != STATUS_OK) {
+        std::cerr << "Failed to start CAN channel " << channel << std::endl;
+        return false;
+    }
+    
+    return true;
+}
+
+bool CanDriver::sendToCan1(VCI_CAN_OBJ& msg) {
+    if (!can1_connected_) {
+        return false;
+    }
+    
+    std::lock_guard<std::mutex> send_lock(send_mutex_);
+    std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
+    
+    int result = VCI_Transmit(device_type_, device_ind_, 0, &msg, 1);
+    
+    if (result != STATUS_OK) {
+        std::cerr << "Failed to send to CAN1, error code: " << result << std::endl;
+        can1_connected_ = false;
+        std::thread(&CanDriver::reconnect, this, 0).detach();
+        return false;
+    }
+    
+    return true;
+}
+
+bool CanDriver::sendToCan2(VCI_CAN_OBJ& msg) {
+    if (!can2_connected_) {
+        return false;
+    }
+    
+    std::lock_guard<std::mutex> send_lock(send_mutex_);
+    std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
+    
+    int result = VCI_Transmit(device_type_, device_ind_, 1, &msg, 1);
+    
+    if (result != STATUS_OK) {
+        std::cerr << "Failed to send to CAN2, error code: " << result << std::endl;
+        can2_connected_ = false;
+        std::thread(&CanDriver::reconnect, this, 1).detach();
+        return false;
+    }
+    
+    return true;
+}
+
+void CanDriver::receiveLoop() {
+    VCI_CAN_OBJ rec[3000];
+    int reclen = 0;
+    
+    while (is_running_) {
+        // 接收CAN1数据
+        if (can1_connected_) {
+            {
+                std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
+                reclen = VCI_Receive(device_type_, device_ind_, 0, rec, 3000, 0); // 0ms超时实现非阻塞读取
+            }
+            
+            if (reclen < 0) {
+                std::cerr << "Error receiving from CAN1, error code: " << reclen << std::endl;
+                can1_connected_ = false;
+                std::thread(&CanDriver::reconnect, this, 0).detach();
+            } else if (reclen > 0) {
+                std::lock_guard<std::mutex> receive_lock(receive_mutex_);
+                for (int j = 0; j < reclen; j++) {
+                    can_msgs_.push_back(rec[j]);
+                }
+                new_data_available_ = true;
+                receive_cv_.notify_one();
+            }
+        }
+        
+        // 接收CAN2数据
+        if (can2_connected_) {
+            {
+                std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
+                reclen = VCI_Receive(device_type_, device_ind_, 1, rec, 3000, 0); // 0ms超时实现非阻塞读取
+            }
+            
+            if (reclen < 0) {
+                std::cerr << "Error receiving from CAN2, error code: " << reclen << std::endl;
+                can2_connected_ = false;
+                std::thread(&CanDriver::reconnect, this, 1).detach();
+            } else if (reclen > 0) {
+                std::lock_guard<std::mutex> receive_lock(receive_mutex_);
+                for (int j = 0; j < reclen; j++) {
+                    can_msgs_.push_back(rec[j]);
+                }
+                new_data_available_ = true;
+                receive_cv_.notify_one();
+            }
+        }
+        
+        // 短暂休眠，避免CPU占用过高
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+}
+
+std::vector<VCI_CAN_OBJ> CanDriver::getReceivedMessages() {
+    std::unique_lock<std::mutex> lock(receive_mutex_);
+    
+    // 等待新数据或超时
+    if (!new_data_available_) {
+        receive_cv_.wait_for(lock, std::chrono::milliseconds(10));
+    }
+    
+    std::vector<VCI_CAN_OBJ> result = can_msgs_;
+    can_msgs_.clear();
+    new_data_available_ = false;
+    return result;
+}
+
+void CanDriver::reconnect(int channel) {
+    std::string channel_name = (channel == 0) ? "CAN1" : "CAN2";
+    int attempt = 0;
+    
+    while (is_running_ && attempt < max_reconnect_attempts_ && 
+          !(channel == 0 ? can1_connected_ : can2_connected_)) 
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_));
+        
+        if (tryReconnectChannel(channel)) {
+            ROS_INFO_STREAM("Successfully reconnected to " << channel_name);
+            break;
+        } else {
+            ROS_WARN_STREAM("Reconnect attempt " << (attempt+1) 
+                          << "/" << max_reconnect_attempts_ 
+                          << " failed for " << channel_name);
+            attempt++;
+        }
+    }
+    
+    if (attempt >= max_reconnect_attempts_) {
+        ROS_ERROR_STREAM("Giving up on reconnecting " << channel_name 
+                      << " after " << max_reconnect_attempts_ << " attempts");
+    }
+}
+
+bool CanDriver::tryReconnectChannel(int channel) {
+    std::lock_guard<std::mutex> reconnect_lock(reconnect_mutex_);
+    std::string channel_name = (channel == 0) ? "CAN1" : "CAN2";
+    
+    // 1. 重置通道
+    if (is_running_) {
+        VCI_ResetCAN(device_type_, device_ind_, channel);
+        VCI_ClearBuffer(device_type_, device_ind_, channel);
+    }
+    
+    // 2. 重新初始化通道
+    VCI_INIT_CONFIG config;
+    config.AccCode = 0;
+    config.AccMask = 0xFFFFFFFF;
+    config.Filter = 1;  // 接收所有帧
+    
+    if(channel == 0){
+        // 设置1000Kbps波特率
+        config.Timing0 = 0x00;
+        config.Timing1 = 0x1C;
+    } else if(channel == 1) {
+        // 设置250Kbps波特率
+        config.Timing0 = 0x01;
+        config.Timing1 = 0x1C;
+    }
+    
+    config.Mode = 0;  // 正常模式
+    
+    if (VCI_InitCAN(device_type_, device_ind_, channel, &config) != STATUS_OK) {
+        return false;
+    }
+    
+    // 启动前短暂延迟
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    
+    if (VCI_StartCAN(device_type_, device_ind_, channel) != STATUS_OK) {
+        return false;
+    }
+    
+    // 3. 更新连接状态
+    if (channel == 0) {
+        can1_connected_ = true;
+    } else {
+        can2_connected_ = true;
+    }
+    
+    // 启动后延迟确保稳定
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    return true;
+}
+
+void CanDriver::stop() {
+    if (is_running_) {
+        is_running_ = false;
+        can1_connected_ = false;
+        can2_connected_ = false;
+        
+        // 通知接收线程退出
+        {
+            std::lock_guard<std::mutex> lock(receive_mutex_);
+            receive_cv_.notify_all();
+        }
+        
+        if (receive_thread_.joinable()) {
+            receive_thread_.join();
+        }
+        
+        // 复位并关闭两路CAN通道
+        VCI_ResetCAN(device_type_, device_ind_, 0);
+        VCI_ResetCAN(device_type_, device_ind_, 1);
+        VCI_CloseDevice(device_type_, device_ind_);
+    }
+}
+
+bool CanDriver::isCan1Connected() {
+    return can1_connected_;
+}
+
+bool CanDriver::isCan2Connected() {
+    return can2_connected_;
+}
+
+} // namespace can_ros_package

sensors/can_driver/src/can_ros_node.cpp

@@ -0,0 +1,120 @@
+#include "can_ros_package/can_ros_node.h"
+#include "ros/ros.h"
+#include "can_msgs/Frame.h"
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "can_ros_node");
+    ros::NodeHandle nh;
+    
+    can_ros_package::CanDriver can_driver;
+    if (!can_driver.init()) {
+        ROS_FATAL("Failed to initialize CAN driver");
+        return -1;
+    }
+    ROS_INFO("CAN driver initialized successfully");
+    
+    ros::Publisher can_pub = nh.advertise<can_msgs::Frame>("/receive_canMessages", 1000);
+    
+    // 创建高优先级线程处理消息发布
+    ros::AsyncSpinner async_spinner(1); // 单线程处理
+    async_spinner.start();
+    
+    // 使用boost::function包装lambda表达式
+    // VCU主题订阅 - 通过CAN1发送
+    boost::function<void(const can_msgs::Frame::ConstPtr&)> vcu_callback = 
+        [&](const can_msgs::Frame::ConstPtr& msg) {
+            if (can_driver.isCan1Connected()) {
+                VCI_CAN_OBJ send_msg{};
+                send_msg.ID = msg->id;
+                send_msg.SendType = 0;
+                send_msg.ExternFlag = msg->is_extended ? 1 : 0;
+                send_msg.DataLen = msg->dlc;
+                
+                for (int i = 0; i < msg->dlc && i < 8; i++) {
+                    send_msg.Data[i] = msg->data[i];
+                }
+                
+                if (!can_driver.sendToCan1(send_msg)) {
+                    ROS_WARN_THROTTLE(1.0, "Failed to send VCU message to CAN1");
+                }
+            } else {
+                ROS_WARN_THROTTLE(1.0, "CAN1 not connected, VCU message dropped");
+            }
+        };
+    ros::Subscriber vcu_sub = nh.subscribe("can_frame_topic", 100, vcu_callback);
+    
+    // BMS主题订阅 - 通过CAN2发送
+    boost::function<void(const can_msgs::Frame::ConstPtr&)> bms_callback = 
+        [&](const can_msgs::Frame::ConstPtr& msg) {
+            if (can_driver.isCan2Connected()) {
+                VCI_CAN_OBJ send_msg{};
+                send_msg.ID = msg->id;
+                send_msg.SendType = 0;
+                send_msg.RemoteFlag = 0;
+                send_msg.ExternFlag = msg->is_extended ? 1 : 0;
+                send_msg.DataLen = msg->dlc;
+                
+                for (int i = 0; i < msg->dlc && i < 8; i++) {
+                    send_msg.Data[i] = msg->data[i];
+                }
+                
+                if (!can_driver.sendToCan2(send_msg)) {
+                    ROS_WARN_THROTTLE(1.0, "Failed to send BMS message to CAN2");
+                }
+            } else {
+                ROS_WARN_THROTTLE(1.0, "CAN2 not connected, BMS message dropped");
+            }
+        };
+    ros::Subscriber bms_sub = nh.subscribe("/sent_messages_bms", 1000, bms_callback);
+    
+    // VCU Driver主题订阅 - 通过CAN1发送
+    boost::function<void(const can_msgs::Frame::ConstPtr&)> vcu_driver_callback = 
+        [&](const can_msgs::Frame::ConstPtr& msg) {
+            if (can_driver.isCan1Connected()) {
+                VCI_CAN_OBJ send_msg{};
+                send_msg.ID = msg->id;
+                send_msg.SendType = 0;
+                send_msg.RemoteFlag = 0;
+                send_msg.ExternFlag = msg->is_extended ? 1 : 0;
+                send_msg.DataLen = msg->dlc;
+                
+                for (int i = 0; i < msg->dlc && i < 8; i++) {
+                    send_msg.Data[i] = msg->data[i];
+                }
+                
+                if (!can_driver.sendToCan1(send_msg)) {
+                    ROS_WARN_THROTTLE(1.0, "Failed to send VCU Driver message to CAN1");
+                }
+            } else {
+                ROS_WARN_THROTTLE(1.0, "CAN1 not connected, VCU Driver message dropped");
+            }
+        };
+    ros::Subscriber vcu_driver_sub = nh.subscribe("/sent_messages_vcuDriver", 1000, vcu_driver_callback);
+    
+    ros::Rate loop_rate(100); // 100Hz
+    
+    while (ros::ok()) {
+        // 发布接收到的CAN消息
+        std::vector<VCI_CAN_OBJ> received_msgs = can_driver.getReceivedMessages();
+        if (!received_msgs.empty()) {
+            for (const auto& msg : received_msgs) {
+                can_msgs::Frame can_msg;
+                can_msg.header.stamp = ros::Time::now();
+                can_msg.id = msg.ID;
+                can_msg.is_extended = msg.ExternFlag;
+                can_msg.dlc = msg.DataLen;
+                for (int i = 0; i < msg.DataLen && i < 8; i++) {
+                    can_msg.data[i] = msg.Data[i];
+                }
+                can_pub.publish(can_msg);
+            }
+        }
+        
+        loop_rate.sleep();
+    }
+    
+    can_driver.stop();
+    return 0;
+}