cxh/sweeper_e800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto commit at 2025-06-12 17:17:18

Browse Source
This commit is contained in:
cxh 2025-06-12 17:17:18 +08:00
parent 80b972bf74
commit 0298d0466d
1 changed files with 59 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
src/mc/src/mc.cpp
View File

@ -163,77 +163,77 @@ void VCUWakeUp()
void setupTimers(rclcpp::Node::SharedPtr node) void setupTimers(rclcpp::Node::SharedPtr node)
{ {
// VCU 唤醒帧发送定时器，1Hz VCU 唤醒帧发送定时器，1Hz
// static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer(
// std::chrono::seconds(1), [node]() std::chrono::seconds(1), [node]()
// { {
// if (!vcu_awake.load()) if (!vcu_awake.load())
// { {
// RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame..."); RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
// VCUWakeUp(); VCUWakeUp();
// } }); } });
// // vcu报文 watchdog 检查，200ms // vcu报文 watchdog 检查，200ms
// static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer(
// std::chrono::milliseconds(200), [node]() std::chrono::milliseconds(200), [node]()
// { {
// auto now = node->now(); auto now = node->now();
// auto elapsed = now - last_vcu_msg_time; auto elapsed = now - last_vcu_msg_time;
// if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5)) 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."); RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state.");
// vcu_msg_received.store(false); vcu_msg_received.store(false);
// vcu_awake.store(false); vcu_awake.store(false);
// } }); } });
// // MCU控制，50Hz // MCU控制，50Hz
// static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
// std::chrono::milliseconds(20), []() std::chrono::milliseconds(20), []()
// { {
// sweeperMsg::McCtrl msg = get_safe_control(); sweeperMsg::McCtrl msg = get_safe_control();
// mcu_cmd.setEnabled(true); mcu_cmd.setEnabled(true);
// mcu_cmd.setGear(msg.gear); mcu_cmd.setGear(msg.gear);
// mcu_cmd.setRPM(msg.rpm); mcu_cmd.setRPM(msg.rpm);
// mcu_cmd.setBrake(msg.brake); mcu_cmd.setBrake(msg.brake);
// canctl.sendFrame(mcu_cmd.toFrame()); }); canctl.sendFrame(mcu_cmd.toFrame()); });
// // EPS 控制，20Hz // EPS 控制，20Hz
// static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer(
// std::chrono::milliseconds(50), []() std::chrono::milliseconds(50), []()
// { {
// sweeperMsg::McCtrl msg = get_safe_control(); sweeperMsg::McCtrl msg = get_safe_control();
// eps_cmd.setCenterCmd(0); eps_cmd.setCenterCmd(0);
// eps_cmd.setAngle(msg.angle); eps_cmd.setAngle(msg.angle);
// eps_cmd.setAngularSpeed(msg.angle_speed); eps_cmd.setAngularSpeed(msg.angle_speed);
// eps_cmd.pack(); eps_cmd.pack();
// canctl.sendFrame(eps_cmd.toFrame()); }); canctl.sendFrame(eps_cmd.toFrame()); });
// // VCU 控制，10Hz // VCU 控制，10Hz
// static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer(
// std::chrono::milliseconds(100), []() std::chrono::milliseconds(100), []()
// { {
// sweeperMsg::McCtrl msg = get_safe_control(); sweeperMsg::McCtrl msg = get_safe_control();
// vcu1_cmd.setLeftLight(msg.left_light); vcu1_cmd.setLeftLight(msg.left_light);
// vcu1_cmd.setDustShake(msg.dust_shake); vcu1_cmd.setDustShake(msg.dust_shake);
// vcu1_cmd.setHeadLight(msg.headlight); vcu1_cmd.setHeadLight(msg.headlight);
// vcu2_cmd.setBrakeLight(msg.brake_light); vcu2_cmd.setBrakeLight(msg.brake_light);
// vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift); vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
// vcu2_cmd.setHeadlight(msg.headlight); vcu2_cmd.setHeadlight(msg.headlight);
// vcu2_cmd.setMudFlap(msg.mud_flap); vcu2_cmd.setMudFlap(msg.mud_flap);
// vcu2_cmd.setRightLight(msg.right_light); vcu2_cmd.setRightLight(msg.right_light);
// vcu2_cmd.setSpray(msg.spray); vcu2_cmd.setSpray(msg.spray);
// vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl); vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
// // canctl.sendFrame(vcu1_cmd.toFrame()); // canctl.sendFrame(vcu1_cmd.toFrame());
// // canctl.sendFrame(vcu2_cmd.toFrame()); // canctl.sendFrame(vcu2_cmd.toFrame());
// }); });
} }
int main(int argc, char **argv) int main(int argc, char **argv)