From 624c7d1ea23f92fa1ffa6629e7ea2e6111021227 Mon Sep 17 00:00:00 2001 From: cxh Date: Wed, 18 Jun 2025 15:49:41 +0800 Subject: [PATCH] Auto commit at 2025-06-18 15:49:41 --- src/mc/include/mc/can_utils.hpp | 14 ++ src/mc/include/mc/control_cache.hpp | 20 ++ src/mc/include/mc/dumper.h | 11 +- src/mc/include/mc/timer_tasks.hpp | 6 + src/mc/include/mc/vcu_wakeup.hpp | 4 + src/mc/src/can_utils.cpp | 39 ++++ src/mc/src/control_cache.cpp | 44 +++++ src/mc/src/dumper.cpp | 8 +- src/mc/src/mc.cpp | 295 ++-------------------------- src/mc/src/timer_tasks.cpp | 89 +++++++++ src/mc/src/vcu_wakeup.cpp | 21 ++ 11 files changed, 270 insertions(+), 281 deletions(-) create mode 100644 src/mc/include/mc/can_utils.hpp create mode 100644 src/mc/include/mc/control_cache.hpp create mode 100644 src/mc/include/mc/timer_tasks.hpp create mode 100644 src/mc/include/mc/vcu_wakeup.hpp create mode 100644 src/mc/src/can_utils.cpp create mode 100644 src/mc/src/control_cache.cpp create mode 100644 src/mc/src/timer_tasks.cpp create mode 100644 src/mc/src/vcu_wakeup.cpp diff --git a/src/mc/include/mc/can_utils.hpp b/src/mc/include/mc/can_utils.hpp new file mode 100644 index 0000000..4d5f106 --- /dev/null +++ b/src/mc/include/mc/can_utils.hpp @@ -0,0 +1,14 @@ +#pragma once +#include "rclcpp/rclcpp.hpp" +#include "sweeper_interfaces/msg/can_frame.hpp" +#include "mc/can_struct.h" + +extern bool g_can_print_enable; + +struct CanHandlerContext +{ + rclcpp::Node::SharedPtr node; + std::shared_ptr> publisher; +}; + +void receiveHandler(const CANFrame &frame, void *userData); diff --git a/src/mc/include/mc/control_cache.hpp b/src/mc/include/mc/control_cache.hpp new file mode 100644 index 0000000..436c39c --- /dev/null +++ b/src/mc/include/mc/control_cache.hpp @@ -0,0 +1,20 @@ +#pragma once +#include "sweeper_interfaces/msg/mc_ctrl.hpp" +#include +#include + +class ControlCache +{ +public: + void update(const sweeper_interfaces::msg::McCtrl &msg); + bool get(sweeper_interfaces::msg::McCtrl &msg); + +private: + std::mutex mutex_; + sweeper_interfaces::msg::McCtrl latest_msg_; + std::chrono::steady_clock::time_point last_update_time_; + bool has_data_ = false; +}; + +extern ControlCache control_cache; +sweeper_interfaces::msg::McCtrl get_safe_control(); // 获取带超时判断的控制指令 diff --git a/src/mc/include/mc/dumper.h b/src/mc/include/mc/dumper.h index 55255a5..42cd514 100644 --- a/src/mc/include/mc/dumper.h +++ b/src/mc/include/mc/dumper.h @@ -28,11 +28,11 @@ class DumperController public: DumperController(CANDriver &driver); - void onStartup(); // 上电复位（只在内部用） - void tryStartup(); // 唤醒后自动执行复位（防重） - void updateDial(int new_dial, int current_state); // 拨钮变化检测与处理 - void resetAwake(); // 清除唤醒状态（VCU 掉线） - + void onStartup(); // 上电复位（只在内部用） + void tryStartup(); // 唤醒后自动执行复位（防重） + void updateDial(int new_dial, int current_state); // 拨钮变化检测与处理 + void resetAwake(); // 清除唤醒状态（VCU 掉线） + void setSweeping(bool sweeping) { sweeping_ = sweeping; } // 外部设置扫地状态， int getSimpleState() const; private: @@ -53,6 +53,7 @@ private: std::queue action_queue_; std::atomic busy_; std::atomic has_pending_task_; + std::atomic sweeping_ = false; // 扫地任务标志 DumperState current_state_; CANDriver &can_; }; diff --git a/src/mc/include/mc/timer_tasks.hpp b/src/mc/include/mc/timer_tasks.hpp new file mode 100644 index 0000000..2ffee92 --- /dev/null +++ b/src/mc/include/mc/timer_tasks.hpp @@ -0,0 +1,6 @@ +#pragma once +#include "rclcpp/rclcpp.hpp" +#include "mc/can_driver.h" +#include "mc/dumper.h" + +void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl); diff --git a/src/mc/include/mc/vcu_wakeup.hpp b/src/mc/include/mc/vcu_wakeup.hpp new file mode 100644 index 0000000..d5a5f12 --- /dev/null +++ b/src/mc/include/mc/vcu_wakeup.hpp @@ -0,0 +1,4 @@ +#pragma once +#include "mc/can_struct.h" + +void VCUWakeUp(CANDriver &driver); // 发送唤醒帧 diff --git a/src/mc/src/can_utils.cpp b/src/mc/src/can_utils.cpp new file mode 100644 index 0000000..30e9179 --- /dev/null +++ b/src/mc/src/can_utils.cpp @@ -0,0 +1,39 @@ +#include "mc/can_utils.hpp" +#include +#include + +bool g_can_print_enable = false; +extern std::atomic vcu_msg_received; +extern std::atomic vcu_awake; +extern rclcpp::Time last_vcu_msg_time; + +void receiveHandler(const CANFrame &frame, void *userData) +{ + auto *context = static_cast(userData); + auto node = context->node; + auto pub = context->publisher; + auto now = node->now(); + + sweeper_interfaces::msg::CanFrame msg; + msg.id = frame.id; + msg.dlc = frame.dlc; + msg.data.assign(frame.data, frame.data + frame.dlc); + pub->publish(msg); + + if (g_can_print_enable) + { + std::stringstream ss; + ss << "CAN ID: " << std::hex << std::uppercase + << std::setw((frame.id > 0x7FF) ? 8 : 5) << std::setfill(' ') << frame.id << " Data: "; + for (int i = 0; i < frame.dlc; ++i) + ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " "; + RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str()); + } + + if (frame.id == 0x18FA0121) + { + last_vcu_msg_time = now; + vcu_msg_received.store(true); + vcu_awake.store(true); + } +} diff --git a/src/mc/src/control_cache.cpp b/src/mc/src/control_cache.cpp new file mode 100644 index 0000000..3f0aa32 --- /dev/null +++ b/src/mc/src/control_cache.cpp @@ -0,0 +1,44 @@ +#include "mc/control_cache.hpp" + +ControlCache control_cache; + +void ControlCache::update(const sweeper_interfaces::msg::McCtrl &msg) +{ + std::lock_guard lock(mutex_); + latest_msg_ = msg; + last_update_time_ = std::chrono::steady_clock::now(); + has_data_ = true; +} + +bool ControlCache::get(sweeper_interfaces::msg::McCtrl &msg) +{ + std::lock_guard lock(mutex_); + if (!has_data_) + return false; + + auto now = std::chrono::steady_clock::now(); + if (std::chrono::duration_cast(now - last_update_time_).count() > 100) + return false; + + msg = latest_msg_; + return true; +} + +sweeper_interfaces::msg::McCtrl get_safe_control() +{ + sweeper_interfaces::msg::McCtrl msg; + if (!control_cache.get(msg)) + { + msg.brake = 1; + msg.gear = 0; + msg.rpm = 0; + msg.angle = 0; + msg.angle_speed = 120; + msg.edge_brush_lift = false; + msg.sweep_ctrl = false; + msg.spray = false; + msg.mud_flap = false; + msg.dust_shake = false; + } + return msg; +} diff --git a/src/mc/src/dumper.cpp b/src/mc/src/dumper.cpp index d9e3ad0..e9ff969 100644 --- a/src/mc/src/dumper.cpp +++ b/src/mc/src/dumper.cpp @@ -124,7 +124,13 @@ void DumperController::runActionSequence(const std::vector &action // 每个动作的具体 CAN 指令 + 延时 void DumperController::doAction(DumperAction act) { - constexpr int delay_ms = 10000; // 可统一调整延时时间（单位：毫秒） + if (sweeping_) + { + std::cout << "[Warning] Sweeping active, dumper action blocked.\n"; + return; + } + + constexpr int delay_ms = 15000; // 可统一调整延时时间（单位：毫秒） using namespace std::chrono_literals; CANFrame frame1; diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp index 771dbf8..0598b43 100644 --- a/src/mc/src/mc.cpp +++ b/src/mc/src/mc.cpp @@ -1,283 +1,36 @@ -#include -#include -#include -#include "mc/can_driver.h" -#include "mc/can_struct.h" -#include "mc/get_config.h" -#include "mc/dumper.h" #include "rclcpp/rclcpp.hpp" +#include "mc/get_config.h" +#include "mc/can_driver.h" +#include "mc/dumper.h" +#include "mc/can_utils.hpp" +#include "mc/control_cache.hpp" +#include "mc/timer_tasks.hpp" +#include "mc/vcu_wakeup.hpp" #include "sweeper_interfaces/msg/can_frame.hpp" #include "sweeper_interfaces/msg/mc_ctrl.hpp" -// 全局开关，控制是否打印 CAN 消息到终端 -bool g_can_print_enable = false; - namespace sweeperMsg = sweeper_interfaces::msg; -CANDriver canctl; // can驱动实例 -DumperController dumper(canctl); // 斗控制器实例 - -struct CanHandlerContext -{ - rclcpp::Node::SharedPtr node; - std::shared_ptr> publisher; -}; - -std::atomic vcu_msg_received = false; // 是否收到过vcu数据帧 -std::atomic vcu_awake = false; // 是否唤醒成功 -rclcpp::Time last_vcu_msg_time; // 上次收到vcu数据帧的时间 - -struct ControlCache -{ - std::mutex mutex; // 防止多线程冲突 - sweeperMsg::McCtrl latest_msg; - std::chrono::steady_clock::time_point last_update_time; - bool has_data = false; - - void update(const sweeperMsg::McCtrl &msg) - { - std::lock_guard lock(mutex); - latest_msg = msg; - last_update_time = std::chrono::steady_clock::now(); - has_data = true; - } - - bool get(sweeperMsg::McCtrl &msg) - { - std::lock_guard lock(mutex); - if (!has_data) - return false; - - auto now = std::chrono::steady_clock::now(); - auto duration = std::chrono::duration_cast(now - last_update_time); - - if (duration.count() > 100) // ms超时 - { - return false; // 数据过期 - } - - msg = latest_msg; - return true; - } -}; - -ControlCache control_cache; - -// 发布检测 -sweeperMsg::McCtrl get_safe_control() -{ - sweeperMsg::McCtrl msg; - if (!control_cache.get(msg)) - { - // 超时或未接收到控制数据，进入安全状态 - msg.brake = 1; - msg.gear = 0; - msg.rpm = 0; - msg.angle = 0; - msg.angle_speed = 120; - msg.edge_brush_lift = false; - msg.sweep_ctrl = false; - msg.spray = false; - msg.mud_flap = false; - msg.dust_shake = false; - } - return msg; -} - -// CAN消息接收回调 -void receiveHandler(const CANFrame &frame, void *userData) -{ - // 获取 ROS 2 Publisher - auto context = static_cast(userData); - auto publisher = context->publisher; - auto node = context->node; - auto now = node->now(); - - // 创建并发布 CAN 消息 - auto msg = sweeperMsg::CanFrame(); - msg.id = frame.id; - msg.dlc = frame.dlc; - msg.data.assign(frame.data, frame.data + frame.dlc); - publisher->publish(msg); - - // 根据开关决定是否打印 CAN 消息 - if (g_can_print_enable) - { - std::stringstream ss; - ss << "CAN ID: "; - if (frame.id > 0x7FF) - ss << std::hex << std::uppercase << frame.id; - else - ss << std::hex << std::uppercase << std::setw(5) << std::setfill(' ') << frame.id; - - ss << " Data: "; - for (int i = 0; i < frame.dlc; ++i) - { - ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " "; - } - - RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str()); - } - - // VCU报文检查 - if (frame.id == 0x18FA0121) - { - last_vcu_msg_time = now; // 更新时间 - vcu_msg_received.store(true); - vcu_awake.store(true); // 唤醒成功 - } -} +CANDriver canctl; +DumperController dumper(canctl); +std::atomic vcu_msg_received = false; +std::atomic vcu_awake = false; +rclcpp::Time last_vcu_msg_time; void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg) { + dumper.setSweeping(msg->sweep_ctrl); control_cache.update(*msg); } -void VCUWakeUp() -{ - CANFrame frame; - - // 公共设置 - frame.id = 0x18FF8017; - frame.dlc = 8; - frame.ext = true; // 使用扩展帧 - frame.rtr = false; // 不是远程帧 - - // 第1帧：握手帧 - frame.data[0] = 0x55; - frame.data[1] = 0x21; - for (int i = 2; i < 8; ++i) - frame.data[i] = 0xFF; - canctl.sendFrame(frame); - - // 延时，例如 50ms - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - - // 第2帧：启动帧 - frame.data[0] = 0xAA; - frame.data[1] = 0x21; - for (int i = 2; i < 8; ++i) - frame.data[i] = 0xFF; - canctl.sendFrame(frame); -} - -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报文 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); - dumper.resetAwake(); - } - - if (vcu_awake.load()) - { - dumper.tryStartup(); - - sweeperMsg::McCtrl msg = get_safe_control(); - - int to=static_cast(msg.dump); - int from =dumper.getSimpleState(); - - dumper.updateDial(to, from); - } }); - - // DBS控制，100Hz - static rclcpp::TimerBase::SharedPtr timer_dbs = node->create_wall_timer( - std::chrono::milliseconds(10), []() - { - sweeperMsg::McCtrl msg = get_safe_control(); - - dbs_cmd.setEnabled(msg.ehb_anable); - dbs_cmd.setPressure(msg.ehb_brake_pressure); - canctl.sendFrame(dbs_cmd.toFrame()); }); - - // 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); - - 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_cmd.setCenterCmd(0); - eps_cmd.setAngle(msg.angle); - eps_cmd.setAngularSpeed(msg.angle_speed); - eps_cmd.pack(); - - 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(); - - 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); - - // canctl.sendFrame(vcu1_cmd.toFrame()); - // canctl.sendFrame(vcu2_cmd.toFrame()); - }); -} - int main(int argc, char **argv) { - rclcpp::init(argc, argv); // 初始化 ROS 2 - - // 创建一个 ROS 2 节点 + rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("can_driver_node"); - RCLCPP_INFO(node->get_logger(), "Starting mc package..."); - // 创建一个 Publisher - auto can_publisher = node->create_publisher("can_data", 10); - - // 创建 Subscriber（收控制指令） - auto subscriber = node->create_subscription( - "mc_ctrl", // 订阅的 topic 名字 - 10, // 队列长度 - mcCtrlCallback // 收到消息后的回调 - ); - + auto pub = node->create_publisher("can_data", 10); + auto sub = node->create_subscription("mc_ctrl", 10, mcCtrlCallback); last_vcu_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type()); Config mc_config; @@ -289,24 +42,16 @@ int main(int argc, char **argv) return -1; } - // 创建上下文 auto context = std::make_shared(); context->node = node; - context->publisher = can_publisher; + context->publisher = pub; + canctl.setReceiveCallback(receiveHandler, context.get()); - // 设置接收回调并传递上下文 - canctl.setReceiveCallback(receiveHandler, (void *)context.get()); + setupTimers(node, dumper, canctl); - // 添加定时器设置 - setupTimers(node); - - // 在 ROS shutdown 时自动清理 rclcpp::on_shutdown([&]() { canctl.close(); }); - - // 事件循环 rclcpp::spin(node); - - rclcpp::shutdown(); // 关闭 ROS 2 + rclcpp::shutdown(); return 0; } diff --git a/src/mc/src/timer_tasks.cpp b/src/mc/src/timer_tasks.cpp new file mode 100644 index 0000000..690e350 --- /dev/null +++ b/src/mc/src/timer_tasks.cpp @@ -0,0 +1,89 @@ +#include "mc/timer_tasks.hpp" +#include "mc/control_cache.hpp" +#include "mc/vcu_wakeup.hpp" +#include "mc/can_struct.h" + +extern std::atomic vcu_awake; +extern std::atomic vcu_msg_received; +extern rclcpp::Time last_vcu_msg_time; + +void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl) +{ + static auto timer_wakeup = node->create_wall_timer( + std::chrono::seconds(1), [node, &canctl]() + { + if (!vcu_awake.load()) + { + RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame..."); + VCUWakeUp(canctl); + } }); + + static auto timer_watchdog = node->create_wall_timer( + std::chrono::milliseconds(200), [node, &dumper]() + { + auto now = node->now(); + if (vcu_msg_received.load() && (now - last_vcu_msg_time > rclcpp::Duration::from_seconds(0.5))) + { + RCLCPP_WARN(node->get_logger(), "[VCU] Timeout, resetting state."); + vcu_awake.store(false); + vcu_msg_received.store(false); + dumper.resetAwake(); + } + + if (vcu_awake.load()) + { + dumper.tryStartup(); + auto msg = get_safe_control(); + dumper.updateDial(static_cast(msg.dump), dumper.getSimpleState()); + } }); + + static auto timer_dbs = node->create_wall_timer( + std::chrono::milliseconds(10), [&canctl]() + { + auto msg = get_safe_control(); + dbs_cmd.setEnabled(msg.ehb_anable); + dbs_cmd.setPressure(msg.ehb_brake_pressure); + canctl.sendFrame(dbs_cmd.toFrame()); }); + + static auto timer_mcu = node->create_wall_timer( + std::chrono::milliseconds(20), [&canctl]() + { + auto msg = get_safe_control(); + mcu_cmd.setEnabled(true); + mcu_cmd.setGear(msg.gear); + mcu_cmd.setRPM(msg.rpm); + mcu_cmd.setBrake(msg.brake); + canctl.sendFrame(mcu_cmd.toFrame()); }); + + static auto timer_eps = node->create_wall_timer( + std::chrono::milliseconds(50), [&canctl]() + { + auto msg = get_safe_control(); + eps_cmd.setCenterCmd(0); + eps_cmd.setAngle(msg.angle); + eps_cmd.setAngularSpeed(msg.angle_speed); + eps_cmd.pack(); + canctl.sendFrame(eps_cmd.toFrame()); }); + + static auto timer_vcu = node->create_wall_timer( + std::chrono::milliseconds(100), [&canctl, &dumper]() + { + auto msg = get_safe_control(); + 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); + + if (msg.sweep_ctrl && dumper.getSimpleState() == 0) + { + canctl.sendFrame(vcu1_cmd.toFrame()); + canctl.sendFrame(vcu2_cmd.toFrame()); + } }); +} diff --git a/src/mc/src/vcu_wakeup.cpp b/src/mc/src/vcu_wakeup.cpp new file mode 100644 index 0000000..f48a33d --- /dev/null +++ b/src/mc/src/vcu_wakeup.cpp @@ -0,0 +1,21 @@ +#include "mc/vcu_wakeup.hpp" +#include + +void VCUWakeUp(CANDriver &driver) +{ + CANFrame frame; + frame.id = 0x18FF8017; + frame.dlc = 8; + frame.ext = true; + frame.rtr = false; + + frame.data[0] = 0x55; + frame.data[1] = 0x21; + std::fill(frame.data + 2, frame.data + 8, 0xFF); + driver.sendFrame(frame); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + frame.data[0] = 0xAA; + driver.sendFrame(frame); +}