349 lines
9.3 KiB
C++
349 lines
9.3 KiB
C++
#include <cstdio>
|
||
#include <iostream>
|
||
#include <iomanip>
|
||
#include "mc/can_driver.h"
|
||
#include "mc/can_struct.h"
|
||
#include "mc/get_config.h"
|
||
#include "rclcpp/rclcpp.hpp"
|
||
#include "mc/msg/can_frame.hpp"
|
||
#include "mc/msg/mc_ctrl.hpp"
|
||
|
||
CANDriver canctl;
|
||
|
||
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
|
||
{
|
||
std::mutex mutex; // 防止多线程冲突
|
||
mc::msg::McCtrl latest_msg;
|
||
std::chrono::steady_clock::time_point last_update_time;
|
||
bool has_data = false;
|
||
|
||
void update(const mc::msg::McCtrl &msg)
|
||
{
|
||
std::lock_guard<std::mutex> lock(mutex);
|
||
latest_msg = msg;
|
||
last_update_time = std::chrono::steady_clock::now();
|
||
has_data = true;
|
||
}
|
||
|
||
bool get(mc::msg::McCtrl &msg)
|
||
{
|
||
std::lock_guard<std::mutex> lock(mutex);
|
||
if (!has_data)
|
||
return false;
|
||
|
||
auto now = std::chrono::steady_clock::now();
|
||
auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time);
|
||
|
||
if (duration.count() > 100) // ms超时
|
||
{
|
||
return false; // 数据过期
|
||
}
|
||
|
||
msg = latest_msg;
|
||
return true;
|
||
}
|
||
};
|
||
|
||
ControlCache control_cache;
|
||
|
||
// 发布检测
|
||
mc::msg::McCtrl get_safe_control()
|
||
{
|
||
mc::msg::McCtrl msg;
|
||
if (!control_cache.get(msg))
|
||
{
|
||
// 超时或未接收到控制数据，进入安全状态
|
||
msg.mcu_enabled = false;
|
||
msg.brake = 1;
|
||
msg.gear = 0;
|
||
msg.rpm = 0;
|
||
msg.brake_time_ms = 500;
|
||
msg.angle = 0;
|
||
msg.angle_speed = 120;
|
||
msg.main_brush_lift = false;
|
||
msg.edge_brush_lift = false;
|
||
msg.vacuum = false;
|
||
msg.spray = false;
|
||
msg.mud_flap = false;
|
||
msg.dust_shake = false;
|
||
msg.main_brush_spin = false;
|
||
msg.edge_brush_spin = false;
|
||
}
|
||
return msg;
|
||
}
|
||
|
||
// CAN消息接收回调
|
||
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 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";
|
||
|
||
// 检查是否为急停报文
|
||
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; // 急停状态位
|
||
|
||
// 急停被按下
|
||
if (estop_status != 0x03)
|
||
{
|
||
if (!estop_active.load())
|
||
{
|
||
std::cout << "!!! E-STOP TRIGGERED, entering safe mode" << std::endl;
|
||
}
|
||
|
||
estop_active.store(true);
|
||
|
||
// 立即更新控制缓存，强制进入安全状态
|
||
mc::msg::McCtrl safe_msg;
|
||
safe_msg.mcu_enabled = false;
|
||
safe_msg.brake = 1;
|
||
safe_msg.gear = 0;
|
||
safe_msg.rpm = 0;
|
||
safe_msg.brake_time_ms = 500;
|
||
safe_msg.angle = 0;
|
||
safe_msg.angle_speed = 120;
|
||
safe_msg.main_brush_lift = false;
|
||
safe_msg.edge_brush_lift = false;
|
||
safe_msg.vacuum = false;
|
||
safe_msg.spray = false;
|
||
safe_msg.mud_flap = false;
|
||
safe_msg.dust_shake = false;
|
||
safe_msg.main_brush_spin = false;
|
||
safe_msg.edge_brush_spin = false;
|
||
|
||
control_cache.update(safe_msg); // 用这个覆盖已有控制指令
|
||
}
|
||
else
|
||
{
|
||
if (estop_active.load())
|
||
{
|
||
std::cout << "E-STOP released, resuming control" << std::endl;
|
||
}
|
||
estop_active.store(false);
|
||
}
|
||
}
|
||
}
|
||
|
||
void mcCtrlCallback(const mc::msg::McCtrl::SharedPtr msg)
|
||
{
|
||
if (estop_active)
|
||
return;
|
||
|
||
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), []()
|
||
{
|
||
mc::msg::McCtrl msg = get_safe_control();
|
||
|
||
mcu_cmd.setEnabled(msg.mcu_enabled);
|
||
mcu_cmd.setGear(msg.gear);
|
||
mcu_cmd.setRPM(msg.rpm);
|
||
mcu_cmd.setBrakeTime(msg.brake_time_ms);
|
||
mcu_cmd.setBrake(msg.brake);
|
||
mcu_cmd.pack();
|
||
|
||
canctl.sendFrame(mcu_cmd.toFrame()); });
|
||
|
||
// EPS 控制，20Hz
|
||
static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer(
|
||
std::chrono::milliseconds(50), []()
|
||
{
|
||
mc::msg::McCtrl msg = get_safe_control();
|
||
|
||
eps_cmd.setCenterCmd(0);
|
||
eps_cmd.setAngle(static_cast<int16_t>(msg.angle * 5));
|
||
eps_cmd.setAngularSpeed(msg.angle_speed);
|
||
eps_cmd.setControlMode(0x20);
|
||
eps_cmd.pack();
|
||
|
||
canctl.sendFrame(eps_cmd.toFrame()); });
|
||
|
||
// VCU2 控制，10Hz
|
||
static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer(
|
||
std::chrono::milliseconds(100), []()
|
||
{
|
||
mc::msg::McCtrl msg = get_safe_control();
|
||
|
||
vcu1_cmd.setLeftLight(msg.left_light);
|
||
vcu1_cmd.setNightLightPercent(msg.night_light);
|
||
|
||
vcu2_cmd.setBrakeLight(msg.brake_light);
|
||
vcu2_cmd.setDustShake(msg.dust_shake);
|
||
vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
|
||
vcu2_cmd.setHeadlight(msg.headlight);
|
||
vcu2_cmd.setMainBrushLift(msg.main_brush_lift);
|
||
vcu2_cmd.setMudFlap(msg.mud_flap);
|
||
vcu2_cmd.setRightLight(msg.right_light);
|
||
vcu2_cmd.setSpray(msg.spray);
|
||
vcu2_cmd.setVacuum(msg.vacuum);
|
||
|
||
canctl.sendFrame(vcu1_cmd.toFrame());
|
||
canctl.sendFrame(vcu2_cmd.toFrame()); });
|
||
|
||
// 刷子控制，3.3Hz
|
||
static rclcpp::TimerBase::SharedPtr timer_brush = node->create_wall_timer(
|
||
std::chrono::milliseconds(300), []()
|
||
{
|
||
mc::msg::McCtrl msg = get_safe_control();
|
||
|
||
main_brush_cmd.setBrushEnable(msg.main_brush_spin);
|
||
main_brush_cmd.setBrushPwm(msg.main_brush_pwm);
|
||
|
||
edge_brush_cmd.setBrushEnable(msg.edge_brush_spin);
|
||
edge_brush_cmd.setBrushPwm(msg.edge_brush_pwm);
|
||
|
||
canctl.sendFrame(main_brush_cmd.toFrame());
|
||
canctl.sendFrame(edge_brush_cmd.toFrame()); });
|
||
}
|
||
|
||
int main(int argc, char **argv)
|
||
{
|
||
rclcpp::init(argc, argv); // 初始化 ROS 2
|
||
|
||
printf("Hello world mc package\n");
|
||
|
||
last_estop_msg_time = rclcpp::Clock().now();
|
||
|
||
// 创建一个 ROS 2 节点
|
||
auto node = rclcpp::Node::make_shared("can_driver_node");
|
||
|
||
// 创建一个 Publisher
|
||
auto can_publisher = node->create_publisher<mc::msg::CanFrame>("can_data", 10);
|
||
|
||
// 创建 Subscriber（收控制指令）
|
||
auto subscriber = node->create_subscription<mc::msg::McCtrl>(
|
||
"mc_ctrl", // 订阅的 topic 名字
|
||
10, // 队列长度
|
||
mcCtrlCallback // 收到消息后的回调
|
||
);
|
||
|
||
Config mc_config;
|
||
load_config(mc_config);
|
||
|
||
if (!canctl.open(mc_config.can_dev))
|
||
{
|
||
std::cerr << "Failed to open CAN interface :";
|
||
std::cerr << mc_config.can_dev << std::endl;
|
||
return -1;
|
||
}
|
||
|
||
// 创建上下文
|
||
auto context = std::make_shared<CanHandlerContext>();
|
||
context->node = node;
|
||
context->publisher = can_publisher;
|
||
|
||
// 设置接收回调并传递上下文
|
||
canctl.setReceiveCallback(receiveHandler, (void *)context.get());
|
||
|
||
// 添加定时器设置
|
||
setupTimers(node);
|
||
|
||
// ROS 2 spin
|
||
rclcpp::spin(node);
|
||
|
||
// 关闭 CAN 接口
|
||
canctl.close();
|
||
rclcpp::shutdown(); // 关闭 ROS 2
|
||
return 0;
|
||
}
|