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

持久化日志并格式化代码

Browse Source
This commit is contained in:
lyq 2026-01-20 14:18:54 +08:00
parent 885277d97a
commit 06adb527c8
55 changed files with 1344 additions and 1443 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/autonomy/fu/CMakeLists.txt
View File

@ -16,13 +16,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
endif() endif()
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang") if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
add_compile_options(-w) # 禁用所有警告 add_compile_options(-w) # 禁用所有警告
endif() endif()
include_directories( include_directories(include/fu ${catkin_INCLUDE_DIRS})
include/fu
${catkin_INCLUDE_DIRS}
)
# find dependencies # find dependencies
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
@ -30,19 +27,23 @@ find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(sensor_msgs REQUIRED) find_package(sensor_msgs REQUIRED)
find_package(mc REQUIRED) find_package(mc REQUIRED)
find_package(logger REQUIRED)
include_directories(include) include_directories(include)
add_executable(fu_node src/fu_node.cpp src/jsoncpp.cpp) add_executable(fu_node src/fu_node.cpp src/jsoncpp.cpp)
ament_target_dependencies(fu_node rclcpp std_msgs sweeper_interfaces sensor_msgs mc) ament_target_dependencies(
install(TARGETS
fu_node fu_node
DESTINATION lib/${PROJECT_NAME} rclcpp
) std_msgs
sweeper_interfaces
sensor_msgs
mc
logger)
install(DIRECTORY launch config install(TARGETS fu_node DESTINATION lib/${PROJECT_NAME})
DESTINATION share/${PROJECT_NAME})
install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
@ -53,6 +54,6 @@ if(BUILD_TESTING)
# uncomment the line when this package is not in a git repo # uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE) #set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies() ament_lint_auto_find_test_dependencies()
endif() endif()
ament_package() ament_package()

710
src/autonomy/fu/src/fu_node.cpp
View File

File diff suppressed because it is too large Load Diff

20
src/autonomy/pl/CMakeLists.txt
View File

@ -21,19 +21,19 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
# find_package(mc REQUIRED) # find_package(mc REQUIRED)
find_package(logger REQUIRED)
include_directories( include_directories(include/pl ${catkin_INCLUDE_DIRS})
include/pl
${catkin_INCLUDE_DIRS}
)
add_executable(pl_node src/pl_node.cpp src/pl.cpp src/jsoncpp.cpp) add_executable(pl_node src/pl_node.cpp src/pl.cpp src/jsoncpp.cpp)
ament_target_dependencies(pl_node rclcpp std_msgs sweeper_interfaces) ament_target_dependencies(
install(TARGETS
pl_node pl_node
DESTINATION lib/${PROJECT_NAME} rclcpp
) std_msgs
sweeper_interfaces
logger)
install(TARGETS pl_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
@ -44,6 +44,6 @@ if(BUILD_TESTING)
# uncomment the line when this package is not in a git repo # uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE) #set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies() ament_lint_auto_find_test_dependencies()
endif() endif()
ament_package() ament_package()

88
src/autonomy/pl/src/pl.cpp
View File

@ -1,21 +1,24 @@
#include "pl.hpp" #include "pl.hpp"
#include <iostream>
#include <unistd.h>
#include <string.h>
#include <math.h> #include <math.h>
#include <string.h>
#include <unistd.h>
#include <iostream>
#include "logger/logger.h"
using namespace std; using namespace std;
#define WRC_MAX_POINTS_IN_PATH 2000 #define WRC_MAX_POINTS_IN_PATH 2000
#define Lat_Origin 32.045062 // 地面平面坐标系原点的纬度 #define Lat_Origin 32.045062 // 地面平面坐标系原点的纬度
#define Lon_Origin 118.845200366 // 地面平面坐标系原点的经度 #define Lon_Origin 118.845200366 // 地面平面坐标系原点的经度
#define Re 6378137 #define Re 6378137
#define Rn 6356755 #define Rn 6356755
#define deg_rad (0.01745329252) // Transfer from angle degree to rad #define deg_rad (0.01745329252) // Transfer from angle degree to rad
#define R_LATI (6378137) #define R_LATI (6378137)
#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度) #define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度)
float nl_radius; float nl_radius;
int nl_within_radius; int nl_within_radius;
@ -39,18 +42,19 @@ TaskStatus task_status = TaskStatus::PENDING;
double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2) double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
{ {
double x, y, length; double x, y, length;
x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式 x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式
y = (R_LATI) * (lati2 - lati1) * deg_rad; y = (R_LATI) * (lati2 - lati1) * deg_rad;
length = sqrt(x * x + y * y); length = sqrt(x * x + y * y);
return length; return length;
} }
// Cur_navAngle指定y轴正方向.Cur_lonti，Cur_lati为原点；x_diff,y_diff是Dest_lonti,Dest_lati在该坐标系的直角坐标:m // Cur_navAngle指定y轴正方向.Cur_lonti，Cur_lati为原点；x_diff,y_diff是Dest_lonti,Dest_lati在该坐标系的直角坐标:m
int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati, double *x_diff, double *y_diff) int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati,
double* x_diff, double* y_diff)
{ {
double Navi_rad, x, y; double Navi_rad, x, y;
float k1, k2, k3, k4; // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta); float k1, k2, k3, k4; // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta);
Navi_rad = Cur_navAngle * deg_rad; // 角度转化为弧度 Navi_rad = Cur_navAngle * deg_rad; // 角度转化为弧度
// 以当前位置为原点，以正北为y正轴，以正东为x正轴； // 以当前位置为原点，以正北为y正轴，以正东为x正轴；
x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT; x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT;
y = (Dest_lati - Cur_lati) * deg_rad * R_LATI; y = (Dest_lati - Cur_lati) * deg_rad * R_LATI;
@ -66,31 +70,30 @@ int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, doubl
} }
// Find_Nearest_Point // Find_Nearest_Point
int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info) int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point* Road_info)
{ {
if (start_index < 0) if (start_index < 0)
{ {
return -1; return -1;
} }
int Nearest_point_index = start_index; // 初始化为起始索引 int Nearest_point_index = start_index; // 初始化为起始索引
double min_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, double min_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
Road_info[start_index].LongitudeInfo, Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo);
Road_info[start_index].LatitudeInfo);
// 从 start_index + 1 开始搜索 // 从 start_index + 1 开始搜索
for (int i = 1; i < GpsPointNum; i++) for (int i = 1; i < GpsPointNum; i++)
{ {
int current_index = (start_index + i) % GpsPointNum; // 环绕访问 int current_index = (start_index + i) % GpsPointNum; // 环绕访问
double current_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, double current_distance =
Road_info[current_index].LongitudeInfo, ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
Road_info[current_index].LatitudeInfo); Road_info[current_index].LongitudeInfo, Road_info[current_index].LatitudeInfo);
if (current_distance < min_distance) if (current_distance < min_distance)
{ {
min_distance = current_distance; min_distance = current_distance;
Nearest_point_index = current_index; // 更新最近点的索引 Nearest_point_index = current_index; // 更新最近点的索引
// 如果距离小于 0.8 m，提前退出 // 如果距离小于 0.8 m，提前退出
if (min_distance < 0.8) if (min_distance < 0.8)
@ -100,7 +103,7 @@ int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_in
} }
} }
return Nearest_point_index; // 返回最近点的索引 return Nearest_point_index; // 返回最近点的索引
} }
/* /*
int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info) int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info)
@ -112,13 +115,15 @@ int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_in
} }
int Nearest_point_index = 0; int Nearest_point_index = 0;
double aft_distance = 0, pre_distance = 0; double aft_distance = 0, pre_distance = 0;
pre_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo); pre_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo);
int start = start_index + 1; int start = start_index + 1;
for (int i = start; i < GpsPointNum; i++) for (int i = start; i < GpsPointNum; i++)
{ {
aft_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo); aft_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo);
// printf("i: %d , aft_distance: %lf \t",i,aft_distance); // printf("i: %d , aft_distance: %lf \t",i,aft_distance);
if (aft_distance < pre_distance) if (aft_distance < pre_distance)
{ {
@ -152,7 +157,7 @@ int Road_Planning_Find_Aim_Point(int start_index, int dest_num)
return (dest_index); return (dest_index);
} }
int star_find_nearest_point(double lon, double lat, WRC_GPS_Point *Road_info) int star_find_nearest_point(double lon, double lat, WRC_GPS_Point* Road_info)
{ {
double dis = 10000; double dis = 10000;
int point = 0; int point = 0;
@ -164,7 +169,7 @@ int star_find_nearest_point(double lon, double lat, WRC_GPS_Point *Road_info)
dis = dis_temp; dis = dis_temp;
point = i; point = i;
} }
printf("i: %d , dis_temp: %lf", i, dis_temp); LOG_DEBUG("i: %d , dis_temp: %lf", i, dis_temp);
} }
return point; return point;
} }
@ -174,8 +179,7 @@ Direction straight_or_turn(double cur_direction, double des_direction, int thres
{ {
double diff = std::fmod(std::fabs(cur_direction - des_direction), 360.0); double diff = std::fmod(std::fabs(cur_direction - des_direction), 360.0);
if (diff > 180) if (diff > 180) diff = 360 - diff;
diff = 360 - diff;
// std::cout << "diff : " << diff << std::endl; // std::cout << "diff : " << diff << std::endl;
@ -211,8 +215,8 @@ void PL_ProcThread()
// double head_raw = g_NavInfo.Yaw_rad; // double head_raw = g_NavInfo.Yaw_rad;
road_pos = Road_Planning_Find_Nearest_Point(des_pos - 3, g_NavInfo, GPSPointQueue); road_pos = Road_Planning_Find_Nearest_Point(des_pos - 3, g_NavInfo, GPSPointQueue);
des_pos = Road_Planning_Find_Aim_Point(road_pos, 3); // 找到当前点后第 3 个点，作为目标点 des_pos = Road_Planning_Find_Aim_Point(road_pos, 3); // 找到当前点后第 3 个点，作为目标点
direction_pos = Road_Planning_Find_Aim_Point(road_pos, 6); // 找到当前点后第 6 个点，判断直行/转弯 direction_pos = Road_Planning_Find_Aim_Point(road_pos, 6); // 找到当前点后第 6 个点，判断直行/转弯
// printf("当前位置: %d\n", road_pos); // printf("当前位置: %d\n", road_pos);
// printf("目标位置: %d\n", des_pos); // printf("目标位置: %d\n", des_pos);
@ -238,16 +242,15 @@ void PL_ProcThread()
// cout << " * * * * * * * * * * * * * * * * * * * * *" << endl; // cout << " * * * * * * * * * * * * * * * * * * * * *" << endl;
// cout << endl; // cout << endl;
// cout << "--- -target_head " << target_head << "--- current_head " << current_head << " " << abs(target_head - current_head) << endl; // cout << "--- -target_head " << target_head << "--- current_head " << current_head << " " <<
// cout << endl; // abs(target_head - current_head) << endl; cout << endl; cout << "--- -distance " << distance << "- -nl_radius
// cout << "--- -distance " << distance << "- -nl_radius " << nl_radius << endl; // " << nl_radius << endl;
// cout << endl; // cout << endl;
// cout << " * * * * * * * * * * * * * * * * * * * * *" << endl; // cout << " * * * * * * * * * * * * * * * * * * * * *" << endl;
if (road_pos >= (GpsPointNum - 1)) if (road_pos >= (GpsPointNum - 1))
{ {
x = 0.0; x = 0.0;
y = 1.0; y = 1.0;
pl_speed = 0; pl_speed = 0;
@ -260,10 +263,8 @@ void PL_ProcThread()
double x_cm_tmp = 0; double x_cm_tmp = 0;
double y_cm_tmp = 0; double y_cm_tmp = 0;
ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat, ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat, GPSPointQueue[des_pos].LongitudeInfo,
GPSPointQueue[des_pos].LongitudeInfo, GPSPointQueue[des_pos].LatitudeInfo, &x_cm_tmp, &y_cm_tmp);
GPSPointQueue[des_pos].LatitudeInfo,
&x_cm_tmp, &y_cm_tmp);
x = x_cm_tmp; x = x_cm_tmp;
y = y_cm_tmp; y = y_cm_tmp;
@ -277,20 +278,20 @@ void PL_ProcThread()
return; return;
} }
void *pl_thread(void *args) void* pl_thread(void* args)
{ {
(void)args; (void)args;
sleep(3); sleep(3);
memset(GPSPointQueue, 0, sizeof(GPSPointQueue)); memset(GPSPointQueue, 0, sizeof(GPSPointQueue));
FILE *fp_gps = fopen("gps_load_now.txt", "r"); FILE* fp_gps = fopen("gps_load_now.txt", "r");
if (NULL == fp_gps) if (NULL == fp_gps)
{ {
printf("open route file error\n"); LOG_ERROR("open route file error");
return NULL; return NULL;
} }
std::cout << "have opened " << "gps_load_now.txt" << std::endl; LOG_INFO("have opened gps_load_now.txt");
char mystring[200]; char mystring[200];
memset(mystring, '\0', 200); memset(mystring, '\0', 200);
@ -318,11 +319,10 @@ void *pl_thread(void *args)
} }
GpsPointNum = i / 4; GpsPointNum = i / 4;
printf("point_num:%d\n", GpsPointNum); LOG_INFO("point_num:%d", GpsPointNum);
fclose(fp_gps); fclose(fp_gps);
std::cout << std::endl;
printf("#################### auto drive on! ####################\n"); LOG_INFO("#################### auto drive on! ####################");
PL_ProcThread(); PL_ProcThread();
return NULL; return NULL;

98
src/autonomy/pl/src/pl_node.cpp
View File

@ -1,22 +1,24 @@
#include <arpa/inet.h>
#include <pthread.h>
#include <sys/socket.h>
#include <unistd.h>
#include <chrono> // 新增：用于时间戳处理
#include <cstring>
#include <fstream>
#include <iostream>
#include "json.h"
#include "logger/logger.h"
#include "pl.hpp"
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/rtk.hpp"
#include "sweeper_interfaces/msg/pl.hpp" #include "sweeper_interfaces/msg/pl.hpp"
#include "sweeper_interfaces/msg/route.hpp" #include "sweeper_interfaces/msg/route.hpp"
#include "sweeper_interfaces/msg/rtk.hpp"
#include "sweeper_interfaces/msg/task.hpp" #include "sweeper_interfaces/msg/task.hpp"
#include "json.h"
#include <pthread.h>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include "pl.hpp"
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <cstring>
#include <chrono> // 新增：用于时间戳处理
using namespace std; using namespace std;
using namespace std::chrono; // 新增：时间戳命名空间 using namespace std::chrono; // 新增：时间戳命名空间
pthread_t pl_thread_t; pthread_t pl_thread_t;
int is_start = 0; int is_start = 0;
@ -29,34 +31,39 @@ const duration<double> rtk_timeout = duration<double>(0.5);
class pl_node : public rclcpp::Node class pl_node : public rclcpp::Node
{ {
public: public:
pl_node(std::string name) : Node(name) pl_node(std::string name) : Node(name)
{ {
RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); LOG_INFO("%s节点已经启动.", name.c_str());
// 初始化最后接收时间为当前时间（避免初始状态直接超时） // 初始化最后接收时间为当前时间（避免初始状态直接超时）
last_rtk_time = system_clock::now(); last_rtk_time = system_clock::now();
// 创建订阅者订阅话题 // 创建订阅者订阅话题
msg_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>("rtk_message", 10, std::bind(&pl_node::msg_callback, this, std::placeholders::_1)); msg_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>(
task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>("task_message/download", 10, std::bind(&pl_node::task_callback, this, std::placeholders::_1)); "rtk_message", 10, std::bind(&pl_node::msg_callback, this, std::placeholders::_1));
task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>(
"task_message/download", 10, std::bind(&pl_node::task_callback, this, std::placeholders::_1));
// 创建发布者 // 创建发布者
msg_publisher_ = this->create_publisher<sweeper_interfaces::msg::Pl>("pl_message", 10); msg_publisher_ = this->create_publisher<sweeper_interfaces::msg::Pl>("pl_message", 10);
task_publisher_ = this->create_publisher<sweeper_interfaces::msg::Task>("task_message/upload", 10); task_publisher_ = this->create_publisher<sweeper_interfaces::msg::Task>("task_message/upload", 10);
// 创建定时器，定时发布 // 创建定时器，定时发布
timer_pl = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&pl_node::timer_callback_pl, this)); timer_pl =
timer_task = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&pl_node::timer_callback_task, this)); this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&pl_node::timer_callback_pl, this));
timer_task =
this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&pl_node::timer_callback_task, this));
} }
private: private:
// 收到话题数据的回调函数 // 收到话题数据的回调函数
void msg_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg) void msg_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg)
{ {
// 新增：更新最后接收时间戳 // 新增：更新最后接收时间戳
last_rtk_time = system_clock::now(); last_rtk_time = system_clock::now();
// RCLCPP_INFO(this->get_logger(), "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", // LOG_INFO(
// "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'",
// msg->lat, msg->lon, msg->head, msg->speed, msg->p_quality, msg->h_quality); // msg->lat, msg->lon, msg->head, msg->speed, msg->p_quality, msg->h_quality);
g_rtk.reliability = 1; g_rtk.reliability = 1;
@ -75,19 +82,19 @@ private:
if (is_start == 0 && msg->task_status == 1) if (is_start == 0 && msg->task_status == 1)
{ {
pthread_create(&pl_thread_t, NULL, pl_thread, NULL); pthread_create(&pl_thread_t, NULL, pl_thread, NULL);
cout << " started" << endl; LOG_INFO("started");
is_start = 1; is_start = 1;
task_status = TaskStatus::PENDING; task_status = TaskStatus::PENDING;
} }
else if (is_start == 1 && msg->task_status == 0) else if (is_start == 1 && msg->task_status == 0)
{ {
pthread_cancel(pl_thread_t); pthread_cancel(pl_thread_t);
cout << "pl_thread_t is canceled " << endl; LOG_INFO("pl_thread_t is canceled");
is_start = 0; is_start = 0;
task_status = TaskStatus::COMPLETED; task_status = TaskStatus::COMPLETED;
} }
// RCLCPP_INFO(this->get_logger(), " ==================================== is_start : %d", is_start); // LOG_INFO(" ==================================== is_start : %d", is_start);
// RCLCPP_INFO(this->get_logger(), " ==================================== task_status : %d", msg->task_status); // LOG_INFO(" ==================================== task_status : %d", msg->task_status);
} }
void timer_callback_pl() void timer_callback_pl()
@ -98,12 +105,12 @@ private:
if (is_start == 1) if (is_start == 1)
{ {
if (sock < 0) // 首次连接 if (sock < 0) // 首次连接
{ {
struct sockaddr_in serv_addr; struct sockaddr_in serv_addr;
if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0)
{ {
RCLCPP_ERROR(this->get_logger(), "Socket creation error"); LOG_ERROR("Socket creation error");
return; return;
} }
@ -112,31 +119,31 @@ private:
if (inet_pton(AF_INET, "192.168.5.1", &serv_addr.sin_addr) <= 0) if (inet_pton(AF_INET, "192.168.5.1", &serv_addr.sin_addr) <= 0)
{ {
RCLCPP_ERROR(this->get_logger(), "Invalid address/ Address not supported"); LOG_ERROR("Invalid address/ Address not supported");
close(sock); close(sock);
return; return;
} }
if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) if (connect(sock, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0)
{ {
RCLCPP_ERROR(this->get_logger(), "Connection Failed"); LOG_ERROR("Connection Failed");
close(sock); close(sock);
sock = -1; sock = -1;
return; return;
} }
} }
const char *message = "Status: RUNNING"; const char* message = "Status: RUNNING";
if (send(sock, message, strlen(message), 0) < 0) // 发送失败则重建连接 if (send(sock, message, strlen(message), 0) < 0) // 发送失败则重建连接
{ {
close(sock); close(sock);
sock = -1; sock = -1;
return timer_callback_pl(); // 重试 return timer_callback_pl(); // 重试
} }
} }
else if (sock >= 0) else if (sock >= 0)
{ {
close(sock); // 停止时关闭连接 close(sock); // 停止时关闭连接
sock = -1; sock = -1;
} }
@ -151,11 +158,11 @@ private:
message.speed = 0; message.speed = 0;
if (time_since_last > rtk_timeout) if (time_since_last > rtk_timeout)
{ {
RCLCPP_ERROR(this->get_logger(), "RTK数据超时(%.2fs)，停车！！！", time_since_last.count()); LOG_ERROR("RTK数据超时(%.2fs)，停车！！！", time_since_last.count());
} }
else else
{ {
RCLCPP_ERROR(this->get_logger(), "RTK信号差，停车！！！"); LOG_ERROR("RTK信号差，停车！！！");
} }
} }
else else
@ -163,17 +170,18 @@ private:
message.reliability = g_rtk.reliability; message.reliability = g_rtk.reliability;
// message.drive_mode = drive_mode; // message.drive_mode = drive_mode;
if (drive_mode == Direction::STRAIGHT_D) // 直行 if (drive_mode == Direction::STRAIGHT_D) // 直行
message.drive_mode = 0; message.drive_mode = 0;
else if (drive_mode == Direction::LEFT_TURN || drive_mode == Direction::RIGHT_TURN) // 转弯 else if (drive_mode == Direction::LEFT_TURN || drive_mode == Direction::RIGHT_TURN) // 转弯
message.drive_mode = 1; message.drive_mode = 1;
message.enter_range = nl_within_radius; message.enter_range = nl_within_radius;
message.is_start = is_start; message.is_start = is_start;
// RCLCPP_INFO(this->get_logger(), " ==================================== message.is_start : %d", message.is_start); // LOG_INFO(" ==================================== message.is_start : %d",
// message.is_start);
// 日志打印 // 日志打印
// RCLCPP_INFO(this->get_logger(), "x:'%lf',y:'%lf',speed:'%lf',drive_mode:'%d'", // LOG_INFO("x:'%lf',y:'%lf',speed:'%lf',drive_mode:'%d'",
// message.x, message.y, message.speed, message.drive_mode); // message.x, message.y, message.speed, message.drive_mode);
// 发布消息 // 发布消息
msg_publisher_->publish(message); msg_publisher_->publish(message);
@ -207,24 +215,28 @@ void initConfig()
std::ifstream in("config.json", std::ios::binary); std::ifstream in("config.json", std::ios::binary);
if (!in.is_open()) if (!in.is_open())
{ {
std::cout << "read config file error" << std::endl; LOG_ERROR("read config file error");
return; return;
} }
if (reader.parse(in, root)) if (reader.parse(in, root))
{ {
nl_radius = root["detect_line_tolerance"].as<double>(); nl_radius = root["detect_line_tolerance"].as<double>();
} }
in.close(); // 关闭文件流 in.close(); // 关闭文件流
} }
int main(int argc, char **argv) int main(int argc, char** argv)
{ {
initConfig(); initConfig();
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
// 初始化日志系统
logger::Logger::Init("pl", "./log");
/*创建对应节点的共享指针对象*/ /*创建对应节点的共享指针对象*/
auto node = std::make_shared<pl_node>("pl_node"); auto node = std::make_shared<pl_node>("pl_node");
/* 运行节点，并检测退出信号*/ /* 运行节点，并检测退出信号*/
rclcpp::spin(node); rclcpp::spin(node);
rclcpp::shutdown(); rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
// pthread_join(pl_thread_t, NULL); // pthread_join(pl_thread_t, NULL);
return 0; return 0;

27
src/autonomy/route/CMakeLists.txt
View File

@ -21,23 +21,20 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(CURL REQUIRED) find_package(CURL REQUIRED)
find_package(logger REQUIRED)
include_directories(include/route ${catkin_INCLUDE_DIRS})
include_directories( add_executable(route_node src/route_node.cpp src/md5.cpp src/jsoncpp.cpp)
include/route ament_target_dependencies(
${catkin_INCLUDE_DIRS}
)
add_executable(route_node
src/route_node.cpp
src/md5.cpp
src/jsoncpp.cpp)
ament_target_dependencies(route_node rclcpp std_msgs sweeper_interfaces CURL)
install(TARGETS
route_node route_node
DESTINATION lib/${PROJECT_NAME} rclcpp
) std_msgs
sweeper_interfaces
CURL
logger)
install(TARGETS route_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
@ -48,5 +45,5 @@ if(BUILD_TESTING)
# uncomment the line when this package is not in a git repo # uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE) #set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies() ament_lint_auto_find_test_dependencies()
endif() endif()
ament_package() ament_package()

1
src/autonomy/route/package.xml
View File

@ -12,6 +12,7 @@
<depend>rclcpp</depend> <depend>rclcpp</depend>
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>sweeper_interfaces</depend> <depend>sweeper_interfaces</depend>
<depend>logger</depend>
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_common</test_depend>

120
src/autonomy/route/src/route_node.cpp
View File

@ -1,25 +1,27 @@
#include <curl/curl.h>
#include <pthread.h>
#include <unistd.h>
#include <chrono>
#include <cstdio>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <mutex>
#include <sstream>
#include "json.h"
#include "logger/logger.h"
#include "md5.h"
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/rtk.hpp" #include "sweeper_interfaces/msg/rtk.hpp"
#include "sweeper_interfaces/msg/sub.hpp" #include "sweeper_interfaces/msg/sub.hpp"
#include <curl/curl.h>
#include "md5.h"
#include "json.h"
#include <pthread.h>
#include <iostream>
#include <unistd.h>
#include <cstdio>
#include <chrono>
#include <fstream> std::mutex collect_mutex; // 互斥锁，用于保护 isCollecting 变量
#include <sstream>
#include <iomanip>
#include <mutex>
std::mutex collect_mutex; // 互斥锁，用于保护 isCollecting 变量 #define deg_rad (0.01745329252) // Transfer from angle degree to rad
#define deg_rad (0.01745329252) // Transfer from angle degree to rad
#define R_LATI (6378137) #define R_LATI (6378137)
#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度) #define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度)
using namespace std; using namespace std;
/* 存储接收RTK的GPS点信息 */ /* 存储接收RTK的GPS点信息 */
struct struct_rtk struct struct_rtk
@ -40,83 +42,87 @@ struct_rtk last_g_rtk = {};
string vid; string vid;
string upload_URL; string upload_URL;
std::string destinationFilePath1 = "./gps_load_now.txt"; std::string destinationFilePath1 = "./gps_load_now.txt";
void *route_thread(void *args); void* route_thread(void* args);
string calculate_md5(string filename) string calculate_md5(string filename)
{ {
MD5 md5; MD5 md5;
ifstream file; ifstream file;
file.open(filename, ios::binary); file.open(filename, ios::binary);
md5.update(file); md5.update(file);
cout << md5.toString() << endl; LOG_INFO("%s", md5.toString().c_str());
return md5.toString(); return md5.toString();
} }
bool upload_file(string filename) bool upload_file(string filename)
{ {
CURL *curl; CURL* curl;
CURLcode ret; CURLcode ret;
curl = curl_easy_init(); curl = curl_easy_init();
struct curl_httppost *post = NULL; struct curl_httppost* post = NULL;
struct curl_httppost *last = NULL; struct curl_httppost* last = NULL;
curl_formadd(&post, &last, CURLFORM_PTRNAME, "vid", CURLFORM_PTRCONTENTS, vid.c_str(), CURLFORM_END); // form-data key(path) 和 value(device_cover) curl_formadd(&post, &last, CURLFORM_PTRNAME, "vid", CURLFORM_PTRCONTENTS, vid.c_str(),
CURLFORM_END); // form-data key(path) 和 value(device_cover)
curl_formadd(&post, &last, CURLFORM_COPYNAME, "file", CURLFORM_FILE, filename.c_str(), CURLFORM_END); curl_formadd(&post, &last, CURLFORM_COPYNAME, "file", CURLFORM_FILE, filename.c_str(), CURLFORM_END);
curl_formadd(&post, &last, CURLFORM_COPYNAME, "md5", CURLFORM_COPYCONTENTS, calculate_md5(filename).c_str(), CURLFORM_END); curl_formadd(&post, &last, CURLFORM_COPYNAME, "md5", CURLFORM_COPYCONTENTS, calculate_md5(filename).c_str(),
CURLFORM_END);
curl_easy_setopt(curl, CURLOPT_URL, upload_URL.c_str()); curl_easy_setopt(curl, CURLOPT_URL, upload_URL.c_str());
curl_easy_setopt(curl, CURLOPT_HTTPPOST, post); curl_easy_setopt(curl, CURLOPT_HTTPPOST, post);
ret = curl_easy_perform(curl); ret = curl_easy_perform(curl);
if (ret != CURLE_OK) if (ret != CURLE_OK)
{ {
fprintf(stderr, "curl_easy_perform() failed: %s\n", curl_easy_strerror(ret)); LOG_ERROR("curl_easy_perform() failed: %s", curl_easy_strerror(ret));
return false; return false;
} }
curl_formfree(post); curl_formfree(post);
curl_easy_cleanup(curl); curl_easy_cleanup(curl);
cout << "上传成功" << endl; LOG_INFO("上传成功");
return true; return true;
} }
void copyFileAndOverwrite(const std::string &sourceFilePath, const std::string &destinationFilePath) void copyFileAndOverwrite(const std::string& sourceFilePath, const std::string& destinationFilePath)
{ {
std::ifstream src(sourceFilePath, std::ios::binary); std::ifstream src(sourceFilePath, std::ios::binary);
std::ofstream dst(destinationFilePath, std::ios::binary); std::ofstream dst(destinationFilePath, std::ios::binary);
if (!src) if (!src)
{ {
std::cerr << "无法打开源文件: " << sourceFilePath << std::endl; LOG_ERROR("无法打开源文件: %s", sourceFilePath.c_str());
return; return;
} }
if (!dst) if (!dst)
{ {
std::cerr << "无法打开目标文件: " << destinationFilePath << std::endl; LOG_ERROR("无法打开目标文件: %s", destinationFilePath.c_str());
return; return;
} }
dst << src.rdbuf(); // 将源文件内容复制到目标文件 dst << src.rdbuf(); // 将源文件内容复制到目标文件
if (!dst) if (!dst)
{ {
std::cerr << "复制文件时出错" << std::endl; LOG_ERROR("复制文件时出错");
} }
} }
class route_node : public rclcpp::Node class route_node : public rclcpp::Node
{ {
public: public:
route_node(std::string name) : Node(name) route_node(std::string name) : Node(name)
{ {
RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); LOG_INFO("%s节点已经启动.", name.c_str());
// 创建一个订阅者订阅话题 // 创建一个订阅者订阅话题
sub_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Sub>("gather", 10, std::bind(&route_node::sub_callback, this, std::placeholders::_1)); sub_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Sub>(
"gather", 10, std::bind(&route_node::sub_callback, this, std::placeholders::_1));
msg_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>("rtk_message", 10, std::bind(&route_node::msg_callback, this, std::placeholders::_1)); msg_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>(
"rtk_message", 10, std::bind(&route_node::msg_callback, this, std::placeholders::_1));
} }
private: private:
// 收到话题数据的回调函数 // 收到话题数据的回调函数
void msg_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg) void msg_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg)
{ {
if (msg->p_quality < 4 || msg->h_quality < 4) if (msg->p_quality < 4 || msg->h_quality < 4)
{ {
g_rtk.reliability = 0; g_rtk.reliability = 0;
RCLCPP_INFO(this->get_logger(), "rtk信号差!"); LOG_INFO("rtk信号差!");
} }
if (first_flag) if (first_flag)
{ {
@ -133,40 +139,34 @@ private:
if (!isCollecting) if (!isCollecting)
{ {
// cout << "等待采集...." << endl; LOG_INFO("等待采集.....");
RCLCPP_INFO(this->get_logger(), "等待采集.....");
} }
else else
{ {
// cout << "平台采集中" << endl; LOG_INFO("平台采集中.....");
RCLCPP_INFO(this->get_logger(), "平台采集中.....");
} }
} }
void sub_callback(const sweeper_interfaces::msg::Sub::SharedPtr msg) void sub_callback(const sweeper_interfaces::msg::Sub::SharedPtr msg)
{ {
if (msg->get_route && !isCollecting) if (msg->get_route && !isCollecting)
{ {
std::lock_guard<std::mutex> lock(collect_mutex); // 加锁 std::lock_guard<std::mutex> lock(collect_mutex); // 加锁
isCollecting = true; isCollecting = true;
// cout << "平台开始采集" << endl; LOG_INFO("平台开始采集");
RCLCPP_INFO(this->get_logger(), "平台开始采集");
pthread_create(&route_thread_t, NULL, route_thread, NULL); pthread_create(&route_thread_t, NULL, route_thread, NULL);
} }
else if (!msg->get_route && isCollecting) else if (!msg->get_route && isCollecting)
{ {
isCollecting = false; isCollecting = false;
// cout << "平台结束采集" << endl; LOG_INFO("平台结束采集");
RCLCPP_INFO(this->get_logger(), "平台结束采集");
pthread_cancel(route_thread_t); pthread_cancel(route_thread_t);
if (upload_file(filename)) if (upload_file(filename))
{ {
// cout << "上传成功" << endl; LOG_INFO("上传成功");
RCLCPP_INFO(this->get_logger(), "上传成功");
} }
else else
{ {
// cout << "上传失败" << endl; LOG_INFO("上传失败");
RCLCPP_INFO(this->get_logger(), "上传失败");
} }
} }
} }
@ -178,13 +178,13 @@ private:
double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2) double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
{ {
double x, y, length; double x, y, length;
x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式 x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式
y = (R_LATI) * (lati2 - lati1) * deg_rad; y = (R_LATI) * (lati2 - lati1) * deg_rad;
length = sqrt(x * x + y * y); length = sqrt(x * x + y * y);
return length; return length;
} }
void *route_thread(void *args) void* route_thread(void* args)
{ {
(void)args; (void)args;
filename = ""; filename = "";
@ -199,12 +199,12 @@ void *route_thread(void *args)
oss << "routes/gps_load_" << timestamp << ".txt"; oss << "routes/gps_load_" << timestamp << ".txt";
filename = oss.str(); filename = oss.str();
FILE *fp_routes = fopen(filename.c_str(), "w"); FILE* fp_routes = fopen(filename.c_str(), "w");
usleep(5000); usleep(5000);
while (1) while (1)
{ {
double distance = ntzx_GPS_length(last_g_rtk.lon, last_g_rtk.lat, g_rtk.lon, g_rtk.lat); double distance = ntzx_GPS_length(last_g_rtk.lon, last_g_rtk.lat, g_rtk.lon, g_rtk.lat);
if (distance >= 1.0) // 每隔1米采样一次 if (distance >= 1.0) // 每隔1米采样一次
{ {
fprintf(fp_routes, "%.10lf\n%.11lf\n%lf\n%lf\n", g_rtk.lat, g_rtk.lon, g_rtk.head, 0.0); fprintf(fp_routes, "%.10lf\n%.11lf\n%lf\n%lf\n", g_rtk.lat, g_rtk.lon, g_rtk.head, 0.0);
fflush(fp_routes); fflush(fp_routes);
@ -212,7 +212,7 @@ void *route_thread(void *args)
last_g_rtk.lat = g_rtk.lat; last_g_rtk.lat = g_rtk.lat;
last_g_rtk.lon = g_rtk.lon; last_g_rtk.lon = g_rtk.lon;
last_g_rtk.head = g_rtk.head; last_g_rtk.head = g_rtk.head;
printf("hit!\n"); LOG_DEBUG("hit!");
} }
} }
} }
@ -224,27 +224,31 @@ void init_main()
std::ifstream in("config.json", std::ios::binary); std::ifstream in("config.json", std::ios::binary);
if (!in.is_open()) if (!in.is_open())
{ {
std::cout << "read config file error" << std::endl; LOG_ERROR("read config file error");
return; return;
} }
if (reader.parse(in, root)) if (reader.parse(in, root))
{ {
vid = root["mqtt"]["vid"].asString(); vid = root["mqtt"]["vid"].asString();
upload_URL = root["mqtt"]["upload_url"].asString(); upload_URL = root["mqtt"]["upload_url"].asString();
cout << "vid:" << vid << endl; LOG_INFO("vid:%s", vid.c_str());
} }
in.close(); // 关闭文件流 in.close(); // 关闭文件流
} }
int main(int argc, char **argv) int main(int argc, char** argv)
{ {
init_main(); init_main();
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
/*初始化日志系统*/
logger::Logger::Init("route", "./log");
/*创建对应节点的共享指针对象*/ /*创建对应节点的共享指针对象*/
auto node = std::make_shared<route_node>("route_node"); auto node = std::make_shared<route_node>("route_node");
/* 运行节点，并检测退出信号*/ /* 运行节点，并检测退出信号*/
rclcpp::spin(node); rclcpp::spin(node);
rclcpp::shutdown(); rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
// pthread_join(route_thread_t, NULL); // pthread_join(route_thread_t, NULL);
return 0; return 0;
} }

17
src/base/mc/CMakeLists.txt
View File

@ -11,6 +11,7 @@ find_package(ament_index_cpp REQUIRED)
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(logger REQUIRED)
# 搜索 src 目录下所有 .cpp 文件 # 搜索 src 目录下所有 .cpp 文件
file(GLOB SRC_FILES src/*.cpp) file(GLOB SRC_FILES src/*.cpp)
@ -18,15 +19,19 @@ file(GLOB SRC_FILES src/*.cpp)
# 创建可执行文件 # 创建可执行文件
add_executable(mc_node ${SRC_FILES}) add_executable(mc_node ${SRC_FILES})
ament_target_dependencies(mc_node ament_index_cpp rclcpp std_msgs sweeper_interfaces) ament_target_dependencies(
mc_node
ament_index_cpp
rclcpp
std_msgs
sweeper_interfaces
logger)
# Set include directories for the target # Set include directories for the target
target_include_directories( target_include_directories(mc_node PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
mc_node $<INSTALL_INTERFACE:include>)
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17) target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17)
# Install target # Install target
install(TARGETS mc_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS mc_node DESTINATION lib/${PROJECT_NAME})

6
src/base/mc/include/mc/can_struct.h
View File

@ -175,12 +175,12 @@ struct can_MCU_cmd
frame.ext = EXT_FLAG; frame.ext = EXT_FLAG;
frame.rtr = RTR_FLAG; frame.rtr = RTR_FLAG;
// std::cout << "MCU frame : "; // LOG_INFO("MCU frame : ");
// for (int i = 0; i < 8; ++i) // for (int i = 0; i < 8; ++i)
// { // {
// printf("0X%x ", frame.data[i]); // LOG_INFO("0X%x ", frame.data[i]);
// } // }
// printf("\n"); // LOG_INFO("");
return frame; return frame;
} }

1
src/base/mc/package.xml
View File

@ -11,6 +11,7 @@
<depend>sweeper_interfaces</depend> <depend>sweeper_interfaces</depend>
<depend>rclcpp</depend> <depend>rclcpp</depend>
<depend>ament_index_cpp</depend> <depend>ament_index_cpp</depend>
<depend>logger</depend>
<exec_depend>rosidl_default_runtime</exec_depend> <exec_depend>rosidl_default_runtime</exec_depend>
<export> <export>

66
src/base/mc/src/can_driver.cpp
View File

@ -1,27 +1,25 @@
#include "mc/can_driver.h" #include "mc/can_driver.h"
#include <cstring>
#include <stdexcept>
#include <iostream>
#include <thread>
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h>
#include <poll.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h> #include <linux/can.h>
#include <linux/can/raw.h> #include <linux/can/raw.h>
#include <net/if.h>
#include <poll.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <cstring>
#include <iostream>
#include <stdexcept>
#include <thread>
CANDriver::CANDriver() = default; CANDriver::CANDriver() = default;
CANDriver::~CANDriver() CANDriver::~CANDriver() { close(); }
{
close();
}
bool CANDriver::open(const std::string &interface) bool CANDriver::open(const std::string& interface)
{ {
if (running) if (running) return false;
return false;
sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW); sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sockfd < 0) if (sockfd < 0)
@ -45,7 +43,7 @@ bool CANDriver::open(const std::string &interface)
addr.can_family = AF_CAN; addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex; addr.can_ifindex = ifr.ifr_ifindex;
if (bind(sockfd, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr)) < 0) if (bind(sockfd, reinterpret_cast<struct sockaddr*>(&addr), sizeof(addr)) < 0)
{ {
perror("bind"); perror("bind");
::close(sockfd); ::close(sockfd);
@ -63,12 +61,10 @@ bool CANDriver::open(const std::string &interface)
void CANDriver::close() void CANDriver::close()
{ {
if (!running) if (!running) return;
return;
running = false; running = false;
if (receiveThread.joinable()) if (receiveThread.joinable()) receiveThread.join();
receiveThread.join();
if (sockfd >= 0) if (sockfd >= 0)
{ {
@ -77,17 +73,14 @@ void CANDriver::close()
} }
} }
bool CANDriver::sendFrame(const CANFrame &frame) bool CANDriver::sendFrame(const CANFrame& frame)
{ {
if (!running) if (!running) return false;
return false;
struct can_frame raw_frame{}; struct can_frame raw_frame{};
raw_frame.can_id = frame.id; raw_frame.can_id = frame.id;
if (frame.ext) if (frame.ext) raw_frame.can_id |= CAN_EFF_FLAG;
raw_frame.can_id |= CAN_EFF_FLAG; if (frame.rtr) raw_frame.can_id |= CAN_RTR_FLAG;
if (frame.rtr)
raw_frame.can_id |= CAN_RTR_FLAG;
raw_frame.can_dlc = frame.dlc; raw_frame.can_dlc = frame.dlc;
std::memcpy(raw_frame.data, frame.data, frame.dlc); std::memcpy(raw_frame.data, frame.data, frame.dlc);
@ -100,28 +93,27 @@ bool CANDriver::sendFrame(const CANFrame &frame)
return true; return true;
} }
void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData) void CANDriver::setReceiveCallback(ReceiveCallback callback, void* userData)
{ {
this->callback = callback; this->callback = callback;
this->userData = userData; this->userData = userData;
} }
bool CANDriver::setFilter(const std::vector<can_filter> &filters) bool CANDriver::setFilter(const std::vector<can_filter>& filters)
{ {
if (!running) if (!running) return false;
return false;
filters_ = filters; filters_ = filters;
return applyFilters(); return applyFilters();
} }
bool CANDriver::addFilter(const can_filter &filter) bool CANDriver::addFilter(const can_filter& filter)
{ {
filters_.push_back(filter); filters_.push_back(filter);
return applyFilters(); return applyFilters();
} }
bool CANDriver::addFilters(const std::vector<can_filter> &filters) bool CANDriver::addFilters(const std::vector<can_filter>& filters)
{ {
filters_.insert(filters_.end(), filters.begin(), filters.end()); filters_.insert(filters_.end(), filters.begin(), filters.end());
return applyFilters(); return applyFilters();
@ -129,8 +121,7 @@ bool CANDriver::addFilters(const std::vector<can_filter> &filters)
bool CANDriver::applyFilters() bool CANDriver::applyFilters()
{ {
if (!running) if (!running) return false;
return false;
if (filters_.empty()) if (filters_.empty())
{ {
@ -138,8 +129,7 @@ bool CANDriver::applyFilters()
return true; return true;
} }
if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, filters_.data(), filters_.size() * sizeof(can_filter)) < 0)
filters_.data(), filters_.size() * sizeof(can_filter)) < 0)
{ {
perror("setsockopt"); perror("setsockopt");
return false; return false;
@ -157,7 +147,7 @@ void CANDriver::receiveThreadFunc()
while (running) while (running)
{ {
int ret = poll(&fds, 1, 100); // 等待最多100ms int ret = poll(&fds, 1, 100); // 等待最多100ms
if (ret < 0) if (ret < 0)
{ {
perror("poll"); perror("poll");

15
src/base/mc/src/can_utils.cpp
View File

@ -1,12 +1,15 @@
#include "mc/can_utils.hpp" #include "mc/can_utils.hpp"
#include <sstream>
#include <iomanip> #include <iomanip>
#include <sstream>
#include "logger/logger.h"
bool g_can_print_enable = false; bool g_can_print_enable = false;
void receiveHandler(const CANFrame &frame, void *userData) void receiveHandler(const CANFrame& frame, void* userData)
{ {
auto *context = static_cast<CanHandlerContext *>(userData); auto* context = static_cast<CanHandlerContext*>(userData);
auto node = context->node; auto node = context->node;
auto pub = context->publisher; auto pub = context->publisher;
auto now = node->now(); auto now = node->now();
@ -20,10 +23,10 @@ void receiveHandler(const CANFrame &frame, void *userData)
if (g_can_print_enable) if (g_can_print_enable)
{ {
std::stringstream ss; std::stringstream ss;
ss << "CAN ID: " << std::hex << std::uppercase ss << "CAN ID: " << std::hex << std::uppercase << std::setw((frame.id > 0x7FF) ? 8 : 5) << std::setfill(' ')
<< std::setw((frame.id > 0x7FF) ? 8 : 5) << std::setfill(' ') << frame.id << " Data: "; << frame.id << " Data: ";
for (int i = 0; i < frame.dlc; ++i) for (int i = 0; i < frame.dlc; ++i)
ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " "; ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str()); LOG_INFO("%s", ss.str().c_str());
} }
} }

10
src/base/mc/src/control_cache.cpp
View File

@ -2,7 +2,7 @@
ControlCache control_cache; ControlCache control_cache;
void ControlCache::update(const sweeper_interfaces::msg::McCtrl &msg) void ControlCache::update(const sweeper_interfaces::msg::McCtrl& msg)
{ {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
latest_msg_ = msg; latest_msg_ = msg;
@ -10,15 +10,13 @@ void ControlCache::update(const sweeper_interfaces::msg::McCtrl &msg)
has_data_ = true; has_data_ = true;
} }
bool ControlCache::get(sweeper_interfaces::msg::McCtrl &msg) bool ControlCache::get(sweeper_interfaces::msg::McCtrl& msg)
{ {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
if (!has_data_) if (!has_data_) return false;
return false;
auto now = std::chrono::steady_clock::now(); auto now = std::chrono::steady_clock::now();
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time_).count() > 100) if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time_).count() > 100) return false;
return false;
msg = latest_msg_; msg = latest_msg_;
return true; return true;

4
src/base/mc/src/get_config.cpp
View File

@ -1,6 +1,6 @@
#include "mc/get_config.h" #include "mc/get_config.h"
bool load_config(Config &config) bool load_config(Config& config)
{ {
try try
{ {
@ -20,7 +20,7 @@ bool load_config(Config &config)
return true; return true;
} }
catch (const std::exception &e) catch (const std::exception& e)
{ {
std::cerr << "Error parsing MQTT config: " << e.what() << std::endl; std::cerr << "Error parsing MQTT config: " << e.what() << std::endl;
return false; return false;

103
src/base/mc/src/mc.cpp
View File

@ -1,73 +1,78 @@
#include "rclcpp/rclcpp.hpp" #include <iostream>
#include "mc/get_config.h"
#include "logger/logger.h"
#include "mc/can_driver.h" #include "mc/can_driver.h"
#include "mc/can_utils.hpp" #include "mc/can_utils.hpp"
#include "mc/control_cache.hpp" #include "mc/control_cache.hpp"
#include "mc/get_config.h"
#include "mc/timer_tasks.hpp" #include "mc/timer_tasks.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp" #include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp" #include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include <iostream>
namespace sweeperMsg = sweeper_interfaces::msg; namespace sweeperMsg = sweeper_interfaces::msg;
CANDriver canctl; CANDriver canctl;
void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg) void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
{ {
// std::cout << "\n 刹车: " << (msg->brake ? "已刹车" : "未刹车") // LOG_INFO("\n 刹车: %s", (msg->brake ? "已刹车" : "未刹车"));
// << "\n 挡位: "; // LOG_INFO(" 挡位: ");
// switch (msg->gear) // switch (msg->gear)
// { // {
// case 0: // case 0:
// std::cout << "空挡"; // LOG_INFO("空挡");
// break; // break;
// case 2: // case 2:
// std::cout << "前进挡"; // LOG_INFO("前进挡");
// break; // break;
// case 1: // case 1:
// std::cout << "后退挡"; // LOG_INFO("后退挡");
// break; // break;
// default: // default:
// std::cout << "未知挡位(" << static_cast<int>(msg->gear) << ")"; // LOG_INFO("未知挡位(%d)", static_cast<int>(msg->gear));
// break; // break;
// } // }
// std::cout << "\n 行走电机转速: " << static_cast<int>(msg->rpm) << " RPM" // LOG_INFO(" 行走电机转速: %d RPM", static_cast<int>(msg->rpm));
// << "\n 轮端转向角度: " << msg->angle << "°" // LOG_INFO(" 轮端转向角度: %.1f°", msg->angle);
// << "\n 清扫状态: " << (msg->sweep ? "正在清扫" : "未清扫") // LOG_INFO(" 清扫状态: %s", (msg->sweep ? "正在清扫" : "未清扫"));
// << std::endl;
control_cache.update(*msg); control_cache.update(*msg);
} }
int main(int argc, char **argv) int main(int argc, char** argv)
{ {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("can_driver_node"); /*初始化日志系统*/
RCLCPP_INFO(node->get_logger(), "Starting mc package..."); logger::Logger::Init("mc", "./log");
auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10); auto node = rclcpp::Node::make_shared("can_driver_node");
auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback); LOG_INFO("Starting mc package...");
Config mc_config; auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
load_config(mc_config); auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback);
if (!canctl.open(mc_config.can_dev)) Config mc_config;
{ load_config(mc_config);
RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str());
return -1;
}
auto context = std::make_shared<CanHandlerContext>(); if (!canctl.open(mc_config.can_dev))
context->node = node; {
context->publisher = pub; LOG_ERROR("Failed to open CAN interface: %s", mc_config.can_dev.c_str());
canctl.setReceiveCallback(receiveHandler, context.get()); logger::Logger::Shutdown();
return -1;
}
setupTimers(node, canctl); auto context = std::make_shared<CanHandlerContext>();
context->node = node;
context->publisher = pub;
canctl.setReceiveCallback(receiveHandler, context.get());
rclcpp::on_shutdown([&]() setupTimers(node, canctl);
{ canctl.close(); });
rclcpp::spin(node); rclcpp::on_shutdown([&]() { canctl.close(); });
rclcpp::shutdown(); rclcpp::spin(node);
return 0; rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
return 0;
} }

187
src/base/mc/src/timer_tasks.cpp
View File

@ -1,134 +1,127 @@
#include "mc/timer_tasks.hpp" #include "mc/timer_tasks.hpp"
#include "mc/control_cache.hpp"
#include "logger/logger.h"
#include "mc/can_struct.h" #include "mc/can_struct.h"
#include "mc/control_cache.hpp"
// 定时器回调：MCU 控制 // 定时器回调：MCU 控制
void mcuTimerTask(CANDriver &canctl) void mcuTimerTask(CANDriver& canctl)
{ {
auto msg = get_safe_control(); auto msg = get_safe_control();
mcu_cmd.setEnabled(true); mcu_cmd.setEnabled(true);
mcu_cmd.setGear(msg.gear); mcu_cmd.setGear(msg.gear);
mcu_cmd.setRPM(msg.rpm); mcu_cmd.setRPM(msg.rpm);
mcu_cmd.setBrake(msg.brake); mcu_cmd.setBrake(msg.brake);
canctl.sendFrame(mcu_cmd.toFrame()); canctl.sendFrame(mcu_cmd.toFrame());
// std::cout << "mcuTimerTask" << std::endl; // LOG_INFO("mcuTimerTask");
// std::cout << "msg.gear " << msg.gear << std::endl; // LOG_INFO("msg.gear %d", msg.gear);
// std::cout << "msg.brake " << msg.brake << std::endl; // LOG_INFO("msg.brake %d", msg.brake);
// std::cout << "msg.rpm " << msg.rpm << std::endl; // LOG_INFO("msg.rpm %d", msg.rpm);
} }
// 定时器回调：EPS 控制 // 定时器回调：EPS 控制
void epsTimerTask(CANDriver &canctl) void epsTimerTask(CANDriver& canctl)
{ {
auto msg = get_safe_control(); auto msg = get_safe_control();
eps_cmd.setCenterCmd(0); eps_cmd.setCenterCmd(0);
eps_cmd.setAngle(msg.angle); eps_cmd.setAngle(msg.angle);
eps_cmd.setAngularSpeed(msg.angle_speed); eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack(); eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame()); canctl.sendFrame(eps_cmd.toFrame());
// std::cout << "epsTimerTask" << std::endl; // LOG_INFO("epsTimerTask");
} }
// 定时器回调：VCU 扫地控制 // 定时器回调：VCU 扫地控制
// 修改timer_tasks.cpp中的vcuTimerTask函数 // 修改timer_tasks.cpp中的vcuTimerTask函数
void vcuTimerTask(CANDriver &canctl) void vcuTimerTask(CANDriver& canctl)
{ {
static bool vcu_initialized = false; static bool vcu_initialized = false;
static bool last_sweep_state = false; // 记录上一次清扫状态 static bool last_sweep_state = false; // 记录上一次清扫状态
auto msg = get_safe_control(); auto msg = get_safe_control();
// 首次运行时初始化VCU控制 // 首次运行时初始化VCU控制
if (!vcu_initialized) if (!vcu_initialized)
{ {
// 发送CAN使能指令 // 发送CAN使能指令
vcu_enable_cmd.setCANEnable(true); vcu_enable_cmd.setCANEnable(true);
canctl.sendFrame(vcu_enable_cmd.toFrame()); canctl.sendFrame(vcu_enable_cmd.toFrame());
// std::cout << "mcuTimerTask" << std::endl; // LOG_INFO("mcuTimerTask");
vcu_initialized = true; vcu_initialized = true;
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled"); LOG_INFO("[VCU] CAN control enabled");
} }
// 检查清扫状态是否变化 // 检查清扫状态是否变化
bool sweep_state_changed = (msg.sweep != last_sweep_state); bool sweep_state_changed = (msg.sweep != last_sweep_state);
if (sweep_state_changed) if (sweep_state_changed)
{ {
// 根据清扫状态设置所有部件 // 根据清扫状态设置所有部件
int8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止 int8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止
int8_t target_value2 = msg.sweep ? -100 : 100; // -100表示下沉，100表示抬升 int8_t target_value2 = msg.sweep ? -100 : 100; // -100表示下沉，100表示抬升
// ===== 控制0x211报文 (电机M1-M7) ===== // ===== 控制0x211报文 (电机M1-M7) =====
vcu_motor1_cmd.setByte0(msg.sweep ? 1 : 0); // 1表示正向运行，0表示停止 vcu_motor1_cmd.setByte0(msg.sweep ? 1 : 0); // 1表示正向运行，0表示停止
vcu_motor1_cmd.setByte1(target_value); vcu_motor1_cmd.setByte1(target_value);
vcu_motor1_cmd.setByte2(target_value); vcu_motor1_cmd.setByte2(target_value);
vcu_motor1_cmd.setByte3(target_value); vcu_motor1_cmd.setByte3(target_value);
vcu_motor1_cmd.setByte4(target_value2); vcu_motor1_cmd.setByte4(target_value2);
vcu_motor1_cmd.setByte5(target_value2); vcu_motor1_cmd.setByte5(target_value2);
vcu_motor1_cmd.setByte6(target_value); vcu_motor1_cmd.setByte6(target_value);
vcu_motor1_cmd.setByte7(target_value); vcu_motor1_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor1_cmd.toFrame()); canctl.sendFrame(vcu_motor1_cmd.toFrame());
// std::cout << "vcuTimerTask1" << std::endl; // LOG_INFO("vcuTimerTask1");
// ===== 控制0x212报文 (电机M8和LED输出) ===== // ===== 控制0x212报文 (电机M8和LED输出) =====
vcu_motor2_cmd.setByte0(target_value); vcu_motor2_cmd.setByte0(target_value);
vcu_motor2_cmd.setByte1(target_value); vcu_motor2_cmd.setByte1(target_value);
vcu_motor2_cmd.setByte2(target_value); vcu_motor2_cmd.setByte2(target_value);
vcu_motor2_cmd.setByte3(target_value); vcu_motor2_cmd.setByte3(target_value);
vcu_motor2_cmd.setByte4(target_value); vcu_motor2_cmd.setByte4(target_value);
vcu_motor2_cmd.setByte5(target_value); vcu_motor2_cmd.setByte5(target_value);
vcu_motor2_cmd.setByte6(target_value); vcu_motor2_cmd.setByte6(target_value);
vcu_motor2_cmd.setByte7(target_value); vcu_motor2_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor2_cmd.toFrame()); canctl.sendFrame(vcu_motor2_cmd.toFrame());
// std::cout << "vcuTimerTask2" << std::endl; // LOG_INFO("vcuTimerTask2");
// RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), // LOG_INFO("[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
// "[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
// 更新状态记录 // 更新状态记录
last_sweep_state = msg.sweep; last_sweep_state = msg.sweep;
} }
} }
// 定时器回调：BMS 查询任务 // 定时器回调：BMS 查询任务
void bmsTimerTask(CANDriver &canctl) void bmsTimerTask(CANDriver& canctl)
{ {
static bool bms_initialized = false; static bool bms_initialized = false;
// 首次运行时初始化日志 // 首次运行时初始化日志
if (!bms_initialized) if (!bms_initialized)
{ {
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[BMS] Query task started"); LOG_INFO("[BMS] Query task started");
bms_initialized = true; bms_initialized = true;
} }
// 周期性发送 // 周期性发送
canctl.sendFrame(bms_query_0x100.toFrame()); canctl.sendFrame(bms_query_0x100.toFrame());
canctl.sendFrame(bms_query_0x101.toFrame()); canctl.sendFrame(bms_query_0x101.toFrame());
} }
// 注册所有定时器任务 // 注册所有定时器任务
void setupTimers(rclcpp::Node::SharedPtr node, CANDriver &canctl) void setupTimers(rclcpp::Node::SharedPtr node, CANDriver& canctl)
{ {
static auto timer_mcu = node->create_wall_timer( static auto timer_mcu =
std::chrono::milliseconds(50), node->create_wall_timer(std::chrono::milliseconds(50), [&canctl]() { mcuTimerTask(canctl); });
[&canctl]()
{ mcuTimerTask(canctl); });
static auto timer_eps = node->create_wall_timer( static auto timer_eps =
std::chrono::milliseconds(50), node->create_wall_timer(std::chrono::milliseconds(50), [&canctl]() { epsTimerTask(canctl); });
[&canctl]()
{ epsTimerTask(canctl); });
static auto timer_vcu = node->create_wall_timer( static auto timer_vcu =
std::chrono::milliseconds(100), node->create_wall_timer(std::chrono::milliseconds(100), [&canctl]() { vcuTimerTask(canctl); });
[&canctl]()
{ vcuTimerTask(canctl); });
static auto timer_bms = node->create_wall_timer( static auto timer_bms =
std::chrono::milliseconds(200), node->create_wall_timer(std::chrono::milliseconds(200), [&canctl]() { bmsTimerTask(canctl); });
[&canctl]()
{ bmsTimerTask(canctl); });
RCLCPP_INFO(node->get_logger(), "[TIMER] All timers setup completed"); LOG_INFO("[TIMER] All timers setup completed");
} }

24
src/base/vehicle_params/CMakeLists.txt
View File

@ -13,40 +13,30 @@ endif()
# ========================= # =========================
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(sweeper_interfaces REQUIRED) # ⭐ 新增 find_package(sweeper_interfaces REQUIRED)
find_package(logger REQUIRED)
# ========================= # =========================
# include 路径 # include 路径
# ========================= # =========================
include_directories( include_directories(include)
include
)
# ========================= # =========================
# 可执行文件 # 可执行文件
# ========================= # =========================
add_executable(vehicle_params_node add_executable(vehicle_params_node src/vehicle_params_node.cpp)
src/vehicle_params_node.cpp
)
# ========================= # =========================
# 链接依赖 # 链接依赖
# ========================= # =========================
ament_target_dependencies(vehicle_params_node ament_target_dependencies(vehicle_params_node rclcpp sweeper_interfaces logger)
rclcpp
sweeper_interfaces # ⭐ 新增
)
# ========================= # =========================
# 安装 # 安装
# ========================= # =========================
install(TARGETS vehicle_params_node install(TARGETS vehicle_params_node DESTINATION lib/${PROJECT_NAME})
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY include/ install(DIRECTORY include/ DESTINATION include/)
DESTINATION include/
)
# ========================= # =========================
# 导出 # 导出

11
src/base/vehicle_params/package.xml
View File

@ -6,10 +6,8 @@
<name>vehicle_params</name> <name>vehicle_params</name>
<version>0.1.0</version> <version>0.1.0</version>
<description> <description> Central vehicle identity provider node. Fetches IMEI / VID from B-side service and
Central vehicle identity provider node. publishes them as ROS2 messages. </description>
Fetches IMEI / VID from B-side service and publishes them as ROS2 messages.
</description>
<maintainer email="zxwl@44.com">nvidia</maintainer> <maintainer email="zxwl@44.com">nvidia</maintainer>
<license>Apache-2.0</license> <license>Apache-2.0</license>
@ -19,7 +17,8 @@
<!-- runtime deps --> <!-- runtime deps -->
<depend>rclcpp</depend> <depend>rclcpp</depend>
<depend>sweeper_interfaces</depend> <!-- ⭐ 新增 --> <depend>sweeper_interfaces</depend>
<depend>logger</depend>
<!-- tests --> <!-- tests -->
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
@ -28,4 +27,4 @@
<export> <export>
<build_type>ament_cmake</build_type> <build_type>ament_cmake</build_type>
</export> </export>
</package> </package>

53
src/base/vehicle_params/src/vehicle_params_node.cpp
View File

@ -1,46 +1,42 @@
#include <rclcpp/rclcpp.hpp>
#include <nlohmann/json.hpp>
#include <atomic> #include <atomic>
#include <chrono> #include <chrono>
#include <nlohmann/json.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string> #include <string>
#include <thread> #include <thread>
#include "vehicle_params/httplib.h" #include "logger/logger.h"
#include "sweeper_interfaces/msg/vehicle_identity.hpp" #include "sweeper_interfaces/msg/vehicle_identity.hpp"
#include "vehicle_params/httplib.h"
using sweeper_interfaces::msg::VehicleIdentity; using sweeper_interfaces::msg::VehicleIdentity;
class VehicleParamsNode : public rclcpp::Node class VehicleParamsNode : public rclcpp::Node
{ {
public: public:
VehicleParamsNode() VehicleParamsNode() : Node("vehicle_params_node")
: Node("vehicle_params_node")
{ {
RCLCPP_INFO(this->get_logger(), "vehicle_params_node starting..."); LOG_INFO("vehicle_params_node starting...");
// ---------- neardi ---------- // ---------- neardi ----------
this->declare_parameter<std::string>("neardi.host", "192.168.5.1"); this->declare_parameter<std::string>("neardi.host", "192.168.5.1");
this->declare_parameter<int>("neardi.port", 8080); this->declare_parameter<int>("neardi.port", 8080);
// ---------- publisher ---------- // ---------- publisher ----------
identity_pub_ = this->create_publisher<VehicleIdentity>( identity_pub_ =
"/vehicle/identity", this->create_publisher<VehicleIdentity>("/vehicle/identity", rclcpp::QoS(1).transient_local().reliable());
rclcpp::QoS(1).transient_local().reliable());
// ---------- worker ---------- // ---------- worker ----------
worker_ = std::thread([this]() worker_ = std::thread([this]() { fetch_from_neardi_loop(); });
{ fetch_from_neardi_loop(); });
} }
~VehicleParamsNode() ~VehicleParamsNode()
{ {
running_.store(false); running_.store(false);
if (worker_.joinable()) if (worker_.joinable()) worker_.join();
worker_.join();
} }
private: private:
std::atomic<bool> running_{true}; std::atomic<bool> running_{true};
std::thread worker_; std::thread worker_;
rclcpp::Publisher<VehicleIdentity>::SharedPtr identity_pub_; rclcpp::Publisher<VehicleIdentity>::SharedPtr identity_pub_;
@ -58,15 +54,13 @@ private:
while (rclcpp::ok() && running_.load()) while (rclcpp::ok() && running_.load())
{ {
RCLCPP_INFO(this->get_logger(), LOG_INFO("Requesting vehicle identity from neardi (%s:%d)...", host.c_str(), port);
"Requesting vehicle identity from neardi (%s:%d)...",
host.c_str(), port);
auto res = cli.Post("/api/v1/device/register"); auto res = cli.Post("/api/v1/device/register");
if (!res || res->status != 200) if (!res || res->status != 200)
{ {
RCLCPP_WARN(this->get_logger(), "neardi request failed, retrying..."); LOG_WARN("neardi request failed, retrying...");
retry_sleep(); retry_sleep();
continue; continue;
} }
@ -97,11 +91,9 @@ private:
identity_pub_->publish(msg); identity_pub_->publish(msg);
RCLCPP_INFO(this->get_logger(), LOG_INFO("Vehicle identity published: IMEI=%s, VID=%s", imei.c_str(), vid.c_str());
"Vehicle identity published: IMEI=%s, VID=%s",
imei.c_str(), vid.c_str());
return; // 成功一次即可 return; // 成功一次即可
} }
catch (...) catch (...)
{ {
@ -110,16 +102,19 @@ private:
} }
} }
void retry_sleep() void retry_sleep() { std::this_thread::sleep_for(std::chrono::seconds(2)); }
{
std::this_thread::sleep_for(std::chrono::seconds(2));
}
}; };
int main(int argc, char **argv) int main(int argc, char** argv)
{ {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
/*初始化日志系统*/
logger::Logger::Init("vehicle_params", "./log");
rclcpp::spin(std::make_shared<VehicleParamsNode>()); rclcpp::spin(std::make_shared<VehicleParamsNode>());
rclcpp::shutdown(); rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
return 0; return 0;
} }

4
src/communication/mqtt_report/CMakeLists.txt
View File

@ -26,6 +26,7 @@ find_package(sweeper_interfaces REQUIRED)
find_package(radio_ctrl REQUIRED) find_package(radio_ctrl REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(ament_index_cpp REQUIRED) find_package(ament_index_cpp REQUIRED)
find_package(logger REQUIRED)
include_directories(include ${catkin_INCLUDE_DIRS}) include_directories(include ${catkin_INCLUDE_DIRS})
@ -44,7 +45,8 @@ ament_target_dependencies(
ament_index_cpp ament_index_cpp
rclcpp rclcpp
sweeper_interfaces sweeper_interfaces
std_msgs) std_msgs
logger)
# 包含 radio_ctrl 头文件 # 包含 radio_ctrl 头文件
target_include_directories(mqtt_report_node PRIVATE ${radio_ctrl_INCLUDE_DIRS}) target_include_directories(mqtt_report_node PRIVATE ${radio_ctrl_INCLUDE_DIRS})

9
src/communication/mqtt_report/include/mqtt_report/fault_codes.h
View File

@ -27,12 +27,11 @@ class ErrorCodeSet
// 打印所有当前错误码 // 打印所有当前错误码
void printErrors() const void printErrors() const
{ {
// std::cout << "Current error codes: "; // LOG_INFO("Current error codes: ");
// for (int code : errors) // for (int code : getAllErrorCodes()) {
// { // LOG_INFO("%d ", code);
// std::cout << code << " ";
// } // }
// std::cout << std::endl; // LOG_INFO("");
} }
// 获取所有当前错误码 // 获取所有当前错误码

3
src/communication/mqtt_report/package.xml
View File

@ -11,6 +11,7 @@
<depend>sweeper_interfaces</depend> <depend>sweeper_interfaces</depend>
<depend>radio_ctrl</depend> <depend>radio_ctrl</depend>
<depend>ament_index_cpp</depend> <depend>ament_index_cpp</depend>
<depend>logger</depend>
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
@ -20,4 +21,4 @@
<export> <export>
<build_type>ament_cmake</build_type> <build_type>ament_cmake</build_type>
</export> </export>
</package> </package>

14
src/communication/mqtt_report/src/can/can_decoder.cpp
View File

@ -5,6 +5,7 @@
#include <iomanip> #include <iomanip>
#include <iostream> #include <iostream>
#include "logger/logger.h"
#include "mqtt_report/fault_codes.h" #include "mqtt_report/fault_codes.h"
// ===================== ctor ===================== // ===================== ctor =====================
@ -49,11 +50,8 @@ void CanDecoder::handle_bms_100(const sweeperMsg::CanFrame& msg)
ctx_.info.chargeStatus = (current >= 0); ctx_.info.chargeStatus = (current >= 0);
} }
std::cout << std::fixed << std::setprecision(2); LOG_INFO("[0x100] 总电压: %.2f V, 电流: %.2f A, 剩余容量: %.2f Ah, 是否在充电: %s", voltage, current, capacity,
std::cout << "[0x100] 总电压: " << voltage << " V, " ((current >= 0) ? "是" : "否"));
<< "电流: " << current << " A, "
<< "剩余容量: " << capacity << " Ah, "
<< "是否在充电: " << ((current >= 0) ? "是" : "否") << std::endl;
} }
void CanDecoder::handle_bms_101(const sweeperMsg::CanFrame& msg) void CanDecoder::handle_bms_101(const sweeperMsg::CanFrame& msg)
@ -71,10 +69,8 @@ void CanDecoder::handle_bms_101(const sweeperMsg::CanFrame& msg)
ctx_.info.power = rsoc; ctx_.info.power = rsoc;
} }
std::cout << std::fixed << std::setprecision(2); LOG_INFO("[0x101] 充满容量: %.2f Ah, 循环次数: %d 次, 剩余容量百分比(RSOC): %.1f %", full_capacity, cycle_count,
std::cout << "[0x101] 充满容量: " << full_capacity << " Ah, " rsoc);
<< "循环次数: " << cycle_count << " 次, "
<< "剩余容量百分比(RSOC): " << rsoc << " %" << std::endl;
} }
// ===================== MCU ===================== // ===================== MCU =====================

9
src/communication/mqtt_report/src/core/input_health_monitor.cpp
View File

@ -2,6 +2,8 @@
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include "logger/logger.h"
static constexpr int64_t WARN_INTERVAL_MS = 10 * 1000; static constexpr int64_t WARN_INTERVAL_MS = 10 * 1000;
static constexpr int64_t IDENTITY_TIMEOUT_MS = 15 * 1000; static constexpr int64_t IDENTITY_TIMEOUT_MS = 15 * 1000;
static constexpr int64_t GPS_TIMEOUT_MS = 5 * 1000; static constexpr int64_t GPS_TIMEOUT_MS = 5 * 1000;
@ -31,7 +33,7 @@ void InputHealthMonitor::check(rclcpp::Logger logger, int64_t now, bool mqtt_con
{ {
if (!mqtt_connected) if (!mqtt_connected)
{ {
RCLCPP_WARN(logger, "MQTT connection lost."); LOG_WARN("MQTT connection lost.");
} }
check_input("vehicle identity (/vehicle/identity)", identity_, now, IDENTITY_TIMEOUT_MS); check_input("vehicle identity (/vehicle/identity)", identity_, now, IDENTITY_TIMEOUT_MS);
@ -46,15 +48,14 @@ void InputHealthMonitor::check_input(const char* name, InputMonitor& mon, int64_
if (mon.ok && now - mon.last_rx_ts > timeout_ms) if (mon.ok && now - mon.last_rx_ts > timeout_ms)
{ {
mon.ok = false; mon.ok = false;
RCLCPP_WARN(rclcpp::get_logger("input_monitor"), "%s timeout (%ld ms without data).", name, LOG_WARN("%s timeout (%ld ms without data).", name, now - mon.last_rx_ts);
now - mon.last_rx_ts);
mon.last_warn_ts = now; mon.last_warn_ts = now;
return; return;
} }
if (!mon.ok && now - mon.last_warn_ts > WARN_INTERVAL_MS) if (!mon.ok && now - mon.last_warn_ts > WARN_INTERVAL_MS)
{ {
RCLCPP_WARN(rclcpp::get_logger("input_monitor"), "Waiting for %s...", name); LOG_WARN("Waiting for %s...", name);
mon.last_warn_ts = now; mon.last_warn_ts = now;
} }
} }

20
src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp
View File

@ -5,6 +5,7 @@
#include <string> #include <string>
#include <vector> #include <vector>
#include "logger/logger.h"
#include "mqtt_report/can/can_decoder.h" #include "mqtt_report/can/can_decoder.h"
#include "mqtt_report/core/input_health_monitor.h" #include "mqtt_report/core/input_health_monitor.h"
#include "mqtt_report/core/vehicle_context.h" #include "mqtt_report/core/vehicle_context.h"
@ -75,7 +76,7 @@ class VehicleReportNode : public rclcpp::Node
ctx_.vid = msg->vid; ctx_.vid = msg->vid;
ctx_.ready = msg->ready; ctx_.ready = msg->ready;
RCLCPP_INFO(get_logger(), "Vehicle identity ready: IMEI=%s, VID=%s", msg->imei.c_str(), msg->vid.c_str()); LOG_INFO("Vehicle identity ready: IMEI=%s, VID=%s", msg->imei.c_str(), msg->vid.c_str());
int64_t now = getCurrentTimestampMs(); int64_t now = getCurrentTimestampMs();
@ -168,28 +169,31 @@ int main(int argc, char** argv)
} }
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
/*初始化日志系统*/
logger::Logger::Init("mqtt_report", "./log");
Config config; Config config;
auto logger = rclcpp::get_logger("main");
if (!load_config(config_path, config)) if (!load_config(config_path, config))
{ {
RCLCPP_ERROR(logger, "Failed to load config: %s", config_path.c_str()); LOG_ERROR("Failed to load config: %s", config_path.c_str());
logger::Logger::Shutdown();
return 1; return 1;
} }
if (mqtt_reconnect_interval <= 0) mqtt_reconnect_interval = 5000; if (mqtt_reconnect_interval <= 0) mqtt_reconnect_interval = 5000;
RCLCPP_INFO(logger, "Config loaded: broker=%s:%d, user=%s", config.mqtt_ip.c_str(), config.mqtt_port, LOG_INFO("Config loaded: broker=%s:%d, user=%s", config.mqtt_ip.c_str(), config.mqtt_port,
config.mqtt_username.empty() ? "<anonymous>" : config.mqtt_username.c_str()); config.mqtt_username.empty() ? "<anonymous>" : config.mqtt_username.c_str());
RCLCPP_INFO(logger, "Topic templates: gps='%s', info='%s', fault='%s'", config.gps_topic_template.c_str(), LOG_INFO("Topic templates: gps='%s', info='%s', fault='%s'", config.gps_topic_template.c_str(),
config.info_topic_template.c_str(), config.fault_topic_template.c_str()); config.info_topic_template.c_str(), config.fault_topic_template.c_str());
auto node = std::make_shared<VehicleReportNode>(config); auto node = std::make_shared<VehicleReportNode>(config);
rclcpp::spin(node); rclcpp::spin(node);
rclcpp::shutdown(); rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
return 0; return 0;
} }

4
src/communication/sub/CMakeLists.txt
View File

@ -25,6 +25,7 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(CURL REQUIRED) find_package(CURL REQUIRED)
find_package(logger REQUIRED)
add_executable( add_executable(
sub_node sub_node
@ -43,7 +44,8 @@ ament_target_dependencies(
rclcpp rclcpp
std_msgs std_msgs
sweeper_interfaces sweeper_interfaces
CURL) CURL
logger)
if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64) if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64)
target_link_libraries(sub_node ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c-static.a) target_link_libraries(sub_node ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c-static.a)

1
src/communication/sub/package.xml
View File

@ -12,6 +12,7 @@
<depend>rclcpp</depend> <depend>rclcpp</depend>
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>sweeper_interfaces</depend> <depend>sweeper_interfaces</depend>
<depend>logger</depend>
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_common</test_depend>

13
src/communication/sub/src/main.cpp
View File

@ -3,6 +3,7 @@
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <string> #include <string>
#include "logger/logger.h"
#include "sub/control_state.hpp" #include "sub/control_state.hpp"
#include "sub/json.h" #include "sub/json.h"
#include "sub/mqtt_receiver.hpp" #include "sub/mqtt_receiver.hpp"
@ -66,9 +67,6 @@ int main(int argc, char** argv)
return 1; return 1;
} }
std::cout << "MQTT address: " << cfg.address << "\n";
std::cout << "MQTT topic template: " << cfg.remote_topic_template << "\n";
// 2) shared state // 2) shared state
ControlState state; ControlState state;
{ {
@ -82,10 +80,19 @@ int main(int argc, char** argv)
// 4) ROS2 spin // 4) ROS2 spin
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
/*初始化日志系统*/
logger::Logger::Init("sub", "./log");
LOG_INFO("MQTT address: %s", cfg.address.c_str());
LOG_INFO("MQTT topic template: %s", cfg.remote_topic_template.c_str());
auto node = std::make_shared<SubNode>(state, "sub_node"); auto node = std::make_shared<SubNode>(state, "sub_node");
rclcpp::spin(node); rclcpp::spin(node);
rclcpp::shutdown(); rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
// 5) stop mqtt // 5) stop mqtt
mqtt.stop(); mqtt.stop();
return 0; return 0;

9
src/communication/sub/src/mqtt_receiver.cpp
View File

@ -9,6 +9,7 @@
#include <thread> #include <thread>
#include <vector> #include <vector>
#include "logger/logger.h"
#include "sub/json.h" #include "sub/json.h"
namespace sub_node_pkg namespace sub_node_pkg
@ -287,7 +288,7 @@ void MqttReceiver::runLoop()
} }
else else
{ {
std::cout << "[MQTT] Connected to broker\n"; LOG_INFO("[MQTT] Connected to broker");
} }
std::string topic = get_sub_topic(); std::string topic = get_sub_topic();
@ -306,7 +307,7 @@ void MqttReceiver::runLoop()
} }
state_.mqtt_connected = true; state_.mqtt_connected = true;
std::cout << "[MQTT] Connected & subscribed: " << topic << "\n"; LOG_INFO("[MQTT] Connected & subscribed: %s", topic.c_str());
return true; return true;
}; };
@ -333,7 +334,7 @@ void MqttReceiver::runLoop()
// 2) identity 从 false->true（或 vid 变化）时，主动订阅一次 // 2) identity 从 false->true（或 vid 变化）时，主动订阅一次
if (ready && (!last_ready || topic != last_topic)) if (ready && (!last_ready || topic != last_topic))
{ {
std::cout << "[MQTT] Identity ready/topic changed, subscribing: " << topic << "\n"; LOG_INFO("[MQTT] Identity ready/topic changed, subscribing: %s", topic.c_str());
// 已连接就直接 subscribe；未连接则走 do_connect_and_sub // 已连接就直接 subscribe；未连接则走 do_connect_and_sub
if (MQTTClient_isConnected(client_) != 0) if (MQTTClient_isConnected(client_) != 0)
@ -342,7 +343,7 @@ void MqttReceiver::runLoop()
if (rc_sub == MQTTCLIENT_SUCCESS) if (rc_sub == MQTTCLIENT_SUCCESS)
{ {
state_.mqtt_connected = true; state_.mqtt_connected = true;
std::cout << "[MQTT] Subscribed: " << topic << "\n"; LOG_INFO("[MQTT] Subscribed: %s", topic.c_str());
last_topic = topic; last_topic = topic;
} }
else else

5
src/communication/sub/src/sub_node.cpp
View File

@ -2,6 +2,7 @@
#include <chrono> #include <chrono>
#include "logger/logger.h"
#include "sub/control_mapper.hpp" #include "sub/control_mapper.hpp"
namespace sub_node_pkg namespace sub_node_pkg
@ -9,7 +10,7 @@ namespace sub_node_pkg
SubNode::SubNode(ControlState& state, const std::string& name) : Node(name), state_(state) SubNode::SubNode(ControlState& state, const std::string& name) : Node(name), state_(state)
{ {
RCLCPP_INFO(this->get_logger(), "%s node started.", name.c_str()); LOG_INFO("%s node started.", name.c_str());
pub_mc_ = this->create_publisher<sweeper_interfaces::msg::McCtrl>("remote_mc_ctrl", 10); pub_mc_ = this->create_publisher<sweeper_interfaces::msg::McCtrl>("remote_mc_ctrl", 10);
pub_gather_ = this->create_publisher<sweeper_interfaces::msg::Sub>("gather", 10); pub_gather_ = this->create_publisher<sweeper_interfaces::msg::Sub>("gather", 10);
@ -70,7 +71,7 @@ void SubNode::identityCallback(const sweeper_interfaces::msg::VehicleIdentity::S
state_.vid = msg->vid; state_.vid = msg->vid;
state_.identity_ready = msg->ready; state_.identity_ready = msg->ready;
RCLCPP_INFO(get_logger(), "Identity: VID=%s ready=%d", msg->vid.c_str(), msg->ready); LOG_INFO("Identity: VID=%s ready=%d", msg->vid.c_str(), msg->ready);
} }
} // namespace sub_node_pkg } // namespace sub_node_pkg

4
src/communication/task_manager/CMakeLists.txt
View File

@ -25,6 +25,7 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(CURL REQUIRED) find_package(CURL REQUIRED)
find_package(logger REQUIRED)
include_directories(include/task_manager include/paho_mqtt_3c ${catkin_INCLUDE_DIRS}) include_directories(include/task_manager include/paho_mqtt_3c ${catkin_INCLUDE_DIRS})
@ -41,7 +42,8 @@ ament_target_dependencies(
rclcpp rclcpp
std_msgs std_msgs
sweeper_interfaces sweeper_interfaces
CURL) CURL
logger)
if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64) if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64)
target_link_libraries(task_manager_node ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c-static.a) target_link_libraries(task_manager_node ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c-static.a)

1
src/communication/task_manager/package.xml
View File

@ -12,6 +12,7 @@
<depend>rclcpp</depend> <depend>rclcpp</depend>
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>sweeper_interfaces</depend> <depend>sweeper_interfaces</depend>
<depend>logger</depend>
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_common</test_depend>

22
src/communication/task_manager/src/mqtt_manager.cpp
View File

@ -4,6 +4,8 @@
#include <iostream> #include <iostream>
#include <memory> #include <memory>
#include "logger/logger.h"
MQTTManager::MQTTManager() MQTTManager::MQTTManager()
: client_(nullptr), : client_(nullptr),
is_running_(false), is_running_(false),
@ -58,7 +60,7 @@ bool MQTTManager::reconnect()
{ {
std::lock_guard<std::mutex> lock(client_mutex_); std::lock_guard<std::mutex> lock(client_mutex_);
std::cout << "Attempting to reconnect to MQTT server..." << std::endl; LOG_INFO("Attempting to reconnect to MQTT server.");
int rc = MQTTClient_connect(client_, &conn_opts_); int rc = MQTTClient_connect(client_, &conn_opts_);
if (rc != MQTTCLIENT_SUCCESS) if (rc != MQTTCLIENT_SUCCESS)
@ -74,7 +76,7 @@ bool MQTTManager::reconnect()
return false; return false;
} }
std::cout << "Successfully reconnected to MQTT server." << std::endl; LOG_INFO("Successfully reconnected to MQTT server.");
reconnect_attempts_ = 0; reconnect_attempts_ = 0;
is_heartbeat_lost_ = false; is_heartbeat_lost_ = false;
last_message_time_ = std::chrono::steady_clock::now(); last_message_time_ = std::chrono::steady_clock::now();
@ -105,7 +107,7 @@ bool MQTTManager::subscribe(const string& topic, int qos)
return false; return false;
} }
std::cout << "Successfully subscribed to topic: " << topic << std::endl; LOG_INFO("Successfully subscribed to topic: %s", topic.c_str());
return true; return true;
} }
@ -148,7 +150,7 @@ void MQTTManager::start()
{ {
if (is_running_) if (is_running_)
{ {
std::cout << "MQTT manager is already running." << std::endl; LOG_INFO("MQTT manager is already running.");
return; return;
} }
@ -161,7 +163,7 @@ void MQTTManager::start()
is_running_ = true; is_running_ = true;
mqtt_thread_ = std::thread(&MQTTManager::mqttLoop, this); mqtt_thread_ = std::thread(&MQTTManager::mqttLoop, this);
std::cout << "MQTT manager started." << std::endl; LOG_INFO("MQTT manager started.");
} }
void MQTTManager::stop() void MQTTManager::stop()
@ -182,7 +184,7 @@ void MQTTManager::stop()
} }
MQTTClient_destroy(&client_); MQTTClient_destroy(&client_);
std::cout << "MQTT manager stopped." << std::endl; LOG_INFO("MQTT manager stopped.");
} }
void MQTTManager::mqttLoop() void MQTTManager::mqttLoop()
@ -215,7 +217,7 @@ void MQTTManager::mqttLoop()
if (duration > heartbeat_timeout_ && !is_heartbeat_lost_) if (duration > heartbeat_timeout_ && !is_heartbeat_lost_)
{ {
is_heartbeat_lost_ = true; is_heartbeat_lost_ = true;
std::cout << "Heartbeat timeout: No message received in " << heartbeat_timeout_ << "ms." << std::endl; LOG_WARN("Heartbeat timeout: No message received in %dms.", heartbeat_timeout_);
if (conn_lost_callback_) if (conn_lost_callback_)
{ {
conn_lost_callback_((char*)"Heartbeat timeout"); conn_lost_callback_((char*)"Heartbeat timeout");
@ -235,7 +237,7 @@ int MQTTManager::onMessageArrived(void* context, char* topicName, int topicLen,
pThis->last_message_time_ = std::chrono::steady_clock::now(); pThis->last_message_time_ = std::chrono::steady_clock::now();
pThis->is_heartbeat_lost_ = false; pThis->is_heartbeat_lost_ = false;
std::cout << "Message arrived on topic: " << topicName << std::endl; LOG_INFO("Message arrived on topic: %s", topicName);
if (pThis->message_callback_) if (pThis->message_callback_)
{ {
@ -248,7 +250,7 @@ int MQTTManager::onMessageArrived(void* context, char* topicName, int topicLen,
void MQTTManager::onConnectionLost(void* context, char* cause) void MQTTManager::onConnectionLost(void* context, char* cause)
{ {
MQTTManager* pThis = static_cast<MQTTManager*>(context); MQTTManager* pThis = static_cast<MQTTManager*>(context);
std::cout << "Connection lost. Cause: " << (cause ? cause : "Unknown") << std::endl; LOG_WARN("Connection lost. Cause: %s", (cause ? cause : "Unknown"));
if (pThis->conn_lost_callback_) if (pThis->conn_lost_callback_)
{ {
@ -259,7 +261,7 @@ void MQTTManager::onConnectionLost(void* context, char* cause)
void MQTTManager::onDelivered(void* context, MQTTClient_deliveryToken dt) void MQTTManager::onDelivered(void* context, MQTTClient_deliveryToken dt)
{ {
MQTTManager* pThis = static_cast<MQTTManager*>(context); MQTTManager* pThis = static_cast<MQTTManager*>(context);
std::cout << "Message with token " << dt << " delivered." << std::endl; LOG_INFO("Message with token %d delivered.", dt);
if (pThis->delivered_callback_) if (pThis->delivered_callback_)
{ {

89
src/communication/task_manager/src/task_manager.cpp
View File

@ -1,40 +1,38 @@
#include "task_manager.hpp" #include "task_manager.hpp"
#include "md5.h"
#include <iostream>
#include <fstream>
#include <cstdlib>
#include <unistd.h> #include <unistd.h>
TaskManager::TaskManager() #include <cstdlib>
: is_running_(false), task_status_(0), #include <fstream>
destination_file_path_("./gps_load_now.txt") #include <iostream>
#include "logger/logger.h"
#include "md5.h"
TaskManager::TaskManager() : is_running_(false), task_status_(0), destination_file_path_("./gps_load_now.txt")
{ {
current_task_.id = 0; current_task_.id = 0;
current_task_.status = TaskStatus::PENDING; current_task_.status = TaskStatus::PENDING;
} }
TaskManager::~TaskManager() TaskManager::~TaskManager() { stop(); }
{
stop();
}
void TaskManager::start() void TaskManager::start()
{ {
if (is_running_) if (is_running_)
{ {
std::cout << "TaskManager is already running." << std::endl; LOG_WARN("TaskManager is already running.");
return; return;
} }
is_running_ = true; is_running_ = true;
task_thread_ = std::thread(&TaskManager::processTasksLoop, this); task_thread_ = std::thread(&TaskManager::processTasksLoop, this);
std::cout << "TaskManager started." << std::endl; LOG_INFO("TaskManager started.");
} }
void TaskManager::stop() void TaskManager::stop()
{ {
if (!is_running_) if (!is_running_) return;
return;
{ {
std::lock_guard<std::mutex> lock(queue_mutex_); std::lock_guard<std::mutex> lock(queue_mutex_);
@ -47,10 +45,10 @@ void TaskManager::stop()
task_thread_.join(); task_thread_.join();
} }
std::cout << "TaskManager stopped." << std::endl; LOG_INFO("TaskManager stopped.");
} }
void TaskManager::addTask(const MQTT_JSON &mqtt_json) void TaskManager::addTask(const MQTT_JSON& mqtt_json)
{ {
{ {
std::lock_guard<std::mutex> lock(queue_mutex_); std::lock_guard<std::mutex> lock(queue_mutex_);
@ -83,39 +81,38 @@ void TaskManager::updateCurrentTask(int taskId, TaskStatus status)
current_task_.id = taskId; current_task_.id = taskId;
current_task_.status = status; current_task_.status = status;
current_task_.lastUpdateTime = std::chrono::steady_clock::now(); current_task_.lastUpdateTime = std::chrono::steady_clock::now();
std::cout << "Task updated: ID=" << taskId << ", Status=" << status << std::endl; LOG_INFO("Task updated: ID=%d, Status=%d", taskId, status);
} }
string TaskManager::calculate_md5(const string &filename) string TaskManager::calculate_md5(const string& filename)
{ {
MD5 md5; MD5 md5;
std::ifstream file; std::ifstream file;
file.open(filename, std::ios::binary); file.open(filename, std::ios::binary);
if (!file) if (!file)
{ {
std::cerr << "Failed to open file for MD5 calculation: " << filename << std::endl; LOG_ERROR("Failed to open file for MD5 calculation: %s", filename.c_str());
return ""; return "";
} }
md5.update(file); md5.update(file);
return md5.toString(); return md5.toString();
} }
bool TaskManager::downloadFile(const string &url, const string &expected_md5, bool TaskManager::downloadFile(const string& url, const string& expected_md5, const string& filename)
const string &filename)
{ {
if (url.empty()) if (url.empty())
{ {
std::cerr << "Download URL is empty." << std::endl; LOG_ERROR("Download URL is empty.");
return false; return false;
} }
std::string command = "wget -O routes/" + filename + " \"" + url + "\""; std::string command = "wget -O routes/" + filename + " \"" + url + "\"";
std::cout << "Executing command: " << command << std::endl; LOG_INFO("Executing command: %s", command.c_str());
int result = system(command.c_str()); int result = system(command.c_str());
if (result != 0) if (result != 0)
{ {
std::cerr << "Download failed: " << url << std::endl; LOG_ERROR("Download failed: %s", url.c_str());
return false; return false;
} }
@ -124,31 +121,29 @@ bool TaskManager::downloadFile(const string &url, const string &expected_md5,
std::string actual_md5 = calculate_md5(filepath); std::string actual_md5 = calculate_md5(filepath);
if (actual_md5 != expected_md5) if (actual_md5 != expected_md5)
{ {
std::cerr << "MD5 verification failed. Expected: " << expected_md5 LOG_ERROR("MD5 verification failed. Expected: %s, Actual: %s", expected_md5.c_str(), actual_md5.c_str());
<< ", Actual: " << actual_md5 << std::endl;
remove(filepath.c_str()); remove(filepath.c_str());
return false; return false;
} }
std::cout << "File downloaded and verified: " << filepath << std::endl; LOG_INFO("File downloaded and verified: %s", filepath.c_str());
return true; return true;
} }
void TaskManager::copyFileAndOverwrite(const string &sourceFilePath, void TaskManager::copyFileAndOverwrite(const string& sourceFilePath, const string& destinationFilePath)
const string &destinationFilePath)
{ {
std::ifstream src(sourceFilePath, std::ios::binary); std::ifstream src(sourceFilePath, std::ios::binary);
std::ofstream dst(destinationFilePath, std::ios::binary); std::ofstream dst(destinationFilePath, std::ios::binary);
if (!src) if (!src)
{ {
std::cerr << "Failed to open source file: " << sourceFilePath << std::endl; LOG_ERROR("Failed to open source file: %s", sourceFilePath.c_str());
return; return;
} }
if (!dst) if (!dst)
{ {
std::cerr << "Failed to open destination file: " << destinationFilePath << std::endl; LOG_ERROR("Failed to open destination file: %s", destinationFilePath.c_str());
return; return;
} }
@ -156,15 +151,15 @@ void TaskManager::copyFileAndOverwrite(const string &sourceFilePath,
if (!dst) if (!dst)
{ {
std::cerr << "Error while copying file." << std::endl; LOG_ERROR("Error while copying file.");
} }
} }
bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo) bool TaskManager::processStartTask(const JSON_DATA& json_data, long seqNo)
{ {
std::cout << "Processing start task ID: " << json_data.value.id << std::endl; LOG_INFO("Processing start task ID: %d", json_data.value.id);
std::cout << "URL: " << json_data.value.routeInfo.url << std::endl; LOG_INFO("URL: %s", json_data.value.routeInfo.url.c_str());
std::cout << "MD5: " << json_data.value.routeInfo.md5 << std::endl; LOG_INFO("MD5: %s", json_data.value.routeInfo.md5.c_str());
// 更新当前任务ID // 更新当前任务ID
updateCurrentTask(json_data.value.id, TaskStatus::RUNNING); updateCurrentTask(json_data.value.id, TaskStatus::RUNNING);
@ -175,11 +170,10 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo)
// 检查文件是否已存在 // 检查文件是否已存在
if (access(filepath.c_str(), F_OK) == -1) if (access(filepath.c_str(), F_OK) == -1)
{ {
std::cout << "File not found, downloading: " << downFileName << std::endl; LOG_INFO("File not found, downloading: %s", downFileName.c_str());
if (!downloadFile(json_data.value.routeInfo.url, if (!downloadFile(json_data.value.routeInfo.url, json_data.value.routeInfo.md5, downFileName))
json_data.value.routeInfo.md5, downFileName))
{ {
std::cout << "Download failed, skipping task." << std::endl; LOG_WARN("Download failed, skipping task.");
updateCurrentTask(json_data.value.id, TaskStatus::FAILED); updateCurrentTask(json_data.value.id, TaskStatus::FAILED);
if (send_response_callback_) if (send_response_callback_)
{ {
@ -191,7 +185,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo)
} }
else else
{ {
std::cout << "File already exists, skipping download: " << filepath << std::endl; LOG_INFO("File already exists, skipping download: %s", filepath.c_str());
} }
// 复制文件 // 复制文件
@ -200,7 +194,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo)
// 启动任务 // 启动任务
task_status_ = 1; task_status_ = 1;
updateCurrentTask(json_data.value.id, TaskStatus::RUNNING); updateCurrentTask(json_data.value.id, TaskStatus::RUNNING);
std::cout << "Task started successfully: " << json_data.value.id << std::endl; LOG_INFO("Task started successfully: %d", json_data.value.id);
// 发送响应（响应主题由调用方决定） // 发送响应（响应主题由调用方决定）
if (send_response_callback_) if (send_response_callback_)
@ -213,7 +207,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo)
bool TaskManager::processStopTask(long taskId, long seqNo) bool TaskManager::processStopTask(long taskId, long seqNo)
{ {
std::cout << "Processing stop task ID: " << taskId << std::endl; LOG_INFO("Processing stop task ID: %d", taskId);
task_status_ = 0; task_status_ = 0;
updateCurrentTask(taskId, TaskStatus::COMPLETED); updateCurrentTask(taskId, TaskStatus::COMPLETED);
@ -232,8 +226,7 @@ void TaskManager::processTasksLoop()
std::unique_lock<std::mutex> lock(queue_mutex_); std::unique_lock<std::mutex> lock(queue_mutex_);
// 等待队列中有任务或线程需要停止 // 等待队列中有任务或线程需要停止
queue_cv_.wait(lock, [this] queue_cv_.wait(lock, [this] { return !task_queue_.empty() || !is_running_; });
{ return !task_queue_.empty() || !is_running_; });
// 检查是否需要退出 // 检查是否需要退出
if (!is_running_ && task_queue_.empty()) if (!is_running_ && task_queue_.empty())
@ -263,9 +256,9 @@ void TaskManager::processTasksLoop()
processStopTask(taskId, mqtt_json.seqNo); processStopTask(taskId, mqtt_json.seqNo);
} }
} }
catch (const boost::bad_any_cast &e) catch (const boost::bad_any_cast& e)
{ {
std::cerr << "Bad any_cast in task processing: " << e.what() << std::endl; LOG_ERROR("Bad any_cast in task processing: %s", e.what());
} }
} }
} }

106
src/communication/task_manager/src/task_manager_main.cpp
View File

@ -6,6 +6,7 @@
#include <random> #include <random>
#include "json.h" #include "json.h"
#include "logger/logger.h"
#include "mqtt_manager.hpp" #include "mqtt_manager.hpp"
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/task.hpp" #include "sweeper_interfaces/msg/task.hpp"
@ -52,12 +53,12 @@ string generate_mqtt_client_id();
// 信号处理函数 // 信号处理函数
void signalHandler(int signum) void signalHandler(int signum)
{ {
std::cout << "Interrupt signal (" << signum << ") received." << std::endl; LOG_INFO("Interrupt signal (%d) received.", signum);
signal_received = signum; signal_received = signum;
if (signum == SIGINT && rclcpp::ok()) if (signum == SIGINT && rclcpp::ok())
{ {
std::cout << "Shutting down ROS2 node..." << std::endl; LOG_INFO("Shutting down ROS2 node.");
rclcpp::shutdown(); rclcpp::shutdown();
} }
} }
@ -87,26 +88,26 @@ ROUTE_INFO get_route_info(const Json::Value& root)
if (root.isMember("routeName") && root["routeName"].isString()) if (root.isMember("routeName") && root["routeName"].isString())
route_info.routeName = root["routeName"].asString(); route_info.routeName = root["routeName"].asString();
else else
std::cerr << "Missing or invalid 'routeName' field" << std::endl; LOG_ERROR("Missing or invalid 'routeName' field");
if (root.isMember("fileName") && root["fileName"].isString()) if (root.isMember("fileName") && root["fileName"].isString())
route_info.fileName = root["fileName"].asString(); route_info.fileName = root["fileName"].asString();
else else
std::cerr << "Missing or invalid 'fileName' field" << std::endl; LOG_ERROR("Missing or invalid 'fileName' field");
if (root.isMember("url") && root["url"].isString()) if (root.isMember("url") && root["url"].isString())
route_info.url = root["url"].asString(); route_info.url = root["url"].asString();
else else
std::cerr << "Missing or invalid 'url' field" << std::endl; LOG_ERROR("Missing or invalid 'url' field");
if (root.isMember("md5") && root["md5"].isString()) if (root.isMember("md5") && root["md5"].isString())
route_info.md5 = root["md5"].asString(); route_info.md5 = root["md5"].asString();
else else
std::cerr << "Missing or invalid 'md5' field" << std::endl; LOG_ERROR("Missing or invalid 'md5' field");
} }
else else
{ {
std::cerr << "routeInfo is not a valid JSON object" << std::endl; LOG_ERROR("routeInfo is not a valid JSON object");
} }
return route_info; return route_info;
@ -122,31 +123,31 @@ TASK_VALUE get_task_value(const Json::Value& root)
if (root.isMember("id") && root["id"].isInt()) if (root.isMember("id") && root["id"].isInt())
task_value.id = root["id"].asInt(); task_value.id = root["id"].asInt();
else else
std::cerr << "Missing or invalid 'id' field" << std::endl; LOG_ERROR("Missing or invalid 'id' field");
if (root.isMember("name") && root["name"].isString()) if (root.isMember("name") && root["name"].isString())
task_value.name = root["name"].asString(); task_value.name = root["name"].asString();
else else
std::cerr << "Missing or invalid 'name' field" << std::endl; LOG_ERROR("Missing or invalid 'name' field");
if (root.isMember("mode") && root["mode"].isInt()) if (root.isMember("mode") && root["mode"].isInt())
task_value.mode = root["mode"].asInt(); task_value.mode = root["mode"].asInt();
else else
std::cerr << "Missing or invalid 'mode' field" << std::endl; LOG_ERROR("Missing or invalid 'mode' field");
if (root.isMember("count") && root["count"].isInt()) if (root.isMember("count") && root["count"].isInt())
task_value.count = root["count"].asInt(); task_value.count = root["count"].asInt();
else else
std::cerr << "Missing or invalid 'count' field" << std::endl; LOG_ERROR("Missing or invalid 'count' field");
if (root.isMember("routeInfo") && root["routeInfo"].isObject()) if (root.isMember("routeInfo") && root["routeInfo"].isObject())
task_value.routeInfo = get_route_info(root["routeInfo"]); task_value.routeInfo = get_route_info(root["routeInfo"]);
else else
std::cerr << "Missing or invalid 'routeInfo' field" << std::endl; LOG_ERROR("Missing or invalid 'routeInfo' field");
} }
else else
{ {
std::cerr << "task value is not a valid JSON object" << std::endl; LOG_ERROR("task value is not a valid JSON object");
} }
return task_value; return task_value;
@ -161,13 +162,13 @@ bool loadConfig(const string& config_file)
if (!in.is_open()) if (!in.is_open())
{ {
std::cout << "Failed to read config file: " << config_file << std::endl; LOG_ERROR("Failed to read config file: %s", config_file.c_str());
return false; return false;
} }
if (!reader.parse(in, root)) if (!reader.parse(in, root))
{ {
std::cout << "Failed to parse config file." << std::endl; LOG_ERROR("Failed to parse config file.");
return false; return false;
} }
@ -182,10 +183,10 @@ bool loadConfig(const string& config_file)
in.close(); in.close();
std::cout << "Config loaded successfully." << std::endl; LOG_INFO("Config loaded successfully.");
std::cout << "MQTT Address: " << config.mqtt_address << std::endl; LOG_INFO("MQTT Address: %s", config.mqtt_address.c_str());
std::cout << "Topic Sub template: " << config.mqtt_topic_sub_task_tmpl << std::endl; LOG_INFO("Topic Sub template: %s", config.mqtt_topic_sub_task_tmpl.c_str());
std::cout << "Topic Pub template: " << config.mqtt_topic_push_status_tmpl << std::endl; LOG_INFO("Topic Pub template: %s", config.mqtt_topic_push_status_tmpl.c_str());
return true; return true;
} }
@ -206,11 +207,11 @@ void sendGeneralResponse(const string& topic, long seqNo, int code, const string
Json::FastWriter writer; Json::FastWriter writer;
string responseJson = writer.write(responseData); string responseJson = writer.write(responseData);
std::cout << "[General Response] seqNo=" << seqNo << ", code=" << code << ", msg=" << msg << std::endl; LOG_INFO("[General Response] seqNo=%d, code=%d, msg=%s", seqNo, code, msg.c_str());
std::cout << "Response JSON: " << responseJson << std::endl; LOG_DEBUG("Response JSON: %s", responseJson.c_str());
bool success = mqtt_manager.publish(topic, responseJson, 0); bool success = mqtt_manager.publish(topic, responseJson, 0);
std::cout << "General response publish to [" << topic << "] " << (success ? "[OK]" : "[FAILED]") << std::endl; LOG_INFO("General response publish to [%s] %s", topic.c_str(), (success ? "[OK]" : "[FAILED]"));
} }
// 发送任务专用应答（包含任务信息） // 发送任务专用应答（包含任务信息）
@ -235,19 +236,18 @@ void sendTaskResponse(long seqNo, int code, const string& msg)
Json::FastWriter writer; Json::FastWriter writer;
string responseJson = writer.write(responseData); string responseJson = writer.write(responseData);
std::cout << "[Task Response] seqNo=" << seqNo << ", code=" << code << ", taskId=" << current_task_id LOG_INFO("[Task Response] seqNo=%ld, code=%d, taskId=%d, taskStatus=%d", seqNo, code, current_task_id,
<< ", taskStatus=" << (int)current_status << std::endl; (int)current_status);
std::cout << "Response JSON: " << responseJson << std::endl; LOG_DEBUG("Response JSON: %s", responseJson.c_str());
if (mqtt_topic_sub_task.empty()) if (mqtt_topic_sub_task.empty())
{ {
std::cout << "[TASK] response topic not ready, drop response. seqNo=" << seqNo << std::endl; LOG_WARN("[TASK] response topic not ready, drop response. seqNo=%ld", seqNo);
return; return;
} }
bool success = mqtt_manager.publish(mqtt_topic_sub_task, responseJson, 0); bool success = mqtt_manager.publish(mqtt_topic_sub_task, responseJson, 0);
std::cout << "Task response publish to [" << mqtt_topic_sub_task << "] " << (success ? "[OK]" : "[FAILED]") LOG_INFO("Task response publish to [%s] %s", mqtt_topic_sub_task.c_str(), (success ? "[OK]" : "[FAILED]"));
<< std::endl;
} }
// MQTT消息回调 // MQTT消息回调
@ -265,12 +265,12 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message
if (!reader.parse(buffer, root)) if (!reader.parse(buffer, root))
{ {
std::cout << "Failed to parse JSON from MQTT message." << std::endl; LOG_ERROR("Failed to parse JSON from MQTT message.");
delete[] buffer; delete[] buffer;
return 1; return 1;
} }
std::cout << "MQTT message received on topic: " << topicName << std::endl; LOG_INFO("MQTT message received on topic: %s", topicName);
if (root.isMember("type") && root["type"].asString() == "request") if (root.isMember("type") && root["type"].asString() == "request")
{ {
@ -301,12 +301,12 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message
mqtt_json.seqNo = seqNo; mqtt_json.seqNo = seqNo;
mqtt_json.data = json_data; mqtt_json.data = json_data;
std::cout << "Start task: " << json_data.value.id << std::endl; LOG_INFO("Start task: %d", json_data.value.id);
task_manager.addTask(mqtt_json); task_manager.addTask(mqtt_json);
} }
catch (const std::exception& e) catch (const std::exception& e)
{ {
std::cerr << "Error processing start command: " << e.what() << std::endl; LOG_ERROR("Error processing start command: %s", e.what());
sendTaskResponse(seqNo, 400, "Failed to process start command"); sendTaskResponse(seqNo, 400, "Failed to process start command");
} }
} }
@ -321,14 +321,14 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message
mqtt_json.seqNo = seqNo; mqtt_json.seqNo = seqNo;
mqtt_json.data = stop_id; mqtt_json.data = stop_id;
std::cout << "Stop task: " << stop_id << std::endl; LOG_INFO("Stop task: %d", stop_id);
task_manager.addTask(mqtt_json); task_manager.addTask(mqtt_json);
sendTaskResponse(seqNo, 200, "Task stop command received"); sendTaskResponse(seqNo, 200, "Task stop command received");
} }
catch (const std::exception& e) catch (const std::exception& e)
{ {
std::cerr << "Error processing stop command: " << e.what() << std::endl; LOG_ERROR("Error processing stop command: %s", e.what());
sendTaskResponse(seqNo, 400, "Failed to process stop command"); sendTaskResponse(seqNo, 400, "Failed to process stop command");
} }
} }
@ -344,11 +344,11 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message
} }
else if (root.isMember("type") && root["type"].asString() == "noReply") else if (root.isMember("type") && root["type"].asString() == "noReply")
{ {
std::cout << "NoReply message received" << std::endl; LOG_INFO("NoReply message received");
} }
else else
{ {
std::cout << "Unknown message type" << std::endl; LOG_INFO("Unknown message type");
} }
delete[] buffer; delete[] buffer;
@ -364,14 +364,14 @@ void try_subscribe_task_topic()
if (subscribed.exchange(true)) return; // 已经订阅过了 if (subscribed.exchange(true)) return; // 已经订阅过了
std::cout << "[TASK] subscribe MQTT topic: " << mqtt_topic_sub_task << std::endl; LOG_INFO("[TASK] subscribe MQTT topic: %s", mqtt_topic_sub_task.c_str());
mqtt_manager.subscribe(mqtt_topic_sub_task, 0); mqtt_manager.subscribe(mqtt_topic_sub_task, 0);
} }
// 周期性上报任务状态到MQTT（200ms间隔持续上报） // 周期性上报任务状态到MQTT（200ms间隔持续上报）
void reportTaskStatusToMqtt() void reportTaskStatusToMqtt()
{ {
std::cout << "DEBUG: Status report thread started" << std::endl; LOG_DEBUG("Status report thread started");
while (rclcpp::ok() && !signal_received) while (rclcpp::ok() && !signal_received)
{ {
@ -396,7 +396,7 @@ void reportTaskStatusToMqtt()
} }
} }
std::cout << "DEBUG: Status report thread exiting" << std::endl; LOG_DEBUG("Status report thread exiting");
} }
// ROS2节点类 // ROS2节点类
@ -405,7 +405,7 @@ class TaskManagerNode : public rclcpp::Node
public: public:
TaskManagerNode(string name) : Node(name) TaskManagerNode(string name) : Node(name)
{ {
RCLCPP_INFO(this->get_logger(), "Node %s started.", name.c_str()); LOG_INFO("Node %s started.", name.c_str());
msg_publisher_ = this->create_publisher<sweeper_interfaces::msg::Task>("task_message/download", 10); msg_publisher_ = this->create_publisher<sweeper_interfaces::msg::Task>("task_message/download", 10);
task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>( task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>(
"task_message/upload", 10, std::bind(&TaskManagerNode::task_callback, this, std::placeholders::_1)); "task_message/upload", 10, std::bind(&TaskManagerNode::task_callback, this, std::placeholders::_1));
@ -432,8 +432,8 @@ class TaskManagerNode : public rclcpp::Node
pos = mqtt_topic_push_status.find("{vid}"); pos = mqtt_topic_push_status.find("{vid}");
if (pos != std::string::npos) mqtt_topic_push_status.replace(pos, 5, vid); if (pos != std::string::npos) mqtt_topic_push_status.replace(pos, 5, vid);
RCLCPP_INFO(this->get_logger(), "[TASK] identity ready VID=%s => sub=%s pub=%s", vid.c_str(), LOG_INFO("[TASK] identity ready VID=%s => sub=%s pub=%s", vid.c_str(), mqtt_topic_sub_task.c_str(),
mqtt_topic_sub_task.c_str(), mqtt_topic_push_status.c_str()); mqtt_topic_push_status.c_str());
try_subscribe_task_topic(); try_subscribe_task_topic();
}); });
@ -448,7 +448,7 @@ class TaskManagerNode : public rclcpp::Node
if (msg.task_status != last_status) if (msg.task_status != last_status)
{ {
msg_publisher_->publish(msg); msg_publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(), "Task status published: %d", msg.task_status); LOG_INFO("Task status published: %d", msg.task_status);
} }
last_status = msg.task_status; last_status = msg.task_status;
@ -462,7 +462,7 @@ class TaskManagerNode : public rclcpp::Node
} }
else else
{ {
RCLCPP_WARN(this->get_logger(), "Invalid task status: %d", msg->task_status); LOG_WARN("Invalid task status: %d", msg->task_status);
} }
} }
@ -477,6 +477,9 @@ int main(int argc, char** argv)
{ {
signal(SIGINT, signalHandler); signal(SIGINT, signalHandler);
// 初始化日志系统
logger::Logger::Init("task_manager", "./log");
// 默认配置路径 // 默认配置路径
std::string config_path = "config.json"; std::string config_path = "config.json";
@ -491,11 +494,11 @@ int main(int argc, char** argv)
} }
} }
std::cout << "[TASK] Using config: " << config_path << std::endl; LOG_INFO("[TASK] Using config: %s", config_path.c_str());
if (!loadConfig(config_path)) if (!loadConfig(config_path))
{ {
std::cerr << "Failed to load configuration." << std::endl; LOG_ERROR("Failed to load configuration.");
return 1; return 1;
} }
@ -503,7 +506,7 @@ int main(int argc, char** argv)
string client_id = generate_mqtt_client_id(); string client_id = generate_mqtt_client_id();
if (!mqtt_manager.init(config.mqtt_address, client_id, config.mqtt_username, config.mqtt_password)) if (!mqtt_manager.init(config.mqtt_address, client_id, config.mqtt_username, config.mqtt_password))
{ {
std::cerr << "Failed to initialize MQTT manager." << std::endl; LOG_ERROR("Failed to initialize MQTT manager.");
return 1; return 1;
} }
@ -516,7 +519,7 @@ int main(int argc, char** argv)
mqtt_manager.setConnRestoredCallback( mqtt_manager.setConnRestoredCallback(
[](const char* cause) [](const char* cause)
{ {
std::cout << "[TASK] MQTT reconnected: " << cause << std::endl; LOG_INFO("[TASK] MQTT reconnected: %s", cause);
subscribed.store(false); subscribed.store(false);
try_subscribe_task_topic(); try_subscribe_task_topic();
}); });
@ -530,7 +533,7 @@ int main(int argc, char** argv)
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
auto node = std::make_shared<TaskManagerNode>("task_manager_node"); auto node = std::make_shared<TaskManagerNode>("task_manager_node");
std::cout << "Starting status report thread..." << std::endl; LOG_INFO("Starting status report thread...");
status_report_thread = std::thread(reportTaskStatusToMqtt); status_report_thread = std::thread(reportTaskStatusToMqtt);
rclcpp::executors::SingleThreadedExecutor executor; rclcpp::executors::SingleThreadedExecutor executor;
@ -542,7 +545,7 @@ int main(int argc, char** argv)
std::this_thread::sleep_for(std::chrono::milliseconds(100)); std::this_thread::sleep_for(std::chrono::milliseconds(100));
} }
std::cout << "Shutting down components..." << std::endl; LOG_INFO("Shutting down components...");
task_manager.stop(); task_manager.stop();
mqtt_manager.stop(); mqtt_manager.stop();
@ -551,5 +554,8 @@ int main(int argc, char** argv)
status_report_thread.join(); status_report_thread.join();
} }
// 关闭日志系统
logger::Logger::Shutdown();
return 0; return 0;
} }

11
src/control/ctrl_arbiter/CMakeLists.txt
View File

@ -10,21 +10,20 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(logger REQUIRED)
file(GLOB SRC_FILES "src/*.cpp") file(GLOB SRC_FILES "src/*.cpp")
add_executable(ctrl_arbiter_node ${SRC_FILES}) add_executable(ctrl_arbiter_node ${SRC_FILES})
ament_target_dependencies(ctrl_arbiter_node ament_target_dependencies(
ctrl_arbiter_node
rclcpp rclcpp
sweeper_interfaces sweeper_interfaces
std_msgs std_msgs
) logger)
install(TARGETS install(TARGETS ctrl_arbiter_node DESTINATION lib/${PROJECT_NAME})
ctrl_arbiter_node
DESTINATION lib/${PROJECT_NAME}
)
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)

88
src/control/ctrl_arbiter/src/ctrl_arbiter.cpp
View File

@ -1,57 +1,54 @@
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include <chrono> #include <chrono>
#include <mutex> #include <mutex>
#include "logger/logger.h"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
using namespace std::chrono_literals; using namespace std::chrono_literals;
namespace sweeperMsg = sweeper_interfaces::msg; namespace sweeperMsg = sweeper_interfaces::msg;
class ArbitrationNode : public rclcpp::Node class ArbitrationNode : public rclcpp::Node
{ {
public: public:
ArbitrationNode() ArbitrationNode() : Node("control_arbitrator")
: Node("control_arbitrator")
{ {
timeout_ms_ = 200; // 200毫秒超时阈值，可调整 timeout_ms_ = 200; // 200毫秒超时阈值，可调整
publisher_ = this->create_publisher<sweeperMsg::McCtrl>("/mc_ctrl", 10); publisher_ = this->create_publisher<sweeperMsg::McCtrl>("/mc_ctrl", 10);
sub_radio_ = this->create_subscription<sweeperMsg::McCtrl>( sub_radio_ = this->create_subscription<sweeperMsg::McCtrl>("/radio_mc_ctrl", 10,
"/radio_mc_ctrl", 10, [this](const sweeperMsg::McCtrl::SharedPtr msg)
[this](const sweeperMsg::McCtrl::SharedPtr msg) {
{ std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard<std::mutex> lock(mutex_); radio_msg_ = *msg;
radio_msg_ = *msg; radio_last_time_ = this->now();
radio_last_time_ = this->now(); radio_valid_ = true;
radio_valid_ = true; });
});
sub_remote_ = this->create_subscription<sweeperMsg::McCtrl>( sub_remote_ = this->create_subscription<sweeperMsg::McCtrl>("/remote_mc_ctrl", 10,
"/remote_mc_ctrl", 10, [this](const sweeperMsg::McCtrl::SharedPtr msg)
[this](const sweeperMsg::McCtrl::SharedPtr msg) {
{ std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard<std::mutex> lock(mutex_); remote_msg_ = *msg;
remote_msg_ = *msg; remote_last_time_ = this->now();
remote_last_time_ = this->now(); remote_valid_ = true;
remote_valid_ = true; });
});
sub_auto_ = this->create_subscription<sweeperMsg::McCtrl>( sub_auto_ = this->create_subscription<sweeperMsg::McCtrl>("/auto_mc_ctrl", 10,
"/auto_mc_ctrl", 10, [this](const sweeperMsg::McCtrl::SharedPtr msg)
[this](const sweeperMsg::McCtrl::SharedPtr msg) {
{ std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard<std::mutex> lock(mutex_); auto_msg_ = *msg;
auto_msg_ = *msg; auto_last_time_ = this->now();
auto_last_time_ = this->now(); auto_valid_ = true;
auto_valid_ = true; });
});
timer_ = this->create_wall_timer(20ms, [this]() timer_ = this->create_wall_timer(20ms, [this]() { this->arbitrateAndPublish(); });
{ this->arbitrateAndPublish(); }); LOG_INFO("ArbitrationNode started, waiting for control sources...");
RCLCPP_INFO(this->get_logger(), "ArbitrationNode started, waiting for control sources...");
} }
private: private:
void arbitrateAndPublish() void arbitrateAndPublish()
{ {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
@ -61,21 +58,21 @@ private:
if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000) if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000)
{ {
publisher_->publish(radio_msg_); publisher_->publish(radio_msg_);
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using RADIO control"); LOG_INFO_THROTTLE(2000, "[ARBITER] Using RADIO control");
return; return;
} }
if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000) if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000)
{ {
publisher_->publish(remote_msg_); publisher_->publish(remote_msg_);
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using REMOTE control"); LOG_INFO_THROTTLE(2000, "[ARBITER] Using REMOTE control");
return; return;
} }
if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000) if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000)
{ {
publisher_->publish(auto_msg_); publisher_->publish(auto_msg_);
RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using AUTO control"); LOG_INFO_THROTTLE(2000, "[ARBITER] Using AUTO control");
return; return;
} }
@ -90,8 +87,7 @@ private:
safe_msg.sweep = false; safe_msg.sweep = false;
publisher_->publish(safe_msg); publisher_->publish(safe_msg);
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, LOG_WARN_THROTTLE(1000, "[ARBITER] All sources timeout, publishing FAILSAFE control");
"[ARBITER] All sources timeout, publishing FAILSAFE control");
} }
rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr publisher_; rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr publisher_;
@ -109,11 +105,17 @@ private:
int timeout_ms_; int timeout_ms_;
}; };
int main(int argc, char *argv[]) int main(int argc, char* argv[])
{ {
// 初始化日志系统
logger::Logger::Init("ctrl_arbiter", "./log");
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
auto node = std::make_shared<ArbitrationNode>(); auto node = std::make_shared<ArbitrationNode>();
rclcpp::spin(node); rclcpp::spin(node);
rclcpp::shutdown(); rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
return 0; return 0;
} }

17
src/control/radio_ctrl/CMakeLists.txt
View File

@ -14,13 +14,19 @@ find_package(rclcpp REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(ament_index_cpp REQUIRED) find_package(ament_index_cpp REQUIRED)
find_package(logger REQUIRED)
add_executable(radio_ctrl_node src/radio_ctrl.cpp src/uart_handler.cpp) add_executable(radio_ctrl_node src/radio_ctrl.cpp src/uart_handler.cpp)
target_include_directories( target_include_directories(radio_ctrl_node PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
radio_ctrl_node PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
ament_target_dependencies(radio_ctrl_node ament_index_cpp rclcpp std_msgs sweeper_interfaces) ament_target_dependencies(
radio_ctrl_node
ament_index_cpp
rclcpp
std_msgs
sweeper_interfaces
logger)
install(TARGETS radio_ctrl_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS radio_ctrl_node DESTINATION lib/${PROJECT_NAME})
@ -28,10 +34,7 @@ install(TARGETS radio_ctrl_node DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
# 安装头文件到正确路径 # 安装头文件到正确路径
install( install(DIRECTORY include/ DESTINATION include)
DIRECTORY include/
DESTINATION include
)
# 导出头文件路径，使其他包能发现 # 导出头文件路径，使其他包能发现
ament_export_include_directories(include) ament_export_include_directories(include)

7
src/control/radio_ctrl/include/radio_ctrl/get_config.h
View File

@ -1,8 +1,9 @@
#pragma once #pragma once
#include <fstream> #include <fstream>
#include <string>
#include <iostream> #include <iostream>
#include <string>
#include "ament_index_cpp/get_package_share_directory.hpp" #include "ament_index_cpp/get_package_share_directory.hpp"
#include "nlohmann/json.hpp" #include "nlohmann/json.hpp"
@ -16,7 +17,7 @@ struct RmoCtlConfig
double eps_angle_max; double eps_angle_max;
}; };
bool load_config(RmoCtlConfig &config) bool load_config(RmoCtlConfig& config)
{ {
try try
{ {
@ -39,7 +40,7 @@ bool load_config(RmoCtlConfig &config)
return true; return true;
} }
catch (const std::exception &e) catch (const std::exception& e)
{ {
std::cerr << "Error parsing config: " << e.what() << std::endl; std::cerr << "Error parsing config: " << e.what() << std::endl;
return false; return false;

45
src/control/radio_ctrl/include/radio_ctrl/uart_handler.h
View File

@ -1,31 +1,32 @@
#ifndef UART_HANDLER_H #ifndef UART_HANDLER_H
#define UART_HANDLER_H #define UART_HANDLER_H
#include <thread> #include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <algorithm>
#include <atomic> #include <atomic>
#include <string>
#include <cstring> #include <cstring>
#include <iostream> #include <iostream>
#include <fcntl.h> #include <string>
#include <unistd.h> #include <thread>
#include <errno.h>
#include <termios.h>
#include <vector> #include <vector>
#include <algorithm>
class UartHandler class UartHandler
{ {
public: public:
UartHandler(const std::string &device, int baudrate = 100000); UartHandler(const std::string& device, int baudrate = 100000);
~UartHandler(); ~UartHandler();
bool open_serial(); // 打开串口 bool open_serial(); // 打开串口
void start_reading(); // 启动读取数据的线程 void start_reading(); // 启动读取数据的线程
void stop_reading(); // 停止读取数据的线程 void stop_reading(); // 停止读取数据的线程
int get_channel_value(int channel); // 获取指定通道的数据 int get_channel_value(int channel); // 获取指定通道的数据
bool get_data_safe(); // 获取数据安全性 bool get_data_safe(); // 获取数据安全性
private: private:
std::string serial_device; std::string serial_device;
int baudrate; int baudrate;
int fd; int fd;
@ -44,13 +45,13 @@ private:
SBUS_SIGNAL_OK SBUS_SIGNAL_OK
}; };
std::atomic<bool> reading; // 控制读取线程的状态 std::atomic<bool> reading; // 控制读取线程的状态
std::thread read_thread; // 读取数据的线程 std::thread read_thread; // 读取数据的线程
void read_loop(); // 持续读取串口数据 void read_loop(); // 持续读取串口数据
void parse_data(std::vector<uint8_t> &buffer); // 解析数据 void parse_data(std::vector<uint8_t>& buffer); // 解析数据
int sbus_parse(); // SBUS 数据解析 int sbus_parse(); // SBUS 数据解析
void print_hex(uint8_t *buf, int len); // 打印数据（调试用） void print_hex(uint8_t* buf, int len); // 打印数据（调试用）
}; };
#endif // UART_HANDLER_H #endif // UART_HANDLER_H

3
src/control/radio_ctrl/package.xml
View File

@ -13,6 +13,7 @@
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>ament_index_cpp</depend> <depend>ament_index_cpp</depend>
<depend>sweeper_interfaces</depend> <depend>sweeper_interfaces</depend>
<depend>logger</depend>
<build_depend>ament_cmake</build_depend> <build_depend>ament_cmake</build_depend>
<exec_depend>ament_cmake</exec_depend> <exec_depend>ament_cmake</exec_depend>
@ -23,4 +24,4 @@
<export> <export>
<build_type>ament_cmake</build_type> <build_type>ament_cmake</build_type>
</export> </export>
</package> </package>

97
src/control/radio_ctrl/src/radio_ctrl.cpp
View File

@ -1,29 +1,31 @@
#include "radio_ctrl/uart_handler.h"
#include "radio_ctrl/get_config.h"
#include <iostream>
#include <algorithm> #include <algorithm>
#include <iostream>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include "logger/logger.h"
#include "radio_ctrl/get_config.h"
#include "radio_ctrl/uart_handler.h"
#include "sweeper_interfaces/msg/can_frame.hpp" #include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
namespace sweeperMsg = sweeper_interfaces::msg; namespace sweeperMsg = sweeper_interfaces::msg;
constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f; constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f;
constexpr float GEAR_RATIO = 16.5f; constexpr float GEAR_RATIO = 16.5f;
constexpr float DELTA_T = 0.02f; // 20ms constexpr float DELTA_T = 0.02f; // 20ms
constexpr int PRINT_INTERVAL = 25; // 打印间隔：每25次回调打印一次（25*20ms=500ms，即2Hz） constexpr int PRINT_INTERVAL = 25; // 打印间隔：每25次回调打印一次（25*20ms=500ms，即2Hz）
class SBUSNode : public rclcpp::Node class SBUSNode : public rclcpp::Node
{ {
public: public:
SBUSNode() : Node("sbus_node"), current_feedback_angle(0.0f), print_counter(0) // 初始化计数器 SBUSNode() : Node("sbus_node"), current_feedback_angle(0.0f), print_counter(0) // 初始化计数器
{ {
if (load_config(config)) if (load_config(config))
{ {
std::cout << "Serial Port: " << config.serial_port << "\n"; LOG_INFO("Serial Port: %s", config.serial_port.c_str());
std::cout << "Baudrate: " << config.baudrate << "\n"; LOG_INFO("Baudrate: %d", config.baudrate);
std::cout << "MCU RPM Max: " << config.mcu_rpm_max << "\n"; LOG_INFO("MCU RPM Max: %d", config.mcu_rpm_max);
std::cout << "EPS Angle Max: " << config.eps_angle_max << "\n"; LOG_INFO("EPS Angle Max: %f", config.eps_angle_max);
} }
// 发布控制指令消息的发布器 // 发布控制指令消息的发布器
@ -31,22 +33,19 @@ public:
// 订阅CAN反馈的回调函数 // 订阅CAN反馈的回调函数
can_sub_ = this->create_subscription<sweeperMsg::CanFrame>( can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
"can_data", 10, "can_data", 10, std::bind(&SBUSNode::can_callback, this, std::placeholders::_1));
std::bind(&SBUSNode::can_callback, this, std::placeholders::_1));
// 初始化串口读取（读取遥控器数据） // 初始化串口读取（读取遥控器数据）
uart_handler_ = std::make_shared<UartHandler>(config.serial_port, config.baudrate); uart_handler_ = std::make_shared<UartHandler>(config.serial_port, config.baudrate);
if (!uart_handler_->open_serial()) if (!uart_handler_->open_serial())
{ {
RCLCPP_ERROR(this->get_logger(), "Failed to open serial port!"); LOG_ERROR("Failed to open serial port!");
return; return;
} }
uart_handler_->start_reading(); // 启动串口后台读线程 uart_handler_->start_reading(); // 启动串口后台读线程
// 创建周期定时器（每20ms回调一次控制逻辑，50Hz） // 创建周期定时器（每20ms回调一次控制逻辑，50Hz）
timer_ = this->create_wall_timer( timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&SBUSNode::timer_callback, this));
std::chrono::milliseconds(20),
std::bind(&SBUSNode::timer_callback, this));
} }
~SBUSNode() ~SBUSNode()
@ -57,44 +56,44 @@ public:
} }
} }
private: private:
// 定时器回调函数，用于周期性发布控制指令 // 定时器回调函数，用于周期性发布控制指令
void timer_callback() void timer_callback()
{ {
static int MCU_RPM_MAX = config.mcu_rpm_max; static int MCU_RPM_MAX = config.mcu_rpm_max;
static float EPS_ANGLE_MAX = config.eps_angle_max; static float EPS_ANGLE_MAX = config.eps_angle_max;
bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性 bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性
auto msg = sweeperMsg::McCtrl(); // 控制消息对象 auto msg = sweeperMsg::McCtrl(); // 控制消息对象
uint16_t ch_data[10]; // 各通道遥控数据 uint16_t ch_data[10]; // 各通道遥控数据
if (data_safe) // 数据安全，进行数据解析并发布 if (data_safe) // 数据安全，进行数据解析并发布
{ {
// 赋值与打印（注释掉原有的高频打印） // 赋值与打印（注释掉原有的高频打印）
for (int i = 0; i < 10; ++i) for (int i = 0; i < 10; ++i)
{ {
ch_data[i] = uart_handler_->get_channel_value(i); ch_data[i] = uart_handler_->get_channel_value(i);
// printf("ch[%d]:%d ", i, ch_data[i]); // 高频打印已注释 // LOG_DEBUG("ch[%d]:%d ", i, ch_data[i]); // 高频打印已注释
} }
// printf("\n"); // 高频打印已注释 // LOG_DEBUG("\n"); // 高频打印已注释
uint16_t gear = ch_data[7]; // 挡位 最下1792，中间992，最上192 uint16_t gear = ch_data[7]; // 挡位 最下1792，中间992，最上192
uint16_t sweep = ch_data[6]; // 清扫 最上192，最下1792 uint16_t sweep = ch_data[6]; // 清扫 最上192，最下1792
int16_t speed = ch_data[2] - 992; // 左边摇杆，最下192，中间992，最上1792 -> [-800,800] int16_t speed = ch_data[2] - 992; // 左边摇杆，最下192，中间992，最上1792 -> [-800,800]
// 挡位选择 // 挡位选择
if (gear == 192) if (gear == 192)
{ {
msg.gear = 2; // D挡 msg.gear = 2; // D挡
} }
else if (gear == 992) else if (gear == 992)
{ {
msg.gear = 0; // N挡 msg.gear = 0; // N挡
} }
else if (gear == 1792) else if (gear == 1792)
{ {
msg.gear = 1; // R挡 msg.gear = 1; // R挡
} }
// 油门 / 刹车逻辑 // 油门 / 刹车逻辑
@ -130,10 +129,8 @@ private:
float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO; float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
// 限制电机转速到 [120, 1500] 范围，防止过小/过大 // 限制电机转速到 [120, 1500] 范围，防止过小/过大
uint16_t can_speed = std::clamp( uint16_t can_speed =
static_cast<uint16_t>(motor_rpm), std::clamp(static_cast<uint16_t>(motor_rpm), static_cast<uint16_t>(120), static_cast<uint16_t>(1500));
static_cast<uint16_t>(120),
static_cast<uint16_t>(1500));
msg.angle = target_angle; msg.angle = target_angle;
msg.angle_speed = can_speed; msg.angle_speed = can_speed;
@ -144,25 +141,25 @@ private:
// 降低打印频率：每 PRINT_INTERVAL 次回调打印一次 // 降低打印频率：每 PRINT_INTERVAL 次回调打印一次
// if (++print_counter >= PRINT_INTERVAL) // if (++print_counter >= PRINT_INTERVAL)
// { // {
// std::cout << "\n=====================================" // LOG_INFO("\n=====================================")
// << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车") // << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车")
// << "\n 挡位: "; // << "\n 挡位: ";
// switch (msg.gear) // switch (msg.gear)
// { // {
// case 0: // case 0:
// std::cout << "空挡"; // LOG_INFO("空挡");
// break; // break;
// case 2: // case 2:
// std::cout << "前进挡"; // LOG_INFO("前进挡");
// break; // break;
// case 1: // case 1:
// std::cout << "后退挡"; // LOG_INFO("后退挡");
// break; // break;
// default: // default:
// std::cout << "未知挡位(" << static_cast<int>(msg.gear) << ")"; // LOG_INFO("未知挡位(%d)", static_cast<int>(msg.gear));
// break; // break;
// } // }
// std::cout << "\n 行走电机转速: " << static_cast<int>(msg.rpm) << " RPM" // LOG_INFO("\n 行走电机转速: %d RPM", static_cast<int>(msg.rpm));
// << "\n 轮端转向角度: " << msg.angle << "°" // << "\n 轮端转向角度: " << msg.angle << "°"
// << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫") // << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫")
// << "\n=====================================" << std::endl; // << "\n=====================================" << std::endl;
@ -174,7 +171,7 @@ private:
// 低频率打印等待信息（每2次回调打印一次，避免刷屏） // 低频率打印等待信息（每2次回调打印一次，避免刷屏）
if (++print_counter >= PRINT_INTERVAL / 2) if (++print_counter >= PRINT_INTERVAL / 2)
{ {
RCLCPP_INFO(this->get_logger(), "Waiting for radio control data..."); LOG_INFO("Waiting for radio control data...");
print_counter = 0; print_counter = 0;
} }
return; return;
@ -201,15 +198,21 @@ private:
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
std::shared_ptr<UartHandler> uart_handler_; std::shared_ptr<UartHandler> uart_handler_;
float current_feedback_angle; // 当前反馈角度（从CAN中读取） float current_feedback_angle; // 当前反馈角度（从CAN中读取）
RmoCtlConfig config; RmoCtlConfig config;
int print_counter; // 打印计数器（新增） int print_counter; // 打印计数器（新增）
}; };
int main(int argc, char *argv[]) int main(int argc, char* argv[])
{ {
// 初始化日志系统
logger::Logger::Init("radio_ctrl", "./log");
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<SBUSNode>()); // 启动节点 rclcpp::spin(std::make_shared<SBUSNode>()); // 启动节点
rclcpp::shutdown(); rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
return 0; return 0;
} }

40
src/control/radio_ctrl/src/uart_handler.cpp
View File

@ -1,14 +1,18 @@
#include "radio_ctrl/uart_handler.h" #include "radio_ctrl/uart_handler.h"
#include <errno.h>
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <termios.h> #include <termios.h>
#include <string.h> #include <unistd.h>
#include <stdio.h>
#include <errno.h>
#include <stdint.h>
#include <iostream> #include <iostream>
#include "logger/logger.h"
#ifndef BOTHER #ifndef BOTHER
#define BOTHER 0010000 #define BOTHER 0010000
#endif #endif
@ -29,10 +33,10 @@ struct termios2
#define TCSETS2 _IOW('T', 0x2B, struct termios2) #define TCSETS2 _IOW('T', 0x2B, struct termios2)
// 构造函数 // 构造函数
UartHandler::UartHandler(const std::string &device, int baudrate) UartHandler::UartHandler(const std::string& device, int baudrate)
: serial_device(device), baudrate(baudrate), fd(-1), failsafe_status(SBUS_SIGNAL_LOST) : serial_device(device), baudrate(baudrate), fd(-1), failsafe_status(SBUS_SIGNAL_LOST)
{ {
std::fill(std::begin(sbus_channels), std::end(sbus_channels), 0); // 初始化通道数据 std::fill(std::begin(sbus_channels), std::end(sbus_channels), 0); // 初始化通道数据
} }
// 析构函数，关闭串口 // 析构函数，关闭串口
@ -63,8 +67,7 @@ bool UartHandler::open_serial()
return false; return false;
} }
tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
IGNCR | ICRNL | IXON);
tio.c_iflag |= (INPCK | IGNPAR); tio.c_iflag |= (INPCK | IGNPAR);
tio.c_oflag &= ~OPOST; tio.c_oflag &= ~OPOST;
tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
@ -84,7 +87,7 @@ bool UartHandler::open_serial()
return false; return false;
} }
// std::cout << "Serial port " << serial_device << " opened and configured.\n"; // LOG_INFO("Serial port %s opened and configured.", serial_device.c_str());
return true; return true;
} }
@ -130,16 +133,16 @@ void UartHandler::read_loop()
} }
} }
void UartHandler::print_hex(uint8_t *buf, int len) void UartHandler::print_hex(uint8_t* buf, int len)
{ {
for (int i = 0; i < len; ++i) for (int i = 0; i < len; ++i)
{ {
printf("%02X ", buf[i]); LOG_DEBUG("%02X ", buf[i]);
} }
printf("\n"); LOG_DEBUG("\n");
} }
void UartHandler::parse_data(std::vector<uint8_t> &buffer) void UartHandler::parse_data(std::vector<uint8_t>& buffer)
{ {
while (buffer.size() >= kSbusFrameLength) while (buffer.size() >= kSbusFrameLength)
{ {
@ -147,7 +150,7 @@ void UartHandler::parse_data(std::vector<uint8_t> &buffer)
auto it = std::find(buffer.begin(), buffer.end(), 0x0F); auto it = std::find(buffer.begin(), buffer.end(), 0x0F);
if (it == buffer.end()) if (it == buffer.end())
{ {
buffer.clear(); // 没找到帧头，清空 buffer buffer.clear(); // 没找到帧头，清空 buffer
failsafe_status = SBUS_SIGNAL_LOST; failsafe_status = SBUS_SIGNAL_LOST;
return; return;
} }
@ -159,8 +162,7 @@ void UartHandler::parse_data(std::vector<uint8_t> &buffer)
if (buffer.size() - index < kSbusFrameLength) if (buffer.size() - index < kSbusFrameLength)
{ {
// 数据不够，等下次再处理 // 数据不够，等下次再处理
if (index > 0) if (index > 0) buffer.erase(buffer.begin(), buffer.begin() + index);
buffer.erase(buffer.begin(), buffer.begin() + index);
return; return;
} }
@ -215,9 +217,9 @@ int UartHandler::get_channel_value(int channel)
{ {
if (channel >= 0 && channel < 8) if (channel >= 0 && channel < 8)
{ {
return sbus_channels[channel]; // 返回指定通道的值 return sbus_channels[channel]; // 返回指定通道的值
} }
return 0; // 如果无效的通道，返回默认值 return 0; // 如果无效的通道，返回默认值
} }
bool UartHandler::get_data_safe() bool UartHandler::get_data_safe()

8
src/control/remote_ctrl/CMakeLists.txt
View File

@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(ament_index_cpp REQUIRED) find_package(ament_index_cpp REQUIRED)
find_package(logger REQUIRED)
# ======== MQTT libs ======== # ======== MQTT libs ========
if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64") if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
@ -41,7 +42,12 @@ target_link_libraries(
ssl ssl
crypto) crypto)
ament_target_dependencies(remote_ctrl_node rclcpp sweeper_interfaces ament_index_cpp) ament_target_dependencies(
remote_ctrl_node
rclcpp
sweeper_interfaces
ament_index_cpp
logger)
# ===== install executable ===== # ===== install executable =====
install(TARGETS remote_ctrl_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS remote_ctrl_node DESTINATION lib/${PROJECT_NAME})

100
src/control/remote_ctrl/include/remote_ctrl/mqtt_client.hpp
View File

@ -1,27 +1,23 @@
#pragma once #pragma once
#include "paho_mqtt_3c/MQTTClient.h"
#include <string>
#include <atomic> #include <atomic>
#include <thread>
#include <mutex>
#include <iostream>
#include <functional>
#include <chrono> #include <chrono>
#include <functional>
#include <iostream>
#include <mutex>
#include <string>
#include <thread>
#include "paho_mqtt_3c/MQTTClient.h"
class MQTTClientWrapper class MQTTClientWrapper
{ {
public: public:
using MessageCallback = using MessageCallback = std::function<void(const std::string&, const std::string&)>;
std::function<void(const std::string &, const std::string &)>; using ConnectedCallback = std::function<void()>;
using ConnectedCallback =
std::function<void()>;
MQTTClientWrapper(const std::string &server_uri, MQTTClientWrapper(const std::string& server_uri, const std::string& client_id, const std::string& username = "",
const std::string &client_id, const std::string& password = "", int reconnect_ms = 3000)
const std::string &username = "",
const std::string &password = "",
int reconnect_ms = 3000)
: server_uri_(server_uri), : server_uri_(server_uri),
client_id_(client_id), client_id_(client_id),
username_(username), username_(username),
@ -30,11 +26,8 @@ public:
connected_(false), connected_(false),
stop_flag_(false) stop_flag_(false)
{ {
int rc = MQTTClient_create(&client_, int rc =
server_uri_.c_str(), MQTTClient_create(&client_, server_uri_.c_str(), client_id_.c_str(), MQTTCLIENT_PERSISTENCE_NONE, nullptr);
client_id_.c_str(),
MQTTCLIENT_PERSISTENCE_NONE,
nullptr);
if (rc != MQTTCLIENT_SUCCESS) if (rc != MQTTCLIENT_SUCCESS)
{ {
@ -44,32 +37,26 @@ public:
} }
MQTTClient_setCallbacks(client_, this, MQTTClient_setCallbacks(client_, this,
connLostCB, // 连接丢失回调 connLostCB, // 连接丢失回调
msgArrivedCB, // 消息回调 msgArrivedCB, // 消息回调
nullptr); nullptr);
// 启动网络循环线程 // 启动网络循环线程
loop_thread_ = std::thread([this]() loop_thread_ = std::thread([this]() { loopFunc(); });
{ loopFunc(); });
} }
~MQTTClientWrapper() ~MQTTClientWrapper() { stop(); }
{
stop();
}
void stop() void stop()
{ {
stop_flag_ = true; stop_flag_ = true;
if (loop_thread_.joinable()) if (loop_thread_.joinable()) loop_thread_.join();
loop_thread_.join();
std::lock_guard<std::mutex> lk(mtx_); std::lock_guard<std::mutex> lk(mtx_);
if (client_) if (client_)
{ {
if (connected_) if (connected_) MQTTClient_disconnect(client_, 2000);
MQTTClient_disconnect(client_, 2000);
MQTTClient_destroy(&client_); MQTTClient_destroy(&client_);
} }
@ -88,13 +75,11 @@ public:
connected_cb_ = std::move(cb); connected_cb_ = std::move(cb);
} }
bool subscribe(const std::string &topic, int qos = 0) bool subscribe(const std::string& topic, int qos = 0)
{ {
std::lock_guard<std::mutex> lk(mtx_); std::lock_guard<std::mutex> lk(mtx_);
if (!client_ || if (!client_ || !connected_.load(std::memory_order_acquire) || !ready_.load(std::memory_order_acquire))
!connected_.load(std::memory_order_acquire) ||
!ready_.load(std::memory_order_acquire))
return false; return false;
int rc = MQTTClient_subscribe(client_, topic.c_str(), qos); int rc = MQTTClient_subscribe(client_, topic.c_str(), qos);
@ -108,19 +93,15 @@ public:
return true; return true;
} }
bool publish(const std::string &topic, bool publish(const std::string& topic, const std::string& payload, int qos = 0)
const std::string &payload,
int qos = 0)
{ {
std::lock_guard<std::mutex> lk(mtx_); std::lock_guard<std::mutex> lk(mtx_);
if (!client_ || if (!client_ || !connected_.load(std::memory_order_acquire) || !ready_.load(std::memory_order_acquire))
!connected_.load(std::memory_order_acquire) ||
!ready_.load(std::memory_order_acquire))
return false; return false;
MQTTClient_message msg = MQTTClient_message_initializer; MQTTClient_message msg = MQTTClient_message_initializer;
msg.payload = (void *)payload.data(); msg.payload = (void*)payload.data();
msg.payloadlen = payload.size(); msg.payloadlen = payload.size();
msg.qos = qos; msg.qos = qos;
@ -137,16 +118,12 @@ public:
return true; return true;
} }
bool connect() bool connect() { return doConnect(); }
{
return doConnect();
}
private: private:
bool doConnect() bool doConnect()
{ {
if (!client_) if (!client_) return false;
return false;
MQTTClient_connectOptions opts = MQTTClient_connectOptions_initializer; MQTTClient_connectOptions opts = MQTTClient_connectOptions_initializer;
opts.keepAliveInterval = 15; opts.keepAliveInterval = 15;
@ -175,24 +152,22 @@ private:
return false; return false;
} }
static void connLostCB(void *ctx, char *cause) static void connLostCB(void* ctx, char* cause)
{ {
auto self = static_cast<MQTTClientWrapper *>(ctx); auto self = static_cast<MQTTClientWrapper*>(ctx);
self->connected_.store(false, std::memory_order_release); self->connected_.store(false, std::memory_order_release);
self->ready_.store(false, std::memory_order_release); self->ready_.store(false, std::memory_order_release);
std::cerr << "[MQTT] lost connection: " std::cerr << "[MQTT] lost connection: " << (cause ? cause : "unknown") << std::endl;
<< (cause ? cause : "unknown") << std::endl;
} }
static int msgArrivedCB(void *ctx, char *topic, static int msgArrivedCB(void* ctx, char* topic, int topicLen, MQTTClient_message* message)
int topicLen, MQTTClient_message *message)
{ {
auto self = static_cast<MQTTClientWrapper *>(ctx); auto self = static_cast<MQTTClientWrapper*>(ctx);
std::string t(topic, topicLen > 0 ? topicLen : strlen(topic)); std::string t(topic, topicLen > 0 ? topicLen : strlen(topic));
std::string payload((char *)message->payload, message->payloadlen); std::string payload((char*)message->payload, message->payloadlen);
if (self->msg_cb_) if (self->msg_cb_)
{ {
@ -235,8 +210,7 @@ private:
// ⭐ 在 ready 状态触发 connected callback // ⭐ 在 ready 状态触发 connected callback
if (need_callback_.exchange(false)) if (need_callback_.exchange(false))
{ {
if (connected_cb_) if (connected_cb_) connected_cb_();
connected_cb_();
} }
} }
} }
@ -246,7 +220,7 @@ private:
} }
} }
private: private:
MQTTClient client_{nullptr}; MQTTClient client_{nullptr};
std::string server_uri_, client_id_; std::string server_uri_, client_id_;
std::string username_, password_; std::string username_, password_;
@ -261,6 +235,6 @@ private:
MessageCallback msg_cb_; MessageCallback msg_cb_;
ConnectedCallback connected_cb_; ConnectedCallback connected_cb_;
std::atomic<bool> ready_{false}; // 是否真正进入可用状态 std::atomic<bool> ready_{false}; // 是否真正进入可用状态
std::atomic<bool> need_callback_{false}; std::atomic<bool> need_callback_{false};
}; };

7
src/control/remote_ctrl/package.xml
View File

@ -4,9 +4,7 @@
<name>remote_ctrl</name> <name>remote_ctrl</name>
<version>0.1.0</version> <version>0.1.0</version>
<description> <description> Remote control node using MQTT to receive vehicle control commands. </description>
Remote control node using MQTT to receive vehicle control commands.
</description>
<maintainer email="zxwl@56.com">cxh</maintainer> <maintainer email="zxwl@56.com">cxh</maintainer>
@ -20,6 +18,7 @@
<depend>rclcpp</depend> <depend>rclcpp</depend>
<depend>sweeper_interfaces</depend> <depend>sweeper_interfaces</depend>
<depend>ament_index_cpp</depend> <depend>ament_index_cpp</depend>
<depend>logger</depend>
<!-- Tests --> <!-- Tests -->
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
@ -28,4 +27,4 @@
<export> <export>
<build_type>ament_cmake</build_type> <build_type>ament_cmake</build_type>
</export> </export>
</package> </package>

6
src/control/remote_ctrl/src/config.cpp
View File

@ -4,6 +4,8 @@
#include <iostream> #include <iostream>
#include <nlohmann/json.hpp> #include <nlohmann/json.hpp>
#include "logger/logger.h"
using json = nlohmann::json; using json = nlohmann::json;
bool config::load(const std::string& path, Config& cfg) bool config::load(const std::string& path, Config& cfg)
@ -13,7 +15,7 @@ bool config::load(const std::string& path, Config& cfg)
std::ifstream ifs(path); std::ifstream ifs(path);
if (!ifs.is_open()) if (!ifs.is_open())
{ {
std::cerr << "Failed to open config file: " << path << std::endl; LOG_ERROR("Failed to open config file: %s", path.c_str());
return false; return false;
} }
@ -34,7 +36,7 @@ bool config::load(const std::string& path, Config& cfg)
} }
catch (const std::exception& e) catch (const std::exception& e)
{ {
std::cerr << "Error parsing remote_ctrl config: " << e.what() << std::endl; LOG_ERROR("Error parsing remote_ctrl config: %s", e.what());
return false; return false;
} }
} }

39
src/control/remote_ctrl/src/remote_ctrl_node.cpp
View File

@ -7,6 +7,7 @@
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <sstream> #include <sstream>
#include "logger/logger.h"
#include "nlohmann/json.hpp" #include "nlohmann/json.hpp"
#include "remote_ctrl/config.hpp" #include "remote_ctrl/config.hpp"
#include "remote_ctrl/mqtt_client.hpp" #include "remote_ctrl/mqtt_client.hpp"
@ -83,7 +84,7 @@ class RemoteCtrlNode : public rclcpp::Node
if (!identity_ready_.load()) if (!identity_ready_.load())
{ {
RCLCPP_WARN(this->get_logger(), "[REMOTE] MQTT connected but identity not ready"); LOG_WARN("[REMOTE] MQTT connected but identity not ready");
return; return;
} }
@ -100,14 +101,14 @@ class RemoteCtrlNode : public rclcpp::Node
vid_ = msg->vid; vid_ = msg->vid;
identity_ready_.store(!vid_.empty()); identity_ready_.store(!vid_.empty());
RCLCPP_INFO(this->get_logger(), "[REMOTE] Identity ready: VID=%s", vid_.c_str()); LOG_INFO("[REMOTE] Identity ready: VID=%s", vid_.c_str());
ctrl_topic_ = cfg_.remote_topic_template; ctrl_topic_ = cfg_.remote_topic_template;
if (ctrl_topic_.find("{vid}") == std::string::npos) if (ctrl_topic_.find("{vid}") == std::string::npos)
{ {
RCLCPP_WARN(this->get_logger(), "[REMOTE] remote_topic_template missing '{vid}', template=%s", LOG_WARN("[REMOTE] remote_topic_template missing '{vid}', template=%s",
cfg_.remote_topic_template.c_str()); cfg_.remote_topic_template.c_str());
} }
auto pos = ctrl_topic_.find("{vid}"); auto pos = ctrl_topic_.find("{vid}");
@ -116,8 +117,7 @@ class RemoteCtrlNode : public rclcpp::Node
try_subscribe_ctrl_topic(); try_subscribe_ctrl_topic();
}); });
RCLCPP_INFO(this->get_logger(), "RemoteCtrlNode started mqtt=%s client_id=%s", uri.str().c_str(), LOG_INFO("RemoteCtrlNode started mqtt=%s client_id=%s", uri.str().c_str(), client_id.c_str());
client_id.c_str());
// ===== 看门狗 & 定时发布 ===== // ===== 看门狗 & 定时发布 =====
last_msg_time_ = std::chrono::steady_clock::now(); last_msg_time_ = std::chrono::steady_clock::now();
@ -137,8 +137,7 @@ class RemoteCtrlNode : public rclcpp::Node
// 只在第一次进入“失活”时打印日志 // 只在第一次进入“失活”时打印日志
if (remote_alive_.exchange(false)) if (remote_alive_.exchange(false))
{ {
RCLCPP_WARN(this->get_logger(), LOG_WARN("[REMOTE] control timeout, enter safe-stop");
"[REMOTE] control timeout, enter safe-stop");
} }
} }
}); });
@ -213,7 +212,7 @@ class RemoteCtrlNode : public rclcpp::Node
if (already) return; // 已经订阅过，退出 if (already) return; // 已经订阅过，退出
RCLCPP_INFO(this->get_logger(), "[REMOTE] subscribe ctrl topic: %s", ctrl_topic_.c_str()); LOG_INFO("[REMOTE] subscribe ctrl topic: %s", ctrl_topic_.c_str());
mqtt_->subscribe(ctrl_topic_, 1); mqtt_->subscribe(ctrl_topic_, 1);
} }
@ -227,12 +226,12 @@ class RemoteCtrlNode : public rclcpp::Node
bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed); bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed);
if (was_dead) if (was_dead)
{ {
RCLCPP_INFO(this->get_logger(), "[REMOTE] control recovered"); LOG_INFO("[REMOTE] control recovered");
} }
last_msg_time_ = std::chrono::steady_clock::now(); last_msg_time_ = std::chrono::steady_clock::now();
RCLCPP_DEBUG(this->get_logger(), "[REMOTE] recv MQTT payload: %s", payload.c_str()); LOG_DEBUG("[REMOTE] recv MQTT payload: %s", payload.c_str());
try try
{ {
@ -258,7 +257,7 @@ class RemoteCtrlNode : public rclcpp::Node
remote_authorized_.store(true, std::memory_order_release); remote_authorized_.store(true, std::memory_order_release);
remote_alive_.store(true, std::memory_order_relaxed); remote_alive_.store(true, std::memory_order_relaxed);
RCLCPP_INFO(this->get_logger(), "[REMOTE] authorized (mode=3)"); LOG_INFO("[REMOTE] authorized (mode=3)");
} }
else if (mode == 0) else if (mode == 0)
{ {
@ -269,7 +268,7 @@ class RemoteCtrlNode : public rclcpp::Node
// 明确清空目标状态 // 明确清空目标状态
desired_ = {}; desired_ = {};
RCLCPP_INFO(this->get_logger(), "[REMOTE] deauthorized (mode=0)"); LOG_INFO("[REMOTE] deauthorized (mode=0)");
} }
// mode 指令处理完直接返回 // mode 指令处理完直接返回
@ -357,7 +356,7 @@ class RemoteCtrlNode : public rclcpp::Node
} }
catch (const std::exception& e) catch (const std::exception& e)
{ {
RCLCPP_WARN(this->get_logger(), "[REMOTE] JSON parse error: %s", e.what()); LOG_WARN("[REMOTE] JSON parse error: %s", e.what());
} }
} }
@ -392,6 +391,9 @@ class RemoteCtrlNode : public rclcpp::Node
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
// 初始化日志系统
logger::Logger::Init("remote_ctrl", "./log");
// =============================== // ===============================
// 1. 默认配置路径 + 手动解析 --config // 1. 默认配置路径 + 手动解析 --config
// =============================== // ===============================
@ -414,12 +416,12 @@ int main(int argc, char* argv[])
Config cfg; Config cfg;
if (!config::load(config_path, cfg)) if (!config::load(config_path, cfg))
{ {
RCLCPP_ERROR(rclcpp::get_logger("main"), "Failed to load config file: %s", config_path.c_str()); LOG_ERROR("Failed to load config file: %s", config_path.c_str());
return 1; return 1;
} }
RCLCPP_INFO(rclcpp::get_logger("main"), "Config loaded: broker=%s:%d remote_topic_template=%s", cfg.mqtt_ip.c_str(), LOG_INFO("Config loaded: broker=%s:%d remote_topic_template=%s", cfg.mqtt_ip.c_str(), cfg.mqtt_port,
cfg.mqtt_port, cfg.remote_topic_template.c_str()); cfg.remote_topic_template.c_str());
// =============================== // ===============================
// 3. 把配置传入 Node 构造函数 // 3. 把配置传入 Node 构造函数
@ -430,5 +432,8 @@ int main(int argc, char* argv[])
node.reset(); node.reset();
rclcpp::shutdown(); rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
return 0; return 0;
} }

310
src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp
View File

@ -1,24 +1,23 @@
#include <rclcpp/rclcpp.hpp> #include <pcl/filters/crop_box.h>
#include <sensor_msgs/msg/point_cloud2.hpp> #include <pcl/filters/statistical_outlier_removal.h>
#include <tf2_ros/transform_listener.h> #include <pcl/filters/voxel_grid.h>
#include <tf2_ros/buffer.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <deque>
#include <unordered_map>
#include <mutex>
#include <memory>
#include <chrono>
#include <pcl/point_cloud.h> #include <pcl/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h> #include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <chrono>
#include <deque>
#include <functional>
#include <memory>
#include <mutex>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/int32_multi_array.hpp> #include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/multi_array_dimension.hpp> #include <std_msgs/msg/multi_array_dimension.hpp>
#include <functional> #include <unordered_map>
using sensor_msgs::msg::PointCloud2; using sensor_msgs::msg::PointCloud2;
using std::placeholders::_1; using std::placeholders::_1;
@ -28,14 +27,14 @@ struct CloudFrame
{ {
std::shared_ptr<PointCloud2> cloud; std::shared_ptr<PointCloud2> cloud;
rclcpp::Time stamp; rclcpp::Time stamp;
rclcpp::Time received_time; // 添加接收时间用于延时分析 rclcpp::Time received_time; // 添加接收时间用于延时分析
std::string source; std::string source;
}; };
// 内存池实现 // 内存池实现
class PointCloud2Pool class PointCloud2Pool
{ {
public: public:
std::shared_ptr<PointCloud2> acquire() std::shared_ptr<PointCloud2> acquire()
{ {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
@ -53,8 +52,7 @@ public:
void release(std::shared_ptr<PointCloud2> cloud) void release(std::shared_ptr<PointCloud2> cloud)
{ {
if (!cloud) if (!cloud) return;
return;
cloud->data.clear(); cloud->data.clear();
cloud->width = 0; cloud->width = 0;
@ -62,20 +60,20 @@ public:
cloud->row_step = 0; cloud->row_step = 0;
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
if (pool_.size() < 10) // 限制池大小 if (pool_.size() < 10) // 限制池大小
{ {
pool_.push_back(cloud); pool_.push_back(cloud);
} }
} }
private: private:
std::vector<std::shared_ptr<PointCloud2>> pool_; std::vector<std::shared_ptr<PointCloud2>> pool_;
std::mutex mutex_; std::mutex mutex_;
}; };
class LidarMerger : public rclcpp::Node class LidarMerger : public rclcpp::Node
{ {
public: public:
LidarMerger() : Node("lidar_merger") LidarMerger() : Node("lidar_merger")
{ {
/* ---------- 参数 ---------- */ /* ---------- 参数 ---------- */
@ -83,11 +81,11 @@ public:
rear_topic_ = declare_parameter<std::string>("rear_topic", "/rslidar_points/rear_lidar"); rear_topic_ = declare_parameter<std::string>("rear_topic", "/rslidar_points/rear_lidar");
output_topic_ = declare_parameter<std::string>("output_topic", "/rslidar_points"); output_topic_ = declare_parameter<std::string>("output_topic", "/rslidar_points");
target_frame_ = declare_parameter<std::string>("frame_id", "rslidar"); target_frame_ = declare_parameter<std::string>("frame_id", "rslidar");
queue_size_ = declare_parameter<int>("queue_size", 3); // 减小队列大小 queue_size_ = declare_parameter<int>("queue_size", 3); // 减小队列大小
cache_size_ = declare_parameter<int>("cache_size", 10); // 增加缓存以应对延时 cache_size_ = declare_parameter<int>("cache_size", 10); // 增加缓存以应对延时
time_tolerance_ = declare_parameter<double>("time_tolerance", 0.1); // 放宽时间容差 time_tolerance_ = declare_parameter<double>("time_tolerance", 0.1); // 放宽时间容差
max_wait_time_ = declare_parameter<double>("max_wait_time", 1.0); // 增加到1.0s以适应实际延时 max_wait_time_ = declare_parameter<double>("max_wait_time", 1.0); // 增加到1.0s以适应实际延时
enable_debug_ = declare_parameter<bool>("enable_debug", false); // 默认关闭调试减少日志开销 enable_debug_ = declare_parameter<bool>("enable_debug", false); // 默认关闭调试减少日志开销
// 点云处理参数 // 点云处理参数
filter_min_x_ = declare_parameter<float>("filter_min_x", -10.0f); filter_min_x_ = declare_parameter<float>("filter_min_x", -10.0f);
@ -96,71 +94,57 @@ public:
filter_max_y_ = declare_parameter<float>("filter_max_y", 10.0f); filter_max_y_ = declare_parameter<float>("filter_max_y", 10.0f);
filter_min_z_ = declare_parameter<float>("filter_min_z", -2.0f); filter_min_z_ = declare_parameter<float>("filter_min_z", -2.0f);
filter_max_z_ = declare_parameter<float>("filter_max_z", 2.0f); filter_max_z_ = declare_parameter<float>("filter_max_z", 2.0f);
voxel_size_ = declare_parameter<float>("voxel_size", 0.1f); // 降采样体素大小 voxel_size_ = declare_parameter<float>("voxel_size", 0.1f); // 降采样体素大小
stat_mean_k_ = declare_parameter<int>("stat_mean_k", 30); // 统计离群点去除的k值 stat_mean_k_ = declare_parameter<int>("stat_mean_k", 30); // 统计离群点去除的k值
stat_std_thresh_ = declare_parameter<float>("stat_std_thresh", 1.0f); // 统计离群点去除的标准差阈值 stat_std_thresh_ = declare_parameter<float>("stat_std_thresh", 1.0f); // 统计离群点去除的标准差阈值
grid_size_ = declare_parameter<int>("grid_size", 100); // 栅格大小 grid_size_ = declare_parameter<int>("grid_size", 100); // 栅格大小
grid_range_ = declare_parameter<float>("grid_range", 20.0f); // 栅格范围 grid_range_ = declare_parameter<float>("grid_range", 20.0f); // 栅格范围
enable_print_ = declare_parameter<bool>("enable_print", true); enable_print_ = declare_parameter<bool>("enable_print", true);
// 车身过滤参数 // 车身过滤参数
filter_car_ = declare_parameter<bool>("filter_car", true); // 是否启用车身过滤 filter_car_ = declare_parameter<bool>("filter_car", true); // 是否启用车身过滤
car_length_ = declare_parameter<float>("car_length", 2.3f); // 车长（米） car_length_ = declare_parameter<float>("car_length", 2.3f); // 车长（米）
car_width_ = declare_parameter<float>("car_width", 1.32f); // 车宽（米） car_width_ = declare_parameter<float>("car_width", 1.32f); // 车宽（米）
car_lidar_offset_x_ = declare_parameter<float>("car_lidar_offset_x", 0.0f); // LiDAR在x轴的安装偏移 car_lidar_offset_x_ = declare_parameter<float>("car_lidar_offset_x", 0.0f); // LiDAR在x轴的安装偏移
car_lidar_offset_y_ = declare_parameter<float>("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移 car_lidar_offset_y_ = declare_parameter<float>("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移
// 打印所有参数值（添加到构造函数末尾） // 打印所有参数值（添加到构造函数末尾）
RCLCPP_INFO_STREAM(this->get_logger(), RCLCPP_INFO_STREAM(
"\n\n---------- 点云融合节点参数配置 ----------" this->get_logger(),
<< "\n [输入输出]" "\n\n---------- 点云融合节点参数配置 ----------"
<< "\n 前雷达话题: " << front_topic_ << "\n [输入输出]"
<< "\n 后雷达话题: " << rear_topic_ << "\n 前雷达话题: " << front_topic_ << "\n 后雷达话题: " << rear_topic_
<< "\n 输出话题: " << output_topic_ << "\n 输出话题: " << output_topic_ << "\n 目标坐标系: " << target_frame_ << "\n [同步参数]"
<< "\n 目标坐标系: " << target_frame_ << "\n 队列大小: " << queue_size_ << "\n 缓存大小: " << cache_size_ << "\n 时间容差(s): "
<< "\n [同步参数]" << time_tolerance_ << "\n 最大等待时间(s): " << max_wait_time_ << "\n [调试选项]"
<< "\n 队列大小: " << queue_size_ << "\n 调试模式: " << (enable_debug_ ? "开启" : "关闭") << "\n [点云处理]"
<< "\n 缓存大小: " << cache_size_ << "\n 空间过滤范围(m): [" << filter_min_x_ << ", " << filter_max_x_ << "] x [" << filter_min_y_
<< "\n 时间容差(s): " << time_tolerance_ << ", " << filter_max_y_ << "] x [" << filter_min_z_ << ", " << filter_max_z_ << "]"
<< "\n 最大等待时间(s): " << max_wait_time_ << "\n 体素大小(m): " << voxel_size_ << "\n 离群点k值: " << stat_mean_k_
<< "\n [调试选项]" << "\n 离群点标准差阈值: " << stat_std_thresh_ << "\n 栅格大小: " << grid_size_ << "x"
<< "\n 调试模式: " << (enable_debug_ ? "开启" : "关闭") << grid_size_ << "\n 栅格范围(m): " << grid_range_
<< "\n [点云处理]" << "\n 打印栅格: " << (enable_print_ ? "开启" : "关闭") << "\n [车身过滤]"
<< "\n 空间过滤范围(m): [" << filter_min_x_ << ", " << filter_max_x_ << "\n 启用车身过滤: " << (filter_car_ ? "是" : "否") << "\n 车身尺寸(m): " << car_length_
<< "] x [" << filter_min_y_ << ", " << filter_max_y_ << " x " << car_width_ << "\n LiDAR偏移(m): (" << car_lidar_offset_x_ << ", " << car_lidar_offset_y_
<< "] x [" << filter_min_z_ << ", " << filter_max_z_ << "]" << ")"
<< "\n 体素大小(m): " << voxel_size_ << "\n--------------------------------------------\n");
<< "\n 离群点k值: " << stat_mean_k_
<< "\n 离群点标准差阈值: " << stat_std_thresh_
<< "\n 栅格大小: " << grid_size_ << "x" << grid_size_
<< "\n 栅格范围(m): " << grid_range_
<< "\n 打印栅格: " << (enable_print_ ? "开启" : "关闭")
<< "\n [车身过滤]"
<< "\n 启用车身过滤: " << (filter_car_ ? "是" : "否")
<< "\n 车身尺寸(m): " << car_length_ << " x " << car_width_
<< "\n LiDAR偏移(m): (" << car_lidar_offset_x_ << ", " << car_lidar_offset_y_ << ")"
<< "\n--------------------------------------------\n");
/* ---------- TF ---------- */ /* ---------- TF ---------- */
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock()); tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_); tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
/* ---------- QoS - 优化为低延时 ---------- */ /* ---------- QoS - 优化为低延时 ---------- */
auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)) auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)).best_effort().durability_volatile();
.best_effort()
.durability_volatile();
// 移除deadline约束，避免因延时导致的消息丢弃 // 移除deadline约束，避免因延时导致的消息丢弃
/* ---------- 订阅 ---------- */ /* ---------- 订阅 ---------- */
sub_front_ = create_subscription<PointCloud2>( sub_front_ = create_subscription<PointCloud2>(front_topic_, qos,
front_topic_, qos, std::bind(&LidarMerger::frontCB, this, std::placeholders::_1)); std::bind(&LidarMerger::frontCB, this, std::placeholders::_1));
sub_rear_ = create_subscription<PointCloud2>( sub_rear_ = create_subscription<PointCloud2>(rear_topic_, qos,
rear_topic_, qos, std::bind(&LidarMerger::rearCB, this, std::placeholders::_1)); std::bind(&LidarMerger::rearCB, this, std::placeholders::_1));
/* ---------- 发布 ---------- */ /* ---------- 发布 ---------- */
auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)) auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)).reliable().durability_volatile();
.reliable()
.durability_volatile();
pub_ = create_publisher<PointCloud2>(output_topic_, pub_qos); pub_ = create_publisher<PointCloud2>(output_topic_, pub_qos);
// 创建栅格发布者 // 创建栅格发布者
@ -176,7 +160,7 @@ public:
cache_size_, time_tolerance_, max_wait_time_); cache_size_, time_tolerance_, max_wait_time_);
} }
private: private:
/* ---------- 回调函数 - 同步处理 ---------- */ /* ---------- 回调函数 - 同步处理 ---------- */
void frontCB(const PointCloud2::SharedPtr msg) void frontCB(const PointCloud2::SharedPtr msg)
{ {
@ -207,8 +191,7 @@ private:
} }
/* ---------- 同步处理点云 ---------- */ /* ---------- 同步处理点云 ---------- */
void processCloudSync(const PointCloud2::SharedPtr msg, const std::string &source, void processCloudSync(const PointCloud2::SharedPtr msg, const std::string& source, const rclcpp::Time& receive_time)
const rclcpp::Time &receive_time)
{ {
// 立即进行坐标变换 // 立即进行坐标变换
auto transformed_cloud = transformCloud(*msg); auto transformed_cloud = transformCloud(*msg);
@ -219,11 +202,8 @@ private:
} }
// 创建CloudFrame // 创建CloudFrame
auto cloud_frame = std::make_shared<CloudFrame>(CloudFrame{ auto cloud_frame = std::make_shared<CloudFrame>(
transformed_cloud, CloudFrame{transformed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source});
rclcpp::Time(msg->header.stamp),
receive_time,
source});
// 添加到缓存并立即尝试合并 // 添加到缓存并立即尝试合并
{ {
@ -241,7 +221,7 @@ private:
/* ---------- 添加到缓存 ---------- */ /* ---------- 添加到缓存 ---------- */
void addToCache(std::shared_ptr<CloudFrame> frame) void addToCache(std::shared_ptr<CloudFrame> frame)
{ {
auto &cache = (frame->source == "front") ? front_clouds_ : rear_clouds_; auto& cache = (frame->source == "front") ? front_clouds_ : rear_clouds_;
// 保持时间排序（最新的在前） // 保持时间排序（最新的在前）
auto it = cache.begin(); auto it = cache.begin();
@ -262,14 +242,12 @@ private:
/* ---------- 尝试合并 - 触发式 ---------- */ /* ---------- 尝试合并 - 触发式 ---------- */
void tryMerge() void tryMerge()
{ {
if (front_clouds_.empty() || rear_clouds_.empty()) if (front_clouds_.empty() || rear_clouds_.empty()) return;
return;
// 查找最佳匹配 // 查找最佳匹配
auto [front_frame, rear_frame] = findBestMatchOptimized(); auto [front_frame, rear_frame] = findBestMatchOptimized();
if (!front_frame || !rear_frame) if (!front_frame || !rear_frame) return;
return;
// 自适应延时检查 - 基于实际网络条件调整 // 自适应延时检查 - 基于实际网络条件调整
auto now_time = now(); auto now_time = now();
@ -279,7 +257,7 @@ private:
// 动态调整max_wait_time基于观察到的延时 // 动态调整max_wait_time基于观察到的延时
static double observed_max_delay = 0.0; static double observed_max_delay = 0.0;
double current_max_delay = std::max(front_age, rear_age); double current_max_delay = std::max(front_age, rear_age);
observed_max_delay = std::max(observed_max_delay * 0.95, current_max_delay); // 指数衰减 observed_max_delay = std::max(observed_max_delay * 0.95, current_max_delay); // 指数衰减
// 使用观察到的延时来决定是否处理，而不是固定阈值 // 使用观察到的延时来决定是否处理，而不是固定阈值
double adaptive_threshold = std::min(max_wait_time_, observed_max_delay + 0.1); double adaptive_threshold = std::min(max_wait_time_, observed_max_delay + 0.1);
@ -289,8 +267,8 @@ private:
if (enable_debug_) if (enable_debug_)
{ {
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000,
"Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs", "Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs", front_age,
front_age, rear_age, adaptive_threshold); rear_age, adaptive_threshold);
} }
// 不直接返回，而是清理最旧的数据后重试 // 不直接返回，而是清理最旧的数据后重试
@ -314,8 +292,7 @@ private:
auto merged = mergeClouds(*front_frame, *rear_frame); auto merged = mergeClouds(*front_frame, *rear_frame);
auto merge_end = std::chrono::high_resolution_clock::now(); auto merge_end = std::chrono::high_resolution_clock::now();
if (!merged) if (!merged) return;
return;
// 设置时间戳和frame_id // 设置时间戳和frame_id
auto front_time = front_frame->stamp.nanoseconds(); auto front_time = front_frame->stamp.nanoseconds();
@ -333,8 +310,7 @@ private:
// ========================= 点云 ========================= // ========================= 点云 =========================
auto processed_pcl = processPointCloud(merged_pcl); auto processed_pcl = processPointCloud(merged_pcl);
if (!processed_pcl) if (!processed_pcl) return;
return;
// 将处理后的PCL点云转回ROS消息 // 将处理后的PCL点云转回ROS消息
auto processed = std::make_shared<sensor_msgs::msg::PointCloud2>(); auto processed = std::make_shared<sensor_msgs::msg::PointCloud2>();
@ -346,28 +322,27 @@ private:
// ========================= 点云 ========================= // ========================= 点云 =========================
// ========================= 栅格 ========================= // ========================= 栅格 =========================
auto grid = processPointCloud_grid(merged_pcl); auto grid = processPointCloud_grid(merged_pcl);
if (grid.empty()) if (grid.empty()) return;
return;
// 可视化栅格 // 可视化栅格
if (enable_print_) if (enable_print_) visualizeGridInTerminal(grid);
visualizeGridInTerminal(grid);
// 发布栅格到ROS话题 // 发布栅格到ROS话题
publishGrid(grid); publishGrid(grid);
// ========================= 栅格 ========================= // ========================= 栅格 =========================
// 延时分析（降低频率） // 延时分析（降低频率）
if (enable_debug_ && merged_count_ % 10 == 0) // 每10次输出一次 if (enable_debug_ && merged_count_ % 10 == 0) // 每10次输出一次
{ {
auto total_delay = (now_time - processed->header.stamp).seconds(); auto total_delay = (now_time - processed->header.stamp).seconds();
auto process_time = std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - merge_start).count(); auto process_time =
std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - merge_start).count();
RCLCPP_INFO(get_logger(), RCLCPP_INFO(
"Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs", get_logger(),
merged_count_, merged->data.size() / merged->point_step, "Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs",
processed->data.size() / processed->point_step, merged_count_, merged->data.size() / merged->point_step, processed->data.size() / processed->point_step,
total_delay, process_time * 1000, adaptive_threshold); total_delay, process_time * 1000, adaptive_threshold);
} }
// 清理已使用的点云 // 清理已使用的点云
@ -375,17 +350,16 @@ private:
} }
/* ---------- 点云处理流程 ---------- */ /* ---------- 点云处理流程 ---------- */
pcl::PointCloud<pcl::PointXYZ>::Ptr processPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) pcl::PointCloud<pcl::PointXYZ>::Ptr processPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
if (!cloud || cloud->empty()) if (!cloud || cloud->empty()) return nullptr;
return nullptr;
// 1. 条件过滤 // 1. 条件过滤
auto filtered = applyConditionalFiltering(cloud); auto filtered = applyConditionalFiltering(cloud);
if (filtered->empty()) if (filtered->empty())
{ {
RCLCPP_WARN(get_logger(), "[processPointCloud] Filtered cloud is empty!"); RCLCPP_WARN(get_logger(), "[processPointCloud] Filtered cloud is empty!");
return cloud; // 返回原始云而不是nullptr return cloud; // 返回原始云而不是nullptr
} }
// 2. 降采样 // 2. 降采样
@ -406,10 +380,9 @@ private:
return outlier_removed; return outlier_removed;
} }
std::vector<std::vector<int>> processPointCloud_grid(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) std::vector<std::vector<int>> processPointCloud_grid(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
if (!cloud || cloud->empty()) if (!cloud || cloud->empty()) return {}; // 返回空栅格
return {}; // 返回空栅格
// 1. 条件过滤 // 1. 条件过滤
auto filtered = applyConditionalFiltering(cloud); auto filtered = applyConditionalFiltering(cloud);
@ -440,16 +413,15 @@ private:
} }
/* ---------- 条件过滤 ---------- */ /* ---------- 条件过滤 ---------- */
pcl::PointCloud<pcl::PointXYZ>::Ptr applyConditionalFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) pcl::PointCloud<pcl::PointXYZ>::Ptr applyConditionalFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
for (const auto &point : cloud->points) for (const auto& point : cloud->points)
{ {
// 自定义过滤条件：保留特定区域内的点 // 自定义过滤条件：保留特定区域内的点
if (point.x > filter_min_x_ && point.x < filter_max_x_ && if (point.x > filter_min_x_ && point.x < filter_max_x_ && point.y > filter_min_y_ &&
point.y > filter_min_y_ && point.y < filter_max_y_ && point.y < filter_max_y_ && point.z > filter_min_z_ && point.z < filter_max_z_)
point.z > filter_min_z_ && point.z < filter_max_z_)
{ {
filtered->points.push_back(point); filtered->points.push_back(point);
} }
@ -460,8 +432,7 @@ private:
filtered->is_dense = true; filtered->is_dense = true;
// 不启用车身过滤，直接返回 // 不启用车身过滤，直接返回
if (!filter_car_) if (!filter_car_) return filtered;
return filtered;
// RCLCPP_INFO(get_logger(), "启用车身过滤"); // RCLCPP_INFO(get_logger(), "启用车身过滤");
// 使用CropBox移除车身区域 // 使用CropBox移除车身区域
@ -469,17 +440,15 @@ private:
crop.setInputCloud(filtered); crop.setInputCloud(filtered);
// 设置车身区域（最小点和最大点） // 设置车身区域（最小点和最大点）
Eigen::Vector4f min_point(car_lidar_offset_x_ - car_length_ / 2.0f, Eigen::Vector4f min_point(car_lidar_offset_x_ - car_length_ / 2.0f, car_lidar_offset_y_ - car_width_ / 2.0f,
car_lidar_offset_y_ - car_width_ / 2.0f,
filter_min_z_, 1.0); filter_min_z_, 1.0);
Eigen::Vector4f max_point(car_lidar_offset_x_ + car_length_ / 2.0f, Eigen::Vector4f max_point(car_lidar_offset_x_ + car_length_ / 2.0f, car_lidar_offset_y_ + car_width_ / 2.0f,
car_lidar_offset_y_ + car_width_ / 2.0f,
filter_max_z_, 1.0); filter_max_z_, 1.0);
crop.setMin(min_point); crop.setMin(min_point);
crop.setMax(max_point); crop.setMax(max_point);
crop.setNegative(true); // true表示保留外部点，false表示保留内部点 crop.setNegative(true); // true表示保留外部点，false表示保留内部点
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
crop.filter(*output); crop.filter(*output);
@ -489,11 +458,11 @@ private:
// 右侧主刷 (x:0.85~1.25, y:0.6~0.96, z:1.2~1.65) // 右侧主刷 (x:0.85~1.25, y:0.6~0.96, z:1.2~1.65)
pcl::CropBox<pcl::PointXYZ> crop1; pcl::CropBox<pcl::PointXYZ> crop1;
crop1.setInputCloud(output); crop1.setInputCloud(output);
Eigen::Vector4f min1(0.85f, 0.6f, 1.2f, 1.0); // 右侧主刷最小点 Eigen::Vector4f min1(0.85f, 0.6f, 1.2f, 1.0); // 右侧主刷最小点
Eigen::Vector4f max1(1.25f, 0.96f, 1.65f, 1.0); // 右侧主刷最大点 Eigen::Vector4f max1(1.25f, 0.96f, 1.65f, 1.0); // 右侧主刷最大点
crop1.setMin(min1); crop1.setMin(min1);
crop1.setMax(max1); crop1.setMax(max1);
crop1.setNegative(true); // 剔除右侧主刷内的点 crop1.setNegative(true); // 剔除右侧主刷内的点
pcl::PointCloud<pcl::PointXYZ>::Ptr output1(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr output1(new pcl::PointCloud<pcl::PointXYZ>);
crop1.filter(*output1); crop1.filter(*output1);
@ -502,20 +471,20 @@ private:
pcl::CropBox<pcl::PointXYZ> crop2; pcl::CropBox<pcl::PointXYZ> crop2;
crop2.setInputCloud(output1); crop2.setInputCloud(output1);
// X轴对称：将y坐标取相反数，保持x和z坐标不变 // X轴对称：将y坐标取相反数，保持x和z坐标不变
Eigen::Vector4f min2(0.85f, -0.96f, 1.2f, 1.0); // 左侧主刷最小点（y取反后注意交换大小） Eigen::Vector4f min2(0.85f, -0.96f, 1.2f, 1.0); // 左侧主刷最小点（y取反后注意交换大小）
Eigen::Vector4f max2(1.25f, -0.6f, 1.65f, 1.0); // 左侧主刷最大点（y取反后注意交换大小） Eigen::Vector4f max2(1.25f, -0.6f, 1.65f, 1.0); // 左侧主刷最大点（y取反后注意交换大小）
crop2.setMin(min2); crop2.setMin(min2);
crop2.setMax(max2); crop2.setMax(max2);
crop2.setNegative(true); // 剔除左侧主刷内的点 crop2.setNegative(true); // 剔除左侧主刷内的点
pcl::PointCloud<pcl::PointXYZ>::Ptr final_output(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final_output(new pcl::PointCloud<pcl::PointXYZ>);
crop2.filter(*final_output); crop2.filter(*final_output);
return final_output; // 剔除两侧主刷 return final_output; // 剔除两侧主刷
} }
/* ---------- 降采样（体素网格过滤） ---------- */ /* ---------- 降采样（体素网格过滤） ---------- */
pcl::PointCloud<pcl::PointXYZ>::Ptr applyVoxelGridFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) pcl::PointCloud<pcl::PointXYZ>::Ptr applyVoxelGridFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::VoxelGrid<pcl::PointXYZ> vg;
@ -528,21 +497,21 @@ private:
} }
/* ---------- 离群点去除 ---------- */ /* ---------- 离群点去除 ---------- */
pcl::PointCloud<pcl::PointXYZ>::Ptr applyStatisticalOutlierRemoval(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) pcl::PointCloud<pcl::PointXYZ>::Ptr applyStatisticalOutlierRemoval(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud); sor.setInputCloud(cloud);
sor.setMeanK(stat_mean_k_); // 考虑每个点的k个最近邻 sor.setMeanK(stat_mean_k_); // 考虑每个点的k个最近邻
sor.setStddevMulThresh(stat_std_thresh_); // 标准差倍数阈值 sor.setStddevMulThresh(stat_std_thresh_); // 标准差倍数阈值
sor.filter(*filtered); sor.filter(*filtered);
return filtered; return filtered;
} }
/* ---------- 栅格化处理 ---------- */ /* ---------- 栅格化处理 ---------- */
std::vector<std::vector<int>> generateOccupancyGrid(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) std::vector<std::vector<int>> generateOccupancyGrid(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
// 0:空 1:障碍物 2:车体 // 0:空 1:障碍物 2:车体
std::vector<std::vector<int>> grid(grid_size_, std::vector<int>(grid_size_, 0)); std::vector<std::vector<int>> grid(grid_size_, std::vector<int>(grid_size_, 0));
@ -552,8 +521,10 @@ private:
if (filter_car_) if (filter_car_)
{ {
// 计算车体在栅格中的位置 // 计算车体在栅格中的位置
int car_min_x = static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution); int car_min_x =
int car_max_x = static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution); static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution);
int car_max_x =
static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution);
int car_min_y = static_cast<int>((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) / resolution); int car_min_y = static_cast<int>((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) / resolution);
int car_max_y = static_cast<int>((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) / resolution); int car_max_y = static_cast<int>((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) / resolution);
@ -574,7 +545,7 @@ private:
} }
// 标记点云障碍物 // 标记点云障碍物
for (const auto &point : cloud->points) for (const auto& point : cloud->points)
{ {
// X轴（前向）映射到行索引（从上到下为正X到负X） // X轴（前向）映射到行索引（从上到下为正X到负X）
int grid_x = static_cast<int>((grid_range_ / 2 - point.x) / resolution); int grid_x = static_cast<int>((grid_range_ / 2 - point.x) / resolution);
@ -596,7 +567,7 @@ private:
} }
/* ---------- 可视化栅格 - 终端打印 ---------- */ /* ---------- 可视化栅格 - 终端打印 ---------- */
void visualizeGridInTerminal(const std::vector<std::vector<int>> &grid) void visualizeGridInTerminal(const std::vector<std::vector<int>>& grid)
{ {
std::system("clear"); std::system("clear");
@ -634,24 +605,24 @@ private:
} }
/* ---------- 发布栅格 ---------- */ /* ---------- 发布栅格 ---------- */
void publishGrid(const std::vector<std::vector<int>> &grid) void publishGrid(const std::vector<std::vector<int>>& grid)
{ {
// 创建Int32MultiArray消息 // 创建Int32MultiArray消息
auto msg = std_msgs::msg::Int32MultiArray(); auto msg = std_msgs::msg::Int32MultiArray();
// 设置维度信息 // 设置维度信息
msg.layout.dim.resize(2); msg.layout.dim.resize(2);
msg.layout.dim[0].label = "height"; // 行（对应X轴） msg.layout.dim[0].label = "height"; // 行（对应X轴）
msg.layout.dim[0].size = grid.size(); msg.layout.dim[0].size = grid.size();
msg.layout.dim[0].stride = grid.size() * grid[0].size(); msg.layout.dim[0].stride = grid.size() * grid[0].size();
msg.layout.dim[1].label = "width"; // 列（对应Y轴） msg.layout.dim[1].label = "width"; // 列（对应Y轴）
msg.layout.dim[1].size = grid[0].size(); msg.layout.dim[1].size = grid[0].size();
msg.layout.dim[1].stride = grid[0].size(); msg.layout.dim[1].stride = grid[0].size();
// 设置数据（按行优先展开） // 设置数据（按行优先展开）
msg.data.clear(); msg.data.clear();
for (const auto &row : grid) for (const auto& row : grid)
{ {
msg.data.insert(msg.data.end(), row.begin(), row.end()); msg.data.insert(msg.data.end(), row.begin(), row.end());
} }
@ -662,8 +633,7 @@ private:
/* ---------- 优化的匹配算法 ---------- */ /* ---------- 优化的匹配算法 ---------- */
std::pair<std::shared_ptr<CloudFrame>, std::shared_ptr<CloudFrame>> findBestMatchOptimized() std::pair<std::shared_ptr<CloudFrame>, std::shared_ptr<CloudFrame>> findBestMatchOptimized()
{ {
if (front_clouds_.empty() || rear_clouds_.empty()) if (front_clouds_.empty() || rear_clouds_.empty()) return {nullptr, nullptr};
return {nullptr, nullptr};
// 快速策略：优先使用最新的点云进行匹配 // 快速策略：优先使用最新的点云进行匹配
auto front_candidate = front_clouds_.front(); auto front_candidate = front_clouds_.front();
@ -708,8 +678,7 @@ private:
} }
/* ---------- 移除已使用的点云 ---------- */ /* ---------- 移除已使用的点云 ---------- */
void removeUsedClouds(std::shared_ptr<CloudFrame> used_front, void removeUsedClouds(std::shared_ptr<CloudFrame> used_front, std::shared_ptr<CloudFrame> used_rear)
std::shared_ptr<CloudFrame> used_rear)
{ {
// 从front_clouds_中移除 // 从front_clouds_中移除
auto front_it = std::find(front_clouds_.begin(), front_clouds_.end(), used_front); auto front_it = std::find(front_clouds_.begin(), front_clouds_.end(), used_front);
@ -735,7 +704,7 @@ private:
void cleanupExpiredClouds() void cleanupExpiredClouds()
{ {
auto now_time = now(); auto now_time = now();
auto cleanup_threshold = max_wait_time_; // 使用配置的等待时间 auto cleanup_threshold = max_wait_time_; // 使用配置的等待时间
// 清理front_clouds_中过期的数据 // 清理front_clouds_中过期的数据
while (!front_clouds_.empty()) while (!front_clouds_.empty())
@ -777,13 +746,12 @@ private:
} }
/* ---------- 合并点云 ---------- */ /* ---------- 合并点云 ---------- */
std::shared_ptr<PointCloud2> mergeClouds(const CloudFrame &front, const CloudFrame &rear) std::shared_ptr<PointCloud2> mergeClouds(const CloudFrame& front, const CloudFrame& rear)
{ {
size_t front_points = front.cloud->data.size() / front.cloud->point_step; size_t front_points = front.cloud->data.size() / front.cloud->point_step;
size_t rear_points = rear.cloud->data.size() / rear.cloud->point_step; size_t rear_points = rear.cloud->data.size() / rear.cloud->point_step;
if (front.cloud->fields != rear.cloud->fields || if (front.cloud->fields != rear.cloud->fields || front.cloud->point_step != rear.cloud->point_step ||
front.cloud->point_step != rear.cloud->point_step ||
front.cloud->is_bigendian != rear.cloud->is_bigendian) front.cloud->is_bigendian != rear.cloud->is_bigendian)
{ {
RCLCPP_ERROR(get_logger(), "PointCloud2 formats do not match!"); RCLCPP_ERROR(get_logger(), "PointCloud2 formats do not match!");
@ -811,7 +779,7 @@ private:
} }
/* ---------- 坐标变换 - 简化版 ---------- */ /* ---------- 坐标变换 - 简化版 ---------- */
std::shared_ptr<PointCloud2> transformCloud(const PointCloud2 &in) std::shared_ptr<PointCloud2> transformCloud(const PointCloud2& in)
{ {
if (in.header.frame_id == target_frame_) if (in.header.frame_id == target_frame_)
{ {
@ -823,20 +791,16 @@ private:
try try
{ {
// 使用最新可用的变换，不等待 // 使用最新可用的变换，不等待
auto tf = tf_buffer_->lookupTransform( auto tf = tf_buffer_->lookupTransform(target_frame_, in.header.frame_id, tf2::TimePointZero);
target_frame_, in.header.frame_id,
tf2::TimePointZero);
auto out = cloud_pool_.acquire(); auto out = cloud_pool_.acquire();
tf2::doTransform(in, *out, tf); tf2::doTransform(in, *out, tf);
return out; return out;
} }
catch (const tf2::TransformException &e) catch (const tf2::TransformException& e)
{ {
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "TF failed (%s -> %s): %s",
"TF failed (%s -> %s): %s", in.header.frame_id.c_str(), target_frame_.c_str(), e.what());
in.header.frame_id.c_str(),
target_frame_.c_str(), e.what());
return nullptr; return nullptr;
} }
} }
@ -845,12 +809,10 @@ private:
void printStats() void printStats()
{ {
auto current_time = now(); auto current_time = now();
if ((current_time - last_stats_time_).seconds() >= 5.0) // 每5秒输出一次 if ((current_time - last_stats_time_).seconds() >= 5.0) // 每5秒输出一次
{ {
RCLCPP_INFO(get_logger(), RCLCPP_INFO(get_logger(), "Stats: Front=%d, Rear=%d, Merged=%d, Cache(F:%zu,R:%zu)", front_count_,
"Stats: Front=%d, Rear=%d, Merged=%d, Cache(F:%zu,R:%zu)", rear_count_, merged_count_, front_clouds_.size(), rear_clouds_.size());
front_count_, rear_count_, merged_count_,
front_clouds_.size(), rear_clouds_.size());
last_stats_time_ = current_time; last_stats_time_ = current_time;
} }
@ -866,11 +828,11 @@ private:
float filter_min_x_, filter_max_x_; float filter_min_x_, filter_max_x_;
float filter_min_y_, filter_max_y_; float filter_min_y_, filter_max_y_;
float filter_min_z_, filter_max_z_; float filter_min_z_, filter_max_z_;
float voxel_size_; // 降采样体素大小 float voxel_size_; // 降采样体素大小
int stat_mean_k_; // 统计离群点去除的k值 int stat_mean_k_; // 统计离群点去除的k值
float stat_std_thresh_; // 统计离群点去除的标准差阈值 float stat_std_thresh_; // 统计离群点去除的标准差阈值
int grid_size_; // 栅格大小 int grid_size_; // 栅格大小
float grid_range_; // 栅格范围 float grid_range_; // 栅格范围
bool enable_print_; bool enable_print_;
// 车身过滤参数 // 车身过滤参数
@ -901,7 +863,7 @@ private:
int front_count_, rear_count_, merged_count_; int front_count_, rear_count_, merged_count_;
}; };
int main(int argc, char **argv) int main(int argc, char** argv)
{ {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);

24
src/perception/rtk/CMakeLists.txt
View File

@ -20,25 +20,25 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(logger REQUIRED)
#find_package(Boost 1.71.0 REQUIRED COMPONENTS thread atomic system regex) #find_package(Boost 1.71.0 REQUIRED COMPONENTS thread atomic system regex)
#if(Boost_FOUND) #if(Boost_FOUND)
#include_directories(${Boost_INCLUDE_DIRS}) #include_directories(${Boost_INCLUDE_DIRS})
#endif() #endif()
include_directories( include_directories(include/rtk ${catkin_INCLUDE_DIRS})
include/rtk
${catkin_INCLUDE_DIRS}
)
add_executable(rtk_node src/rtk_node.cpp src/serial_read.cpp) add_executable(rtk_node src/rtk_node.cpp src/serial_read.cpp)
target_link_libraries(rtk_node ${Boost_LIBRARIES}) target_link_libraries(rtk_node ${Boost_LIBRARIES})
ament_target_dependencies(rtk_node rclcpp std_msgs sweeper_interfaces) ament_target_dependencies(
install(TARGETS
rtk_node rtk_node
DESTINATION lib/${PROJECT_NAME} rclcpp
) std_msgs
sweeper_interfaces
logger)
install(TARGETS rtk_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
@ -49,8 +49,6 @@ if(BUILD_TESTING)
# uncomment the line when this package is not in a git repo # uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE) #set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies() ament_lint_auto_find_test_dependencies()
endif() endif()
ament_package() ament_package()

11
src/perception/rtk/include/rtk/serial_read.hpp
View File

@ -1,24 +1,23 @@
#ifndef __UART_READ_H__ #ifndef __UART_READ_H__
#define __UART_READ_H__ #define __UART_READ_H__
#include <iostream>
#include <boost/asio.hpp> #include <boost/asio.hpp>
#include <iostream>
using namespace std; using namespace std;
using namespace boost::asio; using namespace boost::asio;
class Boost_serial class Boost_serial
{ {
public: public:
Boost_serial(); Boost_serial();
~Boost_serial(); ~Boost_serial();
void init(const string port_name); void init(const string port_name);
int serial_read(char buf[],int size); int serial_read(char buf[], int size);
private: private:
io_service service; io_service service;
serial_port *sp; serial_port* sp;
}; };
#endif #endif

1
src/perception/rtk/package.xml
View File

@ -12,6 +12,7 @@
<depend>rclcpp</depend> <depend>rclcpp</depend>
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>sweeper_interfaces</depend> <depend>sweeper_interfaces</depend>
<depend>logger</depend>
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_common</test_depend>

58
src/perception/rtk/src/rtk_node.cpp
View File

@ -1,18 +1,20 @@
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/rtk.hpp"
#include "serial_read.hpp"
#include <string.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h>
#include <queue> #include <queue>
#include "logger/logger.h"
#include "rclcpp/rclcpp.hpp"
#include "serial_read.hpp"
#include "sweeper_interfaces/msg/rtk.hpp"
class rtk_node : public rclcpp::Node class rtk_node : public rclcpp::Node
{ {
public: public:
// 构造函数,有一个参数为节点名称 // 构造函数,有一个参数为节点名称
rtk_node(std::string name) : Node(name) rtk_node(std::string name) : Node(name)
{ {
RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); LOG_INFO("%s节点已经启动.", name.c_str());
lat = 0.0; lat = 0.0;
lon = 0.0; lon = 0.0;
@ -29,17 +31,17 @@ public:
timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&rtk_node::timer_callback, this)); timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&rtk_node::timer_callback, this));
} }
private: private:
void GPYBM_to_gps(char serial_buf[]) void GPYBM_to_gps(char serial_buf[])
{ {
position = 0; position = 0;
line_string.clear(); line_string.clear();
char *p_gpybm = strstr(serial_buf, "GPYBM"); char* p_gpybm = strstr(serial_buf, "GPYBM");
if (p_gpybm == NULL) if (p_gpybm == NULL)
{ {
RCLCPP_INFO(this->get_logger(), "未检测到GPYBM字符串!"); LOG_INFO("未检测到GPYBM字符串!");
return; return;
} }
@ -103,13 +105,13 @@ private:
// 读取串口传来的定位信息 // 读取串口传来的定位信息
memset(serial_buf, 0, sizeof(serial_buf)); memset(serial_buf, 0, sizeof(serial_buf));
int num = boost_serial->serial_read(serial_buf, 200); int num = boost_serial->serial_read(serial_buf, 200);
// printf("num:%d \n",num); // LOG_DEBUG("num:%d \n",num);
// printf("serial_buf: "); // LOG_DEBUG("serial_buf: ");
// for (int i = 0; i < 300; i++) // for (int i = 0; i < 300; i++)
// { // {
// printf("%c", serial_buf[i]); // LOG_DEBUG("%c", serial_buf[i]);
// } // }
// printf("\n"); // LOG_DEBUG("\n");
if (c_queue.size() >= 400) if (c_queue.size() >= 400)
{ {
@ -132,12 +134,12 @@ private:
} }
// 解析定位信息 // 解析定位信息
// printf("gps_buf: "); // LOG_DEBUG("gps_buf: ");
// for (int i = 0; i < 300; i++) // for (int i = 0; i < 300; i++)
// { // {
// printf("%c", gps_buf[i]); // LOG_DEBUG("%c", gps_buf[i]);
// } // }
// printf("\n"); // LOG_DEBUG("\n");
GPYBM_to_gps(gps_buf); GPYBM_to_gps(gps_buf);
@ -152,8 +154,8 @@ private:
message.h_quality = h_quality; message.h_quality = h_quality;
// 日志打印 // 日志打印
RCLCPP_INFO(this->get_logger(), "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", LOG_INFO("lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", message.lat,
message.lat, message.lon, message.head, message.speed, message.p_quality, message.h_quality); message.lon, message.head, message.speed, message.p_quality, message.h_quality);
// 发布消息 // 发布消息
command_publisher_->publish(message); command_publisher_->publish(message);
} }
@ -170,7 +172,7 @@ private:
rclcpp::Publisher<sweeper_interfaces::msg::Rtk>::SharedPtr command_publisher_; rclcpp::Publisher<sweeper_interfaces::msg::Rtk>::SharedPtr command_publisher_;
// 串口读取类指针 // 串口读取类指针
Boost_serial *boost_serial; Boost_serial* boost_serial;
// 串口读取buffer // 串口读取buffer
char serial_buf[300]; char serial_buf[300];
@ -180,8 +182,8 @@ private:
double lon; double lon;
double head; double head;
double speed; double speed;
int p_quality; // 定位解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解 int p_quality; // 定位解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解
int h_quality; // 定向解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解 int h_quality; // 定向解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解
std::queue<char> c_queue; std::queue<char> c_queue;
// 解析定位信息用到的中间变量 // 解析定位信息用到的中间变量
@ -190,15 +192,21 @@ private:
string line_string; string line_string;
}; };
int main(int argc, char **argv) int main(int argc, char** argv)
{ {
unsigned int a = -1; // 初始化日志系统
printf("%u\n", a); logger::Logger::Init("rtk", "./log");
// unsigned int a = -1;
// LOG_DEBUG("%u\n", a);
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
/*创建对应节点的共享指针对象*/ /*创建对应节点的共享指针对象*/
auto node = std::make_shared<rtk_node>("rtk_node"); auto node = std::make_shared<rtk_node>("rtk_node");
/* 运行节点，并检测退出信号*/ /* 运行节点，并检测退出信号*/
rclcpp::spin(node); rclcpp::spin(node);
rclcpp::shutdown(); rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
return 0; return 0;
} }

14
src/perception/rtk/src/serial_read.cpp
View File

@ -1,9 +1,8 @@
#include "serial_read.hpp" #include "serial_read.hpp"
Boost_serial::Boost_serial() #include "logger/logger.h"
{
; Boost_serial::Boost_serial() { ; }
}
Boost_serial::~Boost_serial() Boost_serial::~Boost_serial()
{ {
@ -26,11 +25,11 @@ void Boost_serial::init(const string port_name)
sp->set_option(serial_port::parity(serial_port::parity::none)); sp->set_option(serial_port::parity(serial_port::parity::none));
sp->set_option(serial_port::stop_bits(serial_port::stop_bits::one)); sp->set_option(serial_port::stop_bits(serial_port::stop_bits::one));
sp->set_option(serial_port::character_size(8)); sp->set_option(serial_port::character_size(8));
cout << "打开串口成功！" << endl; LOG_INFO("打开串口成功！");
} }
else else
{ {
cout << "打开串口失败！" << endl; LOG_ERROR("打开串口失败！");
} }
} }
@ -39,8 +38,7 @@ int Boost_serial::serial_read(char buf[], int size)
int num = read(*sp, buffer(buf, size)); int num = read(*sp, buffer(buf, size));
if (num <= 0) if (num <= 0)
{ {
cout << "没有读到数据！" << endl; LOG_WARN("没有读到数据！");
} }
return num; return num;
} }