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()
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
add_compile_options(-w) # 禁用所有警告
add_compile_options(-w) # 禁用所有警告
endif()
include_directories(
include/fu
${catkin_INCLUDE_DIRS}
)
include_directories(include/fu ${catkin_INCLUDE_DIRS})
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
@ -30,19 +27,23 @@ find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(mc REQUIRED)
find_package(logger REQUIRED)
include_directories(include)
add_executable(fu_node src/fu_node.cpp src/jsoncpp.cpp)
ament_target_dependencies(fu_node rclcpp std_msgs sweeper_interfaces sensor_msgs mc)
install(TARGETS
ament_target_dependencies(
fu_node
DESTINATION lib/${PROJECT_NAME}
)
rclcpp
std_msgs
sweeper_interfaces
sensor_msgs
mc
logger)
install(DIRECTORY launch config
DESTINATION share/${PROJECT_NAME})
install(TARGETS fu_node DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
if(BUILD_TESTING)
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
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
endif()
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(sweeper_interfaces REQUIRED)
# find_package(mc REQUIRED)
find_package(logger REQUIRED)
include_directories(
include/pl
${catkin_INCLUDE_DIRS}
)
include_directories(include/pl ${catkin_INCLUDE_DIRS})
add_executable(pl_node src/pl_node.cpp src/pl.cpp src/jsoncpp.cpp)
ament_target_dependencies(pl_node rclcpp std_msgs sweeper_interfaces)
install(TARGETS
ament_target_dependencies(
pl_node
DESTINATION lib/${PROJECT_NAME}
)
rclcpp
std_msgs
sweeper_interfaces
logger)
install(TARGETS pl_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
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
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
endif()
ament_package()

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

@ -1,21 +1,24 @@
#include "pl.hpp"
#include <iostream>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include <iostream>
#include "logger/logger.h"
using namespace std;
#define WRC_MAX_POINTS_IN_PATH 2000
#define Lat_Origin 32.045062 // 地面平面坐标系原点的纬度
#define Lon_Origin 118.845200366 // 地面平面坐标系原点的经度
#define Lat_Origin 32.045062 // 地面平面坐标系原点的纬度
#define Lon_Origin 118.845200366 // 地面平面坐标系原点的经度
#define Re 6378137
#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_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度)
#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度)
float nl_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 x, y, length;
x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式
x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式
y = (R_LATI) * (lati2 - lati1) * deg_rad;
length = sqrt(x * x + y * y);
return length;
}
// 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;
float k1, k2, k3, k4; // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta);
Navi_rad = Cur_navAngle * deg_rad; // 角度转化为弧度
float k1, k2, k3, k4; // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta);
Navi_rad = Cur_navAngle * deg_rad; // 角度转化为弧度
// 以当前位置为原点，以正北为y正轴，以正东为x正轴；
x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT;
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
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)
{
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,
Road_info[start_index].LongitudeInfo,
Road_info[start_index].LatitudeInfo);
Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo);
// 从 start_index + 1 开始搜索
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,
Road_info[current_index].LongitudeInfo,
Road_info[current_index].LatitudeInfo);
double current_distance =
ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
Road_info[current_index].LongitudeInfo, Road_info[current_index].LatitudeInfo);
if (current_distance < min_distance)
{
min_distance = current_distance;
Nearest_point_index = current_index; // 更新最近点的索引
Nearest_point_index = current_index; // 更新最近点的索引
// 如果距离小于 0.8 m，提前退出
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)
@ -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;
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;
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);
if (aft_distance < pre_distance)
{
@ -152,7 +157,7 @@ int Road_Planning_Find_Aim_Point(int start_index, int dest_num)
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;
int point = 0;
@ -164,7 +169,7 @@ int star_find_nearest_point(double lon, double lat, WRC_GPS_Point *Road_info)
dis = dis_temp;
point = i;
}
printf("i: %d , dis_temp: %lf", i, dis_temp);
LOG_DEBUG("i: %d , dis_temp: %lf", i, dis_temp);
}
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);
if (diff > 180)
diff = 360 - diff;
if (diff > 180) diff = 360 - diff;
// std::cout << "diff : " << diff << std::endl;
@ -211,8 +215,8 @@ void PL_ProcThread()
// double head_raw = g_NavInfo.Yaw_rad;
road_pos = Road_Planning_Find_Nearest_Point(des_pos - 3, g_NavInfo, GPSPointQueue);
des_pos = Road_Planning_Find_Aim_Point(road_pos, 3); // 找到当前点后第 3 个点，作为目标点
direction_pos = Road_Planning_Find_Aim_Point(road_pos, 6); // 找到当前点后第 6 个点，判断直行/转弯
des_pos = Road_Planning_Find_Aim_Point(road_pos, 3); // 找到当前点后第 3 个点，作为目标点
direction_pos = Road_Planning_Find_Aim_Point(road_pos, 6); // 找到当前点后第 6 个点，判断直行/转弯
// printf("当前位置: %d\n", road_pos);
// printf("目标位置: %d\n", des_pos);
@ -238,16 +242,15 @@ void PL_ProcThread()
// cout << " * * * * * * * * * * * * * * * * * * * * *" << endl;
// cout << endl;
// cout << "--- -target_head " << target_head << "--- current_head " << current_head << " " << abs(target_head - current_head) << endl;
// cout << endl;
// cout << "--- -distance " << distance << "- -nl_radius " << nl_radius << endl;
// cout << "--- -target_head " << target_head << "--- current_head " << current_head << " " <<
// abs(target_head - current_head) << endl; cout << endl; cout << "--- -distance " << distance << "- -nl_radius
// " << nl_radius << endl;
// cout << endl;
// cout << " * * * * * * * * * * * * * * * * * * * * *" << endl;
if (road_pos >= (GpsPointNum - 1))
{
x = 0.0;
y = 1.0;
pl_speed = 0;
@ -260,10 +263,8 @@ void PL_ProcThread()
double x_cm_tmp = 0;
double y_cm_tmp = 0;
ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat,
GPSPointQueue[des_pos].LongitudeInfo,
GPSPointQueue[des_pos].LatitudeInfo,
&x_cm_tmp, &y_cm_tmp);
ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat, GPSPointQueue[des_pos].LongitudeInfo,
GPSPointQueue[des_pos].LatitudeInfo, &x_cm_tmp, &y_cm_tmp);
x = x_cm_tmp;
y = y_cm_tmp;
@ -277,20 +278,20 @@ void PL_ProcThread()
return;
}
void *pl_thread(void *args)
void* pl_thread(void* args)
{
(void)args;
sleep(3);
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)
{
printf("open route file error\n");
LOG_ERROR("open route file error");
return NULL;
}
std::cout << "have opened " << "gps_load_now.txt" << std::endl;
LOG_INFO("have opened gps_load_now.txt");
char mystring[200];
memset(mystring, '\0', 200);
@ -318,11 +319,10 @@ void *pl_thread(void *args)
}
GpsPointNum = i / 4;
printf("point_num:%d\n", GpsPointNum);
LOG_INFO("point_num:%d", GpsPointNum);
fclose(fp_gps);
std::cout << std::endl;
printf("#################### auto drive on! ####################\n");
LOG_INFO("#################### auto drive on! ####################");
PL_ProcThread();
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 "sweeper_interfaces/msg/rtk.hpp"
#include "sweeper_interfaces/msg/pl.hpp"
#include "sweeper_interfaces/msg/route.hpp"
#include "sweeper_interfaces/msg/rtk.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::chrono; // 新增：时间戳命名空间
using namespace std::chrono; // 新增：时间戳命名空间
pthread_t pl_thread_t;
int is_start = 0;
@ -29,34 +31,39 @@ const duration<double> rtk_timeout = duration<double>(0.5);
class pl_node : public rclcpp::Node
{
public:
public:
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();
// 创建订阅者订阅话题
msg_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>("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_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>(
"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);
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_task = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&pl_node::timer_callback_task, this));
timer_pl =
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)
{
// 新增：更新最后接收时间戳
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);
g_rtk.reliability = 1;
@ -75,19 +82,19 @@ private:
if (is_start == 0 && msg->task_status == 1)
{
pthread_create(&pl_thread_t, NULL, pl_thread, NULL);
cout << " started" << endl;
LOG_INFO("started");
is_start = 1;
task_status = TaskStatus::PENDING;
}
else if (is_start == 1 && msg->task_status == 0)
{
pthread_cancel(pl_thread_t);
cout << "pl_thread_t is canceled " << endl;
LOG_INFO("pl_thread_t is canceled");
is_start = 0;
task_status = TaskStatus::COMPLETED;
}
// RCLCPP_INFO(this->get_logger(), " ==================================== is_start : %d", is_start);
// RCLCPP_INFO(this->get_logger(), " ==================================== task_status : %d", msg->task_status);
// LOG_INFO(" ==================================== is_start : %d", is_start);
// LOG_INFO(" ==================================== task_status : %d", msg->task_status);
}
void timer_callback_pl()
@ -98,12 +105,12 @@ private:
if (is_start == 1)
{
if (sock < 0) // 首次连接
if (sock < 0) // 首次连接
{
struct sockaddr_in serv_addr;
if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0)
{
RCLCPP_ERROR(this->get_logger(), "Socket creation error");
LOG_ERROR("Socket creation error");
return;
}
@ -112,31 +119,31 @@ private:
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);
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);
sock = -1;
return;
}
}
const char *message = "Status: RUNNING";
if (send(sock, message, strlen(message), 0) < 0) // 发送失败则重建连接
const char* message = "Status: RUNNING";
if (send(sock, message, strlen(message), 0) < 0) // 发送失败则重建连接
{
close(sock);
sock = -1;
return timer_callback_pl(); // 重试
return timer_callback_pl(); // 重试
}
}
else if (sock >= 0)
{
close(sock); // 停止时关闭连接
close(sock); // 停止时关闭连接
sock = -1;
}
@ -151,11 +158,11 @@ private:
message.speed = 0;
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
{
RCLCPP_ERROR(this->get_logger(), "RTK信号差，停车！！！");
LOG_ERROR("RTK信号差，停车！！！");
}
}
else
@ -163,17 +170,18 @@ private:
message.reliability = g_rtk.reliability;
// message.drive_mode = drive_mode;
if (drive_mode == Direction::STRAIGHT_D) // 直行
if (drive_mode == Direction::STRAIGHT_D) // 直行
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.enter_range = nl_within_radius;
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);
// 发布消息
msg_publisher_->publish(message);
@ -207,24 +215,28 @@ void initConfig()
std::ifstream in("config.json", std::ios::binary);
if (!in.is_open())
{
std::cout << "read config file error" << std::endl;
LOG_ERROR("read config file error");
return;
}
if (reader.parse(in, root))
{
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();
rclcpp::init(argc, argv);
// 初始化日志系统
logger::Logger::Init("pl", "./log");
/*创建对应节点的共享指针对象*/
auto node = std::make_shared<pl_node>("pl_node");
/* 运行节点，并检测退出信号*/
rclcpp::spin(node);
rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
// pthread_join(pl_thread_t, NULL);
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(sweeper_interfaces REQUIRED)
find_package(CURL REQUIRED)
find_package(logger REQUIRED)
include_directories(include/route ${catkin_INCLUDE_DIRS})
include_directories(
include/route
${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
add_executable(route_node src/route_node.cpp src/md5.cpp src/jsoncpp.cpp)
ament_target_dependencies(
route_node
DESTINATION lib/${PROJECT_NAME}
)
rclcpp
std_msgs
sweeper_interfaces
CURL
logger)
install(TARGETS route_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
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
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
endif()
ament_package()

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

@ -12,6 +12,7 @@
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>sweeper_interfaces</depend>
<depend>logger</depend>
<test_depend>ament_lint_auto</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 "sweeper_interfaces/msg/rtk.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>
#include <sstream>
#include <iomanip>
#include <mutex>
std::mutex collect_mutex; // 互斥锁，用于保护 isCollecting 变量
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_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度)
#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度)
using namespace std;
/* 存储接收RTK的GPS点信息 */
struct struct_rtk
@ -40,83 +42,87 @@ struct_rtk last_g_rtk = {};
string vid;
string upload_URL;
std::string destinationFilePath1 = "./gps_load_now.txt";
void *route_thread(void *args);
void* route_thread(void* args);
string calculate_md5(string filename)
{
MD5 md5;
ifstream file;
file.open(filename, ios::binary);
md5.update(file);
cout << md5.toString() << endl;
LOG_INFO("%s", md5.toString().c_str());
return md5.toString();
}
bool upload_file(string filename)
{
CURL *curl;
CURL* curl;
CURLcode ret;
curl = curl_easy_init();
struct curl_httppost *post = 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)
struct curl_httppost* post = 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_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_HTTPPOST, post);
ret = curl_easy_perform(curl);
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;
}
curl_formfree(post);
curl_easy_cleanup(curl);
cout << "上传成功" << endl;
LOG_INFO("上传成功");
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::ofstream dst(destinationFilePath, std::ios::binary);
if (!src)
{
std::cerr << "无法打开源文件: " << sourceFilePath << std::endl;
LOG_ERROR("无法打开源文件: %s", sourceFilePath.c_str());
return;
}
if (!dst)
{
std::cerr << "无法打开目标文件: " << destinationFilePath << std::endl;
LOG_ERROR("无法打开目标文件: %s", destinationFilePath.c_str());
return;
}
dst << src.rdbuf(); // 将源文件内容复制到目标文件
dst << src.rdbuf(); // 将源文件内容复制到目标文件
if (!dst)
{
std::cerr << "复制文件时出错" << std::endl;
LOG_ERROR("复制文件时出错");
}
}
class route_node : public rclcpp::Node
{
public:
public:
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)
{
if (msg->p_quality < 4 || msg->h_quality < 4)
{
g_rtk.reliability = 0;
RCLCPP_INFO(this->get_logger(), "rtk信号差!");
LOG_INFO("rtk信号差!");
}
if (first_flag)
{
@ -133,40 +139,34 @@ private:
if (!isCollecting)
{
// cout << "等待采集...." << endl;
RCLCPP_INFO(this->get_logger(), "等待采集.....");
LOG_INFO("等待采集.....");
}
else
{
// cout << "平台采集中" << endl;
RCLCPP_INFO(this->get_logger(), "平台采集中.....");
LOG_INFO("平台采集中.....");
}
}
void sub_callback(const sweeper_interfaces::msg::Sub::SharedPtr msg)
{
if (msg->get_route && !isCollecting)
{
std::lock_guard<std::mutex> lock(collect_mutex); // 加锁
std::lock_guard<std::mutex> lock(collect_mutex); // 加锁
isCollecting = true;
// cout << "平台开始采集" << endl;
RCLCPP_INFO(this->get_logger(), "平台开始采集");
LOG_INFO("平台开始采集");
pthread_create(&route_thread_t, NULL, route_thread, NULL);
}
else if (!msg->get_route && isCollecting)
{
isCollecting = false;
// cout << "平台结束采集" << endl;
RCLCPP_INFO(this->get_logger(), "平台结束采集");
LOG_INFO("平台结束采集");
pthread_cancel(route_thread_t);
if (upload_file(filename))
{
// cout << "上传成功" << endl;
RCLCPP_INFO(this->get_logger(), "上传成功");
LOG_INFO("上传成功");
}
else
{
// cout << "上传失败" << endl;
RCLCPP_INFO(this->get_logger(), "上传失败");
LOG_INFO("上传失败");
}
}
}
@ -178,13 +178,13 @@ private:
double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
{
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;
length = sqrt(x * x + y * y);
return length;
}
void *route_thread(void *args)
void* route_thread(void* args)
{
(void)args;
filename = "";
@ -199,12 +199,12 @@ void *route_thread(void *args)
oss << "routes/gps_load_" << timestamp << ".txt";
filename = oss.str();
FILE *fp_routes = fopen(filename.c_str(), "w");
FILE* fp_routes = fopen(filename.c_str(), "w");
usleep(5000);
while (1)
{
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);
fflush(fp_routes);
@ -212,7 +212,7 @@ void *route_thread(void *args)
last_g_rtk.lat = g_rtk.lat;
last_g_rtk.lon = g_rtk.lon;
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);
if (!in.is_open())
{
std::cout << "read config file error" << std::endl;
LOG_ERROR("read config file error");
return;
}
if (reader.parse(in, root))
{
vid = root["mqtt"]["vid"].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();
rclcpp::init(argc, argv);
/*初始化日志系统*/
logger::Logger::Init("route", "./log");
/*创建对应节点的共享指针对象*/
auto node = std::make_shared<route_node>("route_node");
/* 运行节点，并检测退出信号*/
rclcpp::spin(node);
rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
// pthread_join(route_thread_t, NULL);
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(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(logger REQUIRED)
# 搜索 src 目录下所有 .cpp 文件
file(GLOB SRC_FILES src/*.cpp)
@ -18,15 +19,19 @@ file(GLOB SRC_FILES src/*.cpp)
# 创建可执行文件
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
target_include_directories(
mc_node
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_include_directories(mc_node 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(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.rtr = RTR_FLAG;
// std::cout << "MCU frame : ";
// LOG_INFO("MCU frame : ");
// 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;
}

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

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

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

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

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

@ -1,12 +1,15 @@
#include "mc/can_utils.hpp"
#include <sstream>
#include <iomanip>
#include <sstream>
#include "logger/logger.h"
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 pub = context->publisher;
auto now = node->now();
@ -20,10 +23,10 @@ void receiveHandler(const CANFrame &frame, void *userData)
if (g_can_print_enable)
{
std::stringstream ss;
ss << "CAN ID: " << std::hex << std::uppercase
<< std::setw((frame.id > 0x7FF) ? 8 : 5) << std::setfill(' ') << frame.id << " Data: ";
ss << "CAN ID: " << std::hex << std::uppercase << std::setw((frame.id > 0x7FF) ? 8 : 5) << std::setfill(' ')
<< frame.id << " Data: ";
for (int i = 0; i < frame.dlc; ++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;
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_);
latest_msg_ = msg;
@ -10,15 +10,13 @@ void ControlCache::update(const sweeper_interfaces::msg::McCtrl &msg)
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_);
if (!has_data_)
return false;
if (!has_data_) return false;
auto now = std::chrono::steady_clock::now();
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time_).count() > 100)
return false;
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time_).count() > 100) return false;
msg = latest_msg_;
return true;

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

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

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

@ -1,73 +1,78 @@
#include "rclcpp/rclcpp.hpp"
#include "mc/get_config.h"
#include <iostream>
#include "logger/logger.h"
#include "mc/can_driver.h"
#include "mc/can_utils.hpp"
#include "mc/control_cache.hpp"
#include "mc/get_config.h"
#include "mc/timer_tasks.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include <iostream>
namespace sweeperMsg = sweeper_interfaces::msg;
CANDriver canctl;
void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
{
// std::cout << "\n 刹车: " << (msg->brake ? "已刹车" : "未刹车")
// << "\n 挡位: ";
// switch (msg->gear)
// {
// case 0:
// std::cout << "空挡";
// break;
// case 2:
// std::cout << "前进挡";
// break;
// case 1:
// std::cout << "后退挡";
// break;
// default:
// std::cout << "未知挡位(" << static_cast<int>(msg->gear) << ")";
// break;
// }
// std::cout << "\n 行走电机转速: " << static_cast<int>(msg->rpm) << " RPM"
// << "\n 轮端转向角度: " << msg->angle << "°"
// << "\n 清扫状态: " << (msg->sweep ? "正在清扫" : "未清扫")
// << std::endl;
// LOG_INFO("\n 刹车: %s", (msg->brake ? "已刹车" : "未刹车"));
// LOG_INFO(" 挡位: ");
// switch (msg->gear)
// {
// case 0:
// LOG_INFO("空挡");
// break;
// case 2:
// LOG_INFO("前进挡");
// break;
// case 1:
// LOG_INFO("后退挡");
// break;
// default:
// LOG_INFO("未知挡位(%d)", static_cast<int>(msg->gear));
// break;
// }
// LOG_INFO(" 行走电机转速: %d RPM", static_cast<int>(msg->rpm));
// LOG_INFO(" 轮端转向角度: %.1f°", msg->angle);
// LOG_INFO(" 清扫状态: %s", (msg->sweep ? "正在清扫" : "未清扫"));
control_cache.update(*msg);
control_cache.update(*msg);
}
int main(int argc, char **argv)
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("can_driver_node");
RCLCPP_INFO(node->get_logger(), "Starting mc package...");
rclcpp::init(argc, argv);
/*初始化日志系统*/
logger::Logger::Init("mc", "./log");
auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback);
auto node = rclcpp::Node::make_shared("can_driver_node");
LOG_INFO("Starting mc package...");
Config mc_config;
load_config(mc_config);
auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback);
if (!canctl.open(mc_config.can_dev))
{
RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str());
return -1;
}
Config mc_config;
load_config(mc_config);
auto context = std::make_shared<CanHandlerContext>();
context->node = node;
context->publisher = pub;
canctl.setReceiveCallback(receiveHandler, context.get());
if (!canctl.open(mc_config.can_dev))
{
LOG_ERROR("Failed to open CAN interface: %s", mc_config.can_dev.c_str());
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([&]()
{ canctl.close(); });
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
setupTimers(node, canctl);
rclcpp::on_shutdown([&]() { canctl.close(); });
rclcpp::spin(node);
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/control_cache.hpp"
#include "logger/logger.h"
#include "mc/can_struct.h"
#include "mc/control_cache.hpp"
// 定时器回调：MCU 控制
void mcuTimerTask(CANDriver &canctl)
void mcuTimerTask(CANDriver& canctl)
{
auto msg = get_safe_control();
mcu_cmd.setEnabled(true);
mcu_cmd.setGear(msg.gear);
mcu_cmd.setRPM(msg.rpm);
mcu_cmd.setBrake(msg.brake);
canctl.sendFrame(mcu_cmd.toFrame());
// std::cout << "mcuTimerTask" << std::endl;
// std::cout << "msg.gear " << msg.gear << std::endl;
// std::cout << "msg.brake " << msg.brake << std::endl;
// std::cout << "msg.rpm " << msg.rpm << std::endl;
auto msg = get_safe_control();
mcu_cmd.setEnabled(true);
mcu_cmd.setGear(msg.gear);
mcu_cmd.setRPM(msg.rpm);
mcu_cmd.setBrake(msg.brake);
canctl.sendFrame(mcu_cmd.toFrame());
// LOG_INFO("mcuTimerTask");
// LOG_INFO("msg.gear %d", msg.gear);
// LOG_INFO("msg.brake %d", msg.brake);
// LOG_INFO("msg.rpm %d", msg.rpm);
}
// 定时器回调：EPS 控制
void epsTimerTask(CANDriver &canctl)
void epsTimerTask(CANDriver& canctl)
{
auto msg = get_safe_control();
eps_cmd.setCenterCmd(0);
eps_cmd.setAngle(msg.angle);
eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame());
// std::cout << "epsTimerTask" << std::endl;
auto msg = get_safe_control();
eps_cmd.setCenterCmd(0);
eps_cmd.setAngle(msg.angle);
eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame());
// LOG_INFO("epsTimerTask");
}
// 定时器回调：VCU 扫地控制
// 修改timer_tasks.cpp中的vcuTimerTask函数
void vcuTimerTask(CANDriver &canctl)
void vcuTimerTask(CANDriver& canctl)
{
static bool vcu_initialized = false;
static bool last_sweep_state = false; // 记录上一次清扫状态
static bool vcu_initialized = false;
static bool last_sweep_state = false; // 记录上一次清扫状态
auto msg = get_safe_control();
auto msg = get_safe_control();
// 首次运行时初始化VCU控制
if (!vcu_initialized)
{
// 发送CAN使能指令
vcu_enable_cmd.setCANEnable(true);
canctl.sendFrame(vcu_enable_cmd.toFrame());
// std::cout << "mcuTimerTask" << std::endl;
vcu_initialized = true;
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled");
}
// 首次运行时初始化VCU控制
if (!vcu_initialized)
{
// 发送CAN使能指令
vcu_enable_cmd.setCANEnable(true);
canctl.sendFrame(vcu_enable_cmd.toFrame());
// LOG_INFO("mcuTimerTask");
vcu_initialized = true;
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)
{
// 根据清扫状态设置所有部件
int8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止
int8_t target_value2 = msg.sweep ? -100 : 100; // -100表示下沉，100表示抬升
if (sweep_state_changed)
{
// 根据清扫状态设置所有部件
int8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止
int8_t target_value2 = msg.sweep ? -100 : 100; // -100表示下沉，100表示抬升
// ===== 控制0x211报文 (电机M1-M7) =====
vcu_motor1_cmd.setByte0(msg.sweep ? 1 : 0); // 1表示正向运行，0表示停止
vcu_motor1_cmd.setByte1(target_value);
vcu_motor1_cmd.setByte2(target_value);
vcu_motor1_cmd.setByte3(target_value);
vcu_motor1_cmd.setByte4(target_value2);
vcu_motor1_cmd.setByte5(target_value2);
vcu_motor1_cmd.setByte6(target_value);
vcu_motor1_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor1_cmd.toFrame());
// std::cout << "vcuTimerTask1" << std::endl;
// ===== 控制0x211报文 (电机M1-M7) =====
vcu_motor1_cmd.setByte0(msg.sweep ? 1 : 0); // 1表示正向运行，0表示停止
vcu_motor1_cmd.setByte1(target_value);
vcu_motor1_cmd.setByte2(target_value);
vcu_motor1_cmd.setByte3(target_value);
vcu_motor1_cmd.setByte4(target_value2);
vcu_motor1_cmd.setByte5(target_value2);
vcu_motor1_cmd.setByte6(target_value);
vcu_motor1_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor1_cmd.toFrame());
// LOG_INFO("vcuTimerTask1");
// ===== 控制0x212报文 (电机M8和LED输出) =====
vcu_motor2_cmd.setByte0(target_value);
vcu_motor2_cmd.setByte1(target_value);
vcu_motor2_cmd.setByte2(target_value);
vcu_motor2_cmd.setByte3(target_value);
vcu_motor2_cmd.setByte4(target_value);
vcu_motor2_cmd.setByte5(target_value);
vcu_motor2_cmd.setByte6(target_value);
vcu_motor2_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor2_cmd.toFrame());
// std::cout << "vcuTimerTask2" << std::endl;
// ===== 控制0x212报文 (电机M8和LED输出) =====
vcu_motor2_cmd.setByte0(target_value);
vcu_motor2_cmd.setByte1(target_value);
vcu_motor2_cmd.setByte2(target_value);
vcu_motor2_cmd.setByte3(target_value);
vcu_motor2_cmd.setByte4(target_value);
vcu_motor2_cmd.setByte5(target_value);
vcu_motor2_cmd.setByte6(target_value);
vcu_motor2_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor2_cmd.toFrame());
// LOG_INFO("vcuTimerTask2");
// RCLCPP_INFO(rclcpp::get_logger("timer_tasks"),
// "[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
// LOG_INFO("[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
// 更新状态记录
last_sweep_state = msg.sweep;
}
// 更新状态记录
last_sweep_state = msg.sweep;
}
}
// 定时器回调：BMS 查询任务
void bmsTimerTask(CANDriver &canctl)
void bmsTimerTask(CANDriver& canctl)
{
static bool bms_initialized = false;
static bool bms_initialized = false;
// 首次运行时初始化日志
if (!bms_initialized)
{
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[BMS] Query task started");
bms_initialized = true;
}
// 首次运行时初始化日志
if (!bms_initialized)
{
LOG_INFO("[BMS] Query task started");
bms_initialized = true;
}
// 周期性发送
canctl.sendFrame(bms_query_0x100.toFrame());
canctl.sendFrame(bms_query_0x101.toFrame());
// 周期性发送
canctl.sendFrame(bms_query_0x100.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(
std::chrono::milliseconds(50),
[&canctl]()
{ mcuTimerTask(canctl); });
static auto timer_mcu =
node->create_wall_timer(std::chrono::milliseconds(50), [&canctl]() { mcuTimerTask(canctl); });
static auto timer_eps = node->create_wall_timer(
std::chrono::milliseconds(50),
[&canctl]()
{ epsTimerTask(canctl); });
static auto timer_eps =
node->create_wall_timer(std::chrono::milliseconds(50), [&canctl]() { epsTimerTask(canctl); });
static auto timer_vcu = node->create_wall_timer(
std::chrono::milliseconds(100),
[&canctl]()
{ vcuTimerTask(canctl); });
static auto timer_vcu =
node->create_wall_timer(std::chrono::milliseconds(100), [&canctl]() { vcuTimerTask(canctl); });
static auto timer_bms = node->create_wall_timer(
std::chrono::milliseconds(200),
[&canctl]()
{ bmsTimerTask(canctl); });
static auto timer_bms =
node->create_wall_timer(std::chrono::milliseconds(200), [&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(rclcpp REQUIRED)
find_package(sweeper_interfaces REQUIRED) # ⭐ 新增
find_package(sweeper_interfaces REQUIRED)
find_package(logger REQUIRED)
# =========================
# include 路径
# =========================
include_directories(
include
)
include_directories(include)
# =========================
# 可执行文件
# =========================
add_executable(vehicle_params_node
src/vehicle_params_node.cpp
)
add_executable(vehicle_params_node src/vehicle_params_node.cpp)
# =========================
# 链接依赖
# =========================
ament_target_dependencies(vehicle_params_node
rclcpp
sweeper_interfaces # ⭐ 新增
)
ament_target_dependencies(vehicle_params_node rclcpp sweeper_interfaces logger)
# =========================
# 安装
# =========================
install(TARGETS vehicle_params_node
DESTINATION lib/${PROJECT_NAME}
)
install(TARGETS vehicle_params_node DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY include/
DESTINATION include/
)
install(DIRECTORY include/ DESTINATION include/)
# =========================
# 导出

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

@ -6,10 +6,8 @@
<name>vehicle_params</name>
<version>0.1.0</version>
<description>
Central vehicle identity provider node.
Fetches IMEI / VID from B-side service and publishes them as ROS2 messages.
</description>
<description> Central vehicle identity provider node. Fetches IMEI / VID from B-side service and
publishes them as ROS2 messages. </description>
<maintainer email="zxwl@44.com">nvidia</maintainer>
<license>Apache-2.0</license>
@ -19,7 +17,8 @@
<!-- runtime deps -->
<depend>rclcpp</depend>
<depend>sweeper_interfaces</depend> <!-- ⭐ 新增 -->
<depend>sweeper_interfaces</depend>
<depend>logger</depend>
<!-- tests -->
<test_depend>ament_lint_auto</test_depend>
@ -28,4 +27,4 @@
<export>
<build_type>ament_cmake</build_type>
</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 <chrono>
#include <nlohmann/json.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <thread>
#include "vehicle_params/httplib.h"
#include "logger/logger.h"
#include "sweeper_interfaces/msg/vehicle_identity.hpp"
#include "vehicle_params/httplib.h"
using sweeper_interfaces::msg::VehicleIdentity;
class VehicleParamsNode : public rclcpp::Node
{
public:
VehicleParamsNode()
: Node("vehicle_params_node")
public:
VehicleParamsNode() : Node("vehicle_params_node")
{
RCLCPP_INFO(this->get_logger(), "vehicle_params_node starting...");
LOG_INFO("vehicle_params_node starting...");
// ---------- neardi ----------
this->declare_parameter<std::string>("neardi.host", "192.168.5.1");
this->declare_parameter<int>("neardi.port", 8080);
// ---------- publisher ----------
identity_pub_ = this->create_publisher<VehicleIdentity>(
"/vehicle/identity",
rclcpp::QoS(1).transient_local().reliable());
identity_pub_ =
this->create_publisher<VehicleIdentity>("/vehicle/identity", rclcpp::QoS(1).transient_local().reliable());
// ---------- worker ----------
worker_ = std::thread([this]()
{ fetch_from_neardi_loop(); });
worker_ = std::thread([this]() { fetch_from_neardi_loop(); });
}
~VehicleParamsNode()
{
running_.store(false);
if (worker_.joinable())
worker_.join();
if (worker_.joinable()) worker_.join();
}
private:
private:
std::atomic<bool> running_{true};
std::thread worker_;
rclcpp::Publisher<VehicleIdentity>::SharedPtr identity_pub_;
@ -58,15 +54,13 @@ private:
while (rclcpp::ok() && running_.load())
{
RCLCPP_INFO(this->get_logger(),
"Requesting vehicle identity from neardi (%s:%d)...",
host.c_str(), port);
LOG_INFO("Requesting vehicle identity from neardi (%s:%d)...", host.c_str(), port);
auto res = cli.Post("/api/v1/device/register");
if (!res || res->status != 200)
{
RCLCPP_WARN(this->get_logger(), "neardi request failed, retrying...");
LOG_WARN("neardi request failed, retrying...");
retry_sleep();
continue;
}
@ -97,11 +91,9 @@ private:
identity_pub_->publish(msg);
RCLCPP_INFO(this->get_logger(),
"Vehicle identity published: IMEI=%s, VID=%s",
imei.c_str(), vid.c_str());
LOG_INFO("Vehicle identity published: IMEI=%s, VID=%s", imei.c_str(), vid.c_str());
return; // 成功一次即可
return; // 成功一次即可
}
catch (...)
{
@ -110,16 +102,19 @@ private:
}
}
void retry_sleep()
{
std::this_thread::sleep_for(std::chrono::seconds(2));
}
void retry_sleep() { 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);
/*初始化日志系统*/
logger::Logger::Init("vehicle_params", "./log");
rclcpp::spin(std::make_shared<VehicleParamsNode>());
rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
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(std_msgs REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(logger REQUIRED)
include_directories(include ${catkin_INCLUDE_DIRS})
@ -44,7 +45,8 @@ ament_target_dependencies(
ament_index_cpp
rclcpp
sweeper_interfaces
std_msgs)
std_msgs
logger)
# 包含 radio_ctrl 头文件
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
{
// std::cout << "Current error codes: ";
// for (int code : errors)
// {
// std::cout << code << " ";
// LOG_INFO("Current error codes: ");
// for (int code : getAllErrorCodes()) {
// LOG_INFO("%d ", 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>radio_ctrl</depend>
<depend>ament_index_cpp</depend>
<depend>logger</depend>
<buildtool_depend>ament_cmake</buildtool_depend>
@ -20,4 +21,4 @@
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
</package>

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

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

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

@ -2,6 +2,8 @@
#include <rclcpp/rclcpp.hpp>
#include "logger/logger.h"
static constexpr int64_t WARN_INTERVAL_MS = 10 * 1000;
static constexpr int64_t IDENTITY_TIMEOUT_MS = 15 * 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)
{
RCLCPP_WARN(logger, "MQTT connection lost.");
LOG_WARN("MQTT connection lost.");
}
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)
{
mon.ok = false;
RCLCPP_WARN(rclcpp::get_logger("input_monitor"), "%s timeout (%ld ms without data).", name,
now - mon.last_rx_ts);
LOG_WARN("%s timeout (%ld ms without data).", name, now - mon.last_rx_ts);
mon.last_warn_ts = now;
return;
}
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;
}
}

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

@ -5,6 +5,7 @@
#include <string>
#include <vector>
#include "logger/logger.h"
#include "mqtt_report/can/can_decoder.h"
#include "mqtt_report/core/input_health_monitor.h"
#include "mqtt_report/core/vehicle_context.h"
@ -75,7 +76,7 @@ class VehicleReportNode : public rclcpp::Node
ctx_.vid = msg->vid;
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();
@ -168,28 +169,31 @@ int main(int argc, char** argv)
}
rclcpp::init(argc, argv);
/*初始化日志系统*/
logger::Logger::Init("mqtt_report", "./log");
Config config;
auto logger = rclcpp::get_logger("main");
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;
}
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,
config.mqtt_username.empty() ? "<anonymous>" : config.mqtt_username.c_str());
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());
RCLCPP_INFO(logger, "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());
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());
auto node = std::make_shared<VehicleReportNode>(config);
rclcpp::spin(node);
rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
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(sweeper_interfaces REQUIRED)
find_package(CURL REQUIRED)
find_package(logger REQUIRED)
add_executable(
sub_node
@ -43,7 +44,8 @@ ament_target_dependencies(
rclcpp
std_msgs
sweeper_interfaces
CURL)
CURL
logger)
if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64)
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>std_msgs</depend>
<depend>sweeper_interfaces</depend>
<depend>logger</depend>
<test_depend>ament_lint_auto</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 <string>
#include "logger/logger.h"
#include "sub/control_state.hpp"
#include "sub/json.h"
#include "sub/mqtt_receiver.hpp"
@ -66,9 +67,6 @@ int main(int argc, char** argv)
return 1;
}
std::cout << "MQTT address: " << cfg.address << "\n";
std::cout << "MQTT topic template: " << cfg.remote_topic_template << "\n";
// 2) shared state
ControlState state;
{
@ -82,10 +80,19 @@ int main(int argc, char** argv)
// 4) ROS2 spin
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");
rclcpp::spin(node);
rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
// 5) stop mqtt
mqtt.stop();
return 0;

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

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

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

@ -2,6 +2,7 @@
#include <chrono>
#include "logger/logger.h"
#include "sub/control_mapper.hpp"
namespace sub_node_pkg
@ -9,7 +10,7 @@ namespace sub_node_pkg
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_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_.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

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

@ -25,6 +25,7 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED)
find_package(CURL REQUIRED)
find_package(logger REQUIRED)
include_directories(include/task_manager include/paho_mqtt_3c ${catkin_INCLUDE_DIRS})
@ -41,7 +42,8 @@ ament_target_dependencies(
rclcpp
std_msgs
sweeper_interfaces
CURL)
CURL
logger)
if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64)
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>std_msgs</depend>
<depend>sweeper_interfaces</depend>
<depend>logger</depend>
<test_depend>ament_lint_auto</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 <memory>
#include "logger/logger.h"
MQTTManager::MQTTManager()
: client_(nullptr),
is_running_(false),
@ -58,7 +60,7 @@ bool MQTTManager::reconnect()
{
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_);
if (rc != MQTTCLIENT_SUCCESS)
@ -74,7 +76,7 @@ bool MQTTManager::reconnect()
return false;
}
std::cout << "Successfully reconnected to MQTT server." << std::endl;
LOG_INFO("Successfully reconnected to MQTT server.");
reconnect_attempts_ = 0;
is_heartbeat_lost_ = false;
last_message_time_ = std::chrono::steady_clock::now();
@ -105,7 +107,7 @@ bool MQTTManager::subscribe(const string& topic, int qos)
return false;
}
std::cout << "Successfully subscribed to topic: " << topic << std::endl;
LOG_INFO("Successfully subscribed to topic: %s", topic.c_str());
return true;
}
@ -148,7 +150,7 @@ void MQTTManager::start()
{
if (is_running_)
{
std::cout << "MQTT manager is already running." << std::endl;
LOG_INFO("MQTT manager is already running.");
return;
}
@ -161,7 +163,7 @@ void MQTTManager::start()
is_running_ = true;
mqtt_thread_ = std::thread(&MQTTManager::mqttLoop, this);
std::cout << "MQTT manager started." << std::endl;
LOG_INFO("MQTT manager started.");
}
void MQTTManager::stop()
@ -182,7 +184,7 @@ void MQTTManager::stop()
}
MQTTClient_destroy(&client_);
std::cout << "MQTT manager stopped." << std::endl;
LOG_INFO("MQTT manager stopped.");
}
void MQTTManager::mqttLoop()
@ -215,7 +217,7 @@ void MQTTManager::mqttLoop()
if (duration > heartbeat_timeout_ && !is_heartbeat_lost_)
{
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_)
{
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->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_)
{
@ -248,7 +250,7 @@ int MQTTManager::onMessageArrived(void* context, char* topicName, int topicLen,
void MQTTManager::onConnectionLost(void* context, char* cause)
{
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_)
{
@ -259,7 +261,7 @@ void MQTTManager::onConnectionLost(void* context, char* cause)
void MQTTManager::onDelivered(void* context, MQTTClient_deliveryToken dt)
{
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_)
{

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

@ -1,40 +1,38 @@
#include "task_manager.hpp"
#include "md5.h"
#include <iostream>
#include <fstream>
#include <cstdlib>
#include <unistd.h>
TaskManager::TaskManager()
: is_running_(false), task_status_(0),
destination_file_path_("./gps_load_now.txt")
#include <cstdlib>
#include <fstream>
#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_.status = TaskStatus::PENDING;
}
TaskManager::~TaskManager()
{
stop();
}
TaskManager::~TaskManager() { stop(); }
void TaskManager::start()
{
if (is_running_)
{
std::cout << "TaskManager is already running." << std::endl;
LOG_WARN("TaskManager is already running.");
return;
}
is_running_ = true;
task_thread_ = std::thread(&TaskManager::processTasksLoop, this);
std::cout << "TaskManager started." << std::endl;
LOG_INFO("TaskManager started.");
}
void TaskManager::stop()
{
if (!is_running_)
return;
if (!is_running_) return;
{
std::lock_guard<std::mutex> lock(queue_mutex_);
@ -47,10 +45,10 @@ void TaskManager::stop()
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_);
@ -83,39 +81,38 @@ void TaskManager::updateCurrentTask(int taskId, TaskStatus status)
current_task_.id = taskId;
current_task_.status = status;
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;
std::ifstream file;
file.open(filename, std::ios::binary);
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 "";
}
md5.update(file);
return md5.toString();
}
bool TaskManager::downloadFile(const string &url, const string &expected_md5,
const string &filename)
bool TaskManager::downloadFile(const string& url, const string& expected_md5, const string& filename)
{
if (url.empty())
{
std::cerr << "Download URL is empty." << std::endl;
LOG_ERROR("Download URL is empty.");
return false;
}
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());
if (result != 0)
{
std::cerr << "Download failed: " << url << std::endl;
LOG_ERROR("Download failed: %s", url.c_str());
return false;
}
@ -124,31 +121,29 @@ bool TaskManager::downloadFile(const string &url, const string &expected_md5,
std::string actual_md5 = calculate_md5(filepath);
if (actual_md5 != expected_md5)
{
std::cerr << "MD5 verification failed. Expected: " << expected_md5
<< ", Actual: " << actual_md5 << std::endl;
LOG_ERROR("MD5 verification failed. Expected: %s, Actual: %s", expected_md5.c_str(), actual_md5.c_str());
remove(filepath.c_str());
return false;
}
std::cout << "File downloaded and verified: " << filepath << std::endl;
LOG_INFO("File downloaded and verified: %s", filepath.c_str());
return true;
}
void TaskManager::copyFileAndOverwrite(const string &sourceFilePath,
const string &destinationFilePath)
void TaskManager::copyFileAndOverwrite(const string& sourceFilePath, const string& destinationFilePath)
{
std::ifstream src(sourceFilePath, std::ios::binary);
std::ofstream dst(destinationFilePath, std::ios::binary);
if (!src)
{
std::cerr << "Failed to open source file: " << sourceFilePath << std::endl;
LOG_ERROR("Failed to open source file: %s", sourceFilePath.c_str());
return;
}
if (!dst)
{
std::cerr << "Failed to open destination file: " << destinationFilePath << std::endl;
LOG_ERROR("Failed to open destination file: %s", destinationFilePath.c_str());
return;
}
@ -156,15 +151,15 @@ void TaskManager::copyFileAndOverwrite(const string &sourceFilePath,
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;
std::cout << "URL: " << json_data.value.routeInfo.url << std::endl;
std::cout << "MD5: " << json_data.value.routeInfo.md5 << std::endl;
LOG_INFO("Processing start task ID: %d", json_data.value.id);
LOG_INFO("URL: %s", json_data.value.routeInfo.url.c_str());
LOG_INFO("MD5: %s", json_data.value.routeInfo.md5.c_str());
// 更新当前任务ID
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)
{
std::cout << "File not found, downloading: " << downFileName << std::endl;
if (!downloadFile(json_data.value.routeInfo.url,
json_data.value.routeInfo.md5, downFileName))
LOG_INFO("File not found, downloading: %s", downFileName.c_str());
if (!downloadFile(json_data.value.routeInfo.url, 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);
if (send_response_callback_)
{
@ -191,7 +185,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo)
}
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;
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_)
@ -213,7 +207,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, 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;
updateCurrentTask(taskId, TaskStatus::COMPLETED);
@ -232,8 +226,7 @@ void TaskManager::processTasksLoop()
std::unique_lock<std::mutex> lock(queue_mutex_);
// 等待队列中有任务或线程需要停止
queue_cv_.wait(lock, [this]
{ return !task_queue_.empty() || !is_running_; });
queue_cv_.wait(lock, [this] { return !task_queue_.empty() || !is_running_; });
// 检查是否需要退出
if (!is_running_ && task_queue_.empty())
@ -263,9 +256,9 @@ void TaskManager::processTasksLoop()
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 "json.h"
#include "logger/logger.h"
#include "mqtt_manager.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/task.hpp"
@ -52,12 +53,12 @@ string generate_mqtt_client_id();
// 信号处理函数
void signalHandler(int signum)
{
std::cout << "Interrupt signal (" << signum << ") received." << std::endl;
LOG_INFO("Interrupt signal (%d) received.", signum);
signal_received = signum;
if (signum == SIGINT && rclcpp::ok())
{
std::cout << "Shutting down ROS2 node..." << std::endl;
LOG_INFO("Shutting down ROS2 node.");
rclcpp::shutdown();
}
}
@ -87,26 +88,26 @@ ROUTE_INFO get_route_info(const Json::Value& root)
if (root.isMember("routeName") && root["routeName"].isString())
route_info.routeName = root["routeName"].asString();
else
std::cerr << "Missing or invalid 'routeName' field" << std::endl;
LOG_ERROR("Missing or invalid 'routeName' field");
if (root.isMember("fileName") && root["fileName"].isString())
route_info.fileName = root["fileName"].asString();
else
std::cerr << "Missing or invalid 'fileName' field" << std::endl;
LOG_ERROR("Missing or invalid 'fileName' field");
if (root.isMember("url") && root["url"].isString())
route_info.url = root["url"].asString();
else
std::cerr << "Missing or invalid 'url' field" << std::endl;
LOG_ERROR("Missing or invalid 'url' field");
if (root.isMember("md5") && root["md5"].isString())
route_info.md5 = root["md5"].asString();
else
std::cerr << "Missing or invalid 'md5' field" << std::endl;
LOG_ERROR("Missing or invalid 'md5' field");
}
else
{
std::cerr << "routeInfo is not a valid JSON object" << std::endl;
LOG_ERROR("routeInfo is not a valid JSON object");
}
return route_info;
@ -122,31 +123,31 @@ TASK_VALUE get_task_value(const Json::Value& root)
if (root.isMember("id") && root["id"].isInt())
task_value.id = root["id"].asInt();
else
std::cerr << "Missing or invalid 'id' field" << std::endl;
LOG_ERROR("Missing or invalid 'id' field");
if (root.isMember("name") && root["name"].isString())
task_value.name = root["name"].asString();
else
std::cerr << "Missing or invalid 'name' field" << std::endl;
LOG_ERROR("Missing or invalid 'name' field");
if (root.isMember("mode") && root["mode"].isInt())
task_value.mode = root["mode"].asInt();
else
std::cerr << "Missing or invalid 'mode' field" << std::endl;
LOG_ERROR("Missing or invalid 'mode' field");
if (root.isMember("count") && root["count"].isInt())
task_value.count = root["count"].asInt();
else
std::cerr << "Missing or invalid 'count' field" << std::endl;
LOG_ERROR("Missing or invalid 'count' field");
if (root.isMember("routeInfo") && root["routeInfo"].isObject())
task_value.routeInfo = get_route_info(root["routeInfo"]);
else
std::cerr << "Missing or invalid 'routeInfo' field" << std::endl;
LOG_ERROR("Missing or invalid 'routeInfo' field");
}
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;
@ -161,13 +162,13 @@ bool loadConfig(const string& config_file)
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;
}
if (!reader.parse(in, root))
{
std::cout << "Failed to parse config file." << std::endl;
LOG_ERROR("Failed to parse config file.");
return false;
}
@ -182,10 +183,10 @@ bool loadConfig(const string& config_file)
in.close();
std::cout << "Config loaded successfully." << std::endl;
std::cout << "MQTT Address: " << config.mqtt_address << std::endl;
std::cout << "Topic Sub template: " << config.mqtt_topic_sub_task_tmpl << std::endl;
std::cout << "Topic Pub template: " << config.mqtt_topic_push_status_tmpl << std::endl;
LOG_INFO("Config loaded successfully.");
LOG_INFO("MQTT Address: %s", config.mqtt_address.c_str());
LOG_INFO("Topic Sub template: %s", config.mqtt_topic_sub_task_tmpl.c_str());
LOG_INFO("Topic Pub template: %s", config.mqtt_topic_push_status_tmpl.c_str());
return true;
}
@ -206,11 +207,11 @@ void sendGeneralResponse(const string& topic, long seqNo, int code, const string
Json::FastWriter writer;
string responseJson = writer.write(responseData);
std::cout << "[General Response] seqNo=" << seqNo << ", code=" << code << ", msg=" << msg << std::endl;
std::cout << "Response JSON: " << responseJson << std::endl;
LOG_INFO("[General Response] seqNo=%d, code=%d, msg=%s", seqNo, code, msg.c_str());
LOG_DEBUG("Response JSON: %s", responseJson.c_str());
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;
string responseJson = writer.write(responseData);
std::cout << "[Task Response] seqNo=" << seqNo << ", code=" << code << ", taskId=" << current_task_id
<< ", taskStatus=" << (int)current_status << std::endl;
std::cout << "Response JSON: " << responseJson << std::endl;
LOG_INFO("[Task Response] seqNo=%ld, code=%d, taskId=%d, taskStatus=%d", seqNo, code, current_task_id,
(int)current_status);
LOG_DEBUG("Response JSON: %s", responseJson.c_str());
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;
}
bool success = mqtt_manager.publish(mqtt_topic_sub_task, responseJson, 0);
std::cout << "Task response publish to [" << mqtt_topic_sub_task << "] " << (success ? "[OK]" : "[FAILED]")
<< std::endl;
LOG_INFO("Task response publish to [%s] %s", mqtt_topic_sub_task.c_str(), (success ? "[OK]" : "[FAILED]"));
}
// MQTT消息回调
@ -265,12 +265,12 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message
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;
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")
{
@ -301,12 +301,12 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message
mqtt_json.seqNo = seqNo;
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);
}
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");
}
}
@ -321,14 +321,14 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message
mqtt_json.seqNo = seqNo;
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);
sendTaskResponse(seqNo, 200, "Task stop command received");
}
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");
}
}
@ -344,11 +344,11 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message
}
else if (root.isMember("type") && root["type"].asString() == "noReply")
{
std::cout << "NoReply message received" << std::endl;
LOG_INFO("NoReply message received");
}
else
{
std::cout << "Unknown message type" << std::endl;
LOG_INFO("Unknown message type");
}
delete[] buffer;
@ -364,14 +364,14 @@ void try_subscribe_task_topic()
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（200ms间隔持续上报）
void reportTaskStatusToMqtt()
{
std::cout << "DEBUG: Status report thread started" << std::endl;
LOG_DEBUG("Status report thread started");
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节点类
@ -405,7 +405,7 @@ class TaskManagerNode : public rclcpp::Node
public:
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);
task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>(
"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}");
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(),
mqtt_topic_sub_task.c_str(), mqtt_topic_push_status.c_str());
LOG_INFO("[TASK] identity ready VID=%s => sub=%s pub=%s", vid.c_str(), mqtt_topic_sub_task.c_str(),
mqtt_topic_push_status.c_str());
try_subscribe_task_topic();
});
@ -448,7 +448,7 @@ class TaskManagerNode : public rclcpp::Node
if (msg.task_status != last_status)
{
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;
@ -462,7 +462,7 @@ class TaskManagerNode : public rclcpp::Node
}
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);
// 初始化日志系统
logger::Logger::Init("task_manager", "./log");
// 默认配置路径
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))
{
std::cerr << "Failed to load configuration." << std::endl;
LOG_ERROR("Failed to load configuration.");
return 1;
}
@ -503,7 +506,7 @@ int main(int argc, char** argv)
string client_id = generate_mqtt_client_id();
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;
}
@ -516,7 +519,7 @@ int main(int argc, char** argv)
mqtt_manager.setConnRestoredCallback(
[](const char* cause)
{
std::cout << "[TASK] MQTT reconnected: " << cause << std::endl;
LOG_INFO("[TASK] MQTT reconnected: %s", cause);
subscribed.store(false);
try_subscribe_task_topic();
});
@ -530,7 +533,7 @@ int main(int argc, char** argv)
rclcpp::init(argc, argv);
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);
rclcpp::executors::SingleThreadedExecutor executor;
@ -542,7 +545,7 @@ int main(int argc, char** argv)
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
std::cout << "Shutting down components..." << std::endl;
LOG_INFO("Shutting down components...");
task_manager.stop();
mqtt_manager.stop();
@ -551,5 +554,8 @@ int main(int argc, char** argv)
status_report_thread.join();
}
// 关闭日志系统
logger::Logger::Shutdown();
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(sweeper_interfaces REQUIRED)
find_package(std_msgs REQUIRED)
find_package(logger REQUIRED)
file(GLOB SRC_FILES "src/*.cpp")
add_executable(ctrl_arbiter_node ${SRC_FILES})
ament_target_dependencies(ctrl_arbiter_node
ament_target_dependencies(
ctrl_arbiter_node
rclcpp
sweeper_interfaces
std_msgs
)
logger)
install(TARGETS
ctrl_arbiter_node
DESTINATION lib/${PROJECT_NAME}
)
install(TARGETS ctrl_arbiter_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
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 <mutex>
#include "logger/logger.h"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
using namespace std::chrono_literals;
namespace sweeperMsg = sweeper_interfaces::msg;
class ArbitrationNode : public rclcpp::Node
{
public:
ArbitrationNode()
: Node("control_arbitrator")
public:
ArbitrationNode() : Node("control_arbitrator")
{
timeout_ms_ = 200; // 200毫秒超时阈值，可调整
timeout_ms_ = 200; // 200毫秒超时阈值，可调整
publisher_ = this->create_publisher<sweeperMsg::McCtrl>("/mc_ctrl", 10);
sub_radio_ = this->create_subscription<sweeperMsg::McCtrl>(
"/radio_mc_ctrl", 10,
[this](const sweeperMsg::McCtrl::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
radio_msg_ = *msg;
radio_last_time_ = this->now();
radio_valid_ = true;
});
sub_radio_ = this->create_subscription<sweeperMsg::McCtrl>("/radio_mc_ctrl", 10,
[this](const sweeperMsg::McCtrl::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
radio_msg_ = *msg;
radio_last_time_ = this->now();
radio_valid_ = true;
});
sub_remote_ = this->create_subscription<sweeperMsg::McCtrl>(
"/remote_mc_ctrl", 10,
[this](const sweeperMsg::McCtrl::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
remote_msg_ = *msg;
remote_last_time_ = this->now();
remote_valid_ = true;
});
sub_remote_ = this->create_subscription<sweeperMsg::McCtrl>("/remote_mc_ctrl", 10,
[this](const sweeperMsg::McCtrl::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
remote_msg_ = *msg;
remote_last_time_ = this->now();
remote_valid_ = true;
});
sub_auto_ = this->create_subscription<sweeperMsg::McCtrl>(
"/auto_mc_ctrl", 10,
[this](const sweeperMsg::McCtrl::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
auto_msg_ = *msg;
auto_last_time_ = this->now();
auto_valid_ = true;
});
sub_auto_ = this->create_subscription<sweeperMsg::McCtrl>("/auto_mc_ctrl", 10,
[this](const sweeperMsg::McCtrl::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
auto_msg_ = *msg;
auto_last_time_ = this->now();
auto_valid_ = true;
});
timer_ = this->create_wall_timer(20ms, [this]()
{ this->arbitrateAndPublish(); });
RCLCPP_INFO(this->get_logger(), "ArbitrationNode started, waiting for control sources...");
timer_ = this->create_wall_timer(20ms, [this]() { this->arbitrateAndPublish(); });
LOG_INFO("ArbitrationNode started, waiting for control sources...");
}
private:
private:
void arbitrateAndPublish()
{
std::lock_guard<std::mutex> lock(mutex_);
@ -61,21 +58,21 @@ private:
if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000)
{
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;
}
if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000)
{
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;
}
if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000)
{
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;
}
@ -90,8 +87,7 @@ private:
safe_msg.sweep = false;
publisher_->publish(safe_msg);
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
"[ARBITER] All sources timeout, publishing FAILSAFE control");
LOG_WARN_THROTTLE(1000, "[ARBITER] All sources timeout, publishing FAILSAFE control");
}
rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr publisher_;
@ -109,11 +105,17 @@ private:
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);
auto node = std::make_shared<ArbitrationNode>();
rclcpp::spin(node);
rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
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(std_msgs REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(logger REQUIRED)
add_executable(radio_ctrl_node src/radio_ctrl.cpp src/uart_handler.cpp)
target_include_directories(
radio_ctrl_node PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
target_include_directories(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})
@ -28,10 +34,7 @@ install(TARGETS radio_ctrl_node DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
# 安装头文件到正确路径
install(
DIRECTORY include/
DESTINATION include
)
install(DIRECTORY include/ DESTINATION 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
#include <fstream>
#include <string>
#include <iostream>
#include <string>
#include "ament_index_cpp/get_package_share_directory.hpp"
#include "nlohmann/json.hpp"
@ -16,7 +17,7 @@ struct RmoCtlConfig
double eps_angle_max;
};
bool load_config(RmoCtlConfig &config)
bool load_config(RmoCtlConfig& config)
{
try
{
@ -39,7 +40,7 @@ bool load_config(RmoCtlConfig &config)
return true;
}
catch (const std::exception &e)
catch (const std::exception& e)
{
std::cerr << "Error parsing config: " << e.what() << std::endl;
return false;

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

@ -1,31 +1,32 @@
#ifndef 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 <string>
#include <cstring>
#include <iostream>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <termios.h>
#include <string>
#include <thread>
#include <vector>
#include <algorithm>
class UartHandler
{
public:
UartHandler(const std::string &device, int baudrate = 100000);
public:
UartHandler(const std::string& device, int baudrate = 100000);
~UartHandler();
bool open_serial(); // 打开串口
void start_reading(); // 启动读取数据的线程
void stop_reading(); // 停止读取数据的线程
int get_channel_value(int channel); // 获取指定通道的数据
bool get_data_safe(); // 获取数据安全性
bool open_serial(); // 打开串口
void start_reading(); // 启动读取数据的线程
void stop_reading(); // 停止读取数据的线程
int get_channel_value(int channel); // 获取指定通道的数据
bool get_data_safe(); // 获取数据安全性
private:
private:
std::string serial_device;
int baudrate;
int fd;
@ -44,13 +45,13 @@ private:
SBUS_SIGNAL_OK
};
std::atomic<bool> reading; // 控制读取线程的状态
std::thread read_thread; // 读取数据的线程
std::atomic<bool> reading; // 控制读取线程的状态
std::thread read_thread; // 读取数据的线程
void read_loop(); // 持续读取串口数据
void parse_data(std::vector<uint8_t> &buffer); // 解析数据
int sbus_parse(); // SBUS 数据解析
void print_hex(uint8_t *buf, int len); // 打印数据（调试用）
void read_loop(); // 持续读取串口数据
void parse_data(std::vector<uint8_t>& buffer); // 解析数据
int sbus_parse(); // SBUS 数据解析
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>ament_index_cpp</depend>
<depend>sweeper_interfaces</depend>
<depend>logger</depend>
<build_depend>ament_cmake</build_depend>
<exec_depend>ament_cmake</exec_depend>
@ -23,4 +24,4 @@
<export>
<build_type>ament_cmake</build_type>
</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 <iostream>
#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/mc_ctrl.hpp"
namespace sweeperMsg = sweeper_interfaces::msg;
constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f;
constexpr float GEAR_RATIO = 16.5f;
constexpr float DELTA_T = 0.02f; // 20ms
constexpr int PRINT_INTERVAL = 25; // 打印间隔：每25次回调打印一次（25*20ms=500ms，即2Hz）
constexpr float DELTA_T = 0.02f; // 20ms
constexpr int PRINT_INTERVAL = 25; // 打印间隔：每25次回调打印一次（25*20ms=500ms，即2Hz）
class SBUSNode : public rclcpp::Node
{
public:
SBUSNode() : Node("sbus_node"), current_feedback_angle(0.0f), print_counter(0) // 初始化计数器
public:
SBUSNode() : Node("sbus_node"), current_feedback_angle(0.0f), print_counter(0) // 初始化计数器
{
if (load_config(config))
{
std::cout << "Serial Port: " << config.serial_port << "\n";
std::cout << "Baudrate: " << config.baudrate << "\n";
std::cout << "MCU RPM Max: " << config.mcu_rpm_max << "\n";
std::cout << "EPS Angle Max: " << config.eps_angle_max << "\n";
LOG_INFO("Serial Port: %s", config.serial_port.c_str());
LOG_INFO("Baudrate: %d", config.baudrate);
LOG_INFO("MCU RPM Max: %d", config.mcu_rpm_max);
LOG_INFO("EPS Angle Max: %f", config.eps_angle_max);
}
// 发布控制指令消息的发布器
@ -31,22 +33,19 @@ public:
// 订阅CAN反馈的回调函数
can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
"can_data", 10,
std::bind(&SBUSNode::can_callback, this, std::placeholders::_1));
"can_data", 10, std::bind(&SBUSNode::can_callback, this, std::placeholders::_1));
// 初始化串口读取（读取遥控器数据）
uart_handler_ = std::make_shared<UartHandler>(config.serial_port, config.baudrate);
if (!uart_handler_->open_serial())
{
RCLCPP_ERROR(this->get_logger(), "Failed to open serial port!");
LOG_ERROR("Failed to open serial port!");
return;
}
uart_handler_->start_reading(); // 启动串口后台读线程
uart_handler_->start_reading(); // 启动串口后台读线程
// 创建周期定时器（每20ms回调一次控制逻辑，50Hz）
timer_ = this->create_wall_timer(
std::chrono::milliseconds(20),
std::bind(&SBUSNode::timer_callback, this));
timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&SBUSNode::timer_callback, this));
}
~SBUSNode()
@ -57,44 +56,44 @@ public:
}
}
private:
private:
// 定时器回调函数，用于周期性发布控制指令
void timer_callback()
{
static int MCU_RPM_MAX = config.mcu_rpm_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(); // 控制消息对象
uint16_t ch_data[10]; // 各通道遥控数据
auto msg = sweeperMsg::McCtrl(); // 控制消息对象
uint16_t ch_data[10]; // 各通道遥控数据
if (data_safe) // 数据安全，进行数据解析并发布
if (data_safe) // 数据安全，进行数据解析并发布
{
// 赋值与打印（注释掉原有的高频打印）
for (int i = 0; i < 10; ++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 sweep = ch_data[6]; // 清扫 最上192，最下1792
int16_t speed = ch_data[2] - 992; // 左边摇杆，最下192，中间992，最上1792 -> [-800,800]
uint16_t gear = ch_data[7]; // 挡位 最下1792，中间992，最上192
uint16_t sweep = ch_data[6]; // 清扫 最上192，最下1792
int16_t speed = ch_data[2] - 992; // 左边摇杆，最下192，中间992，最上1792 -> [-800,800]
// 挡位选择
if (gear == 192)
{
msg.gear = 2; // D挡
msg.gear = 2; // D挡
}
else if (gear == 992)
{
msg.gear = 0; // N挡
msg.gear = 0; // N挡
}
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;
// 限制电机转速到 [120, 1500] 范围，防止过小/过大
uint16_t can_speed = std::clamp(
static_cast<uint16_t>(motor_rpm),
static_cast<uint16_t>(120),
static_cast<uint16_t>(1500));
uint16_t can_speed =
std::clamp(static_cast<uint16_t>(motor_rpm), static_cast<uint16_t>(120), static_cast<uint16_t>(1500));
msg.angle = target_angle;
msg.angle_speed = can_speed;
@ -144,25 +141,25 @@ private:
// 降低打印频率：每 PRINT_INTERVAL 次回调打印一次
// if (++print_counter >= PRINT_INTERVAL)
// {
// std::cout << "\n====================================="
// LOG_INFO("\n=====================================")
// << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车")
// << "\n 挡位: ";
// switch (msg.gear)
// {
// case 0:
// std::cout << "空挡";
// LOG_INFO("空挡");
// break;
// case 2:
// std::cout << "前进挡";
// LOG_INFO("前进挡");
// break;
// case 1:
// std::cout << "后退挡";
// LOG_INFO("后退挡");
// break;
// default:
// std::cout << "未知挡位(" << static_cast<int>(msg.gear) << ")";
// LOG_INFO("未知挡位(%d)", static_cast<int>(msg.gear));
// 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.sweep ? "正在清扫" : "未清扫")
// << "\n=====================================" << std::endl;
@ -174,7 +171,7 @@ private:
// 低频率打印等待信息（每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;
}
return;
@ -201,15 +198,21 @@ private:
rclcpp::TimerBase::SharedPtr timer_;
std::shared_ptr<UartHandler> uart_handler_;
float current_feedback_angle; // 当前反馈角度（从CAN中读取）
float current_feedback_angle; // 当前反馈角度（从CAN中读取）
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::spin(std::make_shared<SBUSNode>()); // 启动节点
rclcpp::spin(std::make_shared<SBUSNode>()); // 启动节点
rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
return 0;
}

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

@ -1,14 +1,18 @@
#include "radio_ctrl/uart_handler.h"
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include <stdint.h>
#include <unistd.h>
#include <iostream>
#include "logger/logger.h"
#ifndef BOTHER
#define BOTHER 0010000
#endif
@ -29,10 +33,10 @@ 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)
{
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;
}
tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR |
IGNCR | ICRNL | IXON);
tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
tio.c_iflag |= (INPCK | IGNPAR);
tio.c_oflag &= ~OPOST;
tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
@ -84,7 +87,7 @@ bool UartHandler::open_serial()
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;
}
@ -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)
{
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)
{
@ -147,7 +150,7 @@ void UartHandler::parse_data(std::vector<uint8_t> &buffer)
auto it = std::find(buffer.begin(), buffer.end(), 0x0F);
if (it == buffer.end())
{
buffer.clear(); // 没找到帧头，清空 buffer
buffer.clear(); // 没找到帧头，清空 buffer
failsafe_status = SBUS_SIGNAL_LOST;
return;
}
@ -159,8 +162,7 @@ void UartHandler::parse_data(std::vector<uint8_t> &buffer)
if (buffer.size() - index < kSbusFrameLength)
{
// 数据不够，等下次再处理
if (index > 0)
buffer.erase(buffer.begin(), buffer.begin() + index);
if (index > 0) buffer.erase(buffer.begin(), buffer.begin() + index);
return;
}
@ -215,9 +217,9 @@ int UartHandler::get_channel_value(int channel)
{
if (channel >= 0 && channel < 8)
{
return sbus_channels[channel]; // 返回指定通道的值
return sbus_channels[channel]; // 返回指定通道的值
}
return 0; // 如果无效的通道，返回默认值
return 0; // 如果无效的通道，返回默认值
}
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(sweeper_interfaces REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(logger REQUIRED)
# ======== MQTT libs ========
if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
@ -41,7 +42,12 @@ target_link_libraries(
ssl
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(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
#include "paho_mqtt_3c/MQTTClient.h"
#include <string>
#include <atomic>
#include <thread>
#include <mutex>
#include <iostream>
#include <functional>
#include <chrono>
#include <functional>
#include <iostream>
#include <mutex>
#include <string>
#include <thread>
#include "paho_mqtt_3c/MQTTClient.h"
class MQTTClientWrapper
{
public:
using MessageCallback =
std::function<void(const std::string &, const std::string &)>;
using ConnectedCallback =
std::function<void()>;
public:
using MessageCallback = std::function<void(const std::string&, const std::string&)>;
using ConnectedCallback = std::function<void()>;
MQTTClientWrapper(const std::string &server_uri,
const std::string &client_id,
const std::string &username = "",
const std::string &password = "",
int reconnect_ms = 3000)
MQTTClientWrapper(const std::string& server_uri, const std::string& client_id, const std::string& username = "",
const std::string& password = "", int reconnect_ms = 3000)
: server_uri_(server_uri),
client_id_(client_id),
username_(username),
@ -30,11 +26,8 @@ public:
connected_(false),
stop_flag_(false)
{
int rc = MQTTClient_create(&client_,
server_uri_.c_str(),
client_id_.c_str(),
MQTTCLIENT_PERSISTENCE_NONE,
nullptr);
int rc =
MQTTClient_create(&client_, server_uri_.c_str(), client_id_.c_str(), MQTTCLIENT_PERSISTENCE_NONE, nullptr);
if (rc != MQTTCLIENT_SUCCESS)
{
@ -44,32 +37,26 @@ public:
}
MQTTClient_setCallbacks(client_, this,
connLostCB, // 连接丢失回调
msgArrivedCB, // 消息回调
connLostCB, // 连接丢失回调
msgArrivedCB, // 消息回调
nullptr);
// 启动网络循环线程
loop_thread_ = std::thread([this]()
{ loopFunc(); });
loop_thread_ = std::thread([this]() { loopFunc(); });
}
~MQTTClientWrapper()
{
stop();
}
~MQTTClientWrapper() { stop(); }
void stop()
{
stop_flag_ = true;
if (loop_thread_.joinable())
loop_thread_.join();
if (loop_thread_.joinable()) loop_thread_.join();
std::lock_guard<std::mutex> lk(mtx_);
if (client_)
{
if (connected_)
MQTTClient_disconnect(client_, 2000);
if (connected_) MQTTClient_disconnect(client_, 2000);
MQTTClient_destroy(&client_);
}
@ -88,13 +75,11 @@ public:
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_);
if (!client_ ||
!connected_.load(std::memory_order_acquire) ||
!ready_.load(std::memory_order_acquire))
if (!client_ || !connected_.load(std::memory_order_acquire) || !ready_.load(std::memory_order_acquire))
return false;
int rc = MQTTClient_subscribe(client_, topic.c_str(), qos);
@ -108,19 +93,15 @@ public:
return true;
}
bool publish(const std::string &topic,
const std::string &payload,
int qos = 0)
bool publish(const std::string& topic, const std::string& payload, int qos = 0)
{
std::lock_guard<std::mutex> lk(mtx_);
if (!client_ ||
!connected_.load(std::memory_order_acquire) ||
!ready_.load(std::memory_order_acquire))
if (!client_ || !connected_.load(std::memory_order_acquire) || !ready_.load(std::memory_order_acquire))
return false;
MQTTClient_message msg = MQTTClient_message_initializer;
msg.payload = (void *)payload.data();
msg.payload = (void*)payload.data();
msg.payloadlen = payload.size();
msg.qos = qos;
@ -137,16 +118,12 @@ public:
return true;
}
bool connect()
{
return doConnect();
}
bool connect() { return doConnect(); }
private:
private:
bool doConnect()
{
if (!client_)
return false;
if (!client_) return false;
MQTTClient_connectOptions opts = MQTTClient_connectOptions_initializer;
opts.keepAliveInterval = 15;
@ -175,24 +152,22 @@ private:
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->ready_.store(false, std::memory_order_release);
std::cerr << "[MQTT] lost connection: "
<< (cause ? cause : "unknown") << std::endl;
std::cerr << "[MQTT] lost connection: " << (cause ? cause : "unknown") << std::endl;
}
static int msgArrivedCB(void *ctx, char *topic,
int topicLen, MQTTClient_message *message)
static int msgArrivedCB(void* ctx, char* topic, 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 payload((char *)message->payload, message->payloadlen);
std::string payload((char*)message->payload, message->payloadlen);
if (self->msg_cb_)
{
@ -235,8 +210,7 @@ private:
// ⭐ 在 ready 状态触发 connected callback
if (need_callback_.exchange(false))
{
if (connected_cb_)
connected_cb_();
if (connected_cb_) connected_cb_();
}
}
}
@ -246,7 +220,7 @@ private:
}
}
private:
private:
MQTTClient client_{nullptr};
std::string server_uri_, client_id_;
std::string username_, password_;
@ -261,6 +235,6 @@ private:
MessageCallback msg_cb_;
ConnectedCallback connected_cb_;
std::atomic<bool> ready_{false}; // 是否真正进入可用状态
std::atomic<bool> ready_{false}; // 是否真正进入可用状态
std::atomic<bool> need_callback_{false};
};

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

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

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

@ -4,6 +4,8 @@
#include <iostream>
#include <nlohmann/json.hpp>
#include "logger/logger.h"
using json = nlohmann::json;
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);
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;
}
@ -34,7 +36,7 @@ bool config::load(const std::string& path, Config& cfg)
}
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;
}
}

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

@ -7,6 +7,7 @@
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#include "logger/logger.h"
#include "nlohmann/json.hpp"
#include "remote_ctrl/config.hpp"
#include "remote_ctrl/mqtt_client.hpp"
@ -83,7 +84,7 @@ class RemoteCtrlNode : public rclcpp::Node
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;
}
@ -100,14 +101,14 @@ class RemoteCtrlNode : public rclcpp::Node
vid_ = msg->vid;
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;
if (ctrl_topic_.find("{vid}") == std::string::npos)
{
RCLCPP_WARN(this->get_logger(), "[REMOTE] remote_topic_template missing '{vid}', template=%s",
cfg_.remote_topic_template.c_str());
LOG_WARN("[REMOTE] remote_topic_template missing '{vid}', template=%s",
cfg_.remote_topic_template.c_str());
}
auto pos = ctrl_topic_.find("{vid}");
@ -116,8 +117,7 @@ class RemoteCtrlNode : public rclcpp::Node
try_subscribe_ctrl_topic();
});
RCLCPP_INFO(this->get_logger(), "RemoteCtrlNode started mqtt=%s client_id=%s", uri.str().c_str(),
client_id.c_str());
LOG_INFO("RemoteCtrlNode started mqtt=%s client_id=%s", uri.str().c_str(), client_id.c_str());
// ===== 看门狗 & 定时发布 =====
last_msg_time_ = std::chrono::steady_clock::now();
@ -137,8 +137,7 @@ class RemoteCtrlNode : public rclcpp::Node
// 只在第一次进入“失活”时打印日志
if (remote_alive_.exchange(false))
{
RCLCPP_WARN(this->get_logger(),
"[REMOTE] control timeout, enter safe-stop");
LOG_WARN("[REMOTE] control timeout, enter safe-stop");
}
}
});
@ -213,7 +212,7 @@ class RemoteCtrlNode : public rclcpp::Node
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);
}
@ -227,12 +226,12 @@ class RemoteCtrlNode : public rclcpp::Node
bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed);
if (was_dead)
{
RCLCPP_INFO(this->get_logger(), "[REMOTE] control recovered");
LOG_INFO("[REMOTE] control recovered");
}
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
{
@ -258,7 +257,7 @@ class RemoteCtrlNode : public rclcpp::Node
remote_authorized_.store(true, std::memory_order_release);
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)
{
@ -269,7 +268,7 @@ class RemoteCtrlNode : public rclcpp::Node
// 明确清空目标状态
desired_ = {};
RCLCPP_INFO(this->get_logger(), "[REMOTE] deauthorized (mode=0)");
LOG_INFO("[REMOTE] deauthorized (mode=0)");
}
// mode 指令处理完直接返回
@ -357,7 +356,7 @@ class RemoteCtrlNode : public rclcpp::Node
}
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[])
{
// 初始化日志系统
logger::Logger::Init("remote_ctrl", "./log");
// ===============================
// 1. 默认配置路径 + 手动解析 --config
// ===============================
@ -414,12 +416,12 @@ int main(int argc, char* argv[])
Config 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;
}
RCLCPP_INFO(rclcpp::get_logger("main"), "Config loaded: broker=%s:%d remote_topic_template=%s", cfg.mqtt_ip.c_str(),
cfg.mqtt_port, cfg.remote_topic_template.c_str());
LOG_INFO("Config loaded: broker=%s:%d remote_topic_template=%s", cfg.mqtt_ip.c_str(), cfg.mqtt_port,
cfg.remote_topic_template.c_str());
// ===============================
// 3. 把配置传入 Node 构造函数
@ -430,5 +432,8 @@ int main(int argc, char* argv[])
node.reset();
rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
return 0;
}

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

@ -1,24 +1,23 @@
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_ros/transform_listener.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/filters/crop_box.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_cloud.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/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/multi_array_dimension.hpp>
#include <functional>
#include <unordered_map>
using sensor_msgs::msg::PointCloud2;
using std::placeholders::_1;
@ -28,14 +27,14 @@ struct CloudFrame
{
std::shared_ptr<PointCloud2> cloud;
rclcpp::Time stamp;
rclcpp::Time received_time; // 添加接收时间用于延时分析
rclcpp::Time received_time; // 添加接收时间用于延时分析
std::string source;
};
// 内存池实现
class PointCloud2Pool
{
public:
public:
std::shared_ptr<PointCloud2> acquire()
{
std::lock_guard<std::mutex> lock(mutex_);
@ -53,8 +52,7 @@ public:
void release(std::shared_ptr<PointCloud2> cloud)
{
if (!cloud)
return;
if (!cloud) return;
cloud->data.clear();
cloud->width = 0;
@ -62,20 +60,20 @@ public:
cloud->row_step = 0;
std::lock_guard<std::mutex> lock(mutex_);
if (pool_.size() < 10) // 限制池大小
if (pool_.size() < 10) // 限制池大小
{
pool_.push_back(cloud);
}
}
private:
private:
std::vector<std::shared_ptr<PointCloud2>> pool_;
std::mutex mutex_;
};
class LidarMerger : public rclcpp::Node
{
public:
public:
LidarMerger() : Node("lidar_merger")
{
/* ---------- 参数 ---------- */
@ -83,11 +81,11 @@ public:
rear_topic_ = declare_parameter<std::string>("rear_topic", "/rslidar_points/rear_lidar");
output_topic_ = declare_parameter<std::string>("output_topic", "/rslidar_points");
target_frame_ = declare_parameter<std::string>("frame_id", "rslidar");
queue_size_ = declare_parameter<int>("queue_size", 3); // 减小队列大小
cache_size_ = declare_parameter<int>("cache_size", 10); // 增加缓存以应对延时
time_tolerance_ = declare_parameter<double>("time_tolerance", 0.1); // 放宽时间容差
max_wait_time_ = declare_parameter<double>("max_wait_time", 1.0); // 增加到1.0s以适应实际延时
enable_debug_ = declare_parameter<bool>("enable_debug", false); // 默认关闭调试减少日志开销
queue_size_ = declare_parameter<int>("queue_size", 3); // 减小队列大小
cache_size_ = declare_parameter<int>("cache_size", 10); // 增加缓存以应对延时
time_tolerance_ = declare_parameter<double>("time_tolerance", 0.1); // 放宽时间容差
max_wait_time_ = declare_parameter<double>("max_wait_time", 1.0); // 增加到1.0s以适应实际延时
enable_debug_ = declare_parameter<bool>("enable_debug", false); // 默认关闭调试减少日志开销
// 点云处理参数
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_min_z_ = declare_parameter<float>("filter_min_z", -2.0f);
filter_max_z_ = declare_parameter<float>("filter_max_z", 2.0f);
voxel_size_ = declare_parameter<float>("voxel_size", 0.1f); // 降采样体素大小
stat_mean_k_ = declare_parameter<int>("stat_mean_k", 30); // 统计离群点去除的k值
stat_std_thresh_ = declare_parameter<float>("stat_std_thresh", 1.0f); // 统计离群点去除的标准差阈值
grid_size_ = declare_parameter<int>("grid_size", 100); // 栅格大小
grid_range_ = declare_parameter<float>("grid_range", 20.0f); // 栅格范围
voxel_size_ = declare_parameter<float>("voxel_size", 0.1f); // 降采样体素大小
stat_mean_k_ = declare_parameter<int>("stat_mean_k", 30); // 统计离群点去除的k值
stat_std_thresh_ = declare_parameter<float>("stat_std_thresh", 1.0f); // 统计离群点去除的标准差阈值
grid_size_ = declare_parameter<int>("grid_size", 100); // 栅格大小
grid_range_ = declare_parameter<float>("grid_range", 20.0f); // 栅格范围
enable_print_ = declare_parameter<bool>("enable_print", true);
// 车身过滤参数
filter_car_ = declare_parameter<bool>("filter_car", true); // 是否启用车身过滤
car_length_ = declare_parameter<float>("car_length", 2.3f); // 车长（米）
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_y_ = declare_parameter<float>("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移
filter_car_ = declare_parameter<bool>("filter_car", true); // 是否启用车身过滤
car_length_ = declare_parameter<float>("car_length", 2.3f); // 车长（米）
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_y_ = declare_parameter<float>("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移
// 打印所有参数值（添加到构造函数末尾）
RCLCPP_INFO_STREAM(this->get_logger(),
"\n\n---------- 点云融合节点参数配置 ----------"
<< "\n [输入输出]"
<< "\n 前雷达话题: " << front_topic_
<< "\n 后雷达话题: " << rear_topic_
<< "\n 输出话题: " << output_topic_
<< "\n 目标坐标系: " << target_frame_
<< "\n [同步参数]"
<< "\n 队列大小: " << queue_size_
<< "\n 缓存大小: " << cache_size_
<< "\n 时间容差(s): " << time_tolerance_
<< "\n 最大等待时间(s): " << max_wait_time_
<< "\n [调试选项]"
<< "\n 调试模式: " << (enable_debug_ ? "开启" : "关闭")
<< "\n [点云处理]"
<< "\n 空间过滤范围(m): [" << filter_min_x_ << ", " << filter_max_x_
<< "] x [" << filter_min_y_ << ", " << filter_max_y_
<< "] x [" << filter_min_z_ << ", " << filter_max_z_ << "]"
<< "\n 体素大小(m): " << voxel_size_
<< "\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");
RCLCPP_INFO_STREAM(
this->get_logger(),
"\n\n---------- 点云融合节点参数配置 ----------"
<< "\n [输入输出]"
<< "\n 前雷达话题: " << front_topic_ << "\n 后雷达话题: " << rear_topic_
<< "\n 输出话题: " << output_topic_ << "\n 目标坐标系: " << target_frame_ << "\n [同步参数]"
<< "\n 队列大小: " << queue_size_ << "\n 缓存大小: " << cache_size_ << "\n 时间容差(s): "
<< time_tolerance_ << "\n 最大等待时间(s): " << max_wait_time_ << "\n [调试选项]"
<< "\n 调试模式: " << (enable_debug_ ? "开启" : "关闭") << "\n [点云处理]"
<< "\n 空间过滤范围(m): [" << filter_min_x_ << ", " << filter_max_x_ << "] x [" << filter_min_y_
<< ", " << filter_max_y_ << "] x [" << filter_min_z_ << ", " << filter_max_z_ << "]"
<< "\n 体素大小(m): " << voxel_size_ << "\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_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
/* ---------- QoS - 优化为低延时 ---------- */
auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_))
.best_effort()
.durability_volatile();
auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)).best_effort().durability_volatile();
// 移除deadline约束，避免因延时导致的消息丢弃
/* ---------- 订阅 ---------- */
sub_front_ = create_subscription<PointCloud2>(
front_topic_, qos, std::bind(&LidarMerger::frontCB, this, std::placeholders::_1));
sub_rear_ = create_subscription<PointCloud2>(
rear_topic_, qos, std::bind(&LidarMerger::rearCB, this, std::placeholders::_1));
sub_front_ = create_subscription<PointCloud2>(front_topic_, qos,
std::bind(&LidarMerger::frontCB, this, std::placeholders::_1));
sub_rear_ = create_subscription<PointCloud2>(rear_topic_, qos,
std::bind(&LidarMerger::rearCB, this, std::placeholders::_1));
/* ---------- 发布 ---------- */
auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_))
.reliable()
.durability_volatile();
auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)).reliable().durability_volatile();
pub_ = create_publisher<PointCloud2>(output_topic_, pub_qos);
// 创建栅格发布者
@ -176,7 +160,7 @@ public:
cache_size_, time_tolerance_, max_wait_time_);
}
private:
private:
/* ---------- 回调函数 - 同步处理 ---------- */
void frontCB(const PointCloud2::SharedPtr msg)
{
@ -207,8 +191,7 @@ private:
}
/* ---------- 同步处理点云 ---------- */
void processCloudSync(const PointCloud2::SharedPtr msg, const std::string &source,
const rclcpp::Time &receive_time)
void processCloudSync(const PointCloud2::SharedPtr msg, const std::string& source, const rclcpp::Time& receive_time)
{
// 立即进行坐标变换
auto transformed_cloud = transformCloud(*msg);
@ -219,11 +202,8 @@ private:
}
// 创建CloudFrame
auto cloud_frame = std::make_shared<CloudFrame>(CloudFrame{
transformed_cloud,
rclcpp::Time(msg->header.stamp),
receive_time,
source});
auto cloud_frame = std::make_shared<CloudFrame>(
CloudFrame{transformed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source});
// 添加到缓存并立即尝试合并
{
@ -241,7 +221,7 @@ private:
/* ---------- 添加到缓存 ---------- */
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();
@ -262,14 +242,12 @@ private:
/* ---------- 尝试合并 - 触发式 ---------- */
void tryMerge()
{
if (front_clouds_.empty() || rear_clouds_.empty())
return;
if (front_clouds_.empty() || rear_clouds_.empty()) return;
// 查找最佳匹配
auto [front_frame, rear_frame] = findBestMatchOptimized();
if (!front_frame || !rear_frame)
return;
if (!front_frame || !rear_frame) return;
// 自适应延时检查 - 基于实际网络条件调整
auto now_time = now();
@ -279,7 +257,7 @@ private:
// 动态调整max_wait_time基于观察到的延时
static double observed_max_delay = 0.0;
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);
@ -289,8 +267,8 @@ private:
if (enable_debug_)
{
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000,
"Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs",
front_age, rear_age, adaptive_threshold);
"Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs", front_age,
rear_age, adaptive_threshold);
}
// 不直接返回，而是清理最旧的数据后重试
@ -314,8 +292,7 @@ private:
auto merged = mergeClouds(*front_frame, *rear_frame);
auto merge_end = std::chrono::high_resolution_clock::now();
if (!merged)
return;
if (!merged) return;
// 设置时间戳和frame_id
auto front_time = front_frame->stamp.nanoseconds();
@ -333,8 +310,7 @@ private:
// ========================= 点云 =========================
auto processed_pcl = processPointCloud(merged_pcl);
if (!processed_pcl)
return;
if (!processed_pcl) return;
// 将处理后的PCL点云转回ROS消息
auto processed = std::make_shared<sensor_msgs::msg::PointCloud2>();
@ -346,28 +322,27 @@ private:
// ========================= 点云 =========================
// ========================= 栅格 =========================
auto grid = processPointCloud_grid(merged_pcl);
if (grid.empty())
return;
if (grid.empty()) return;
// 可视化栅格
if (enable_print_)
visualizeGridInTerminal(grid);
if (enable_print_) visualizeGridInTerminal(grid);
// 发布栅格到ROS话题
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 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(),
"Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs",
merged_count_, merged->data.size() / merged->point_step,
processed->data.size() / processed->point_step,
total_delay, process_time * 1000, adaptive_threshold);
RCLCPP_INFO(
get_logger(),
"Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs",
merged_count_, merged->data.size() / merged->point_step, processed->data.size() / processed->point_step,
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())
return nullptr;
if (!cloud || cloud->empty()) return nullptr;
// 1. 条件过滤
auto filtered = applyConditionalFiltering(cloud);
if (filtered->empty())
{
RCLCPP_WARN(get_logger(), "[processPointCloud] Filtered cloud is empty!");
return cloud; // 返回原始云而不是nullptr
return cloud; // 返回原始云而不是nullptr
}
// 2. 降采样
@ -406,10 +380,9 @@ private:
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())
return {}; // 返回空栅格
if (!cloud || cloud->empty()) return {}; // 返回空栅格
// 1. 条件过滤
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>);
for (const auto &point : cloud->points)
for (const auto& point : cloud->points)
{
// 自定义过滤条件：保留特定区域内的点
if (point.x > filter_min_x_ && point.x < filter_max_x_ &&
point.y > filter_min_y_ && point.y < filter_max_y_ &&
point.z > filter_min_z_ && point.z < filter_max_z_)
if (point.x > filter_min_x_ && point.x < filter_max_x_ && point.y > filter_min_y_ &&
point.y < filter_max_y_ && point.z > filter_min_z_ && point.z < filter_max_z_)
{
filtered->points.push_back(point);
}
@ -460,8 +432,7 @@ private:
filtered->is_dense = true;
// 不启用车身过滤，直接返回
if (!filter_car_)
return filtered;
if (!filter_car_) return filtered;
// RCLCPP_INFO(get_logger(), "启用车身过滤");
// 使用CropBox移除车身区域
@ -469,17 +440,15 @@ private:
crop.setInputCloud(filtered);
// 设置车身区域（最小点和最大点）
Eigen::Vector4f min_point(car_lidar_offset_x_ - car_length_ / 2.0f,
car_lidar_offset_y_ - car_width_ / 2.0f,
Eigen::Vector4f min_point(car_lidar_offset_x_ - car_length_ / 2.0f, car_lidar_offset_y_ - car_width_ / 2.0f,
filter_min_z_, 1.0);
Eigen::Vector4f max_point(car_lidar_offset_x_ + car_length_ / 2.0f,
car_lidar_offset_y_ + car_width_ / 2.0f,
Eigen::Vector4f max_point(car_lidar_offset_x_ + car_length_ / 2.0f, car_lidar_offset_y_ + car_width_ / 2.0f,
filter_max_z_, 1.0);
crop.setMin(min_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>);
crop.filter(*output);
@ -489,11 +458,11 @@ private:
// 右侧主刷 (x:0.85~1.25, y:0.6~0.96, z:1.2~1.65)
pcl::CropBox<pcl::PointXYZ> crop1;
crop1.setInputCloud(output);
Eigen::Vector4f min1(0.85f, 0.6f, 1.2f, 1.0); // 右侧主刷最小点
Eigen::Vector4f max1(1.25f, 0.96f, 1.65f, 1.0); // 右侧主刷最大点
Eigen::Vector4f min1(0.85f, 0.6f, 1.2f, 1.0); // 右侧主刷最小点
Eigen::Vector4f max1(1.25f, 0.96f, 1.65f, 1.0); // 右侧主刷最大点
crop1.setMin(min1);
crop1.setMax(max1);
crop1.setNegative(true); // 剔除右侧主刷内的点
crop1.setNegative(true); // 剔除右侧主刷内的点
pcl::PointCloud<pcl::PointXYZ>::Ptr output1(new pcl::PointCloud<pcl::PointXYZ>);
crop1.filter(*output1);
@ -502,20 +471,20 @@ private:
pcl::CropBox<pcl::PointXYZ> crop2;
crop2.setInputCloud(output1);
// X轴对称：将y坐标取相反数，保持x和z坐标不变
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 min2(0.85f, -0.96f, 1.2f, 1.0); // 左侧主刷最小点（y取反后注意交换大小）
Eigen::Vector4f max2(1.25f, -0.6f, 1.65f, 1.0); // 左侧主刷最大点（y取反后注意交换大小）
crop2.setMin(min2);
crop2.setMax(max2);
crop2.setNegative(true); // 剔除左侧主刷内的点
crop2.setNegative(true); // 剔除左侧主刷内的点
pcl::PointCloud<pcl::PointXYZ>::Ptr final_output(new pcl::PointCloud<pcl::PointXYZ>);
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::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::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK(stat_mean_k_); // 考虑每个点的k个最近邻
sor.setStddevMulThresh(stat_std_thresh_); // 标准差倍数阈值
sor.setMeanK(stat_mean_k_); // 考虑每个点的k个最近邻
sor.setStddevMulThresh(stat_std_thresh_); // 标准差倍数阈值
sor.filter(*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:车体
std::vector<std::vector<int>> grid(grid_size_, std::vector<int>(grid_size_, 0));
@ -552,8 +521,10 @@ private:
if (filter_car_)
{
// 计算车体在栅格中的位置
int car_min_x = 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_x =
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_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）
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");
@ -634,24 +605,24 @@ private:
}
/* ---------- 发布栅格 ---------- */
void publishGrid(const std::vector<std::vector<int>> &grid)
void publishGrid(const std::vector<std::vector<int>>& grid)
{
// 创建Int32MultiArray消息
auto msg = std_msgs::msg::Int32MultiArray();
// 设置维度信息
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].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].stride = grid[0].size();
// 设置数据（按行优先展开）
msg.data.clear();
for (const auto &row : grid)
for (const auto& row : grid)
{
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()
{
if (front_clouds_.empty() || rear_clouds_.empty())
return {nullptr, nullptr};
if (front_clouds_.empty() || rear_clouds_.empty()) return {nullptr, nullptr};
// 快速策略：优先使用最新的点云进行匹配
auto front_candidate = front_clouds_.front();
@ -708,8 +678,7 @@ private:
}
/* ---------- 移除已使用的点云 ---------- */
void removeUsedClouds(std::shared_ptr<CloudFrame> used_front,
std::shared_ptr<CloudFrame> used_rear)
void removeUsedClouds(std::shared_ptr<CloudFrame> used_front, std::shared_ptr<CloudFrame> used_rear)
{
// 从front_clouds_中移除
auto front_it = std::find(front_clouds_.begin(), front_clouds_.end(), used_front);
@ -735,7 +704,7 @@ private:
void cleanupExpiredClouds()
{
auto now_time = now();
auto cleanup_threshold = max_wait_time_; // 使用配置的等待时间
auto cleanup_threshold = max_wait_time_; // 使用配置的等待时间
// 清理front_clouds_中过期的数据
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 rear_points = rear.cloud->data.size() / rear.cloud->point_step;
if (front.cloud->fields != rear.cloud->fields ||
front.cloud->point_step != rear.cloud->point_step ||
if (front.cloud->fields != rear.cloud->fields || front.cloud->point_step != rear.cloud->point_step ||
front.cloud->is_bigendian != rear.cloud->is_bigendian)
{
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_)
{
@ -823,20 +791,16 @@ private:
try
{
// 使用最新可用的变换，不等待
auto tf = tf_buffer_->lookupTransform(
target_frame_, in.header.frame_id,
tf2::TimePointZero);
auto tf = tf_buffer_->lookupTransform(target_frame_, in.header.frame_id, tf2::TimePointZero);
auto out = cloud_pool_.acquire();
tf2::doTransform(in, *out, tf);
return out;
}
catch (const tf2::TransformException &e)
catch (const tf2::TransformException& e)
{
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
"TF failed (%s -> %s): %s",
in.header.frame_id.c_str(),
target_frame_.c_str(), e.what());
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "TF failed (%s -> %s): %s",
in.header.frame_id.c_str(), target_frame_.c_str(), e.what());
return nullptr;
}
}
@ -845,12 +809,10 @@ private:
void printStats()
{
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(),
"Stats: Front=%d, Rear=%d, Merged=%d, Cache(F:%zu,R:%zu)",
front_count_, rear_count_, merged_count_,
front_clouds_.size(), rear_clouds_.size());
RCLCPP_INFO(get_logger(), "Stats: Front=%d, Rear=%d, Merged=%d, Cache(F:%zu,R:%zu)", front_count_,
rear_count_, merged_count_, front_clouds_.size(), rear_clouds_.size());
last_stats_time_ = current_time;
}
@ -866,11 +828,11 @@ private:
float filter_min_x_, filter_max_x_;
float filter_min_y_, filter_max_y_;
float filter_min_z_, filter_max_z_;
float voxel_size_; // 降采样体素大小
int stat_mean_k_; // 统计离群点去除的k值
float stat_std_thresh_; // 统计离群点去除的标准差阈值
int grid_size_; // 栅格大小
float grid_range_; // 栅格范围
float voxel_size_; // 降采样体素大小
int stat_mean_k_; // 统计离群点去除的k值
float stat_std_thresh_; // 统计离群点去除的标准差阈值
int grid_size_; // 栅格大小
float grid_range_; // 栅格范围
bool enable_print_;
// 车身过滤参数
@ -901,7 +863,7 @@ private:
int front_count_, rear_count_, merged_count_;
};
int main(int argc, char **argv)
int main(int argc, char** 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(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED)
find_package(logger REQUIRED)
#find_package(Boost 1.71.0 REQUIRED COMPONENTS thread atomic system regex)
#if(Boost_FOUND)
#include_directories(${Boost_INCLUDE_DIRS})
#include_directories(${Boost_INCLUDE_DIRS})
#endif()
include_directories(
include/rtk
${catkin_INCLUDE_DIRS}
)
include_directories(include/rtk ${catkin_INCLUDE_DIRS})
add_executable(rtk_node src/rtk_node.cpp src/serial_read.cpp)
target_link_libraries(rtk_node ${Boost_LIBRARIES})
ament_target_dependencies(rtk_node rclcpp std_msgs sweeper_interfaces)
install(TARGETS
ament_target_dependencies(
rtk_node
DESTINATION lib/${PROJECT_NAME}
)
rclcpp
std_msgs
sweeper_interfaces
logger)
install(TARGETS rtk_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
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
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
endif()
ament_package()

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

@ -1,24 +1,23 @@
#ifndef __UART_READ_H__
#define __UART_READ_H__
#include <iostream>
#include <boost/asio.hpp>
#include <iostream>
using namespace std;
using namespace boost::asio;
class Boost_serial
{
public:
public:
Boost_serial();
~Boost_serial();
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;
serial_port *sp;
serial_port* sp;
};
#endif

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

@ -12,6 +12,7 @@
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>sweeper_interfaces</depend>
<depend>logger</depend>
<test_depend>ament_lint_auto</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 <string.h>
#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
{
public:
public:
// 构造函数,有一个参数为节点名称
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;
lon = 0.0;
@ -29,17 +31,17 @@ public:
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[])
{
position = 0;
line_string.clear();
char *p_gpybm = strstr(serial_buf, "GPYBM");
char* p_gpybm = strstr(serial_buf, "GPYBM");
if (p_gpybm == NULL)
{
RCLCPP_INFO(this->get_logger(), "未检测到GPYBM字符串!");
LOG_INFO("未检测到GPYBM字符串!");
return;
}
@ -103,13 +105,13 @@ private:
// 读取串口传来的定位信息
memset(serial_buf, 0, sizeof(serial_buf));
int num = boost_serial->serial_read(serial_buf, 200);
// printf("num:%d \n",num);
// printf("serial_buf: ");
// LOG_DEBUG("num:%d \n",num);
// LOG_DEBUG("serial_buf: ");
// 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)
{
@ -132,12 +134,12 @@ private:
}
// 解析定位信息
// printf("gps_buf: ");
// LOG_DEBUG("gps_buf: ");
// 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);
@ -152,8 +154,8 @@ private:
message.h_quality = h_quality;
// 日志打印
RCLCPP_INFO(this->get_logger(), "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'",
message.lat, message.lon, message.head, message.speed, message.p_quality, message.h_quality);
LOG_INFO("lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", message.lat,
message.lon, message.head, message.speed, message.p_quality, message.h_quality);
// 发布消息
command_publisher_->publish(message);
}
@ -170,7 +172,7 @@ private:
rclcpp::Publisher<sweeper_interfaces::msg::Rtk>::SharedPtr command_publisher_;
// 串口读取类指针
Boost_serial *boost_serial;
Boost_serial* boost_serial;
// 串口读取buffer
char serial_buf[300];
@ -180,8 +182,8 @@ private:
double lon;
double head;
double speed;
int p_quality; // 定位解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解
int h_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浮点解
std::queue<char> c_queue;
// 解析定位信息用到的中间变量
@ -190,15 +192,21 @@ private:
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);
/*创建对应节点的共享指针对象*/
auto node = std::make_shared<rtk_node>("rtk_node");
/* 运行节点，并检测退出信号*/
rclcpp::spin(node);
rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
return 0;
}

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

@ -1,9 +1,8 @@
#include "serial_read.hpp"
Boost_serial::Boost_serial()
{
;
}
#include "logger/logger.h"
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::stop_bits(serial_port::stop_bits::one));
sp->set_option(serial_port::character_size(8));
cout << "打开串口成功！" << endl;
LOG_INFO("打开串口成功！");
}
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));
if (num <= 0)
{
cout << "没有读到数据！" << endl;
LOG_WARN("没有读到数据！");
}
return num;
}