can_filter
This commit is contained in:
parent
eabf181280
commit
d701062b3b
@ -8,7 +8,9 @@ source install/setup.bash
|
||||
ros2 run vehicle_params vehicle_params_node &
|
||||
sleep 0.2
|
||||
|
||||
ros2 run radio_ctrl radio_ctrl_node &
|
||||
# ros2 run radio_ctrl radio_ctrl_node &
|
||||
# sleep 0.2
|
||||
ros2 run can_radio_ctrl can_radio_ctrl_node &
|
||||
sleep 0.2
|
||||
|
||||
ros2 run mc mc_node &
|
||||
|
||||
@ -12,13 +12,11 @@
|
||||
#include <unistd.h>
|
||||
|
||||
#include <cstdint>
|
||||
#include <mutex> // 线程安全互斥锁
|
||||
|
||||
constexpr auto FALSE = 0;
|
||||
constexpr auto TRUE = 1;
|
||||
|
||||
int can_Init(void);
|
||||
void can_Recv_CAN1(void);
|
||||
|
||||
// 遥控器数据结构体
|
||||
struct RadioData
|
||||
{
|
||||
@ -29,6 +27,14 @@ struct RadioData
|
||||
bool data_valid; // 数据有效性标志
|
||||
};
|
||||
|
||||
// 全局变量声明
|
||||
extern RadioData radio_data;
|
||||
extern std::mutex radio_data_mutex; // 保护radio_data的互斥锁
|
||||
|
||||
// 函数声明
|
||||
int can_Init(void);
|
||||
void can_Recv_CAN1(void);
|
||||
RadioData get_radio_data(void); //
|
||||
void can_Stop_Recv(void); //
|
||||
|
||||
#endif // !_CANDEV_H
|
||||
@ -1,11 +1,13 @@
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <cmath> // 补充fabs所需头文件
|
||||
|
||||
#include "can_radio_ctrl/candev.h"
|
||||
#include "can_radio_ctrl/get_config.h"
|
||||
#include "logger/logger.h"
|
||||
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
|
||||
#include "sweeper_interfaces/msg/can_frame.hpp"
|
||||
|
||||
namespace sweeperMsg = sweeper_interfaces::msg;
|
||||
|
||||
@ -19,52 +21,68 @@ class CanRadioCtrlNode : public rclcpp::Node
|
||||
public:
|
||||
CanRadioCtrlNode() : Node("can_radio_ctrl_node"), current_feedback_angle(0.0f), print_counter(0)
|
||||
{
|
||||
// 加载配置（仅初始化一次，避免重复加载）
|
||||
if (load_config(config))
|
||||
{
|
||||
LOG_INFO("MCU RPM Max: %d", config.mcu_rpm_max);
|
||||
LOG_INFO("EPS Angle Max: %f", config.eps_angle_max);
|
||||
mcu_rpm_max_ = config.mcu_rpm_max;
|
||||
eps_angle_max_ = config.eps_angle_max;
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG_ERROR("Failed to load config, use default values!");
|
||||
mcu_rpm_max_ = 1500; // 默认值
|
||||
eps_angle_max_ = 45.0f; // 默认值
|
||||
}
|
||||
|
||||
can_Recv_CAN1();
|
||||
|
||||
// 发布控制指令消息的发布器
|
||||
pub_ = this->create_publisher<sweeperMsg::McCtrl>("radio_mc_ctrl", 10);
|
||||
|
||||
// 订阅CAN反馈的回调函数
|
||||
can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
|
||||
"can_data", 10, std::bind(&CanRadioCtrlNode::can_callback, this, std::placeholders::_1));
|
||||
|
||||
// 创建周期定时器（每20ms回调一次控制逻辑，50Hz）
|
||||
timer_ =
|
||||
this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&CanRadioCtrlNode::timer_callback, this));
|
||||
timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(20),
|
||||
std::bind(&CanRadioCtrlNode::timer_callback, this)
|
||||
);
|
||||
|
||||
LOG_INFO("CanRadioCtrlNode initialized successfully!");
|
||||
}
|
||||
|
||||
~CanRadioCtrlNode() {}
|
||||
~CanRadioCtrlNode()
|
||||
{
|
||||
// 停止CAN接收线程
|
||||
can_Stop_Recv();
|
||||
LOG_INFO("CanRadioCtrlNode destroyed!");
|
||||
}
|
||||
|
||||
private:
|
||||
// 定时器回调函数，用于周期性发布控制指令
|
||||
void timer_callback()
|
||||
{
|
||||
static int MCU_RPM_MAX = config.mcu_rpm_max;
|
||||
static float EPS_ANGLE_MAX = config.eps_angle_max;
|
||||
|
||||
// 接收CAN数据
|
||||
can_Recv_CAN1();
|
||||
// 线程安全获取遥控数据（拷贝而非直接操作全局变量）
|
||||
RadioData radio_data = get_radio_data();
|
||||
|
||||
// 检查数据有效性
|
||||
bool has_received = radio_data.data_valid;
|
||||
bool data_safe = radio_data.data_valid; // 这里可以添加超时判断
|
||||
bool data_safe = radio_data.data_valid;
|
||||
|
||||
auto msg = sweeperMsg::McCtrl();
|
||||
|
||||
if (has_received && data_safe)
|
||||
{
|
||||
// 解析挡位
|
||||
if (radio_data.gear == 0)
|
||||
switch(radio_data.gear)
|
||||
{
|
||||
msg.gear = 1; // R挡（低位）
|
||||
}
|
||||
else if (radio_data.gear == 1)
|
||||
{
|
||||
msg.gear = 0; // N挡（中位）
|
||||
}
|
||||
else if (radio_data.gear == 2)
|
||||
{
|
||||
msg.gear = 2; // D挡（高位）
|
||||
case 0: msg.gear = 1; break; // R挡
|
||||
case 1: msg.gear = 0; break; // N挡
|
||||
case 2: msg.gear = 2; break; // D挡
|
||||
default: msg.gear = 0; LOG_WARN("Unknown gear: %d", radio_data.gear); break;
|
||||
}
|
||||
|
||||
// 解析油门/刹车
|
||||
@ -76,31 +94,29 @@ class CanRadioCtrlNode : public rclcpp::Node
|
||||
else
|
||||
{
|
||||
msg.brake = 0;
|
||||
msg.rpm = static_cast<uint16_t>(std::clamp(MCU_RPM_MAX * radio_data.throttle / 450, 0, MCU_RPM_MAX));
|
||||
float throttle_ratio = static_cast<float>(radio_data.throttle) / 450.0f;
|
||||
int rpm_val = static_cast<int>(mcu_rpm_max_ * throttle_ratio);
|
||||
msg.rpm = static_cast<uint16_t>(std::clamp(rpm_val, 0, mcu_rpm_max_));
|
||||
}
|
||||
|
||||
// 解析清扫开关
|
||||
if (radio_data.sweep == 0)
|
||||
{
|
||||
msg.sweep = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
msg.sweep = true;
|
||||
}
|
||||
msg.sweep = (radio_data.sweep != 0);
|
||||
|
||||
// 解析转向
|
||||
float target_angle = static_cast<float>(radio_data.steering) / 450 * EPS_ANGLE_MAX;
|
||||
float steering_ratio = static_cast<float>(radio_data.steering) / 450.0f;
|
||||
float target_angle = steering_ratio * eps_angle_max_;
|
||||
|
||||
// 角速度（度/秒）
|
||||
float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T;
|
||||
float angle_diff = fabs(target_angle - current_feedback_angle);
|
||||
float angle_speed = angle_diff / DELTA_T;
|
||||
|
||||
// 电机转速（单位：rpm）
|
||||
float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * 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));
|
||||
uint16_t can_speed = static_cast<uint16_t>(
|
||||
std::clamp(motor_rpm, 120.0f, 1500.0f)
|
||||
);
|
||||
|
||||
msg.angle = target_angle;
|
||||
msg.angle_speed = can_speed;
|
||||
@ -108,8 +124,19 @@ class CanRadioCtrlNode : public rclcpp::Node
|
||||
// 发布控制消息
|
||||
pub_->publish(msg);
|
||||
|
||||
// 打印日志
|
||||
if (++print_counter >= PRINT_INTERVAL)
|
||||
{
|
||||
const char* brake_str = msg.brake ? "已刹车" : "未刹车";
|
||||
const char* gear_str = "未知挡位";
|
||||
switch(msg.gear)
|
||||
{
|
||||
case 0: gear_str = "空挡"; break;
|
||||
case 1: gear_str = "后退挡"; break;
|
||||
case 2: gear_str = "前进挡"; break;
|
||||
}
|
||||
const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫";
|
||||
|
||||
LOG_INFO(
|
||||
"=====================================\n"
|
||||
" 刹车: %s\n"
|
||||
@ -118,55 +145,59 @@ class CanRadioCtrlNode : public rclcpp::Node
|
||||
" 轮端转向角度: %.1f °\n"
|
||||
" 清扫状态: %s\n"
|
||||
"=====================================",
|
||||
msg.brake ? "已刹车" : "未刹车",
|
||||
[&]()
|
||||
{
|
||||
switch (msg.gear)
|
||||
{
|
||||
case 0:
|
||||
return "空挡";
|
||||
case 1:
|
||||
return "后退挡";
|
||||
case 2:
|
||||
return "前进挡";
|
||||
default:
|
||||
return "未知挡位";
|
||||
}
|
||||
}(),
|
||||
static_cast<int>(msg.rpm), msg.angle, msg.sweep ? "正在清扫" : "未清扫");
|
||||
brake_str, gear_str, static_cast<int>(msg.rpm), msg.angle, sweep_str
|
||||
);
|
||||
print_counter = 0; // 重置计数器
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 未接收到数据或数据不安全
|
||||
int interval = PRINT_INTERVAL / 2;
|
||||
if (++print_counter >= interval)
|
||||
{
|
||||
if (!has_received)
|
||||
{
|
||||
if (++print_counter >= PRINT_INTERVAL / 2)
|
||||
{
|
||||
LOG_INFO("Waiting for CAN radio control data...");
|
||||
print_counter = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (++print_counter >= PRINT_INTERVAL / 2)
|
||||
{
|
||||
LOG_WARN("CAN radio control data lost or failsafe!");
|
||||
print_counter = 0;
|
||||
}
|
||||
print_counter = 0;
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// ROS2 发布/定时器
|
||||
// CAN反馈回调函数（用于获取当前转向角度）
|
||||
void can_callback(const sweeperMsg::CanFrame::SharedPtr msg)
|
||||
{
|
||||
// 增加空指针检查，避免崩溃
|
||||
if (!msg) return;
|
||||
|
||||
// 判断是否为转向反馈帧（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并除以7，得到角度（单位：度）
|
||||
current_feedback_angle = (raw_value - 1024) / 7.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// ROS2 发布/订阅/定时器
|
||||
rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr pub_;
|
||||
rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr can_sub_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
|
||||
float current_feedback_angle; // 当前反馈角度（从CAN中读取）
|
||||
// 成员变量
|
||||
float current_feedback_angle;
|
||||
RmoCtlConfig config;
|
||||
int print_counter; // 打印计数器
|
||||
int print_counter;
|
||||
int mcu_rpm_max_; // MCU最大转速
|
||||
float eps_angle_max_;// EPS最大角度
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
@ -177,7 +208,7 @@ int main(int argc, char* argv[])
|
||||
// 初始化CAN设备
|
||||
LOG_INFO("Initializing CAN device...");
|
||||
int ifInit = can_Init();
|
||||
while (ifInit != 1)
|
||||
while (ifInit != 1 && rclcpp::ok())
|
||||
{
|
||||
LOG_ERROR("CAN device initialization failed, retrying...");
|
||||
ifInit = can_Init();
|
||||
@ -186,9 +217,13 @@ int main(int argc, char* argv[])
|
||||
LOG_INFO("CAN device initialized successfully!");
|
||||
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<CanRadioCtrlNode>());
|
||||
auto node = std::make_shared<CanRadioCtrlNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
|
||||
// 停止CAN接收线程
|
||||
can_Stop_Recv();
|
||||
|
||||
// 关闭日志系统
|
||||
logger::Logger::Shutdown();
|
||||
return 0;
|
||||
|
||||
@ -2,12 +2,24 @@
|
||||
|
||||
#include <cstring>
|
||||
#include <stdexcept>
|
||||
#include <thread> // 异步接收线程
|
||||
#include <atomic> // 线程退出标志
|
||||
#include <chrono> // 睡眠所需头文件
|
||||
#include <iostream> // 日志输出
|
||||
|
||||
// 全局遥控器数据
|
||||
// 全局变量定义
|
||||
RadioData radio_data = {0, 0, 0, 0, false};
|
||||
std::mutex radio_data_mutex; // 保护radio_data的互斥锁
|
||||
|
||||
// CAN 文件描述符
|
||||
int can_fd = -1;
|
||||
std::atomic<bool> can_recv_running = true; // CAN接收线程运行标志
|
||||
std::thread can_recv_thread; // CAN接收线程
|
||||
|
||||
// 简易LOG_INFO宏
|
||||
#ifndef LOG_INFO
|
||||
#define LOG_INFO(fmt, ...) printf("[CAN] " fmt "\n", ##__VA_ARGS__)
|
||||
#endif
|
||||
|
||||
int can_Init(void)
|
||||
{
|
||||
@ -37,10 +49,27 @@ int can_Init(void)
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
// 设置接收超时
|
||||
// ===================== CAN过滤配置 =====================
|
||||
struct can_filter rfilter[2];
|
||||
rfilter[0].can_id = 0x10B;
|
||||
rfilter[0].can_mask = CAN_SFF_MASK;
|
||||
rfilter[1].can_id = 0x10A;
|
||||
rfilter[1].can_mask = CAN_SFF_MASK;
|
||||
|
||||
if (setsockopt(can_fd, SOL_CAN_RAW, CAN_RAW_FILTER, rfilter, sizeof(rfilter)) < 0)
|
||||
{
|
||||
perror("Error setting CAN filter");
|
||||
close(can_fd);
|
||||
can_fd = -1;
|
||||
return FALSE;
|
||||
}
|
||||
LOG_INFO("CAN filter initialized: only 0x10A and 0x10B frames are allowed");
|
||||
// ======================================================
|
||||
|
||||
// 设置接收超时（5ms）
|
||||
struct timeval tv;
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = 10000; // 10ms
|
||||
tv.tv_usec = 5000;
|
||||
if (setsockopt(can_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0)
|
||||
{
|
||||
perror("Error setting socket timeout");
|
||||
@ -53,15 +82,20 @@ int can_Init(void)
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
void can_Recv_CAN1(void)
|
||||
// CAN数据异步接收循环
|
||||
void can_recv_loop()
|
||||
{
|
||||
while (can_recv_running)
|
||||
{
|
||||
if (can_fd < 0)
|
||||
{
|
||||
printf("CAN device not initialized, reinitializing...\n");
|
||||
if (can_Init() != TRUE)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(radio_data_mutex);
|
||||
radio_data.data_valid = false;
|
||||
return;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
@ -70,39 +104,62 @@ void can_Recv_CAN1(void)
|
||||
|
||||
if (nbytes > 0)
|
||||
{
|
||||
// 遥控器数据采集
|
||||
std::lock_guard<std::mutex> lock(radio_data_mutex);
|
||||
// 仅处理过滤后的0x10A/0x10B报文
|
||||
if (frame.can_id == 0x10B && frame.can_dlc >= 8)
|
||||
{
|
||||
// X2（0-15位）右手左右
|
||||
int16_t x2 = (frame.data[1] << 8) | frame.data[0];
|
||||
// X1（32-47位）左手上下
|
||||
int16_t x1 = (frame.data[5] << 8) | frame.data[4];
|
||||
|
||||
// 油门用x1，方向用x2
|
||||
radio_data.throttle = x1; // 油门（左手上下）
|
||||
radio_data.steering = x2; // 方向（右手左右）
|
||||
|
||||
radio_data.throttle = x1;
|
||||
radio_data.steering = x2;
|
||||
radio_data.data_valid = true;
|
||||
}
|
||||
else if (frame.can_id == 0x10A && frame.can_dlc >= 8)
|
||||
{
|
||||
// E（0-7位）
|
||||
radio_data.gear = frame.data[0];
|
||||
// F（8-15位）
|
||||
radio_data.sweep = frame.data[1];
|
||||
|
||||
radio_data.data_valid = true;
|
||||
}
|
||||
}
|
||||
else if (nbytes < 0)
|
||||
{
|
||||
// 读取超时或其他错误
|
||||
if (errno != EAGAIN && errno != EWOULDBLOCK)
|
||||
{
|
||||
perror("Error reading from CAN socket");
|
||||
close(can_fd);
|
||||
can_fd = -1;
|
||||
std::lock_guard<std::mutex> lock(radio_data_mutex);
|
||||
radio_data.data_valid = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void can_Recv_CAN1(void)
|
||||
{
|
||||
if (!can_recv_thread.joinable() && can_recv_running)
|
||||
{
|
||||
can_recv_thread = std::thread(can_recv_loop);
|
||||
}
|
||||
}
|
||||
|
||||
RadioData get_radio_data(void)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(radio_data_mutex);
|
||||
return radio_data;
|
||||
}
|
||||
|
||||
void can_Stop_Recv(void)
|
||||
{
|
||||
can_recv_running = false;
|
||||
if (can_recv_thread.joinable())
|
||||
{
|
||||
can_recv_thread.join();
|
||||
}
|
||||
if (can_fd >= 0)
|
||||
{
|
||||
close(can_fd);
|
||||
can_fd = -1;
|
||||
}
|
||||
LOG_INFO("CAN receive thread stopped");
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user