From 0298d0466d336e065beabd2c6c051cf8735ad12a Mon Sep 17 00:00:00 2001 From: cxh Date: Thu, 12 Jun 2025 17:17:18 +0800 Subject: [PATCH] Auto commit at 2025-06-12 17:17:18 --- src/mc/src/mc.cpp | 118 +++++++++++++++++++++++----------------------- 1 file changed, 59 insertions(+), 59 deletions(-) diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp index 4cacc57..255e6a7 100644 --- a/src/mc/src/mc.cpp +++ b/src/mc/src/mc.cpp @@ -163,77 +163,77 @@ void VCUWakeUp() void setupTimers(rclcpp::Node::SharedPtr node) { - // VCU 唤醒帧发送定时器，1Hz - // static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer( - // std::chrono::seconds(1), [node]() - // { - // if (!vcu_awake.load()) - // { - // RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame..."); - // VCUWakeUp(); - // } }); + VCU 唤醒帧发送定时器，1Hz + static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer( + std::chrono::seconds(1), [node]() + { + if (!vcu_awake.load()) + { + RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame..."); + VCUWakeUp(); + } }); - // // vcu报文 watchdog 检查，200ms - // static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer( - // std::chrono::milliseconds(200), [node]() - // { - // auto now = node->now(); - // auto elapsed = now - last_vcu_msg_time; + // vcu报文 watchdog 检查，200ms + static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer( + std::chrono::milliseconds(200), [node]() + { + auto now = node->now(); + auto elapsed = now - last_vcu_msg_time; - // if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5)) - // { - // RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state."); - // vcu_msg_received.store(false); - // vcu_awake.store(false); - // } }); + if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5)) + { + RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state."); + vcu_msg_received.store(false); + vcu_awake.store(false); + } }); - // // MCU控制，50Hz - // static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer( - // std::chrono::milliseconds(20), []() - // { - // sweeperMsg::McCtrl msg = get_safe_control(); + // MCU控制，50Hz + static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer( + std::chrono::milliseconds(20), []() + { + sweeperMsg::McCtrl msg = get_safe_control(); - // mcu_cmd.setEnabled(true); - // mcu_cmd.setGear(msg.gear); - // mcu_cmd.setRPM(msg.rpm); - // mcu_cmd.setBrake(msg.brake); + mcu_cmd.setEnabled(true); + mcu_cmd.setGear(msg.gear); + mcu_cmd.setRPM(msg.rpm); + mcu_cmd.setBrake(msg.brake); - // canctl.sendFrame(mcu_cmd.toFrame()); }); + canctl.sendFrame(mcu_cmd.toFrame()); }); - // // EPS 控制，20Hz - // static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer( - // std::chrono::milliseconds(50), []() - // { - // sweeperMsg::McCtrl msg = get_safe_control(); + // EPS 控制，20Hz + static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer( + std::chrono::milliseconds(50), []() + { + sweeperMsg::McCtrl msg = get_safe_control(); - // eps_cmd.setCenterCmd(0); - // eps_cmd.setAngle(msg.angle); - // eps_cmd.setAngularSpeed(msg.angle_speed); - // eps_cmd.pack(); + eps_cmd.setCenterCmd(0); + eps_cmd.setAngle(msg.angle); + eps_cmd.setAngularSpeed(msg.angle_speed); + eps_cmd.pack(); - // canctl.sendFrame(eps_cmd.toFrame()); }); + canctl.sendFrame(eps_cmd.toFrame()); }); - // // VCU 控制，10Hz - // static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer( - // std::chrono::milliseconds(100), []() - // { - // sweeperMsg::McCtrl msg = get_safe_control(); + // VCU 控制，10Hz + static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer( + std::chrono::milliseconds(100), []() + { + sweeperMsg::McCtrl msg = get_safe_control(); - // vcu1_cmd.setLeftLight(msg.left_light); - // vcu1_cmd.setDustShake(msg.dust_shake); - // vcu1_cmd.setHeadLight(msg.headlight); + vcu1_cmd.setLeftLight(msg.left_light); + vcu1_cmd.setDustShake(msg.dust_shake); + vcu1_cmd.setHeadLight(msg.headlight); - // vcu2_cmd.setBrakeLight(msg.brake_light); - // vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift); - // vcu2_cmd.setHeadlight(msg.headlight); - // vcu2_cmd.setMudFlap(msg.mud_flap); - // vcu2_cmd.setRightLight(msg.right_light); - // vcu2_cmd.setSpray(msg.spray); - // vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl); + vcu2_cmd.setBrakeLight(msg.brake_light); + vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift); + vcu2_cmd.setHeadlight(msg.headlight); + vcu2_cmd.setMudFlap(msg.mud_flap); + vcu2_cmd.setRightLight(msg.right_light); + vcu2_cmd.setSpray(msg.spray); + vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl); - // // canctl.sendFrame(vcu1_cmd.toFrame()); - // // canctl.sendFrame(vcu2_cmd.toFrame()); - // }); + // canctl.sendFrame(vcu1_cmd.toFrame()); + // canctl.sendFrame(vcu2_cmd.toFrame()); + }); } int main(int argc, char **argv)