diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index 7513372..6ed86fa 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -11,11 +11,12 @@ namespace sweeperMsg = sweeper_interfaces::msg; constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f; constexpr float GEAR_RATIO = 16.5f; constexpr float DELTA_T = 0.02f; // 20ms +constexpr int PRINT_INTERVAL = 25; // 打印间隔:每25次回调打印一次(25*20ms=500ms,即2Hz) class SBUSNode : public rclcpp::Node { public: - SBUSNode() : Node("sbus_node"), current_feedback_angle(0.0f) + SBUSNode() : Node("sbus_node"), current_feedback_angle(0.0f), print_counter(0) // 初始化计数器 { if (load_config(config)) { @@ -62,7 +63,6 @@ private: { static int MCU_RPM_MAX = config.mcu_rpm_max; static float EPS_ANGLE_MAX = config.eps_angle_max; - // static bool initialized = false; // 新增初始化标志 bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性 @@ -71,13 +71,13 @@ private: if (data_safe) // 数据安全,进行数据解析并发布 { - // 赋值与打印 + // 赋值与打印(注释掉原有的高频打印) for (int i = 0; i < 10; ++i) { ch_data[i] = uart_handler_->get_channel_value(i); - // printf("ch[%d]:%d ", i, ch_data[i]); + // printf("ch[%d]:%d ", i, ch_data[i]); // 高频打印已注释 } - // printf("\n"); + // printf("\n"); // 高频打印已注释 uint16_t gear = ch_data[7]; // 挡位 最下1792,中间992,最上192 uint16_t sweep = ch_data[6]; // 清扫 最上192,最下1792 @@ -141,32 +141,43 @@ private: // 发布控制消息 pub_->publish(msg); - std::cout << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车") - << "\n 挡位: "; - switch (msg.gear) + // 降低打印频率:每 PRINT_INTERVAL 次回调打印一次 + if (++print_counter >= PRINT_INTERVAL) { - case 0: - std::cout << "空挡"; - break; - case 2: - std::cout << "前进挡"; - break; - case 1: - std::cout << "后退挡"; - break; - default: - std::cout << "未知挡位(" << static_cast(msg.gear) << ")"; - break; + std::cout << "\n=====================================" + << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车") + << "\n 挡位: "; + switch (msg.gear) + { + case 0: + std::cout << "空挡"; + break; + case 2: + std::cout << "前进挡"; + break; + case 1: + std::cout << "后退挡"; + break; + default: + std::cout << "未知挡位(" << static_cast(msg.gear) << ")"; + break; + } + std::cout << "\n 行走电机转速: " << static_cast(msg.rpm) << " RPM" + << "\n 轮端转向角度: " << msg.angle << "°" + << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫") + << "\n=====================================" << std::endl; + print_counter = 0; // 重置计数器 } - std::cout << "\n 行走电机转速: " << static_cast(msg.rpm) << " RPM" - << "\n 轮端转向角度: " << msg.angle << "°" - << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫") - << std::endl; } else { - RCLCPP_INFO(this->get_logger(), "Waiting for radio control data..."); - return; // 下次循环,等待数据安全 + // 低频率打印等待信息(每2次回调打印一次,避免刷屏) + if (++print_counter >= PRINT_INTERVAL / 2) + { + RCLCPP_INFO(this->get_logger(), "Waiting for radio control data..."); + print_counter = 0; + } + return; } } @@ -192,6 +203,7 @@ private: float current_feedback_angle; // 当前反馈角度(从CAN中读取) RmoCtlConfig config; + int print_counter; // 打印计数器(新增) }; int main(int argc, char *argv[])