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 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,32 +141,43 @@ private:
// 发布控制消息 // 发布控制消息
pub_->publish(msg); pub_->publish(msg);
std::cout << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车") // 降低打印频率：每 PRINT_INTERVAL 次回调打印一次
<< "\n 挡位: "; if (++print_counter >= PRINT_INTERVAL)
switch (msg.gear)
{ {
case 0: std::cout << "\n====================================="
std::cout << "空挡"; << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车")
break; << "\n 挡位: ";
case 2: switch (msg.gear)
std::cout << "前进挡"; {
break; case 0:
case 1: std::cout << "空挡";
std::cout << "后退挡"; break;
break; case 2:
default: std::cout << "前进挡";
std::cout << "未知挡位(" << static_cast<int>(msg.gear) << ")"; break;
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 else
{ {
RCLCPP_INFO(this->get_logger(), "Waiting for radio control data..."); // 低频率打印等待信息（每2次回调打印一次，避免刷屏）
return; // 下次循环，等待数据安全 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中读取） float current_feedback_angle; // 当前反馈角度（从CAN中读取）
RmoCtlConfig config; RmoCtlConfig config;
int print_counter; // 打印计数器（新增）
}; };
int main(int argc, char *argv[]) int main(int argc, char *argv[])