diff --git a/radio_ctrl.sh b/radio_ctrl.sh index f740717..28d7fde 100755 --- a/radio_ctrl.sh +++ b/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 & diff --git a/src/control/can_radio_ctrl/include/can_radio_ctrl/candev.h b/src/control/can_radio_ctrl/include/can_radio_ctrl/candev.h index b415427..0660050 100644 --- a/src/control/can_radio_ctrl/include/can_radio_ctrl/candev.h +++ b/src/control/can_radio_ctrl/include/can_radio_ctrl/candev.h @@ -12,13 +12,11 @@ #include #include +#include // 线程安全互斥锁 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的互斥锁 -#endif // !_CANDEV_H +// 函数声明 +int can_Init(void); +void can_Recv_CAN1(void); +RadioData get_radio_data(void); // +void can_Stop_Recv(void); // + +#endif // !_CANDEV_H \ No newline at end of file diff --git a/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp b/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp index 360fb7d..f58b73c 100644 --- a/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp +++ b/src/control/can_radio_ctrl/src/can_radio_ctrl.cpp @@ -1,11 +1,13 @@ #include #include #include +#include // 补充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("radio_mc_ctrl", 10); + // 订阅CAN反馈的回调函数 + can_sub_ = this->create_subscription( + "can_data", 10, std::bind(&CanRadioCtrlNode::can_callback, this, std::placeholders::_1)); + // 创建周期定时器（每20ms回调一次控制逻辑，50Hz） - timer_ = - this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&CanRadioCtrlNode::timer_callback, this)); + timer_ = this->create_wall_timer( + 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; - - // 接收CAN数据 - can_Recv_CAN1(); + // 线程安全获取遥控数据（拷贝而非直接操作全局变量） + RadioData radio_data = get_radio_data(); // 检查数据有效性 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挡（低位） - } - else if (radio_data.gear == 1) - { - msg.gear = 0; // N挡（中位） - } - else if (radio_data.gear == 2) - { - msg.gear = 2; // D挡（高位） + case 0: msg.gear = 1; break; // R挡 + case 1: msg.gear = 0; break; // N挡 + case 2: msg.gear = 2; break; // D挡 + default: msg.gear = 0; LOG_WARN("Unknown gear: %d", radio_data.gear); break; } // 解析油门/刹车 @@ -76,31 +94,29 @@ class CanRadioCtrlNode : public rclcpp::Node else { msg.brake = 0; - msg.rpm = static_cast(std::clamp(MCU_RPM_MAX * radio_data.throttle / 450, 0, MCU_RPM_MAX)); + float throttle_ratio = static_cast(radio_data.throttle) / 450.0f; + int rpm_val = static_cast(mcu_rpm_max_ * throttle_ratio); + msg.rpm = static_cast(std::clamp(rpm_val, 0, mcu_rpm_max_)); } // 解析清扫开关 - if (radio_data.sweep == 0) - { - msg.sweep = false; - } - else - { - msg.sweep = true; - } + msg.sweep = (radio_data.sweep != 0); // 解析转向 - float target_angle = static_cast(radio_data.steering) / 450 * EPS_ANGLE_MAX; + float steering_ratio = static_cast(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 = - std::clamp(static_cast(motor_rpm), static_cast(120), static_cast(1500)); + uint16_t can_speed = static_cast( + 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 ? "已刹车" : "未刹车", - [&]() - { - switch (msg.gear) - { - case 0: - return "空挡"; - case 1: - return "后退挡"; - case 2: - return "前进挡"; - default: - return "未知挡位"; - } - }(), - static_cast(msg.rpm), msg.angle, msg.sweep ? "正在清扫" : "未清扫"); + brake_str, gear_str, static_cast(msg.rpm), msg.angle, sweep_str + ); print_counter = 0; // 重置计数器 } } else { // 未接收到数据或数据不安全 - if (!has_received) + int interval = PRINT_INTERVAL / 2; + if (++print_counter >= interval) { - if (++print_counter >= PRINT_INTERVAL / 2) + if (!has_received) { LOG_INFO("Waiting for CAN radio control data..."); - print_counter = 0; } - } - else - { - if (++print_counter >= PRINT_INTERVAL / 2) + else { 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((msg->data[3] << 8) | msg->data[4]); + + // 数据偏移：减去1024并除以7，得到角度（单位：度） + current_feedback_angle = (raw_value - 1024) / 7.0f; + } + } + + // ROS2 发布/订阅/定时器 rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::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,10 +217,14 @@ int main(int argc, char* argv[]) LOG_INFO("CAN device initialized successfully!"); rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + auto node = std::make_shared(); + rclcpp::spin(node); rclcpp::shutdown(); + // 停止CAN接收线程 + can_Stop_Recv(); + // 关闭日志系统 logger::Logger::Shutdown(); return 0; -} +} \ No newline at end of file diff --git a/src/control/can_radio_ctrl/src/candev.cpp b/src/control/can_radio_ctrl/src/candev.cpp index 1de2bf7..ea66fef 100644 --- a/src/control/can_radio_ctrl/src/candev.cpp +++ b/src/control/can_radio_ctrl/src/candev.cpp @@ -2,12 +2,24 @@ #include #include +#include // 异步接收线程 +#include // 线程退出标志 +#include // 睡眠所需头文件 +#include // 日志输出 -// 全局遥控器数据 +// 全局变量定义 RadioData radio_data = {0, 0, 0, 0, false}; +std::mutex radio_data_mutex; // 保护radio_data的互斥锁 // CAN 文件描述符 int can_fd = -1; +std::atomic 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,56 +82,84 @@ int can_Init(void) return TRUE; } -void can_Recv_CAN1(void) +// CAN数据异步接收循环 +void can_recv_loop() { - if (can_fd < 0) + while (can_recv_running) { - printf("CAN device not initialized, reinitializing...\n"); - if (can_Init() != TRUE) + if (can_fd < 0) { - radio_data.data_valid = false; - return; + printf("CAN device not initialized, reinitializing...\n"); + if (can_Init() != TRUE) + { + std::lock_guard lock(radio_data_mutex); + radio_data.data_valid = false; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + continue; + } } - } - struct can_frame frame; - int nbytes = read(can_fd, &frame, sizeof(struct can_frame)); + struct can_frame frame; + int nbytes = read(can_fd, &frame, sizeof(struct can_frame)); - if (nbytes > 0) - { - // 遥控器数据采集 - if (frame.can_id == 0x10B && frame.can_dlc >= 8) + if (nbytes > 0) { - // 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]; - - // 油门用x1，方向用x2 - radio_data.throttle = x1; // 油门（左手上下） - radio_data.steering = x2; // 方向（右手左右） - - radio_data.data_valid = true; + std::lock_guard lock(radio_data_mutex); + // 仅处理过滤后的0x10A/0x10B报文 + if (frame.can_id == 0x10B && frame.can_dlc >= 8) + { + int16_t x2 = (frame.data[1] << 8) | frame.data[0]; + int16_t x1 = (frame.data[5] << 8) | frame.data[4]; + radio_data.throttle = x1; + radio_data.steering = x2; + radio_data.data_valid = true; + } + else if (frame.can_id == 0x10A && frame.can_dlc >= 8) + { + radio_data.gear = frame.data[0]; + radio_data.sweep = frame.data[1]; + radio_data.data_valid = true; + } } - else if (frame.can_id == 0x10A && frame.can_dlc >= 8) + else if (nbytes < 0) { - // 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; - radio_data.data_valid = false; + if (errno != EAGAIN && errno != EWOULDBLOCK) + { + perror("Error reading from CAN socket"); + close(can_fd); + can_fd = -1; + std::lock_guard 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 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"); +} \ No newline at end of file