235 lines
9.0 KiB
C++
235 lines
9.0 KiB
C++
#include "radio_ctrl/uart_handler.h"
|
||
#include "radio_ctrl/get_config.h"
|
||
#include <iostream>
|
||
#include <algorithm>
|
||
#include <rclcpp/rclcpp.hpp>
|
||
#include "mc/msg/mc_ctrl.hpp"
|
||
#include "mc/msg/can_frame.hpp"
|
||
|
||
class SBUSNode : public rclcpp::Node
|
||
{
|
||
public:
|
||
SBUSNode() : Node("sbus_node"), current_feedback_angle(0.0f)
|
||
{
|
||
if (load_config(config))
|
||
{
|
||
std::cout << "Serial Port: " << config.serial_port << "\n";
|
||
std::cout << "Baudrate: " << config.baudrate << "\n";
|
||
std::cout << "MCU RPM Max: " << config.mcu_rpm_max << "\n";
|
||
std::cout << "EPS Angle Max: " << config.eps_angle_max << "\n";
|
||
}
|
||
|
||
// 发布控制指令消息的发布器
|
||
pub_ = this->create_publisher<mc::msg::McCtrl>("mc_ctrl", 10);
|
||
|
||
// 订阅CAN反馈的回调函数
|
||
can_sub_ = this->create_subscription<mc::msg::CanFrame>(
|
||
"can_data", 10,
|
||
std::bind(&SBUSNode::can_callback, this, std::placeholders::_1));
|
||
|
||
// 初始化串口读取（读取遥控器数据）
|
||
uart_handler_ = std::make_shared<UartHandler>(config.serial_port, config.baudrate);
|
||
if (!uart_handler_->open_serial())
|
||
{
|
||
RCLCPP_ERROR(this->get_logger(), "Failed to open serial port!");
|
||
return;
|
||
}
|
||
uart_handler_->start_reading(); // 启动串口后台读线程
|
||
|
||
// 创建周期定时器（每20ms回调一次控制逻辑，50Hz）
|
||
timer_ = this->create_wall_timer(
|
||
std::chrono::milliseconds(20),
|
||
std::bind(&SBUSNode::timer_callback, this));
|
||
}
|
||
|
||
~SBUSNode()
|
||
{
|
||
if (uart_handler_)
|
||
{
|
||
uart_handler_->stop_reading();
|
||
}
|
||
}
|
||
|
||
private:
|
||
// 定时器回调函数，用于周期性发布控制指令
|
||
void timer_callback()
|
||
{
|
||
int MCU_RPM_MAX = config.mcu_rpm_max;
|
||
float EPS_ANGLE_MAX = config.eps_angle_max;
|
||
constexpr float EPS_GEAR_RATIO = 16.5f;
|
||
constexpr float DELTA_T = 0.02f; // 20ms
|
||
|
||
auto msg = mc::msg::McCtrl(); // 控制消息对象
|
||
|
||
// 获取各通道遥控数据
|
||
uint16_t ch_data[8];
|
||
// 获取数据安全性
|
||
bool data_safe = uart_handler_->get_data_safe();
|
||
for (int i = 0; i < 8; ++i)
|
||
{
|
||
ch_data[i] = uart_handler_->get_channel_value(i);
|
||
printf("ch[%d]:%d ", i, ch_data[i]);
|
||
}
|
||
printf("\n");
|
||
// 是否使能车辆控制
|
||
if (ch_data[6] == 192)
|
||
{
|
||
msg.mcu_enabled = true;
|
||
msg.brake = 0;
|
||
// 挡位选择
|
||
if (ch_data[7] == 192)
|
||
{
|
||
msg.gear = 3; // D挡
|
||
}
|
||
else if (ch_data[7] == 1792)
|
||
{
|
||
msg.gear = 1; // R挡
|
||
}
|
||
else if (ch_data[7] == 992)
|
||
{
|
||
msg.gear = 2; // N挡
|
||
}
|
||
else
|
||
{
|
||
msg.gear = 0; // 未知状态
|
||
}
|
||
|
||
// 油门 / 刹车逻辑
|
||
if (ch_data[1] >= 1016)
|
||
{
|
||
msg.brake = 1;
|
||
msg.brake_time_ms = static_cast<uint16_t>(30000 + (500 - 30000) * (ch_data[1] - 1016) / (1816 - 1016));
|
||
msg.rpm = 0;
|
||
}
|
||
else
|
||
{
|
||
msg.brake = 0;
|
||
msg.brake_time_ms = 500;
|
||
msg.rpm = static_cast<uint16_t>(MCU_RPM_MAX * (1016 - ch_data[1]) / (1016 - 216));
|
||
}
|
||
|
||
// 一键清扫
|
||
if (ch_data[6] == 1792)
|
||
{
|
||
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;
|
||
}
|
||
else if (ch_data[6] == 192)
|
||
{
|
||
msg.main_brush_lift = true;
|
||
msg.edge_brush_lift = true;
|
||
msg.vacuum = true;
|
||
msg.spray = true;
|
||
msg.mud_flap = true;
|
||
msg.dust_shake = true;
|
||
msg.main_brush_spin = true;
|
||
msg.edge_brush_spin = true;
|
||
msg.main_brush_pwm = 100;
|
||
msg.edge_brush_pwm = 100;
|
||
}
|
||
|
||
// 转向逻辑
|
||
float target_angle = (static_cast<float>(ch_data[3]) - 800) / 800 * EPS_ANGLE_MAX;
|
||
|
||
// 角速度（度/秒）
|
||
float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T;
|
||
|
||
// 电机转速（单位：rpm）
|
||
float motor_rpm = angle_speed * EPS_GEAR_RATIO;
|
||
|
||
// 限制电机转速到 [120, 1500] 范围，防止过小/过大
|
||
uint16_t can_speed = std::clamp(
|
||
static_cast<uint16_t>(motor_rpm),
|
||
static_cast<uint16_t>(120),
|
||
static_cast<uint16_t>(1500));
|
||
|
||
msg.angle = target_angle;
|
||
msg.angle_speed = can_speed;
|
||
}
|
||
else
|
||
{
|
||
// 未使能状态，发送安全默认控制指令
|
||
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;
|
||
}
|
||
|
||
if (data_safe)
|
||
{
|
||
RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
|
||
<< "\n mcu_enabled: " << msg.mcu_enabled
|
||
<< "\n brake: " << static_cast<int>(msg.brake)
|
||
<< "\n gear: " << static_cast<int>(msg.gear)
|
||
<< "\n rpm: " << msg.rpm
|
||
<< "\n brake_time_ms: " << msg.brake_time_ms
|
||
<< "\n angle: " << msg.angle
|
||
<< "\n angle_speed: " << msg.angle_speed
|
||
<< "\n main_brush_lift: " << msg.main_brush_lift
|
||
<< "\n edge_brush_lift: " << msg.edge_brush_lift
|
||
<< "\n vacuum: " << msg.vacuum
|
||
<< "\n spray: " << msg.spray
|
||
<< "\n mud_flap: " << msg.mud_flap
|
||
<< "\n dust_shake: " << msg.dust_shake
|
||
<< "\n left_light: " << msg.left_light
|
||
<< "\n right_light: " << msg.right_light
|
||
<< "\n night_light: " << msg.night_light
|
||
<< "\n brake_light: " << msg.brake_light
|
||
<< "\n headlight: " << msg.headlight
|
||
<< "\n main_brush_spin: " << msg.main_brush_spin
|
||
<< "\n edge_brush_spin: " << msg.edge_brush_spin
|
||
<< "\n main_brush_pwm: " << static_cast<int>(msg.main_brush_pwm)
|
||
<< "\n edge_brush_pwm: " << static_cast<int>(msg.edge_brush_pwm));
|
||
|
||
pub_->publish(msg); // 发布控制消息
|
||
}
|
||
}
|
||
|
||
// CAN反馈回调函数（用于获取当前转向角度）
|
||
void can_callback(const mc::msg::CanFrame::SharedPtr msg)
|
||
{
|
||
// 判断是否为转向反馈帧（ID=0x401，且数据长度大于5）
|
||
if (msg->id == 0x401 && msg->dlc >= 5)
|
||
{
|
||
// 第3、4字节拼接为原始角度数据
|
||
uint16_t raw_value = static_cast<uint16_t>((msg->data[3] << 8) | msg->data[4]);
|
||
|
||
// 数据偏移：减去1024并除以5，得到角度（单位：度）
|
||
current_feedback_angle = (raw_value - 1024) / 5.0f;
|
||
}
|
||
}
|
||
|
||
// ROS2 发布/订阅/定时器/串口读取器
|
||
rclcpp::Publisher<mc::msg::McCtrl>::SharedPtr pub_;
|
||
rclcpp::Subscription<mc::msg::CanFrame>::SharedPtr can_sub_;
|
||
rclcpp::TimerBase::SharedPtr timer_;
|
||
std::shared_ptr<UartHandler> uart_handler_;
|
||
|
||
float current_feedback_angle; // 当前反馈角度（从CAN中读取）
|
||
RmoCtlConfig config;
|
||
};
|
||
|
||
int main(int argc, char *argv[])
|
||
{
|
||
rclcpp::init(argc, argv);
|
||
rclcpp::spin(std::make_shared<SBUSNode>()); // 启动节点
|
||
rclcpp::shutdown();
|
||
return 0;
|
||
} |