lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

降低打印输出频率

Browse Source
This commit is contained in:
lyq 2025-11-26 09:45:13 +08:00
parent b5686a81b0
commit fdde0c6e24
1 changed files with 38 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/radio_ctrl/src/radio_ctrl.cpp
View File

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