From e15bcea4584f8fc3119b101909a443ce11d3e66e Mon Sep 17 00:00:00 2001 From: cxh Date: Thu, 19 Jun 2025 14:14:52 +0800 Subject: [PATCH] Auto commit at 2025-06-19 14:14:52 --- src/mc/include/mc/dumper.h | 2 ++ src/mc/src/dumper.cpp | 27 ++++++++-------- src/radio_ctrl/src/radio_ctrl.cpp | 51 +++++++++++++++---------------- 3 files changed, 41 insertions(+), 39 deletions(-) diff --git a/src/mc/include/mc/dumper.h b/src/mc/include/mc/dumper.h index 42cd514..6ec3022 100644 --- a/src/mc/include/mc/dumper.h +++ b/src/mc/include/mc/dumper.h @@ -5,6 +5,7 @@ #include "can_driver.h" #include #include +#include enum class DumperState { @@ -56,6 +57,7 @@ private: std::atomic sweeping_ = false; // 扫地任务标志 DumperState current_state_; CANDriver &can_; + rclcpp::Logger logger_; }; #endif \ No newline at end of file diff --git a/src/mc/src/dumper.cpp b/src/mc/src/dumper.cpp index e9ff969..183d371 100644 --- a/src/mc/src/dumper.cpp +++ b/src/mc/src/dumper.cpp @@ -7,14 +7,15 @@ DumperController::DumperController(CANDriver &driver) : busy_(false), has_pending_task_(false), current_state_(DumperState::IDLE), - can_(driver) + can_(driver), + logger_(rclcpp::get_logger("DumperController")) { } // 上电复位：自动收斗 + 降斗 void DumperController::onStartup() { - std::cout << "[Init] Power-on reset: retract and lower...\n"; + RCLCPP_INFO(logger_, "[Init] Power-on reset: retract and lower..."); doAction(DumperAction::RETRACT); doAction(DumperAction::LOWER); in_startup_reset_ = false; @@ -26,7 +27,7 @@ void DumperController::tryStartup() if (awakened_) return; - std::cout << "[Init] Dumper first wake-up: resetting...\n"; + RCLCPP_INFO(logger_, "[Init] Dumper first wake-up: resetting..."); onStartup(); awakened_ = true; } @@ -35,14 +36,14 @@ void DumperController::resetAwake() { awakened_ = false; last_dial_target_ = -1; - std::cout << "[INFO] Dumper awake state reset.\n"; + RCLCPP_INFO(logger_, "[INFO] Dumper awake state reset."); } void DumperController::updateDial(int new_dial, int current_state) { if (in_startup_reset_) { - std::cout << "[Dumper] In startup reset, ignoring remote control dump command.\n"; + RCLCPP_WARN(logger_, "[Dumper] In startup reset, ignoring remote control dump command."); return; } @@ -52,7 +53,7 @@ void DumperController::updateDial(int new_dial, int current_state) if (last_dial_target_ == new_dial) return; - std::cout << "[Dial] Changed from " << current_state << " to " << new_dial << "\n"; + RCLCPP_INFO(logger_, "[Dial] Changed from %d to %d", current_state, new_dial); handleDialChange(current_state, new_dial); last_dial_target_ = new_dial; } @@ -126,7 +127,7 @@ void DumperController::doAction(DumperAction act) { if (sweeping_) { - std::cout << "[Warning] Sweeping active, dumper action blocked.\n"; + RCLCPP_WARN(logger_, "[Warning] Sweeping active, dumper action blocked."); return; } @@ -141,7 +142,7 @@ void DumperController::doAction(DumperAction act) case DumperAction::RAISE: if (raised_) return; - std::cout << "[Action] Raising...\n"; + RCLCPP_INFO(logger_, "[Action] Raising..."); frame1 = {0x18FA1117, {0x00, 0x40, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false}; can_.sendFrame(frame1); current_state_ = DumperState::RAISING; @@ -153,7 +154,7 @@ void DumperController::doAction(DumperAction act) case DumperAction::DUMP: if (dumped_) return; - std::cout << "[Action] Dumping...\n"; + RCLCPP_INFO(logger_, "[Action] Dumping..."); frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x10, 0x20, 0x00, 0x00, 0x00}, 8, true, false}; can_.sendFrame(frame1); current_state_ = DumperState::DUMPING; @@ -164,7 +165,7 @@ void DumperController::doAction(DumperAction act) case DumperAction::RETRACT: if (!dumped_) return; - std::cout << "[Action] Retracting...\n"; + RCLCPP_INFO(logger_, "[Action] Retracting..."); frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false}; frame2 = {0x18F21117, {0x00, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 8, true, false}; can_.sendFrame(frame1); @@ -177,7 +178,7 @@ void DumperController::doAction(DumperAction act) case DumperAction::LOWER: if (!raised_) return; - std::cout << "[Action] Lowering...\n"; + RCLCPP_INFO(logger_, "[Action] Lowering..."); frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false}; frame2 = {0x18F21117, {0x00, 0x00, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00}, 8, true, false}; can_.sendFrame(frame1); @@ -197,13 +198,13 @@ void DumperController::sendControlOff() CANFrame off2 = {0x18F21117, {0, 0, 0, 0, 0, 0, 0, 0}, 8, true, false}; can_.sendFrame(off1); can_.sendFrame(off2); - std::cout << "[INFO] Control OFF sent.\n"; + RCLCPP_INFO(logger_, "[INFO] Control OFF sent."); } void DumperController::logState() const { const char *state_str[] = {"IDLE", "RAISING", "DUMPING", "RETRACTING", "LOWERING"}; - std::cout << "[STATE] Dumper state: " << state_str[static_cast(current_state_)] << "\n"; + RCLCPP_INFO(logger_, "[STATE] Dumper state: %s", state_str[static_cast(current_state_)]); } int DumperController::getSimpleState() const diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index e7f8d79..0c8e43d 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -75,15 +75,14 @@ private: 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 ctrl = ch_data[4]; // 手刹 - uint16_t gear = ch_data[5]; // 挡位 - uint16_t sweep = ch_data[6]; // 清扫 - uint16_t dump = ch_data[7]; // 垃圾倾倒 - printf("dump=%d\n", dump); + uint16_t ctrl = ch_data[4]; // 手刹 + uint16_t gear = ch_data[5]; // 挡位 + uint16_t sweep = ch_data[6]; // 清扫 + uint16_t dump = ch_data[7]; // 垃圾倾倒 int16_t speed = ch_data[1] - 992; //[-800,800] if (!initialized) @@ -214,25 +213,25 @@ private: // 发布控制消息 pub_->publish(msg); - // RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" - // << "\n brake: " << static_cast(msg.brake) // uint8 - // << "\n gear: " << static_cast(msg.gear) // uint8 - // << "\n rpm: " << static_cast(msg.rpm) // uint8 - // << "\n ehb_enable: " << static_cast(msg.ehb_anable) // bool - // << "\n ehb_brake_pressure: " << static_cast(msg.ehb_brake_pressure) // float32 - // << "\n angle: " << msg.angle // float32 - // << "\n angle_speed: " << msg.angle_speed // uint16 - // << "\n edge_brush_lift: " << static_cast(msg.edge_brush_lift) // bool - // << "\n sweep_ctrl: " << static_cast(msg.sweep_ctrl) // bool - // << "\n spray: " << static_cast(msg.spray) // bool - // << "\n mud_flap: " << static_cast(msg.mud_flap) // bool - // << "\n dust_shake: " << static_cast(msg.dust_shake) // bool - // << "\n left_light: " << static_cast(msg.left_light) // bool - // << "\n right_light: " << static_cast(msg.right_light) // bool - // << "\n brake_light: " << static_cast(msg.brake_light) // bool - // << "\n headlight: " << static_cast(msg.headlight) // bool - // << "\n dump: " << static_cast(msg.dump) // uint8 - // ); + RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" + << "\n brake: " << static_cast(msg.brake) // uint8 + << "\n gear: " << static_cast(msg.gear) // uint8 + << "\n rpm: " << static_cast(msg.rpm) // uint8 + << "\n ehb_enable: " << static_cast(msg.ehb_anable) // bool + << "\n ehb_brake_pressure: " << static_cast(msg.ehb_brake_pressure) // float32 + << "\n angle: " << msg.angle // float32 + << "\n angle_speed: " << msg.angle_speed // uint16 + << "\n edge_brush_lift: " << static_cast(msg.edge_brush_lift) // bool + << "\n sweep_ctrl: " << static_cast(msg.sweep_ctrl) // bool + << "\n spray: " << static_cast(msg.spray) // bool + << "\n mud_flap: " << static_cast(msg.mud_flap) // bool + << "\n dust_shake: " << static_cast(msg.dust_shake) // bool + << "\n left_light: " << static_cast(msg.left_light) // bool + << "\n right_light: " << static_cast(msg.right_light) // bool + << "\n brake_light: " << static_cast(msg.brake_light) // bool + << "\n headlight: " << static_cast(msg.headlight) // bool + << "\n dump: " << static_cast(msg.dump) // uint8 + ); } else {