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

Auto commit at 2025-06-12 16:13:00

Browse Source
This commit is contained in:
cxh 2025-06-12 16:13:00 +08:00
parent 29bbad2b81
commit 80b972bf74
1 changed files with 58 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
src/mc/src/mc.cpp
View File

@ -164,76 +164,76 @@ 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)