cxh/sweeper_c200
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; // 是否唤醒成功
rclcpp::Time last_estop_msg_time; // 上次收到急停帧的时间
// 全局开关，控制是否打印 CAN 消息到终端
bool g_can_print_enable = true;
struct ControlCache
{
std::mutex mutex; // 防止多线程冲突
@ -89,35 +92,36 @@ void receiveHandler(const CANFrame &frame, void *userData)
// 获取 ROS 2 Publisher
auto context = static_cast<CanHandlerContext *>(userData);
auto publisher = context->publisher;
auto now = context->node->now();
auto node = context->node;
auto now = node->now();
// 创建一个新的消息
// 创建并发布 CAN 消息
auto msg = mc::msg::CanFrame();
msg.id = frame.id;
msg.dlc = frame.dlc;
msg.data.assign(frame.data, frame.data + frame.dlc);
// 发布消息
publisher->publish(msg);
// 终端打印
// std::cout << "CAN ID: ";
// if (frame.id > 0x7ff)
// {
// std::cout << std::hex << std::uppercase << frame.id;
// }
// else
// {
// std::cout << std::hex << std::uppercase << " " << 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";
// 根据开关决定是否打印 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());
}
// 急停报文检查
if (frame.id == 0x18FA0121 && frame.dlc >= 3)
{
last_estop_msg_time = now; // 更新时间
@ -131,7 +135,7 @@ void receiveHandler(const CANFrame &frame, void *userData)
{
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);
@ -160,7 +164,7 @@ void receiveHandler(const CANFrame &frame, void *userData)
{
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);
}
@ -207,13 +211,13 @@ void setupTimers(rclcpp::Node::SharedPtr node)
{
// VCU 唤醒帧发送定时器，1Hz
static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer(
std::chrono::seconds(1), []()
std::chrono::seconds(1), [node]()
{
if (!vcu_awake.load())
{
std::cout << "[VCU] 未唤醒，发送唤醒帧..." << std::endl;
VCUWakeUp(); // 发唤醒帧函数
} });
RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
VCUWakeUp();
} });
// 急停报文 watchdog 检查，200ms
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))
{
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);
vcu_awake.store(false);
} });
@ -300,11 +304,11 @@ int main(int argc, char **argv)
{
rclcpp::init(argc, argv); // 初始化 ROS 2
printf("Hello world mc package\n");
// 创建一个 ROS 2 节点
auto node = rclcpp::Node::make_shared("can_driver_node");
RCLCPP_INFO(node->get_logger(), "Starting mc package...");
// 创建一个 Publisher
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))
{
std::cerr << "Failed to open CAN interface :";
std::cerr << mc_config.can_dev << std::endl;
RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str());
return -1;
}