Auto commit at 2025-05-29 14:24:58
This commit is contained in:
parent
80eb36066d
commit
579182ba07
@ -10,8 +10,16 @@
|
||||
|
||||
CANDriver canctl;
|
||||
|
||||
// 急停状态量
|
||||
std::atomic<bool> estop_active = false;
|
||||
struct CanHandlerContext
|
||||
{
|
||||
rclcpp::Node::SharedPtr node;
|
||||
std::shared_ptr<rclcpp::Publisher<mc::msg::CanFrame>> publisher;
|
||||
};
|
||||
|
||||
std::atomic<bool> estop_active = false; // 急停状态量
|
||||
std::atomic<bool> estop_msg_received = false; // 是否收到过急停帧
|
||||
std::atomic<bool> vcu_awake = false; // 是否唤醒成功
|
||||
rclcpp::Time last_estop_msg_time; // 上次收到急停帧的时间
|
||||
|
||||
struct ControlCache
|
||||
{
|
||||
@ -79,19 +87,15 @@ mc::msg::McCtrl get_safe_control()
|
||||
void receiveHandler(const CANFrame &frame, void *userData)
|
||||
{
|
||||
// 获取 ROS 2 Publisher
|
||||
auto publisher = *static_cast<std::shared_ptr<rclcpp::Publisher<mc::msg::CanFrame>> *>(userData);
|
||||
auto context = static_cast<CanHandlerContext *>(userData);
|
||||
auto publisher = context->publisher;
|
||||
auto now = context->node->now();
|
||||
|
||||
// 创建一个新的消息
|
||||
auto msg = mc::msg::CanFrame();
|
||||
msg.id = frame.id;
|
||||
msg.dlc = frame.dlc;
|
||||
msg.data.resize(frame.dlc);
|
||||
|
||||
// 复制数据
|
||||
for (int i = 0; i < frame.dlc; ++i)
|
||||
{
|
||||
msg.data[i] = frame.data[i];
|
||||
}
|
||||
msg.data.assign(frame.data, frame.data + frame.dlc);
|
||||
|
||||
// 发布消息
|
||||
publisher->publish(msg);
|
||||
@ -116,6 +120,10 @@ void receiveHandler(const CANFrame &frame, void *userData)
|
||||
// 检查是否为急停报文
|
||||
if (frame.id == 0x18FA0121 && frame.dlc >= 3)
|
||||
{
|
||||
last_estop_msg_time = now; // 更新时间
|
||||
estop_msg_received.store(true);
|
||||
vcu_awake.store(true); // 唤醒成功
|
||||
|
||||
uint8_t estop_status = (frame.data[2] & 0x0c) >> 2; // 急停状态位
|
||||
|
||||
// 急停被按下
|
||||
@ -167,8 +175,60 @@ void mcCtrlCallback(const mc::msg::McCtrl::SharedPtr msg)
|
||||
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), []()
|
||||
{
|
||||
if (!vcu_awake.load())
|
||||
{
|
||||
std::cout << "[VCU] 未唤醒，发送唤醒帧..." << std::endl;
|
||||
VCUWakeUp(); // 发唤醒帧函数
|
||||
} });
|
||||
|
||||
// 急停报文 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_estop_msg_time;
|
||||
|
||||
if (estop_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
|
||||
{
|
||||
std::cout << "[VCU] 急停报文超时，重置唤醒状态。" << std::endl;
|
||||
estop_msg_received.store(false);
|
||||
vcu_awake.store(false);
|
||||
} });
|
||||
|
||||
// MCU、VCU1 控制，50Hz
|
||||
static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
|
||||
std::chrono::milliseconds(20), []()
|
||||
@ -236,34 +296,6 @@ void setupTimers(rclcpp::Node::SharedPtr node)
|
||||
canctl.sendFrame(edge_brush_cmd.toFrame()); });
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv); // 初始化 ROS 2
|
||||
@ -274,7 +306,7 @@ int main(int argc, char **argv)
|
||||
auto node = rclcpp::Node::make_shared("can_driver_node");
|
||||
|
||||
// 创建一个 Publisher
|
||||
auto publisher = node->create_publisher<mc::msg::CanFrame>("can_data", 10);
|
||||
auto can_publisher = node->create_publisher<mc::msg::CanFrame>("can_data", 10);
|
||||
|
||||
// 创建 Subscriber（收控制指令）
|
||||
auto subscriber = node->create_subscription<mc::msg::McCtrl>(
|
||||
@ -293,10 +325,13 @@ int main(int argc, char **argv)
|
||||
return -1;
|
||||
}
|
||||
|
||||
VCUWakeUp();
|
||||
// 创建上下文
|
||||
auto context = std::make_shared<CanHandlerContext>();
|
||||
context->node = node;
|
||||
context->publisher = can_publisher;
|
||||
|
||||
// 设置接收回调并传递 ROS 2 Publisher 作为 userData
|
||||
canctl.setReceiveCallback(receiveHandler, (void *)&publisher);
|
||||
// 设置接收回调并传递上下文
|
||||
canctl.setReceiveCallback(receiveHandler, (void *)context.get());
|
||||
|
||||
// 添加定时器设置
|
||||
setupTimers(node);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user