can_filter

radio_ctrl.sh

@@ -8,7 +8,9 @@ source install/setup.bash
 ros2 run vehicle_params vehicle_params_node &
 sleep 0.2
 
-ros2 run radio_ctrl radio_ctrl_node & 
+# ros2 run radio_ctrl radio_ctrl_node & 
+# sleep 0.2
+ros2 run can_radio_ctrl can_radio_ctrl_node & 
 sleep 0.2
 
 ros2 run mc mc_node &

src/control/can_radio_ctrl/include/can_radio_ctrl/candev.h

@@ -12,13 +12,11 @@
 #include <unistd.h>
 
 #include <cstdint>
+#include <mutex>  // 线程安全互斥锁
 
 constexpr auto FALSE = 0;
 constexpr auto TRUE = 1;
 
-int can_Init(void);
-void can_Recv_CAN1(void);
-
 // 遥控器数据结构体
 struct RadioData
 {
@@ -29,6 +27,14 @@ struct RadioData
     bool data_valid;   // 数据有效性标志
 };
 
+// 全局变量声明
 extern RadioData radio_data;
+extern std::mutex radio_data_mutex;  // 保护radio_data的互斥锁
+
+// 函数声明
+int can_Init(void);
+void can_Recv_CAN1(void);
+RadioData get_radio_data(void);  // 
+void can_Stop_Recv(void);        // 
 
 #endif  // !_CANDEV_H

src/control/can_radio_ctrl/src/can_radio_ctrl.cpp

@@ -1,11 +1,13 @@
 #include <algorithm>
 #include <iostream>
 #include <rclcpp/rclcpp.hpp>
+#include <cmath>  // 补充fabs所需头文件
 
 #include "can_radio_ctrl/candev.h"
 #include "can_radio_ctrl/get_config.h"
 #include "logger/logger.h"
 #include "sweeper_interfaces/msg/mc_ctrl.hpp"
+#include "sweeper_interfaces/msg/can_frame.hpp"
 
 namespace sweeperMsg = sweeper_interfaces::msg;
 
@@ -19,52 +21,68 @@ class CanRadioCtrlNode : public rclcpp::Node
    public:
     CanRadioCtrlNode() : Node("can_radio_ctrl_node"), current_feedback_angle(0.0f), print_counter(0)
     {
+        // 加载配置（仅初始化一次，避免重复加载）
         if (load_config(config))
         {
             LOG_INFO("MCU RPM Max: %d", config.mcu_rpm_max);
             LOG_INFO("EPS Angle Max: %f", config.eps_angle_max);
+            mcu_rpm_max_ = config.mcu_rpm_max;
+            eps_angle_max_ = config.eps_angle_max;
         }
+        else
+        {
+            LOG_ERROR("Failed to load config, use default values!");
+            mcu_rpm_max_ = 1500;  // 默认值
+            eps_angle_max_ = 45.0f; // 默认值
+        }
+
+        can_Recv_CAN1();
 
         // 发布控制指令消息的发布器
         pub_ = this->create_publisher<sweeperMsg::McCtrl>("radio_mc_ctrl", 10);
 
+        // 订阅CAN反馈的回调函数
+        can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
+            "can_data", 10, std::bind(&CanRadioCtrlNode::can_callback, this, std::placeholders::_1));
+
         // 创建周期定时器（每20ms回调一次控制逻辑，50Hz）
-        timer_ =
+        timer_ = this->create_wall_timer(
-            this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&CanRadioCtrlNode::timer_callback, this));
+            std::chrono::milliseconds(20), 
+            std::bind(&CanRadioCtrlNode::timer_callback, this)
+        );
+
+        LOG_INFO("CanRadioCtrlNode initialized successfully!");
     }
 
-    ~CanRadioCtrlNode() {}
+    ~CanRadioCtrlNode() 
+    {
+        // 停止CAN接收线程
+        can_Stop_Recv();
+        LOG_INFO("CanRadioCtrlNode destroyed!");
+    }
 
    private:
     // 定时器回调函数，用于周期性发布控制指令
     void timer_callback()
     {
-        static int MCU_RPM_MAX = config.mcu_rpm_max;
+        // 线程安全获取遥控数据（拷贝而非直接操作全局变量）
-        static float EPS_ANGLE_MAX = config.eps_angle_max;
+        RadioData radio_data = get_radio_data();
-
-        // 接收CAN数据
-        can_Recv_CAN1();
 
         // 检查数据有效性
         bool has_received = radio_data.data_valid;
-        bool data_safe = radio_data.data_valid;  // 这里可以添加超时判断
+        bool data_safe = radio_data.data_valid;
 
         auto msg = sweeperMsg::McCtrl();
 
         if (has_received && data_safe)
         {
             // 解析挡位
-            if (radio_data.gear == 0)
+            switch(radio_data.gear)
             {
-                msg.gear = 1;  // R挡（低位）
+                case 0: msg.gear = 1; break;  // R挡
-            }
+                case 1: msg.gear = 0; break;  // N挡
-            else if (radio_data.gear == 1)
+                case 2: msg.gear = 2; break;  // D挡
-            {
+                default: msg.gear = 0; LOG_WARN("Unknown gear: %d", radio_data.gear); break;
-                msg.gear = 0;  // N挡（中位）
-            }
-            else if (radio_data.gear == 2)
-            {
-                msg.gear = 2;  // D挡（高位）
             }
 
             // 解析油门/刹车
@@ -76,31 +94,29 @@ class CanRadioCtrlNode : public rclcpp::Node
             else
             {
                 msg.brake = 0;
-                msg.rpm = static_cast<uint16_t>(std::clamp(MCU_RPM_MAX * radio_data.throttle / 450, 0, MCU_RPM_MAX));
+                float throttle_ratio = static_cast<float>(radio_data.throttle) / 450.0f;
+                int rpm_val = static_cast<int>(mcu_rpm_max_ * throttle_ratio);
+                msg.rpm = static_cast<uint16_t>(std::clamp(rpm_val, 0, mcu_rpm_max_));
             }
 
             // 解析清扫开关
-            if (radio_data.sweep == 0)
+            msg.sweep = (radio_data.sweep != 0);
-            {
-                msg.sweep = false;
-            }
-            else
-            {
-                msg.sweep = true;
-            }
 
             // 解析转向
-            float target_angle = static_cast<float>(radio_data.steering) / 450 * EPS_ANGLE_MAX;
+            float steering_ratio = static_cast<float>(radio_data.steering) / 450.0f;
+            float target_angle = steering_ratio * eps_angle_max_;
 
             // 角速度（度/秒）
-            float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T;
+            float angle_diff = fabs(target_angle - current_feedback_angle);
+            float angle_speed = angle_diff / DELTA_T;
 
             // 电机转速（单位：rpm）
             float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
 
             // 限制电机转速到 [120, 1500] 范围
-            uint16_t can_speed =
+            uint16_t can_speed = static_cast<uint16_t>(
-                std::clamp(static_cast<uint16_t>(motor_rpm), static_cast<uint16_t>(120), static_cast<uint16_t>(1500));
+                std::clamp(motor_rpm, 120.0f, 1500.0f)
+            );
 
             msg.angle = target_angle;
             msg.angle_speed = can_speed;
@@ -108,8 +124,19 @@ class CanRadioCtrlNode : public rclcpp::Node
             // 发布控制消息
             pub_->publish(msg);
 
+            // 打印日志
             if (++print_counter >= PRINT_INTERVAL)
             {
+                const char* brake_str = msg.brake ? "已刹车" : "未刹车";
+                const char* gear_str = "未知挡位";
+                switch(msg.gear)
+                {
+                    case 0: gear_str = "空挡"; break;
+                    case 1: gear_str = "后退挡"; break;
+                    case 2: gear_str = "前进挡"; break;
+                }
+                const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫";
+
                 LOG_INFO(
                     "=====================================\n"
                     "  刹车: %s\n"
@@ -118,55 +145,59 @@ class CanRadioCtrlNode : public rclcpp::Node
                     "  轮端转向角度: %.1f °\n"
                     "  清扫状态: %s\n"
                     "=====================================",
-                    msg.brake ? "已刹车" : "未刹车",
+                    brake_str, gear_str, static_cast<int>(msg.rpm), msg.angle, sweep_str
-                    [&]()
+                );
-                    {
-                        switch (msg.gear)
-                        {
-                            case 0:
-                                return "空挡";
-                            case 1:
-                                return "后退挡";
-                            case 2:
-                                return "前进挡";
-                            default:
-                                return "未知挡位";
-                        }
-                    }(),
-                    static_cast<int>(msg.rpm), msg.angle, msg.sweep ? "正在清扫" : "未清扫");
                 print_counter = 0;  // 重置计数器
             }
         }
         else
         {
             // 未接收到数据或数据不安全
+            int interval = PRINT_INTERVAL / 2;
+            if (++print_counter >= interval)
+            {
                 if (!has_received)
-            {
-                if (++print_counter >= PRINT_INTERVAL / 2)
                 {
                     LOG_INFO("Waiting for CAN radio control data...");
-                    print_counter = 0;
-                }
                 }
                 else
-            {
-                if (++print_counter >= PRINT_INTERVAL / 2)
                 {
                     LOG_WARN("CAN radio control data lost or failsafe!");
-                    print_counter = 0;
                 }
+                print_counter = 0;
             }
             return;
         }
     }
 
-    // ROS2 发布/定时器
+    // CAN反馈回调函数（用于获取当前转向角度）
+    void can_callback(const sweeperMsg::CanFrame::SharedPtr msg)
+    {
+        // 增加空指针检查，避免崩溃
+        if (!msg) return;
+        
+        // 判断是否为转向反馈帧（ID=0x401，且数据长度大于5）
+        if (msg->id == 0x401 && msg->dlc >= 5)
+        {
+            // 第3、4字节拼接为原始角度数据
+            uint16_t raw_value = static_cast<uint16_t>((msg->data[3] << 8) | msg->data[4]);
+
+            // 数据偏移：减去1024并除以7，得到角度（单位：度）
+            current_feedback_angle = (raw_value - 1024) / 7.0f;
+        }
+    }
+
+    // ROS2 发布/订阅/定时器
     rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr pub_;
+    rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr can_sub_;
     rclcpp::TimerBase::SharedPtr timer_;
 
-    float current_feedback_angle;  // 当前反馈角度（从CAN中读取）
+    // 成员变量
+    float current_feedback_angle;
     RmoCtlConfig config;
-    int print_counter;  // 打印计数器
+    int print_counter;
+    int mcu_rpm_max_;    // MCU最大转速
+    float eps_angle_max_;// EPS最大角度
 };
 
 int main(int argc, char* argv[])
@@ -177,7 +208,7 @@ int main(int argc, char* argv[])
     // 初始化CAN设备
     LOG_INFO("Initializing CAN device...");
     int ifInit = can_Init();
-    while (ifInit != 1)
+    while (ifInit != 1 && rclcpp::ok())
     {
         LOG_ERROR("CAN device initialization failed, retrying...");
         ifInit = can_Init();
@@ -186,9 +217,13 @@ int main(int argc, char* argv[])
     LOG_INFO("CAN device initialized successfully!");
 
     rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<CanRadioCtrlNode>());
+    auto node = std::make_shared<CanRadioCtrlNode>();
+    rclcpp::spin(node);
     rclcpp::shutdown();
 
+    // 停止CAN接收线程
+    can_Stop_Recv();
+
     // 关闭日志系统
     logger::Logger::Shutdown();
     return 0;

src/control/can_radio_ctrl/src/candev.cpp

@@ -2,12 +2,24 @@
 
 #include <cstring>
 #include <stdexcept>
+#include <thread>   // 异步接收线程
+#include <atomic>   // 线程退出标志
+#include <chrono>   // 睡眠所需头文件
+#include <iostream> // 日志输出
 
-// 全局遥控器数据
+// 全局变量定义
 RadioData radio_data = {0, 0, 0, 0, false};
+std::mutex radio_data_mutex;  // 保护radio_data的互斥锁
 
 // CAN 文件描述符
 int can_fd = -1;
+std::atomic<bool> can_recv_running = true;  // CAN接收线程运行标志
+std::thread can_recv_thread;                // CAN接收线程
+
+// 简易LOG_INFO宏
+#ifndef LOG_INFO
+#define LOG_INFO(fmt, ...) printf("[CAN] " fmt "\n", ##__VA_ARGS__)
+#endif
 
 int can_Init(void)
 {
@@ -37,10 +49,27 @@ int can_Init(void)
         return FALSE;
     }
 
-    // 设置接收超时
+    // ===================== CAN过滤配置 =====================
+    struct can_filter rfilter[2];
+    rfilter[0].can_id   = 0x10B;
+    rfilter[0].can_mask = CAN_SFF_MASK;
+    rfilter[1].can_id   = 0x10A;
+    rfilter[1].can_mask = CAN_SFF_MASK;
+
+    if (setsockopt(can_fd, SOL_CAN_RAW, CAN_RAW_FILTER, rfilter, sizeof(rfilter)) < 0)
+    {
+        perror("Error setting CAN filter");
+        close(can_fd);
+        can_fd = -1;
+        return FALSE;
+    }
+    LOG_INFO("CAN filter initialized: only 0x10A and 0x10B frames are allowed");
+    // ======================================================
+
+    // 设置接收超时（5ms）
     struct timeval tv;
     tv.tv_sec = 0;
-    tv.tv_usec = 10000;  // 10ms
+    tv.tv_usec = 5000;
     if (setsockopt(can_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0)
     {
         perror("Error setting socket timeout");
@@ -53,15 +82,20 @@ int can_Init(void)
     return TRUE;
 }
 
-void can_Recv_CAN1(void)
+// CAN数据异步接收循环
+void can_recv_loop()
+{
+    while (can_recv_running)
     {
         if (can_fd < 0)
         {
             printf("CAN device not initialized, reinitializing...\n");
             if (can_Init() != TRUE)
             {
+                std::lock_guard<std::mutex> lock(radio_data_mutex);
                 radio_data.data_valid = false;
-            return;
+                std::this_thread::sleep_for(std::chrono::milliseconds(10));
+                continue;
             }
         }
 
@@ -70,39 +104,62 @@ void can_Recv_CAN1(void)
 
         if (nbytes > 0)
         {
-        // 遥控器数据采集
+            std::lock_guard<std::mutex> lock(radio_data_mutex);
+            // 仅处理过滤后的0x10A/0x10B报文
             if (frame.can_id == 0x10B && frame.can_dlc >= 8)
             {
-            // X2（0-15位）右手左右
                 int16_t x2 = (frame.data[1] << 8) | frame.data[0];
-            // X1（32-47位）左手上下
                 int16_t x1 = (frame.data[5] << 8) | frame.data[4];
-
+                radio_data.throttle = x1;
-            // 油门用x1，方向用x2
+                radio_data.steering = x2;
-            radio_data.throttle = x1;  // 油门（左手上下）
-            radio_data.steering = x2;  // 方向（右手左右）
-
                 radio_data.data_valid = true;
             }
             else if (frame.can_id == 0x10A && frame.can_dlc >= 8)
             {
-            // E（0-7位）
                 radio_data.gear = frame.data[0];
-            // F（8-15位）
                 radio_data.sweep = frame.data[1];
-
                 radio_data.data_valid = true;
             }
         }
         else if (nbytes < 0)
         {
-        // 读取超时或其他错误
             if (errno != EAGAIN && errno != EWOULDBLOCK)
             {
                 perror("Error reading from CAN socket");
                 close(can_fd);
                 can_fd = -1;
+                std::lock_guard<std::mutex> lock(radio_data_mutex);
                 radio_data.data_valid = false;
             }
         }
     }
+}
+
+void can_Recv_CAN1(void)
+{
+    if (!can_recv_thread.joinable() && can_recv_running)
+    {
+        can_recv_thread = std::thread(can_recv_loop);
+    }
+}
+
+RadioData get_radio_data(void)
+{
+    std::lock_guard<std::mutex> lock(radio_data_mutex);
+    return radio_data;
+}
+
+void can_Stop_Recv(void)
+{
+    can_recv_running = false;
+    if (can_recv_thread.joinable())
+    {
+        can_recv_thread.join();
+    }
+    if (can_fd >= 0)
+    {
+        close(can_fd);
+        can_fd = -1;
+    }
+    LOG_INFO("CAN receive thread stopped");
+}