lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

can_filter

Browse Source
This commit is contained in:
lyq 2026-02-12 17:01:41 +08:00
parent eabf181280
commit d701062b3b
4 changed files with 211 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
radio_ctrl.sh
View File

@ -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 &

14
src/control/can_radio_ctrl/include/can_radio_ctrl/candev.h
View File

@ -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的互斥锁
#endif // !_CANDEV_H
// 函数声明
int can_Init(void);
void can_Recv_CAN1(void);
RadioData get_radio_data(void); //
void can_Stop_Recv(void); //
#endif // !_CANDEV_H

159
src/control/can_radio_ctrl/src/can_radio_ctrl.cpp
View File

@ -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
{
// 未接收到数据或数据不安全
if (!has_received)
int interval = PRINT_INTERVAL / 2;
if (++print_counter >= interval)
{
if (++print_counter >= PRINT_INTERVAL / 2)
if (!has_received)
{
LOG_INFO("Waiting for CAN radio control data...");
print_counter = 0;
}
}
else
{
if (++print_counter >= PRINT_INTERVAL / 2)
else
{
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,10 +217,14 @@ 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;
}
}

145
src/control/can_radio_ctrl/src/candev.cpp
View File

@ -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,56 +82,84 @@ int can_Init(void)
return TRUE;
}
void can_Recv_CAN1(void)
// CAN数据异步接收循环
void can_recv_loop()
{
if (can_fd < 0)
while (can_recv_running)
{
printf("CAN device not initialized, reinitializing...\n");
if (can_Init() != TRUE)
if (can_fd < 0)
{
radio_data.data_valid = false;
return;
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;
std::this_thread::sleep_for(std::chrono::milliseconds(10));
continue;
}
}
}
struct can_frame frame;
int nbytes = read(can_fd, &frame, sizeof(struct can_frame));
struct can_frame frame;
int nbytes = read(can_fd, &frame, sizeof(struct can_frame));
if (nbytes > 0)
{
// 遥控器数据采集
if (frame.can_id == 0x10B && frame.can_dlc >= 8)
if (nbytes > 0)
{
// 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.data_valid = true;
std::lock_guard<std::mutex> lock(radio_data_mutex);
// 仅处理过滤后的0x10A/0x10B报文
if (frame.can_id == 0x10B && frame.can_dlc >= 8)
{
int16_t x2 = (frame.data[1] << 8) | frame.data[0];
int16_t x1 = (frame.data[5] << 8) | frame.data[4];
radio_data.throttle = x1;
radio_data.steering = x2;
radio_data.data_valid = true;
}
else if (frame.can_id == 0x10A && frame.can_dlc >= 8)
{
radio_data.gear = frame.data[0];
radio_data.sweep = frame.data[1];
radio_data.data_valid = true;
}
}
else if (frame.can_id == 0x10A && frame.can_dlc >= 8)
else if (nbytes < 0)
{
// 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;
radio_data.data_valid = false;
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");
}