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

Auto commit at 2025-05-29 14:58:54

Browse Source
This commit is contained in:
cxh 2025-05-29 14:58:54 +08:00
parent 258ba11026
commit ec1acd4f53
1 changed files with 35 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
src/mc/src/mc.cpp
View File

@ -21,6 +21,9 @@ std::atomic<bool> estop_msg_received = false; // 是否收到过急停帧
std::atomic<bool> vcu_awake = false; // 是否唤醒成功 std::atomic<bool> vcu_awake = false; // 是否唤醒成功
rclcpp::Time last_estop_msg_time; // 上次收到急停帧的时间 rclcpp::Time last_estop_msg_time; // 上次收到急停帧的时间
// 全局开关，控制是否打印 CAN 消息到终端
bool g_can_print_enable = true;
struct ControlCache struct ControlCache
{ {
std::mutex mutex; // 防止多线程冲突 std::mutex mutex; // 防止多线程冲突
@ -89,35 +92,36 @@ void receiveHandler(const CANFrame &frame, void *userData)
// 获取 ROS 2 Publisher // 获取 ROS 2 Publisher
auto context = static_cast<CanHandlerContext *>(userData); auto context = static_cast<CanHandlerContext *>(userData);
auto publisher = context->publisher; auto publisher = context->publisher;
auto now = context->node->now(); auto node = context->node;
auto now = node->now();
// 创建一个新的消息 // 创建并发布 CAN 消息
auto msg = mc::msg::CanFrame(); auto msg = mc::msg::CanFrame();
msg.id = frame.id; msg.id = frame.id;
msg.dlc = frame.dlc; msg.dlc = frame.dlc;
msg.data.assign(frame.data, frame.data + frame.dlc); msg.data.assign(frame.data, frame.data + frame.dlc);
// 发布消息
publisher->publish(msg); publisher->publish(msg);
// 终端打印 // 根据开关决定是否打印 CAN 消息
// std::cout << "CAN ID: "; if (g_can_print_enable)
// if (frame.id > 0x7ff) {
// { std::stringstream ss;
// std::cout << std::hex << std::uppercase << frame.id; ss << "CAN ID: ";
// } if (frame.id > 0x7FF)
// else ss << std::hex << std::uppercase << frame.id;
// { else
// std::cout << std::hex << std::uppercase << " " << frame.id; ss << std::hex << std::uppercase << std::setw(5) << std::setfill(' ') << frame.id;
// }
// std::cout << " Data: ";
// for (int i = 0; i < frame.dlc; ++i)
// {
// std::cout << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
// }
// std::cout << std::dec << "\n";
// 检查是否为急停报文 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());
}
// 急停报文检查
if (frame.id == 0x18FA0121 && frame.dlc >= 3) if (frame.id == 0x18FA0121 && frame.dlc >= 3)
{ {
last_estop_msg_time = now; // 更新时间 last_estop_msg_time = now; // 更新时间
@ -131,7 +135,7 @@ void receiveHandler(const CANFrame &frame, void *userData)
{ {
if (!estop_active.load()) if (!estop_active.load())
{ {
std::cout << "!!! E-STOP TRIGGERED, entering safe mode" << std::endl; RCLCPP_WARN(node->get_logger(), "E-STOP TRIGGERED, entering safe mode");
} }
estop_active.store(true); estop_active.store(true);
@ -160,7 +164,7 @@ void receiveHandler(const CANFrame &frame, void *userData)
{ {
if (estop_active.load()) if (estop_active.load())
{ {
std::cout << "E-STOP released, resuming control" << std::endl; RCLCPP_INFO(node->get_logger(), "E-STOP released, resuming control");
} }
estop_active.store(false); estop_active.store(false);
} }
@ -207,13 +211,13 @@ 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), []() std::chrono::seconds(1), [node]()
{ {
if (!vcu_awake.load()) if (!vcu_awake.load())
{ {
std::cout << "[VCU] 未唤醒，发送唤醒帧..." << std::endl; RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
VCUWakeUp(); // 发唤醒帧函数 VCUWakeUp();
} }); } });
// 急停报文 watchdog 检查，200ms // 急停报文 watchdog 检查，200ms
static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer( static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer(
@ -224,7 +228,7 @@ void setupTimers(rclcpp::Node::SharedPtr node)
if (estop_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5)) if (estop_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
{ {
std::cout << "[VCU] 急停报文超时，重置唤醒状态。" << std::endl; RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] Emergency stop message timeout, resetting wake-up state.");
estop_msg_received.store(false); estop_msg_received.store(false);
vcu_awake.store(false); vcu_awake.store(false);
} }); } });
@ -300,11 +304,11 @@ int main(int argc, char **argv)
{ {
rclcpp::init(argc, argv); // 初始化 ROS 2 rclcpp::init(argc, argv); // 初始化 ROS 2
printf("Hello world mc package\n");
// 创建一个 ROS 2 节点 // 创建一个 ROS 2 节点
auto node = rclcpp::Node::make_shared("can_driver_node"); auto node = rclcpp::Node::make_shared("can_driver_node");
RCLCPP_INFO(node->get_logger(), "Starting mc package...");
// 创建一个 Publisher // 创建一个 Publisher
auto can_publisher = node->create_publisher<mc::msg::CanFrame>("can_data", 10); auto can_publisher = node->create_publisher<mc::msg::CanFrame>("can_data", 10);
@ -322,8 +326,7 @@ int main(int argc, char **argv)
if (!canctl.open(mc_config.can_dev)) if (!canctl.open(mc_config.can_dev))
{ {
std::cerr << "Failed to open CAN interface :"; RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str());
std::cerr << mc_config.can_dev << std::endl;
return -1; return -1;
} }