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

64
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 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<int>(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<int>(msg.gear) << ")";
break;
}
std::cout << "\n 行走电机转速: " << static_cast<int>(msg.rpm) << " RPM"
<< "\n 轮端转向角度: " << msg.angle << "°"
<< "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫")
<< "\n=====================================" << std::endl;
print_counter = 0; // 重置计数器
}
std::cout << "\n 行走电机转速: " << static_cast<int>(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[])