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)
{
// 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)