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

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

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

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

@ -1,21 +1,23 @@
#include "rclcpp/rclcpp.hpp" #include <math.h>
#include "sweeper_interfaces/msg/pl.hpp" #include <time.h>
#include "sweeper_interfaces/msg/fu.hpp"
#include "sweeper_interfaces/msg/detect_line.hpp" #include <algorithm>
#include "sweeper_interfaces/msg/sub.hpp" #include <iostream>
#include <sensor_msgs/msg/point_cloud2.hpp> #include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp> #include <sensor_msgs/point_cloud2_iterator.hpp>
#include "sweeper_interfaces/msg/can_frame.hpp" #include <sstream>
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include <std_msgs/msg/int32_multi_array.hpp> #include <std_msgs/msg/int32_multi_array.hpp>
#include <vector>
#include <string> #include <string>
#include <algorithm> #include <vector>
#include <math.h> #include "logger/logger.h"
#include <iostream> #include "rclcpp/rclcpp.hpp"
#include <time.h> #include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/detect_line.hpp"
#include "sweeper_interfaces/msg/fu.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include "sweeper_interfaces/msg/pl.hpp"
#include "sweeper_interfaces/msg/sub.hpp"
using namespace std; using namespace std;
// 栅格值定义 // 栅格值定义
@ -31,10 +33,10 @@ int is_start = 0;
class fu_node : public rclcpp::Node class fu_node : public rclcpp::Node
{ {
public: public:
fu_node(std::string name) : Node(name) fu_node(std::string name) : Node(name)
{ {
RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); LOG_INFO("%s节点已经启动.", name.c_str());
// 声明并获取参数（默认开启遇障停车和绕障） // 声明并获取参数（默认开启遇障停车和绕障）
this->declare_parameter("enable_obstacle_stop", true); // 是否是否开启遇障停车 this->declare_parameter("enable_obstacle_stop", true); // 是否是否开启遇障停车
@ -60,24 +62,23 @@ public:
this->get_parameter("right_area_extend", right_area_extend_); this->get_parameter("right_area_extend", right_area_extend_);
// 创建参数回调，支持动态修改 // 创建参数回调，支持动态修改
parameter_callback_handle_ = this->add_on_set_parameters_callback( parameter_callback_handle_ =
std::bind(&fu_node::parameter_callback, this, std::placeholders::_1)); this->add_on_set_parameters_callback(std::bind(&fu_node::parameter_callback, this, std::placeholders::_1));
// 创建订阅器 // 创建订阅器
subscription_grid = this->create_subscription<std_msgs::msg::Int32MultiArray>( subscription_grid = this->create_subscription<std_msgs::msg::Int32MultiArray>(
"grid_raw", 10, std::bind(&fu_node::gridCallback, this, std::placeholders::_1)); "grid_raw", 10, std::bind(&fu_node::gridCallback, this, std::placeholders::_1));
msg_subscribe_pl = this->create_subscription<sweeper_interfaces::msg::Pl>("pl_message", 10, msg_subscribe_pl = this->create_subscription<sweeper_interfaces::msg::Pl>(
std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1)); "pl_message", 10, std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1));
msg_subscribe_DetectLine = this->create_subscription<sweeper_interfaces::msg::DetectLine>("airy_message", 10, msg_subscribe_DetectLine = this->create_subscription<sweeper_interfaces::msg::DetectLine>(
std::bind(&fu_node::msg_callback_DetectLine, this, std::placeholders::_1)); "airy_message", 10, std::bind(&fu_node::msg_callback_DetectLine, this, std::placeholders::_1));
// 发布控制指令 // 发布控制指令
pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("auto_mc_ctrl", 10); pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("auto_mc_ctrl", 10);
// 创建定时器，100ms为周期 // 创建定时器，100ms为周期
timer_ = this->create_wall_timer(std::chrono::milliseconds(100), timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&fu_node::timer_callback, this));
std::bind(&fu_node::timer_callback, this));
// 避障相关参数初始化 // 避障相关参数初始化
avoid_counter_ = 0; avoid_counter_ = 0;
@ -94,22 +95,16 @@ public:
node_clock_ = this->get_clock(); node_clock_ = this->get_clock();
// 打印所有参数值（添加到构造函数末尾） // 打印所有参数值（添加到构造函数末尾）
RCLCPP_INFO_STREAM(this->get_logger(), LOG_INFO(
"\n\n---------- FU节点参数配置 ----------" "\n\n---------- FU节点参数配置 ----------\n 遇障停车功能: %s\n 绕障功能: %s\n 可视化栅格: %s\n "
<< "\n 遇障停车功能: " << (enable_obstacle_stop_ ? "开启" : "关闭") "车前必停区行数: %d\n 车后必停区行数: %d\n 车左必停区列数: %d\n 车右必停区列数: %d\n 前方扩展检测区域: "
<< "\n 绕障功能: " << (enable_obstacle_avoid_ ? "开启" : "关闭") "%d\n 左侧扩展检测区域: %d\n 右侧扩展检测区域: %d\n--------------------------------------------\n",
<< "\n 可视化栅格: " << (enable_visualize_grid_ ? "是" : "否") (enable_obstacle_stop_ ? "开启" : "关闭"), (enable_obstacle_avoid_ ? "开启" : "关闭"),
<< "\n 车前必停区行数: " << FRONT_STOP_ZONE_ROWS_ (enable_visualize_grid_ ? "是" : "否"), FRONT_STOP_ZONE_ROWS_, REAR_STOP_ZONE_ROWS_, LEFT_STOP_ZONE_COLS_,
<< "\n 车后必停区行数: " << REAR_STOP_ZONE_ROWS_ RIGHT_STOP_ZONE_COLS_, front_area_extend_, left_area_extend_, right_area_extend_);
<< "\n 车左必停区列数: " << LEFT_STOP_ZONE_COLS_
<< "\n 车右必停区列数: " << RIGHT_STOP_ZONE_COLS_
<< "\n 前方扩展检测区域: " << front_area_extend_
<< "\n 左侧扩展检测区域: " << left_area_extend_
<< "\n 右侧扩展检测区域: " << right_area_extend_
<< "\n--------------------------------------------\n");
} }
private: private:
rclcpp::Clock::SharedPtr node_clock_; rclcpp::Clock::SharedPtr node_clock_;
enum AvoidState enum AvoidState
@ -176,14 +171,14 @@ private:
} }
} }
double getElapsedTime(const rclcpp::Time &start, const rclcpp::Time &current) const double getElapsedTime(const rclcpp::Time& start, const rclcpp::Time& current) const
{ {
try try
{ {
auto duration = current - start; auto duration = current - start;
return duration.seconds(); return duration.seconds();
} }
catch (const std::runtime_error &e) catch (const std::runtime_error& e)
{ {
return 0.0; return 0.0;
} }
@ -216,15 +211,13 @@ private:
// 评估绕障可行性 // 评估绕障可行性
bool canAvoid() const bool canAvoid() const
{ {
return obstacle_width > 0 && obstacle_width < 10 && return obstacle_width > 0 && obstacle_width < 10 && (free_space_left >= 3 || free_space_right >= 3);
(free_space_left >= 3 || free_space_right >= 3);
} }
// 评估是否需要紧急制动 // 评估是否需要紧急制动
bool needEmergencyBrake(int speed) const bool needEmergencyBrake(int speed) const
{ {
if (!has_front_critical) if (!has_front_critical) return false;
return false;
int safe_distance = calculateSafeDistance(speed); int safe_distance = calculateSafeDistance(speed);
return obstacle_distance <= safe_distance; return obstacle_distance <= safe_distance;
} }
@ -305,22 +298,22 @@ private:
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
// 参数动态更新回调 // 参数动态更新回调
rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector<rclcpp::Parameter> &parameters) rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector<rclcpp::Parameter>& parameters)
{ {
rcl_interfaces::msg::SetParametersResult result; rcl_interfaces::msg::SetParametersResult result;
result.successful = true; result.successful = true;
for (const auto &param : parameters) for (const auto& param : parameters)
{ {
if (param.get_name() == "enable_obstacle_stop") if (param.get_name() == "enable_obstacle_stop")
{ {
enable_obstacle_stop_ = param.as_bool(); enable_obstacle_stop_ = param.as_bool();
RCLCPP_INFO(this->get_logger(), "遇障停车功能 %s", enable_obstacle_stop_ ? "开启" : "关闭"); LOG_INFO("遇障停车功能 %s", enable_obstacle_stop_ ? "开启" : "关闭");
} }
else if (param.get_name() == "enable_obstacle_avoid") else if (param.get_name() == "enable_obstacle_avoid")
{ {
enable_obstacle_avoid_ = param.as_bool(); enable_obstacle_avoid_ = param.as_bool();
RCLCPP_INFO(this->get_logger(), "绕障功能 %s", enable_obstacle_avoid_ ? "开启" : "关闭"); LOG_INFO("绕障功能 %s", enable_obstacle_avoid_ ? "开启" : "关闭");
// 关闭绕障时重置避障状态 // 关闭绕障时重置避障状态
if (!enable_obstacle_avoid_) if (!enable_obstacle_avoid_)
{ {
@ -360,7 +353,7 @@ private:
} }
// 查找车体在栅格中的矩形边界 // 查找车体在栅格中的矩形边界
void findVehicleRectangle(const std::vector<std::vector<int>> &grid) void findVehicleRectangle(const std::vector<std::vector<int>>& grid)
{ {
// 重置车体边界 // 重置车体边界
vehicle_min_x_ = -1; vehicle_min_x_ = -1;
@ -376,23 +369,18 @@ private:
if (grid[i][j] == VEHICLE) if (grid[i][j] == VEHICLE)
{ {
// 更新最小和最大行坐标 // 更新最小和最大行坐标
if (vehicle_min_x_ == -1 || static_cast<int>(i) < vehicle_min_x_) if (vehicle_min_x_ == -1 || static_cast<int>(i) < vehicle_min_x_) vehicle_min_x_ = i;
vehicle_min_x_ = i; if (vehicle_max_x_ == -1 || static_cast<int>(i) > vehicle_max_x_) vehicle_max_x_ = i;
if (vehicle_max_x_ == -1 || static_cast<int>(i) > vehicle_max_x_)
vehicle_max_x_ = i;
// 更新最小和最大列坐标 // 更新最小和最大列坐标
if (vehicle_min_y_ == -1 || static_cast<int>(j) < vehicle_min_y_) if (vehicle_min_y_ == -1 || static_cast<int>(j) < vehicle_min_y_) vehicle_min_y_ = j;
vehicle_min_y_ = j; if (vehicle_max_y_ == -1 || static_cast<int>(j) > vehicle_max_y_) vehicle_max_y_ = j;
if (vehicle_max_y_ == -1 || static_cast<int>(j) > vehicle_max_y_)
vehicle_max_y_ = j;
} }
} }
} }
// 如果找到车体，计算必停区域 // 如果找到车体，计算必停区域
if (vehicle_min_x_ != -1 && vehicle_max_x_ != -1 && if (vehicle_min_x_ != -1 && vehicle_max_x_ != -1 && vehicle_min_y_ != -1 && vehicle_max_y_ != -1)
vehicle_min_y_ != -1 && vehicle_max_y_ != -1)
{ {
calculateSafetyZones(grid.size(), grid[0].size()); calculateSafetyZones(grid.size(), grid[0].size());
} }
@ -431,7 +419,7 @@ private:
// 检查消息是否包含维度信息 // 检查消息是否包含维度信息
if (msg->layout.dim.size() != 2) if (msg->layout.dim.size() != 2)
{ {
RCLCPP_ERROR(this->get_logger(), "Received grid message with invalid dimensions!"); LOG_ERROR("Received grid message with invalid dimensions!");
return; return;
} }
@ -442,8 +430,7 @@ private:
// 检查数据长度是否匹配 // 检查数据长度是否匹配
if (msg->data.size() != height * width) if (msg->data.size() != height * width)
{ {
RCLCPP_ERROR(this->get_logger(), "Grid data size mismatch! Expected %zu, got %zu", LOG_ERROR("Grid data size mismatch! Expected %zu, got %zu", height * width, msg->data.size());
height * width, msg->data.size());
return; return;
} }
@ -466,8 +453,7 @@ private:
analyzeObstacles(grid); analyzeObstacles(grid);
// 可视化栅格 // 可视化栅格
if (enable_visualize_grid_) if (enable_visualize_grid_) visualizeGridInTerminal(grid);
visualizeGridInTerminal(grid);
cacheGridData(grid); // 缓存栅格数据 cacheGridData(grid); // 缓存栅格数据
} }
@ -475,14 +461,14 @@ private:
std::vector<std::vector<int>> cached_grid_; std::vector<std::vector<int>> cached_grid_;
bool grid_data_valid_ = false; bool grid_data_valid_ = false;
void cacheGridData(const std::vector<std::vector<int>> &grid) void cacheGridData(const std::vector<std::vector<int>>& grid)
{ {
cached_grid_ = grid; cached_grid_ = grid;
grid_data_valid_ = true; grid_data_valid_ = true;
} }
// 分析障碍物分布 // 分析障碍物分布
void analyzeObstacles(const std::vector<std::vector<int>> &grid) void analyzeObstacles(const std::vector<std::vector<int>>& grid)
{ {
// 重置检测结果 // 重置检测结果
obstacle_in_front_ = false; obstacle_in_front_ = false;
@ -497,9 +483,7 @@ private:
obstacle_max_x_ = 100; obstacle_max_x_ = 100;
// 如果未找到车体位置，不进行检测 // 如果未找到车体位置，不进行检测
if (vehicle_min_x_ == -1 || vehicle_max_x_ == -1 || if (vehicle_min_x_ == -1 || vehicle_max_x_ == -1 || vehicle_min_y_ == -1 || vehicle_max_y_ == -1) return;
vehicle_min_y_ == -1 || vehicle_max_y_ == -1)
return;
// 检查前方必停区 // 检查前方必停区
bool found_front_obstacle = false; bool found_front_obstacle = false;
@ -507,8 +491,7 @@ private:
{ {
for (int j = front_stop_zone_.min_y; j <= front_stop_zone_.max_y; ++j) for (int j = front_stop_zone_.min_y; j <= front_stop_zone_.max_y; ++j)
{ {
if (static_cast<size_t>(i) < grid.size() && if (static_cast<size_t>(i) < grid.size() && static_cast<size_t>(j) < grid[0].size() &&
static_cast<size_t>(j) < grid[0].size() &&
grid[i][j] == OBSTACLE) grid[i][j] == OBSTACLE)
{ {
obstacle_in_front_ = true; obstacle_in_front_ = true;
@ -533,7 +516,7 @@ private:
} }
// 检查其他必停区 // 检查其他必停区
void checkOtherStopZones(const std::vector<std::vector<int>> &grid) void checkOtherStopZones(const std::vector<std::vector<int>>& grid)
{ {
// 检查后方必停区 // 检查后方必停区
for (int i = rear_stop_zone_.min_x; i <= rear_stop_zone_.max_x && !obstacle_in_rear_; ++i) for (int i = rear_stop_zone_.min_x; i <= rear_stop_zone_.max_x && !obstacle_in_rear_; ++i)
@ -579,7 +562,7 @@ private:
} }
// 分析前方扩展区域 - 优化策略：只检测车辆宽度范围的前方 // 分析前方扩展区域 - 优化策略：只检测车辆宽度范围的前方
void analyzeFrontExtendedArea(const std::vector<std::vector<int>> &grid) void analyzeFrontExtendedArea(const std::vector<std::vector<int>>& grid)
{ {
size_t front_area_min_x = static_cast<size_t>(max(0, vehicle_min_x_ - front_area_extend_)); size_t front_area_min_x = static_cast<size_t>(max(0, vehicle_min_x_ - front_area_extend_));
size_t front_area_max_x = static_cast<size_t>(vehicle_min_x_ - 1); size_t front_area_max_x = static_cast<size_t>(vehicle_min_x_ - 1);
@ -621,7 +604,8 @@ private:
} }
// 计算绕障可用空间 - 在更大范围内分析左右通道 // 计算绕障可用空间 - 在更大范围内分析左右通道
void calculateAvoidanceSpace(const std::vector<std::vector<int>> &grid, int core_left_obstacle, int core_right_obstacle) void calculateAvoidanceSpace(const std::vector<std::vector<int>>& grid, int core_left_obstacle,
int core_right_obstacle)
{ {
if (obstacle_in_front_area_) if (obstacle_in_front_area_)
{ {
@ -635,14 +619,14 @@ private:
// 左侧绕障空间检查：从 车辆左边界 到 障碍物左边界 // 左侧绕障空间检查：从 车辆左边界 到 障碍物左边界
int left_check_start = max(0, vehicle_min_y_ - left_area_extend_); int left_check_start = max(0, vehicle_min_y_ - left_area_extend_);
int left_check_end = obstacle_left_edge_ - 1; int left_check_end = obstacle_left_edge_ - 1;
free_space_left_ = calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, free_space_left_ =
left_check_start, left_check_end); calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, left_check_start, left_check_end);
// 右侧绕障空间检查：从 障碍物右边界 到 车辆右边界 // 右侧绕障空间检查：从 障碍物右边界 到 车辆右边界
int right_check_start = obstacle_right_edge_ + 1; int right_check_start = obstacle_right_edge_ + 1;
int right_check_end = min(static_cast<int>(grid[0].size() - 1), vehicle_max_y_ + right_area_extend_); int right_check_end = min(static_cast<int>(grid[0].size() - 1), vehicle_max_y_ + right_area_extend_);
free_space_right_ = calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, free_space_right_ =
right_check_start, right_check_end); calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, right_check_start, right_check_end);
} }
else else
{ {
@ -655,26 +639,23 @@ private:
} }
// 计算指定方向的可用空间 // 计算指定方向的可用空间
int calculateSpaceInDirection(const std::vector<std::vector<int>> &grid, int calculateSpaceInDirection(const std::vector<std::vector<int>>& grid, size_t min_x, size_t max_x,
size_t min_x, size_t max_x, int check_start_y, int check_end_y) int check_start_y, int check_end_y)
{ {
if (check_start_y > check_end_y) if (check_start_y > check_end_y) return 0;
return 0;
int available_space = 0; int available_space = 0;
// 从起始位置开始，逐列检查直到遇到障碍物 // 从起始位置开始，逐列检查直到遇到障碍物
for (int j = check_start_y; j <= check_end_y; ++j) for (int j = check_start_y; j <= check_end_y; ++j)
{ {
if (j < 0 || j >= static_cast<int>(grid[0].size())) if (j < 0 || j >= static_cast<int>(grid[0].size())) break;
break;
bool column_clear = true; bool column_clear = true;
// 检查该列在检测范围内是否有障碍物 // 检查该列在检测范围内是否有障碍物
for (size_t i = min_x; i <= max_x; ++i) for (size_t i = min_x; i <= max_x; ++i)
{ {
if (i >= grid.size()) if (i >= grid.size()) break;
break;
if (grid[i][j] == OBSTACLE) if (grid[i][j] == OBSTACLE)
{ {
column_clear = false; column_clear = false;
@ -695,28 +676,28 @@ private:
return available_space; return available_space;
} }
void visualizeGridInTerminal(const std::vector<std::vector<int>> &grid) void visualizeGridInTerminal(const std::vector<std::vector<int>>& grid)
{ {
std::system("clear"); std::stringstream ss;
std::cout << " --------------------障碍物矩阵-------------------- " << std::endl; ss << " --------------------障碍物矩阵-------------------- " << std::endl;
// 打印顶部边框 // 打印顶部边框
std::cout << " " << std::string(grid[0].size(), '-') << std::endl; ss << " " << std::string(grid[0].size(), '-') << std::endl;
// 打印列号（仅0-9） // 打印列号（仅0-9）
std::cout << " "; ss << " ";
for (size_t i = 0; i < grid[0].size(); i++) for (size_t i = 0; i < grid[0].size(); i++)
{ {
std::cout << (i % 10); ss << (i % 10);
} }
std::cout << std::endl; ss << std::endl;
// 打印栅格内容（行号 + 栅格） // 打印栅格内容（行号 + 栅格）
for (size_t i = 0; i < grid.size(); i++) for (size_t i = 0; i < grid.size(); i++)
{ {
// 行号显示（两位数，右对齐） // 行号显示（两位数，右对齐）
std::cout << (i < 10 ? " " : "") << i << "|"; ss << (i < 10 ? " " : "") << i << "|";
// 栅格内容 // 栅格内容
for (size_t j = 0; j < grid[i].size(); j++) for (size_t j = 0; j < grid[i].size(); j++)
@ -739,24 +720,26 @@ private:
// 根据栅格值和必停区状态选择显示符号 // 根据栅格值和必停区状态选择显示符号
if (grid[i][j] == OBSTACLE) if (grid[i][j] == OBSTACLE)
std::cout << "@"; ss << "@";
else if (grid[i][j] == VEHICLE) else if (grid[i][j] == VEHICLE)
std::cout << "V"; ss << "V";
else if (in_front_zone) else if (in_front_zone)
std::cout << "F"; // 前方必停区 ss << "F"; // 前方必停区
else if (in_left_zone) else if (in_left_zone)
std::cout << "L"; // 左方必停区 ss << "L"; // 左方必停区
else if (in_right_zone) else if (in_right_zone)
std::cout << "R"; // 右方必停区 ss << "R"; // 右方必停区
else else
std::cout << "."; ss << ".";
} }
std::cout << "|" << std::endl; ss << "|" << std::endl;
} }
// 打印底部边框 // 打印底部边框
std::cout << " " << std::string(grid[0].size(), '-') << std::endl; ss << " " << std::string(grid[0].size(), '-') << std::endl;
LOG_DEBUG("%s", ss.str().c_str());
} }
void msg_callback_pl(const sweeper_interfaces::msg::Pl::SharedPtr msg) void msg_callback_pl(const sweeper_interfaces::msg::Pl::SharedPtr msg)
@ -781,7 +764,7 @@ private:
{ {
publish_speed = 0; publish_speed = 0;
publish_angle = 0; publish_angle = 0;
RCLCPP_INFO(this->get_logger(), "PL速度为0，已停车"); LOG_INFO("PL速度为0，已停车");
} }
else if (is_start) else if (is_start)
{ {
@ -799,7 +782,7 @@ private:
else else
{ {
message.rpm = 0; message.rpm = 0;
RCLCPP_ERROR(this->get_logger(), "rtk信号差,停车！！！"); LOG_ERROR("rtk信号差,停车！！！");
} }
message.angle = publish_angle; // 负数表示左转，正数表示右转 message.angle = publish_angle; // 负数表示左转，正数表示右转
@ -830,11 +813,8 @@ private:
state_str = "未知状态"; state_str = "未知状态";
} }
RCLCPP_INFO_STREAM(this->get_logger(), LOG_INFO("控制: 速度=%d, 角度=%d, 状态=%s, 前方%s障碍物", publish_speed, publish_angle, state_str.c_str(),
"控制: 速度=" << publish_speed (obstacle_in_front_ ? "有" : "无"));
<< ", 角度=" << publish_angle
<< ", 状态=" << state_str
<< ", 前方" << (obstacle_in_front_ ? "有" : "无") << "障碍物");
} }
else else
{ {
@ -860,7 +840,7 @@ private:
publish_speed = 0; publish_speed = 0;
publish_angle = 0; publish_angle = 0;
avoid_ctx_.reset(); avoid_ctx_.reset();
RCLCPP_WARN(this->get_logger(), "未检测到车体位置，安全停车"); LOG_WARN("未检测到车体位置，安全停车");
return; return;
} }
@ -892,9 +872,7 @@ private:
obstacle_analysis_.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1; obstacle_analysis_.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1;
obstacle_analysis_.left_edge = obstacle_left_edge_; obstacle_analysis_.left_edge = obstacle_left_edge_;
obstacle_analysis_.right_edge = obstacle_right_edge_; obstacle_analysis_.right_edge = obstacle_right_edge_;
RCLCPP_INFO(this->get_logger(), "障碍物左侧=%d，障碍物右侧=%d", LOG_INFO("障碍物左侧=%d，障碍物右侧=%d", obstacle_analysis_.left_edge, obstacle_analysis_.right_edge);
obstacle_analysis_.left_edge,
obstacle_analysis_.right_edge);
obstacle_analysis_.free_space_left = free_space_left_; obstacle_analysis_.free_space_left = free_space_left_;
obstacle_analysis_.free_space_right = free_space_right_; obstacle_analysis_.free_space_right = free_space_right_;
@ -907,8 +885,7 @@ private:
// 第一优先级：紧急停车策略 // 第一优先级：紧急停车策略
bool executeEmergencyStop() bool executeEmergencyStop()
{ {
if (!enable_obstacle_stop_) if (!enable_obstacle_stop_) return false;
return false;
// 前方必停区有障碍物，立即停车 // 前方必停区有障碍物，立即停车
if (obstacle_analysis_.has_front_critical) if (obstacle_analysis_.has_front_critical)
@ -921,8 +898,7 @@ private:
// 每2秒打印一次状态 // 每2秒打印一次状态
if (avoid_ctx_.counter % 20 == 0) if (avoid_ctx_.counter % 20 == 0)
{ {
RCLCPP_WARN(this->get_logger(), "紧急停车：前方必停区有障碍物，距离=%d格", LOG_WARN("紧急停车：前方必停区有障碍物，距离=%d格", obstacle_analysis_.obstacle_distance);
obstacle_analysis_.obstacle_distance);
} }
return true; return true;
} }
@ -933,8 +909,7 @@ private:
publish_speed = 0; publish_speed = 0;
publish_angle = 0; publish_angle = 0;
avoid_ctx_.state = AVOID_WAITING; avoid_ctx_.state = AVOID_WAITING;
RCLCPP_WARN(this->get_logger(), "动态紧急制动：障碍物距离%d格，速度%d", LOG_WARN("动态紧急制动：障碍物距离%d格，速度%d", obstacle_analysis_.obstacle_distance, publish_speed);
obstacle_analysis_.obstacle_distance, publish_speed);
return true; return true;
} }
@ -944,15 +919,14 @@ private:
// 第二优先级：转向保护策略 // 第二优先级：转向保护策略
bool executeTurnProtection() bool executeTurnProtection()
{ {
if (!enable_obstacle_stop_) if (!enable_obstacle_stop_) return false;
return false;
// 检查转向方向的安全性 // 检查转向方向的安全性
if (publish_angle < -10 && obstacle_analysis_.has_left_critical) if (publish_angle < -10 && obstacle_analysis_.has_left_critical)
{ {
publish_speed = 0; publish_speed = 0;
publish_angle = 0; publish_angle = 0;
RCLCPP_WARN(this->get_logger(), "转向保护：左转时左侧有障碍物"); LOG_WARN("转向保护：左转时左侧有障碍物");
return true; return true;
} }
@ -960,7 +934,7 @@ private:
{ {
publish_speed = 0; publish_speed = 0;
publish_angle = 0; publish_angle = 0;
RCLCPP_WARN(this->get_logger(), "转向保护：右转时右侧有障碍物"); LOG_WARN("转向保护：右转时右侧有障碍物");
return true; return true;
} }
@ -981,16 +955,14 @@ private:
if (avoid_ctx_.counter % 30 == 0) if (avoid_ctx_.counter % 30 == 0)
{ // 每3秒打印一次 { // 每3秒打印一次
RCLCPP_INFO(this->get_logger(), "障碍物无法绕行(宽度=%d格)，等待通过...", LOG_INFO("障碍物无法绕行(宽度=%d格)，等待通过...", obstacle_analysis_.obstacle_width);
obstacle_analysis_.obstacle_width);
} }
} }
else else
{ {
// 距离足够，可以减速接近 // 距离足够，可以减速接近
publish_speed = std::max(600, publish_speed / 2); publish_speed = std::max(600, publish_speed / 2);
RCLCPP_INFO(this->get_logger(), "障碍物较宽，减速接近中，距离=%d格", LOG_INFO("障碍物较宽，减速接近中，距离=%d格", obstacle_analysis_.obstacle_distance);
obstacle_analysis_.obstacle_distance);
} }
} }
@ -1018,13 +990,9 @@ private:
avoid_ctx_.parallel_start_obstacle_x = -1; avoid_ctx_.parallel_start_obstacle_x = -1;
avoid_ctx_.obstacle_passed = false; avoid_ctx_.obstacle_passed = false;
RCLCPP_INFO(this->get_logger(), LOG_INFO("启动绕障: 方向=%s, 需要偏移=%d格, 障碍物宽度=%d, 左侧空间=%d, 右侧空间=%d",
"启动绕障: 方向=%s, 需要偏移=%d格, 障碍物宽度=%d, 左侧空间=%d, 右侧空间=%d", (direction == -1) ? "左" : "右", required_shift, obstacle_analysis_.obstacle_width,
(direction == -1) ? "左" : "右", obstacle_analysis_.free_space_left, obstacle_analysis_.free_space_right);
required_shift,
obstacle_analysis_.obstacle_width,
obstacle_analysis_.free_space_left,
obstacle_analysis_.free_space_right);
} }
// 方向选择算法 // 方向选择算法
@ -1038,10 +1006,8 @@ private:
right_score += obstacle_analysis_.free_space_right * 20; right_score += obstacle_analysis_.free_space_right * 20;
// 2. 安全性评分(关键因素) // 2. 安全性评分(关键因素)
if (obstacle_analysis_.has_left_critical) if (obstacle_analysis_.has_left_critical) left_score -= 1000;
left_score -= 1000; if (obstacle_analysis_.has_right_critical) right_score -= 1000;
if (obstacle_analysis_.has_right_critical)
right_score -= 1000;
// 3. 障碍物位置分析 - 基于车辆边界而非中心 // 3. 障碍物位置分析 - 基于车辆边界而非中心
if (obstacle_analysis_.left_edge > -1 && obstacle_analysis_.right_edge > -1) if (obstacle_analysis_.left_edge > -1 && obstacle_analysis_.right_edge > -1)
@ -1054,9 +1020,7 @@ private:
// 右侧可用空间 = 障碍物右边界到车辆右边界的距离 // 右侧可用空间 = 障碍物右边界到车辆右边界的距离
int right_clearance = vehicle_max_y_ - obstacle_analysis_.right_edge; int right_clearance = vehicle_max_y_ - obstacle_analysis_.right_edge;
RCLCPP_INFO(this->get_logger(), LOG_INFO("位置分析: 左侧间隙=%d, 右侧间隙=%d, 车宽=%d", left_clearance, right_clearance, vehicle_width);
"位置分析: 左侧间隙=%d, 右侧间隙=%d, 车宽=%d",
left_clearance, right_clearance, vehicle_width);
// 基于实际间隙大小评分 // 基于实际间隙大小评分
if (left_clearance > right_clearance + 2) if (left_clearance > right_clearance + 2)
@ -1073,13 +1037,13 @@ private:
{ {
// 障碍物完全在车辆左侧,应该右绕 // 障碍物完全在车辆左侧,应该右绕
right_score += 30; right_score += 30;
RCLCPP_INFO(this->get_logger(), "障碍物在左侧,倾向右绕"); LOG_INFO("障碍物在左侧,倾向右绕");
} }
else if (obstacle_analysis_.left_edge > vehicle_max_y_) else if (obstacle_analysis_.left_edge > vehicle_max_y_)
{ {
// 障碍物完全在车辆右侧,应该左绕 // 障碍物完全在车辆右侧,应该左绕
left_score += 30; left_score += 30;
RCLCPP_INFO(this->get_logger(), "障碍物在右侧,倾向左绕"); LOG_INFO("障碍物在右侧,倾向左绕");
} }
} }
@ -1087,8 +1051,7 @@ private:
float target_angle = calculate_the_angle(x, y); float target_angle = calculate_the_angle(x, y);
if (target_angle < -15) // 阈值从10提高到15 if (target_angle < -15) // 阈值从10提高到15
left_score += 15; // 权重从20降低到15 left_score += 15; // 权重从20降低到15
if (target_angle > 15) if (target_angle > 15) right_score += 15;
right_score += 15;
// 5. 历史趋势稳定性(避免频繁切换) - 权重提高 // 5. 历史趋势稳定性(避免频繁切换) - 权重提高
if (!avoid_ctx_.angle_history.empty() && avoid_ctx_.angle_history.size() >= 5) // 从3提高到5 if (!avoid_ctx_.angle_history.empty() && avoid_ctx_.angle_history.size() >= 5) // 从3提高到5
@ -1096,8 +1059,8 @@ private:
int recent_avg = 0; int recent_avg = 0;
int count = std::min(5, static_cast<int>(avoid_ctx_.angle_history.size())); int count = std::min(5, static_cast<int>(avoid_ctx_.angle_history.size()));
for (int i = avoid_ctx_.angle_history.size() - count; for (int i = avoid_ctx_.angle_history.size() - count; i < static_cast<int>(avoid_ctx_.angle_history.size());
i < static_cast<int>(avoid_ctx_.angle_history.size()); i++) i++)
{ {
recent_avg += avoid_ctx_.angle_history[i]; recent_avg += avoid_ctx_.angle_history[i];
} }
@ -1105,17 +1068,13 @@ private:
if (recent_avg < -8) // 阈值从5提高到8 if (recent_avg < -8) // 阈值从5提高到8
left_score += 15; // 权重从10提高到15 left_score += 15; // 权重从10提高到15
if (recent_avg > 8) if (recent_avg > 8) right_score += 15;
right_score += 15;
} }
int selected_direction = (left_score > right_score) ? -1 : 1; int selected_direction = (left_score > right_score) ? -1 : 1;
RCLCPP_INFO(this->get_logger(), LOG_INFO("方向选择 - 左评分:%d 右评分:%d → %s (左空间:%d 右空间:%d)", left_score, right_score,
"方向选择 - 左评分:%d 右评分:%d → %s (左空间:%d 右空间:%d)", (selected_direction == -1) ? "左绕" : "右绕", obstacle_analysis_.free_space_left,
left_score, right_score,
(selected_direction == -1) ? "左绕" : "右绕",
obstacle_analysis_.free_space_left,
obstacle_analysis_.free_space_right); obstacle_analysis_.free_space_right);
return selected_direction; return selected_direction;
@ -1158,7 +1117,7 @@ private:
if (avoid_ctx_.state != AVOID_NONE) if (avoid_ctx_.state != AVOID_NONE)
{ {
avoid_ctx_.reset(); avoid_ctx_.reset();
RCLCPP_INFO(this->get_logger(), "前方无障碍物，重置绕障状态"); LOG_INFO("前方无障碍物，重置绕障状态");
} }
return; return;
} }
@ -1166,7 +1125,7 @@ private:
// 评估是否需要绕障 // 评估是否需要绕障
if (!obstacle_analysis_.canAvoid()) if (!obstacle_analysis_.canAvoid())
{ {
RCLCPP_INFO(this->get_logger(), "障碍物无法绕行，减速靠近"); LOG_INFO("障碍物无法绕行，减速靠近");
handleUnpassableObstacle(); handleUnpassableObstacle();
return; return;
} }
@ -1229,8 +1188,8 @@ private:
bool safe_distance_achieved = verifySafeDistance(); bool safe_distance_achieved = verifySafeDistance();
// 所有条件都满足才进入下一阶段 // 所有条件都满足才进入下一阶段
bool shift_complete = time_sufficient && distance_sufficient && bool shift_complete =
lateral_clearance_achieved && safe_distance_achieved; time_sufficient && distance_sufficient && lateral_clearance_achieved && safe_distance_achieved;
if (shift_complete) if (shift_complete)
{ {
@ -1239,19 +1198,16 @@ private:
avoid_ctx_.last_state_change_time_set = true; avoid_ctx_.last_state_change_time_set = true;
avoid_ctx_.parallel_start_obstacle_x = obstacle_max_x_; avoid_ctx_.parallel_start_obstacle_x = obstacle_max_x_;
RCLCPP_INFO(this->get_logger(), LOG_INFO("阶段1完成: 横向偏移完成(%.1fs, %d格), 进入阶段2: 平行前进", shift_time,
"阶段1完成: 横向偏移完成(%.1fs, %d格), 进入阶段2: 平行前进", avoid_ctx_.lateral_shift_achieved);
shift_time, avoid_ctx_.lateral_shift_achieved);
} }
else else
{ {
if (avoid_ctx_.counter % 10 == 0) if (avoid_ctx_.counter % 10 == 0)
{ {
RCLCPP_INFO(this->get_logger(), LOG_INFO("阶段1进行中: 时间%.1fs/%s, 距离%d/%d格, 间隙%s, 安全距离%s", shift_time,
"阶段1进行中: 时间%.1fs/%s, 距离%d/%d格, 间隙%s, 安全距离%s", time_sufficient ? "✓" : "✗", avoid_ctx_.lateral_shift_achieved,
shift_time, time_sufficient ? "✓" : "✗", avoid_ctx_.lateral_shift_distance, lateral_clearance_achieved ? "✓" : "✗",
avoid_ctx_.lateral_shift_achieved, avoid_ctx_.lateral_shift_distance,
lateral_clearance_achieved ? "✓" : "✗",
safe_distance_achieved ? "✓" : "✗"); safe_distance_achieved ? "✓" : "✗");
} }
} }
@ -1265,20 +1221,20 @@ private:
// 检查当前是否已经没有前方障碍物警告 // 检查当前是否已经没有前方障碍物警告
if (obstacle_analysis_.has_front_critical || obstacle_analysis_.has_front_area) if (obstacle_analysis_.has_front_critical || obstacle_analysis_.has_front_area)
{ {
RCLCPP_DEBUG(this->get_logger(), "前方仍有障碍区域"); LOG_DEBUG("前方仍有障碍区域");
return false; return false;
} }
// 检查绕障方向的侧方是否有障碍物 // 检查绕障方向的侧方是否有障碍物
if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical)
{ {
RCLCPP_DEBUG(this->get_logger(), "左绕时左侧仍有障碍"); LOG_DEBUG("左绕时左侧仍有障碍");
return false; return false;
} }
if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical)
{ {
RCLCPP_DEBUG(this->get_logger(), "右绕时右侧仍有障碍"); LOG_DEBUG("右绕时右侧仍有障碍");
return false; return false;
} }
@ -1289,9 +1245,7 @@ private:
{ {
if (obstacle_analysis_.left_edge <= vehicle_max_y_) if (obstacle_analysis_.left_edge <= vehicle_max_y_)
{ {
RCLCPP_DEBUG(this->get_logger(), LOG_DEBUG("左绕时障碍物左边界%d仍在车右边界%d范围内", obstacle_analysis_.left_edge, vehicle_max_y_);
"左绕时障碍物左边界%d仍在车右边界%d范围内",
obstacle_analysis_.left_edge, vehicle_max_y_);
return false; return false;
} }
} }
@ -1299,9 +1253,8 @@ private:
{ {
if (obstacle_analysis_.right_edge >= vehicle_min_y_) if (obstacle_analysis_.right_edge >= vehicle_min_y_)
{ {
RCLCPP_DEBUG(this->get_logger(), LOG_DEBUG("右绕时障碍物右边界%d仍在车左边界%d范围内", obstacle_analysis_.right_edge,
"右绕时障碍物右边界%d仍在车左边界%d范围内", vehicle_min_y_);
obstacle_analysis_.right_edge, vehicle_min_y_);
return false; return false;
} }
} }
@ -1314,7 +1267,8 @@ private:
int calculateLateralShiftAngle() int calculateLateralShiftAngle()
{ {
// 基于可用空间和需求偏移距离计算角度 // 基于可用空间和需求偏移距离计算角度
int available_space = (avoid_ctx_.direction == -1) ? obstacle_analysis_.free_space_left : obstacle_analysis_.free_space_right; int available_space =
(avoid_ctx_.direction == -1) ? obstacle_analysis_.free_space_left : obstacle_analysis_.free_space_right;
int base_angle = 20; // 基础角度 int base_angle = 20; // 基础角度
@ -1343,16 +1297,13 @@ private:
// 检查横向间隙是否足够 // 检查横向间隙是否足够
bool checkLateralClearance() bool checkLateralClearance()
{ {
if (!grid_data_valid_ || cached_grid_.empty()) if (!grid_data_valid_ || cached_grid_.empty()) return false;
return false;
// 最小偏移阈值 // 最小偏移阈值
const int MIN_SHIFT_THRESHOLD = 2; // 2格 const int MIN_SHIFT_THRESHOLD = 2; // 2格
if (avoid_ctx_.lateral_shift_achieved < MIN_SHIFT_THRESHOLD) if (avoid_ctx_.lateral_shift_achieved < MIN_SHIFT_THRESHOLD)
{ {
RCLCPP_DEBUG(this->get_logger(), LOG_DEBUG("横移距离不足: %d < %d", avoid_ctx_.lateral_shift_achieved, MIN_SHIFT_THRESHOLD);
"横移距离不足: %d < %d",
avoid_ctx_.lateral_shift_achieved, MIN_SHIFT_THRESHOLD);
return false; return false;
} }
@ -1366,9 +1317,7 @@ private:
int required_position = obstacle_analysis_.left_edge - SAFE_MARGIN; int required_position = obstacle_analysis_.left_edge - SAFE_MARGIN;
if (vehicle_max_y_ > required_position) // 右边界 if (vehicle_max_y_ > required_position) // 右边界
{ {
RCLCPP_DEBUG(this->get_logger(), LOG_DEBUG("左绕未完成: 车右边界%d > 目标位置%d", vehicle_max_y_, required_position);
"左绕未完成: 车右边界%d > 目标位置%d",
vehicle_max_y_, required_position);
return false; return false;
} }
} }
@ -1377,9 +1326,7 @@ private:
int required_position = obstacle_analysis_.right_edge + SAFE_MARGIN; int required_position = obstacle_analysis_.right_edge + SAFE_MARGIN;
if (vehicle_min_y_ < required_position) if (vehicle_min_y_ < required_position)
{ {
RCLCPP_DEBUG(this->get_logger(), LOG_DEBUG("右绕未完成: 车左边界%d < 目标位置%d", vehicle_min_y_, required_position);
"右绕未完成: 车左边界%d < 目标位置%d",
vehicle_min_y_, required_position);
return false; return false;
} }
} }
@ -1391,20 +1338,16 @@ private:
for (int depth = 1; depth <= CHECK_DEPTH; depth++) for (int depth = 1; depth <= CHECK_DEPTH; depth++)
{ {
int check_row = vehicle_min_x_ - depth; int check_row = vehicle_min_x_ - depth;
if (check_row < 0) if (check_row < 0) break;
break;
// 检查整个车宽范围 // 检查整个车宽范围
for (int col = vehicle_min_y_; col <= vehicle_max_y_; col++) for (int col = vehicle_min_y_; col <= vehicle_max_y_; col++)
{ {
if (col < 0 || col >= static_cast<int>(cached_grid_[0].size())) if (col < 0 || col >= static_cast<int>(cached_grid_[0].size())) continue;
continue;
if (cached_grid_[check_row][col] == OBSTACLE) if (cached_grid_[check_row][col] == OBSTACLE)
{ {
RCLCPP_DEBUG(this->get_logger(), LOG_DEBUG("前方第%d行仍有障碍,位置[%d,%d]", depth, check_row, col);
"前方第%d行仍有障碍,位置[%d,%d]",
depth, check_row, col);
return false; return false;
} }
} }
@ -1418,21 +1361,17 @@ private:
for (int depth = 1; depth <= 5; depth++) for (int depth = 1; depth <= 5; depth++)
{ {
int check_row = vehicle_min_x_ - depth; int check_row = vehicle_min_x_ - depth;
if (check_row < 0) if (check_row < 0) break;
break;
// 检查车辆右侧若干列 // 检查车辆右侧若干列
for (int offset = 1; offset <= 3; offset++) for (int offset = 1; offset <= 3; offset++)
{ {
int check_col = vehicle_max_y_ + offset; int check_col = vehicle_max_y_ + offset;
if (check_col >= static_cast<int>(cached_grid_[0].size())) if (check_col >= static_cast<int>(cached_grid_[0].size())) break;
break;
if (cached_grid_[check_row][check_col] == OBSTACLE) if (cached_grid_[check_row][check_col] == OBSTACLE)
{ {
RCLCPP_DEBUG(this->get_logger(), LOG_DEBUG("右侧仍有障碍[%d,%d],左绕未完全清空", check_row, check_col);
"右侧仍有障碍[%d,%d],左绕未完全清空",
check_row, check_col);
return false; return false;
} }
} }
@ -1443,30 +1382,24 @@ private:
for (int depth = 1; depth <= 5; depth++) for (int depth = 1; depth <= 5; depth++)
{ {
int check_row = vehicle_min_x_ - depth; int check_row = vehicle_min_x_ - depth;
if (check_row < 0) if (check_row < 0) break;
break;
// 检查车辆左侧若干列 // 检查车辆左侧若干列
for (int offset = 1; offset <= 3; offset++) for (int offset = 1; offset <= 3; offset++)
{ {
int check_col = vehicle_min_y_ - offset; int check_col = vehicle_min_y_ - offset;
if (check_col < 0) if (check_col < 0) break;
break;
if (cached_grid_[check_row][check_col] == OBSTACLE) if (cached_grid_[check_row][check_col] == OBSTACLE)
{ {
RCLCPP_DEBUG(this->get_logger(), LOG_DEBUG("左侧仍有障碍[%d,%d],右绕未完全清空", check_row, check_col);
"左侧仍有障碍[%d,%d],右绕未完全清空",
check_row, check_col);
return false; return false;
} }
} }
} }
} }
RCLCPP_INFO(this->get_logger(), LOG_INFO("横向间隙检查通过: 方向=%s, 已移动=%d格", (avoid_ctx_.direction == -1) ? "左" : "右",
"横向间隙检查通过: 方向=%s, 已移动=%d格",
(avoid_ctx_.direction == -1) ? "左" : "右",
avoid_ctx_.lateral_shift_achieved); avoid_ctx_.lateral_shift_achieved);
return true; return true;
@ -1508,17 +1441,13 @@ private:
avoid_ctx_.last_state_change_time_set = true; avoid_ctx_.last_state_change_time_set = true;
avoid_ctx_.obstacle_passed = true; avoid_ctx_.obstacle_passed = true;
RCLCPP_INFO(this->get_logger(), LOG_INFO("阶段2完成: 障碍物已在车身后方(%.1fs), 进入阶段3: 回归轨迹", parallel_time);
"阶段2完成: 障碍物已在车身后方(%.1fs), 进入阶段3: 回归轨迹",
parallel_time);
} }
else else
{ {
if (avoid_ctx_.counter % 10 == 0) if (avoid_ctx_.counter % 10 == 0)
{ {
RCLCPP_INFO(this->get_logger(), LOG_INFO("阶段2进行中: 平行前进 %.1fs, 障碍物位置检查=%s", parallel_time,
"阶段2进行中: 平行前进 %.1fs, 障碍物位置检查=%s",
parallel_time,
obstacle_aligned_or_behind ? "已通过" : "未通过"); obstacle_aligned_or_behind ? "已通过" : "未通过");
} }
} }
@ -1530,8 +1459,7 @@ private:
bool checkObstaclePosition() bool checkObstaclePosition()
{ {
// 方法1: 检查前方核心区域是否清空 // 方法1: 检查前方核心区域是否清空
bool front_core_clear = !obstacle_analysis_.has_front_area && bool front_core_clear = !obstacle_analysis_.has_front_area && !obstacle_analysis_.has_front_critical;
!obstacle_analysis_.has_front_critical;
// 方法2: 检查障碍物是否不再在绕障侧的检测范围内 // 方法2: 检查障碍物是否不再在绕障侧的检测范围内
bool lateral_clear = false; bool lateral_clear = false;
@ -1539,15 +1467,13 @@ private:
{ {
// 检查右侧区域是否清空（障碍物应该在左后方） // 检查右侧区域是否清空（障碍物应该在左后方）
lateral_clear = !obstacle_analysis_.has_right_critical && lateral_clear = !obstacle_analysis_.has_right_critical &&
(obstacle_analysis_.right_edge == -1 || (obstacle_analysis_.right_edge == -1 || obstacle_analysis_.right_edge < vehicle_min_y_);
obstacle_analysis_.right_edge < vehicle_min_y_);
} }
else // 右绕 else // 右绕
{ {
// 检查左侧区域是否清空（障碍物应该在右后方） // 检查左侧区域是否清空（障碍物应该在右后方）
lateral_clear = !obstacle_analysis_.has_left_critical && lateral_clear = !obstacle_analysis_.has_left_critical &&
(obstacle_analysis_.left_edge == -1 || (obstacle_analysis_.left_edge == -1 || obstacle_analysis_.left_edge > vehicle_max_y_);
obstacle_analysis_.left_edge > vehicle_max_y_);
} }
// 综合判断 // 综合判断
@ -1555,11 +1481,8 @@ private:
if (avoid_ctx_.counter % 10 == 0) if (avoid_ctx_.counter % 10 == 0)
{ {
RCLCPP_DEBUG(this->get_logger(), LOG_DEBUG("障碍物位置检查: 前方清空=%s, 侧方清空=%s => %s", front_core_clear ? "是" : "否",
"障碍物位置检查: 前方清空=%s, 侧方清空=%s => %s", lateral_clear ? "是" : "否", obstacle_passed ? "已通过" : "未通过");
front_core_clear ? "是" : "否",
lateral_clear ? "是" : "否",
obstacle_passed ? "已通过" : "未通过");
} }
return obstacle_passed; return obstacle_passed;
@ -1573,7 +1496,7 @@ private:
{ {
publish_speed = 0; publish_speed = 0;
avoid_ctx_.state = AVOID_WAITING; avoid_ctx_.state = AVOID_WAITING;
RCLCPP_ERROR(this->get_logger(), "平行前进时前方出现障碍物！"); LOG_ERROR("平行前进时前方出现障碍物！");
return true; return true;
} }
@ -1582,7 +1505,7 @@ private:
{ {
publish_speed = 0; publish_speed = 0;
avoid_ctx_.state = AVOID_WAITING; avoid_ctx_.state = AVOID_WAITING;
RCLCPP_ERROR(this->get_logger(), "平行前进时左侧出现障碍物！"); LOG_ERROR("平行前进时左侧出现障碍物！");
return true; return true;
} }
@ -1590,7 +1513,7 @@ private:
{ {
publish_speed = 0; publish_speed = 0;
avoid_ctx_.state = AVOID_WAITING; avoid_ctx_.state = AVOID_WAITING;
RCLCPP_ERROR(this->get_logger(), "平行前进时右侧出现障碍物！"); LOG_ERROR("平行前进时右侧出现障碍物！");
return true; return true;
} }
@ -1630,9 +1553,7 @@ private:
publish_speed = 800; publish_speed = 800;
avoid_ctx_.reset(); avoid_ctx_.reset();
RCLCPP_INFO(this->get_logger(), LOG_INFO("✓ 回正完成(%.1fs), 绕障结束! 目标角度=%d°", return_time, target_angle_int);
"✓ 回正完成(%.1fs), 绕障结束! 目标角度=%d°",
return_time, target_angle_int);
} }
else else
{ {
@ -1650,9 +1571,7 @@ private:
if (avoid_ctx_.counter % 10 == 0) if (avoid_ctx_.counter % 10 == 0)
{ {
RCLCPP_INFO(this->get_logger(), LOG_INFO("回正中: 当前角度=%d°, 目标角度=%d°, 差值=%d°", publish_angle, target_angle_int, angle_diff);
"回正中: 当前角度=%d°, 目标角度=%d°, 差值=%d°",
publish_angle, target_angle_int, angle_diff);
} }
} }
@ -1667,7 +1586,7 @@ private:
{ {
publish_speed = 0; publish_speed = 0;
publish_angle = 0; publish_angle = 0;
RCLCPP_WARN(this->get_logger(), "回正暂停: 需要左转但左侧有障碍"); LOG_WARN("回正暂停: 需要左转但左侧有障碍");
return true; return true;
} }
@ -1675,7 +1594,7 @@ private:
{ {
publish_speed = 0; publish_speed = 0;
publish_angle = 0; publish_angle = 0;
RCLCPP_WARN(this->get_logger(), "回正暂停: 需要右转但右侧有障碍"); LOG_WARN("回正暂停: 需要右转但右侧有障碍");
return true; return true;
} }
@ -1684,7 +1603,7 @@ private:
{ {
publish_speed = 0; publish_speed = 0;
publish_angle = 0; publish_angle = 0;
RCLCPP_WARN(this->get_logger(), "回正暂停: 前方出现障碍"); LOG_WARN("回正暂停: 前方出现障碍");
avoid_ctx_.state = AVOID_WAITING; avoid_ctx_.state = AVOID_WAITING;
return true; return true;
} }
@ -1700,7 +1619,7 @@ private:
{ {
publish_speed = 0; publish_speed = 0;
avoid_ctx_.state = AVOID_WAITING; avoid_ctx_.state = AVOID_WAITING;
RCLCPP_ERROR(this->get_logger(), "左绕过程中左侧出现障碍物！"); LOG_ERROR("左绕过程中左侧出现障碍物！");
return true; return true;
} }
@ -1708,7 +1627,7 @@ private:
{ {
publish_speed = 0; publish_speed = 0;
avoid_ctx_.state = AVOID_WAITING; avoid_ctx_.state = AVOID_WAITING;
RCLCPP_ERROR(this->get_logger(), "右绕过程中右侧出现障碍物！"); LOG_ERROR("右绕过程中右侧出现障碍物！");
return true; return true;
} }
@ -1716,11 +1635,16 @@ private:
} }
}; };
int main(int argc, char **argv) int main(int argc, char** argv)
{ {
// 初始化日志系统
logger::Logger::Init("fu", "./log");
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
auto node = std::make_shared<fu_node>("fu_node"); auto node = std::make_shared<fu_node>("fu_node");
rclcpp::spin(node); rclcpp::spin(node);
rclcpp::shutdown(); rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
return 0; return 0;
} }

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

@ -21,19 +21,19 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
# find_package(mc REQUIRED) # find_package(mc REQUIRED)
find_package(logger REQUIRED)
include_directories( include_directories(include/pl ${catkin_INCLUDE_DIRS})
include/pl
${catkin_INCLUDE_DIRS}
)
add_executable(pl_node src/pl_node.cpp src/pl.cpp src/jsoncpp.cpp) add_executable(pl_node src/pl_node.cpp src/pl.cpp src/jsoncpp.cpp)
ament_target_dependencies(pl_node rclcpp std_msgs sweeper_interfaces) ament_target_dependencies(
install(TARGETS
pl_node pl_node
DESTINATION lib/${PROJECT_NAME} rclcpp
) std_msgs
sweeper_interfaces
logger)
install(TARGETS pl_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)

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

@ -1,9 +1,12 @@
#include "pl.hpp" #include "pl.hpp"
#include <iostream>
#include <unistd.h>
#include <string.h>
#include <math.h> #include <math.h>
#include <string.h>
#include <unistd.h>
#include <iostream>
#include "logger/logger.h"
using namespace std; using namespace std;
@ -46,7 +49,8 @@ double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
} }
// Cur_navAngle指定y轴正方向.Cur_lonti，Cur_lati为原点；x_diff,y_diff是Dest_lonti,Dest_lati在该坐标系的直角坐标:m // Cur_navAngle指定y轴正方向.Cur_lonti，Cur_lati为原点；x_diff,y_diff是Dest_lonti,Dest_lati在该坐标系的直角坐标:m
int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati, double *x_diff, double *y_diff) int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati,
double* x_diff, double* y_diff)
{ {
double Navi_rad, x, y; double Navi_rad, x, y;
float k1, k2, k3, k4; // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta); float k1, k2, k3, k4; // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta);
@ -66,7 +70,7 @@ int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, doubl
} }
// Find_Nearest_Point // Find_Nearest_Point
int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info) int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point* Road_info)
{ {
if (start_index < 0) if (start_index < 0)
{ {
@ -75,17 +79,16 @@ int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_in
int Nearest_point_index = start_index; // 初始化为起始索引 int Nearest_point_index = start_index; // 初始化为起始索引
double min_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, double min_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
Road_info[start_index].LongitudeInfo, Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo);
Road_info[start_index].LatitudeInfo);
// 从 start_index + 1 开始搜索 // 从 start_index + 1 开始搜索
for (int i = 1; i < GpsPointNum; i++) for (int i = 1; i < GpsPointNum; i++)
{ {
int current_index = (start_index + i) % GpsPointNum; // 环绕访问 int current_index = (start_index + i) % GpsPointNum; // 环绕访问
double current_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, double current_distance =
Road_info[current_index].LongitudeInfo, ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
Road_info[current_index].LatitudeInfo); Road_info[current_index].LongitudeInfo, Road_info[current_index].LatitudeInfo);
if (current_distance < min_distance) if (current_distance < min_distance)
{ {
@ -112,13 +115,15 @@ int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_in
} }
int Nearest_point_index = 0; int Nearest_point_index = 0;
double aft_distance = 0, pre_distance = 0; double aft_distance = 0, pre_distance = 0;
pre_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo); pre_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo);
int start = start_index + 1; int start = start_index + 1;
for (int i = start; i < GpsPointNum; i++) for (int i = start; i < GpsPointNum; i++)
{ {
aft_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo); aft_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo);
// printf("i: %d , aft_distance: %lf \t",i,aft_distance); // printf("i: %d , aft_distance: %lf \t",i,aft_distance);
if (aft_distance < pre_distance) if (aft_distance < pre_distance)
{ {
@ -152,7 +157,7 @@ int Road_Planning_Find_Aim_Point(int start_index, int dest_num)
return (dest_index); return (dest_index);
} }
int star_find_nearest_point(double lon, double lat, WRC_GPS_Point *Road_info) int star_find_nearest_point(double lon, double lat, WRC_GPS_Point* Road_info)
{ {
double dis = 10000; double dis = 10000;
int point = 0; int point = 0;
@ -164,7 +169,7 @@ int star_find_nearest_point(double lon, double lat, WRC_GPS_Point *Road_info)
dis = dis_temp; dis = dis_temp;
point = i; point = i;
} }
printf("i: %d , dis_temp: %lf", i, dis_temp); LOG_DEBUG("i: %d , dis_temp: %lf", i, dis_temp);
} }
return point; return point;
} }
@ -174,8 +179,7 @@ Direction straight_or_turn(double cur_direction, double des_direction, int thres
{ {
double diff = std::fmod(std::fabs(cur_direction - des_direction), 360.0); double diff = std::fmod(std::fabs(cur_direction - des_direction), 360.0);
if (diff > 180) if (diff > 180) diff = 360 - diff;
diff = 360 - diff;
// std::cout << "diff : " << diff << std::endl; // std::cout << "diff : " << diff << std::endl;
@ -238,16 +242,15 @@ void PL_ProcThread()
// cout << " * * * * * * * * * * * * * * * * * * * * *" << endl; // cout << " * * * * * * * * * * * * * * * * * * * * *" << endl;
// cout << endl; // cout << endl;
// cout << "--- -target_head " << target_head << "--- current_head " << current_head << " " << abs(target_head - current_head) << endl; // cout << "--- -target_head " << target_head << "--- current_head " << current_head << " " <<
// cout << endl; // abs(target_head - current_head) << endl; cout << endl; cout << "--- -distance " << distance << "- -nl_radius
// cout << "--- -distance " << distance << "- -nl_radius " << nl_radius << endl; // " << nl_radius << endl;
// cout << endl; // cout << endl;
// cout << " * * * * * * * * * * * * * * * * * * * * *" << endl; // cout << " * * * * * * * * * * * * * * * * * * * * *" << endl;
if (road_pos >= (GpsPointNum - 1)) if (road_pos >= (GpsPointNum - 1))
{ {
x = 0.0; x = 0.0;
y = 1.0; y = 1.0;
pl_speed = 0; pl_speed = 0;
@ -260,10 +263,8 @@ void PL_ProcThread()
double x_cm_tmp = 0; double x_cm_tmp = 0;
double y_cm_tmp = 0; double y_cm_tmp = 0;
ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat, ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat, GPSPointQueue[des_pos].LongitudeInfo,
GPSPointQueue[des_pos].LongitudeInfo, GPSPointQueue[des_pos].LatitudeInfo, &x_cm_tmp, &y_cm_tmp);
GPSPointQueue[des_pos].LatitudeInfo,
&x_cm_tmp, &y_cm_tmp);
x = x_cm_tmp; x = x_cm_tmp;
y = y_cm_tmp; y = y_cm_tmp;
@ -277,20 +278,20 @@ void PL_ProcThread()
return; return;
} }
void *pl_thread(void *args) void* pl_thread(void* args)
{ {
(void)args; (void)args;
sleep(3); sleep(3);
memset(GPSPointQueue, 0, sizeof(GPSPointQueue)); memset(GPSPointQueue, 0, sizeof(GPSPointQueue));
FILE *fp_gps = fopen("gps_load_now.txt", "r"); FILE* fp_gps = fopen("gps_load_now.txt", "r");
if (NULL == fp_gps) if (NULL == fp_gps)
{ {
printf("open route file error\n"); LOG_ERROR("open route file error");
return NULL; return NULL;
} }
std::cout << "have opened " << "gps_load_now.txt" << std::endl; LOG_INFO("have opened gps_load_now.txt");
char mystring[200]; char mystring[200];
memset(mystring, '\0', 200); memset(mystring, '\0', 200);
@ -318,11 +319,10 @@ void *pl_thread(void *args)
} }
GpsPointNum = i / 4; GpsPointNum = i / 4;
printf("point_num:%d\n", GpsPointNum); LOG_INFO("point_num:%d", GpsPointNum);
fclose(fp_gps); fclose(fp_gps);
std::cout << std::endl;
printf("#################### auto drive on! ####################\n"); LOG_INFO("#################### auto drive on! ####################");
PL_ProcThread(); PL_ProcThread();
return NULL; return NULL;

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

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

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

@ -21,23 +21,20 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(CURL REQUIRED) find_package(CURL REQUIRED)
find_package(logger REQUIRED)
include_directories(include/route ${catkin_INCLUDE_DIRS})
include_directories( add_executable(route_node src/route_node.cpp src/md5.cpp src/jsoncpp.cpp)
include/route ament_target_dependencies(
${catkin_INCLUDE_DIRS}
)
add_executable(route_node
src/route_node.cpp
src/md5.cpp
src/jsoncpp.cpp)
ament_target_dependencies(route_node rclcpp std_msgs sweeper_interfaces CURL)
install(TARGETS
route_node route_node
DESTINATION lib/${PROJECT_NAME} rclcpp
) std_msgs
sweeper_interfaces
CURL
logger)
install(TARGETS route_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)

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

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

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

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

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

@ -11,6 +11,7 @@ find_package(ament_index_cpp REQUIRED)
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(logger REQUIRED)
# 搜索 src 目录下所有 .cpp 文件 # 搜索 src 目录下所有 .cpp 文件
file(GLOB SRC_FILES src/*.cpp) file(GLOB SRC_FILES src/*.cpp)
@ -18,12 +19,16 @@ file(GLOB SRC_FILES src/*.cpp)
# 创建可执行文件 # 创建可执行文件
add_executable(mc_node ${SRC_FILES}) add_executable(mc_node ${SRC_FILES})
ament_target_dependencies(mc_node ament_index_cpp rclcpp std_msgs sweeper_interfaces) ament_target_dependencies(
mc_node
ament_index_cpp
rclcpp
std_msgs
sweeper_interfaces
logger)
# Set include directories for the target # Set include directories for the target
target_include_directories( target_include_directories(mc_node PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
mc_node
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE: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)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -1,9 +1,11 @@
#include "mc/timer_tasks.hpp" #include "mc/timer_tasks.hpp"
#include "mc/control_cache.hpp"
#include "logger/logger.h"
#include "mc/can_struct.h" #include "mc/can_struct.h"
#include "mc/control_cache.hpp"
// 定时器回调：MCU 控制 // 定时器回调：MCU 控制
void mcuTimerTask(CANDriver &canctl) void mcuTimerTask(CANDriver& canctl)
{ {
auto msg = get_safe_control(); auto msg = get_safe_control();
mcu_cmd.setEnabled(true); mcu_cmd.setEnabled(true);
@ -11,14 +13,14 @@ void mcuTimerTask(CANDriver &canctl)
mcu_cmd.setRPM(msg.rpm); mcu_cmd.setRPM(msg.rpm);
mcu_cmd.setBrake(msg.brake); mcu_cmd.setBrake(msg.brake);
canctl.sendFrame(mcu_cmd.toFrame()); canctl.sendFrame(mcu_cmd.toFrame());
// std::cout << "mcuTimerTask" << std::endl; // LOG_INFO("mcuTimerTask");
// std::cout << "msg.gear " << msg.gear << std::endl; // LOG_INFO("msg.gear %d", msg.gear);
// std::cout << "msg.brake " << msg.brake << std::endl; // LOG_INFO("msg.brake %d", msg.brake);
// std::cout << "msg.rpm " << msg.rpm << std::endl; // LOG_INFO("msg.rpm %d", msg.rpm);
} }
// 定时器回调：EPS 控制 // 定时器回调：EPS 控制
void epsTimerTask(CANDriver &canctl) void epsTimerTask(CANDriver& canctl)
{ {
auto msg = get_safe_control(); auto msg = get_safe_control();
eps_cmd.setCenterCmd(0); eps_cmd.setCenterCmd(0);
@ -26,12 +28,12 @@ void epsTimerTask(CANDriver &canctl)
eps_cmd.setAngularSpeed(msg.angle_speed); eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack(); eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame()); canctl.sendFrame(eps_cmd.toFrame());
// std::cout << "epsTimerTask" << std::endl; // LOG_INFO("epsTimerTask");
} }
// 定时器回调：VCU 扫地控制 // 定时器回调：VCU 扫地控制
// 修改timer_tasks.cpp中的vcuTimerTask函数 // 修改timer_tasks.cpp中的vcuTimerTask函数
void vcuTimerTask(CANDriver &canctl) void vcuTimerTask(CANDriver& canctl)
{ {
static bool vcu_initialized = false; static bool vcu_initialized = false;
static bool last_sweep_state = false; // 记录上一次清扫状态 static bool last_sweep_state = false; // 记录上一次清扫状态
@ -44,9 +46,9 @@ void vcuTimerTask(CANDriver &canctl)
// 发送CAN使能指令 // 发送CAN使能指令
vcu_enable_cmd.setCANEnable(true); vcu_enable_cmd.setCANEnable(true);
canctl.sendFrame(vcu_enable_cmd.toFrame()); canctl.sendFrame(vcu_enable_cmd.toFrame());
// std::cout << "mcuTimerTask" << std::endl; // LOG_INFO("mcuTimerTask");
vcu_initialized = true; vcu_initialized = true;
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled"); LOG_INFO("[VCU] CAN control enabled");
} }
// 检查清扫状态是否变化 // 检查清扫状态是否变化
@ -68,7 +70,7 @@ void vcuTimerTask(CANDriver &canctl)
vcu_motor1_cmd.setByte6(target_value); vcu_motor1_cmd.setByte6(target_value);
vcu_motor1_cmd.setByte7(target_value); vcu_motor1_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor1_cmd.toFrame()); canctl.sendFrame(vcu_motor1_cmd.toFrame());
// std::cout << "vcuTimerTask1" << std::endl; // LOG_INFO("vcuTimerTask1");
// ===== 控制0x212报文 (电机M8和LED输出) ===== // ===== 控制0x212报文 (电机M8和LED输出) =====
vcu_motor2_cmd.setByte0(target_value); vcu_motor2_cmd.setByte0(target_value);
@ -80,10 +82,9 @@ void vcuTimerTask(CANDriver &canctl)
vcu_motor2_cmd.setByte6(target_value); vcu_motor2_cmd.setByte6(target_value);
vcu_motor2_cmd.setByte7(target_value); vcu_motor2_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor2_cmd.toFrame()); canctl.sendFrame(vcu_motor2_cmd.toFrame());
// std::cout << "vcuTimerTask2" << std::endl; // LOG_INFO("vcuTimerTask2");
// RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), // LOG_INFO("[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
// "[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
// 更新状态记录 // 更新状态记录
last_sweep_state = msg.sweep; last_sweep_state = msg.sweep;
@ -91,14 +92,14 @@ void vcuTimerTask(CANDriver &canctl)
} }
// 定时器回调：BMS 查询任务 // 定时器回调：BMS 查询任务
void bmsTimerTask(CANDriver &canctl) void bmsTimerTask(CANDriver& canctl)
{ {
static bool bms_initialized = false; static bool bms_initialized = false;
// 首次运行时初始化日志 // 首次运行时初始化日志
if (!bms_initialized) if (!bms_initialized)
{ {
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[BMS] Query task started"); LOG_INFO("[BMS] Query task started");
bms_initialized = true; bms_initialized = true;
} }
@ -108,27 +109,19 @@ void bmsTimerTask(CANDriver &canctl)
} }
// 注册所有定时器任务 // 注册所有定时器任务
void setupTimers(rclcpp::Node::SharedPtr node, CANDriver &canctl) void setupTimers(rclcpp::Node::SharedPtr node, CANDriver& canctl)
{ {
static auto timer_mcu = node->create_wall_timer( static auto timer_mcu =
std::chrono::milliseconds(50), node->create_wall_timer(std::chrono::milliseconds(50), [&canctl]() { mcuTimerTask(canctl); });
[&canctl]()
{ mcuTimerTask(canctl); });
static auto timer_eps = node->create_wall_timer( static auto timer_eps =
std::chrono::milliseconds(50), node->create_wall_timer(std::chrono::milliseconds(50), [&canctl]() { epsTimerTask(canctl); });
[&canctl]()
{ epsTimerTask(canctl); });
static auto timer_vcu = node->create_wall_timer( static auto timer_vcu =
std::chrono::milliseconds(100), node->create_wall_timer(std::chrono::milliseconds(100), [&canctl]() { vcuTimerTask(canctl); });
[&canctl]()
{ vcuTimerTask(canctl); });
static auto timer_bms = node->create_wall_timer( static auto timer_bms =
std::chrono::milliseconds(200), node->create_wall_timer(std::chrono::milliseconds(200), [&canctl]() { bmsTimerTask(canctl); });
[&canctl]()
{ bmsTimerTask(canctl); });
RCLCPP_INFO(node->get_logger(), "[TIMER] All timers setup completed"); LOG_INFO("[TIMER] All timers setup completed");
} }

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

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

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

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

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

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

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

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

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

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

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

@ -11,6 +11,7 @@
<depend>sweeper_interfaces</depend> <depend>sweeper_interfaces</depend>
<depend>radio_ctrl</depend> <depend>radio_ctrl</depend>
<depend>ament_index_cpp</depend> <depend>ament_index_cpp</depend>
<depend>logger</depend>
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -1,22 +1,23 @@
#ifndef UART_HANDLER_H #ifndef UART_HANDLER_H
#define UART_HANDLER_H #define UART_HANDLER_H
#include <thread> #include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <algorithm>
#include <atomic> #include <atomic>
#include <string>
#include <cstring> #include <cstring>
#include <iostream> #include <iostream>
#include <fcntl.h> #include <string>
#include <unistd.h> #include <thread>
#include <errno.h>
#include <termios.h>
#include <vector> #include <vector>
#include <algorithm>
class UartHandler class UartHandler
{ {
public: public:
UartHandler(const std::string &device, int baudrate = 100000); UartHandler(const std::string& device, int baudrate = 100000);
~UartHandler(); ~UartHandler();
bool open_serial(); // 打开串口 bool open_serial(); // 打开串口
@ -25,7 +26,7 @@ public:
int get_channel_value(int channel); // 获取指定通道的数据 int get_channel_value(int channel); // 获取指定通道的数据
bool get_data_safe(); // 获取数据安全性 bool get_data_safe(); // 获取数据安全性
private: private:
std::string serial_device; std::string serial_device;
int baudrate; int baudrate;
int fd; int fd;
@ -48,9 +49,9 @@ private:
std::thread read_thread; // 读取数据的线程 std::thread read_thread; // 读取数据的线程
void read_loop(); // 持续读取串口数据 void read_loop(); // 持续读取串口数据
void parse_data(std::vector<uint8_t> &buffer); // 解析数据 void parse_data(std::vector<uint8_t>& buffer); // 解析数据
int sbus_parse(); // SBUS 数据解析 int sbus_parse(); // SBUS 数据解析
void print_hex(uint8_t *buf, int len); // 打印数据（调试用） void print_hex(uint8_t* buf, int len); // 打印数据（调试用）
}; };
#endif // UART_HANDLER_H #endif // UART_HANDLER_H

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

@ -13,6 +13,7 @@
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>ament_index_cpp</depend> <depend>ament_index_cpp</depend>
<depend>sweeper_interfaces</depend> <depend>sweeper_interfaces</depend>
<depend>logger</depend>
<build_depend>ament_cmake</build_depend> <build_depend>ament_cmake</build_depend>
<exec_depend>ament_cmake</exec_depend> <exec_depend>ament_cmake</exec_depend>

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -1,24 +1,23 @@
#include <rclcpp/rclcpp.hpp> #include <pcl/filters/crop_box.h>
#include <sensor_msgs/msg/point_cloud2.hpp> #include <pcl/filters/statistical_outlier_removal.h>
#include <tf2_ros/transform_listener.h> #include <pcl/filters/voxel_grid.h>
#include <tf2_ros/buffer.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <deque>
#include <unordered_map>
#include <mutex>
#include <memory>
#include <chrono>
#include <pcl/point_cloud.h> #include <pcl/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h> #include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <chrono>
#include <deque>
#include <functional>
#include <memory>
#include <mutex>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/int32_multi_array.hpp> #include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/multi_array_dimension.hpp> #include <std_msgs/msg/multi_array_dimension.hpp>
#include <functional> #include <unordered_map>
using sensor_msgs::msg::PointCloud2; using sensor_msgs::msg::PointCloud2;
using std::placeholders::_1; using std::placeholders::_1;
@ -35,7 +34,7 @@ struct CloudFrame
// 内存池实现 // 内存池实现
class PointCloud2Pool class PointCloud2Pool
{ {
public: public:
std::shared_ptr<PointCloud2> acquire() std::shared_ptr<PointCloud2> acquire()
{ {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
@ -53,8 +52,7 @@ public:
void release(std::shared_ptr<PointCloud2> cloud) void release(std::shared_ptr<PointCloud2> cloud)
{ {
if (!cloud) if (!cloud) return;
return;
cloud->data.clear(); cloud->data.clear();
cloud->width = 0; cloud->width = 0;
@ -68,14 +66,14 @@ public:
} }
} }
private: private:
std::vector<std::shared_ptr<PointCloud2>> pool_; std::vector<std::shared_ptr<PointCloud2>> pool_;
std::mutex mutex_; std::mutex mutex_;
}; };
class LidarMerger : public rclcpp::Node class LidarMerger : public rclcpp::Node
{ {
public: public:
LidarMerger() : Node("lidar_merger") LidarMerger() : Node("lidar_merger")
{ {
/* ---------- 参数 ---------- */ /* ---------- 参数 ---------- */
@ -111,34 +109,24 @@ public:
car_lidar_offset_y_ = declare_parameter<float>("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移 car_lidar_offset_y_ = declare_parameter<float>("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移
// 打印所有参数值（添加到构造函数末尾） // 打印所有参数值（添加到构造函数末尾）
RCLCPP_INFO_STREAM(this->get_logger(), RCLCPP_INFO_STREAM(
this->get_logger(),
"\n\n---------- 点云融合节点参数配置 ----------" "\n\n---------- 点云融合节点参数配置 ----------"
<< "\n [输入输出]" << "\n [输入输出]"
<< "\n 前雷达话题: " << front_topic_ << "\n 前雷达话题: " << front_topic_ << "\n 后雷达话题: " << rear_topic_
<< "\n 后雷达话题: " << rear_topic_ << "\n 输出话题: " << output_topic_ << "\n 目标坐标系: " << target_frame_ << "\n [同步参数]"
<< "\n 输出话题: " << output_topic_ << "\n 队列大小: " << queue_size_ << "\n 缓存大小: " << cache_size_ << "\n 时间容差(s): "
<< "\n 目标坐标系: " << target_frame_ << time_tolerance_ << "\n 最大等待时间(s): " << max_wait_time_ << "\n [调试选项]"
<< "\n [同步参数]" << "\n 调试模式: " << (enable_debug_ ? "开启" : "关闭") << "\n [点云处理]"
<< "\n 队列大小: " << queue_size_ << "\n 空间过滤范围(m): [" << filter_min_x_ << ", " << filter_max_x_ << "] x [" << filter_min_y_
<< "\n 缓存大小: " << cache_size_ << ", " << filter_max_y_ << "] x [" << filter_min_z_ << ", " << filter_max_z_ << "]"
<< "\n 时间容差(s): " << time_tolerance_ << "\n 体素大小(m): " << voxel_size_ << "\n 离群点k值: " << stat_mean_k_
<< "\n 最大等待时间(s): " << max_wait_time_ << "\n 离群点标准差阈值: " << stat_std_thresh_ << "\n 栅格大小: " << grid_size_ << "x"
<< "\n [调试选项]" << grid_size_ << "\n 栅格范围(m): " << grid_range_
<< "\n 调试模式: " << (enable_debug_ ? "开启" : "关闭") << "\n 打印栅格: " << (enable_print_ ? "开启" : "关闭") << "\n [车身过滤]"
<< "\n [点云处理]" << "\n 启用车身过滤: " << (filter_car_ ? "是" : "否") << "\n 车身尺寸(m): " << car_length_
<< "\n 空间过滤范围(m): [" << filter_min_x_ << ", " << filter_max_x_ << " x " << car_width_ << "\n LiDAR偏移(m): (" << car_lidar_offset_x_ << ", " << car_lidar_offset_y_
<< "] 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"); << "\n--------------------------------------------\n");
/* ---------- TF ---------- */ /* ---------- TF ---------- */
@ -146,21 +134,17 @@ public:
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_); tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
/* ---------- QoS - 优化为低延时 ---------- */ /* ---------- QoS - 优化为低延时 ---------- */
auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)) auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)).best_effort().durability_volatile();
.best_effort()
.durability_volatile();
// 移除deadline约束，避免因延时导致的消息丢弃 // 移除deadline约束，避免因延时导致的消息丢弃
/* ---------- 订阅 ---------- */ /* ---------- 订阅 ---------- */
sub_front_ = create_subscription<PointCloud2>( sub_front_ = create_subscription<PointCloud2>(front_topic_, qos,
front_topic_, qos, std::bind(&LidarMerger::frontCB, this, std::placeholders::_1)); std::bind(&LidarMerger::frontCB, this, std::placeholders::_1));
sub_rear_ = create_subscription<PointCloud2>( sub_rear_ = create_subscription<PointCloud2>(rear_topic_, qos,
rear_topic_, qos, std::bind(&LidarMerger::rearCB, this, std::placeholders::_1)); std::bind(&LidarMerger::rearCB, this, std::placeholders::_1));
/* ---------- 发布 ---------- */ /* ---------- 发布 ---------- */
auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)) auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)).reliable().durability_volatile();
.reliable()
.durability_volatile();
pub_ = create_publisher<PointCloud2>(output_topic_, pub_qos); pub_ = create_publisher<PointCloud2>(output_topic_, pub_qos);
// 创建栅格发布者 // 创建栅格发布者
@ -176,7 +160,7 @@ public:
cache_size_, time_tolerance_, max_wait_time_); cache_size_, time_tolerance_, max_wait_time_);
} }
private: private:
/* ---------- 回调函数 - 同步处理 ---------- */ /* ---------- 回调函数 - 同步处理 ---------- */
void frontCB(const PointCloud2::SharedPtr msg) void frontCB(const PointCloud2::SharedPtr msg)
{ {
@ -207,8 +191,7 @@ private:
} }
/* ---------- 同步处理点云 ---------- */ /* ---------- 同步处理点云 ---------- */
void processCloudSync(const PointCloud2::SharedPtr msg, const std::string &source, void processCloudSync(const PointCloud2::SharedPtr msg, const std::string& source, const rclcpp::Time& receive_time)
const rclcpp::Time &receive_time)
{ {
// 立即进行坐标变换 // 立即进行坐标变换
auto transformed_cloud = transformCloud(*msg); auto transformed_cloud = transformCloud(*msg);
@ -219,11 +202,8 @@ private:
} }
// 创建CloudFrame // 创建CloudFrame
auto cloud_frame = std::make_shared<CloudFrame>(CloudFrame{ auto cloud_frame = std::make_shared<CloudFrame>(
transformed_cloud, CloudFrame{transformed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source});
rclcpp::Time(msg->header.stamp),
receive_time,
source});
// 添加到缓存并立即尝试合并 // 添加到缓存并立即尝试合并
{ {
@ -241,7 +221,7 @@ private:
/* ---------- 添加到缓存 ---------- */ /* ---------- 添加到缓存 ---------- */
void addToCache(std::shared_ptr<CloudFrame> frame) void addToCache(std::shared_ptr<CloudFrame> frame)
{ {
auto &cache = (frame->source == "front") ? front_clouds_ : rear_clouds_; auto& cache = (frame->source == "front") ? front_clouds_ : rear_clouds_;
// 保持时间排序（最新的在前） // 保持时间排序（最新的在前）
auto it = cache.begin(); auto it = cache.begin();
@ -262,14 +242,12 @@ private:
/* ---------- 尝试合并 - 触发式 ---------- */ /* ---------- 尝试合并 - 触发式 ---------- */
void tryMerge() void tryMerge()
{ {
if (front_clouds_.empty() || rear_clouds_.empty()) if (front_clouds_.empty() || rear_clouds_.empty()) return;
return;
// 查找最佳匹配 // 查找最佳匹配
auto [front_frame, rear_frame] = findBestMatchOptimized(); auto [front_frame, rear_frame] = findBestMatchOptimized();
if (!front_frame || !rear_frame) if (!front_frame || !rear_frame) return;
return;
// 自适应延时检查 - 基于实际网络条件调整 // 自适应延时检查 - 基于实际网络条件调整
auto now_time = now(); auto now_time = now();
@ -289,8 +267,8 @@ private:
if (enable_debug_) if (enable_debug_)
{ {
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000,
"Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs", "Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs", front_age,
front_age, rear_age, adaptive_threshold); rear_age, adaptive_threshold);
} }
// 不直接返回，而是清理最旧的数据后重试 // 不直接返回，而是清理最旧的数据后重试
@ -314,8 +292,7 @@ private:
auto merged = mergeClouds(*front_frame, *rear_frame); auto merged = mergeClouds(*front_frame, *rear_frame);
auto merge_end = std::chrono::high_resolution_clock::now(); auto merge_end = std::chrono::high_resolution_clock::now();
if (!merged) if (!merged) return;
return;
// 设置时间戳和frame_id // 设置时间戳和frame_id
auto front_time = front_frame->stamp.nanoseconds(); auto front_time = front_frame->stamp.nanoseconds();
@ -333,8 +310,7 @@ private:
// ========================= 点云 ========================= // ========================= 点云 =========================
auto processed_pcl = processPointCloud(merged_pcl); auto processed_pcl = processPointCloud(merged_pcl);
if (!processed_pcl) if (!processed_pcl) return;
return;
// 将处理后的PCL点云转回ROS消息 // 将处理后的PCL点云转回ROS消息
auto processed = std::make_shared<sensor_msgs::msg::PointCloud2>(); auto processed = std::make_shared<sensor_msgs::msg::PointCloud2>();
@ -346,12 +322,10 @@ private:
// ========================= 点云 ========================= // ========================= 点云 =========================
// ========================= 栅格 ========================= // ========================= 栅格 =========================
auto grid = processPointCloud_grid(merged_pcl); auto grid = processPointCloud_grid(merged_pcl);
if (grid.empty()) if (grid.empty()) return;
return;
// 可视化栅格 // 可视化栅格
if (enable_print_) if (enable_print_) visualizeGridInTerminal(grid);
visualizeGridInTerminal(grid);
// 发布栅格到ROS话题 // 发布栅格到ROS话题
publishGrid(grid); publishGrid(grid);
@ -361,12 +335,13 @@ private:
if (enable_debug_ && merged_count_ % 10 == 0) // 每10次输出一次 if (enable_debug_ && merged_count_ % 10 == 0) // 每10次输出一次
{ {
auto total_delay = (now_time - processed->header.stamp).seconds(); auto total_delay = (now_time - processed->header.stamp).seconds();
auto process_time = std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - merge_start).count(); auto process_time =
std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - merge_start).count();
RCLCPP_INFO(get_logger(), RCLCPP_INFO(
get_logger(),
"Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs", "Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs",
merged_count_, merged->data.size() / merged->point_step, merged_count_, merged->data.size() / merged->point_step, processed->data.size() / processed->point_step,
processed->data.size() / processed->point_step,
total_delay, process_time * 1000, adaptive_threshold); total_delay, process_time * 1000, adaptive_threshold);
} }
@ -375,10 +350,9 @@ private:
} }
/* ---------- 点云处理流程 ---------- */ /* ---------- 点云处理流程 ---------- */
pcl::PointCloud<pcl::PointXYZ>::Ptr processPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) pcl::PointCloud<pcl::PointXYZ>::Ptr processPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
if (!cloud || cloud->empty()) if (!cloud || cloud->empty()) return nullptr;
return nullptr;
// 1. 条件过滤 // 1. 条件过滤
auto filtered = applyConditionalFiltering(cloud); auto filtered = applyConditionalFiltering(cloud);
@ -406,10 +380,9 @@ private:
return outlier_removed; return outlier_removed;
} }
std::vector<std::vector<int>> processPointCloud_grid(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) std::vector<std::vector<int>> processPointCloud_grid(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
if (!cloud || cloud->empty()) if (!cloud || cloud->empty()) return {}; // 返回空栅格
return {}; // 返回空栅格
// 1. 条件过滤 // 1. 条件过滤
auto filtered = applyConditionalFiltering(cloud); auto filtered = applyConditionalFiltering(cloud);
@ -440,16 +413,15 @@ private:
} }
/* ---------- 条件过滤 ---------- */ /* ---------- 条件过滤 ---------- */
pcl::PointCloud<pcl::PointXYZ>::Ptr applyConditionalFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) pcl::PointCloud<pcl::PointXYZ>::Ptr applyConditionalFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
for (const auto &point : cloud->points) for (const auto& point : cloud->points)
{ {
// 自定义过滤条件：保留特定区域内的点 // 自定义过滤条件：保留特定区域内的点
if (point.x > filter_min_x_ && point.x < filter_max_x_ && if (point.x > filter_min_x_ && point.x < filter_max_x_ && point.y > filter_min_y_ &&
point.y > filter_min_y_ && point.y < filter_max_y_ && point.y < filter_max_y_ && point.z > filter_min_z_ && point.z < filter_max_z_)
point.z > filter_min_z_ && point.z < filter_max_z_)
{ {
filtered->points.push_back(point); filtered->points.push_back(point);
} }
@ -460,8 +432,7 @@ private:
filtered->is_dense = true; filtered->is_dense = true;
// 不启用车身过滤，直接返回 // 不启用车身过滤，直接返回
if (!filter_car_) if (!filter_car_) return filtered;
return filtered;
// RCLCPP_INFO(get_logger(), "启用车身过滤"); // RCLCPP_INFO(get_logger(), "启用车身过滤");
// 使用CropBox移除车身区域 // 使用CropBox移除车身区域
@ -469,12 +440,10 @@ private:
crop.setInputCloud(filtered); crop.setInputCloud(filtered);
// 设置车身区域（最小点和最大点） // 设置车身区域（最小点和最大点）
Eigen::Vector4f min_point(car_lidar_offset_x_ - car_length_ / 2.0f, Eigen::Vector4f min_point(car_lidar_offset_x_ - car_length_ / 2.0f, car_lidar_offset_y_ - car_width_ / 2.0f,
car_lidar_offset_y_ - car_width_ / 2.0f,
filter_min_z_, 1.0); filter_min_z_, 1.0);
Eigen::Vector4f max_point(car_lidar_offset_x_ + car_length_ / 2.0f, Eigen::Vector4f max_point(car_lidar_offset_x_ + car_length_ / 2.0f, car_lidar_offset_y_ + car_width_ / 2.0f,
car_lidar_offset_y_ + car_width_ / 2.0f,
filter_max_z_, 1.0); filter_max_z_, 1.0);
crop.setMin(min_point); crop.setMin(min_point);
@ -515,7 +484,7 @@ private:
} }
/* ---------- 降采样（体素网格过滤） ---------- */ /* ---------- 降采样（体素网格过滤） ---------- */
pcl::PointCloud<pcl::PointXYZ>::Ptr applyVoxelGridFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) pcl::PointCloud<pcl::PointXYZ>::Ptr applyVoxelGridFiltering(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> vg; pcl::VoxelGrid<pcl::PointXYZ> vg;
@ -528,7 +497,7 @@ private:
} }
/* ---------- 离群点去除 ---------- */ /* ---------- 离群点去除 ---------- */
pcl::PointCloud<pcl::PointXYZ>::Ptr applyStatisticalOutlierRemoval(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) pcl::PointCloud<pcl::PointXYZ>::Ptr applyStatisticalOutlierRemoval(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
@ -542,7 +511,7 @@ private:
} }
/* ---------- 栅格化处理 ---------- */ /* ---------- 栅格化处理 ---------- */
std::vector<std::vector<int>> generateOccupancyGrid(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) std::vector<std::vector<int>> generateOccupancyGrid(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{ {
// 0:空 1:障碍物 2:车体 // 0:空 1:障碍物 2:车体
std::vector<std::vector<int>> grid(grid_size_, std::vector<int>(grid_size_, 0)); std::vector<std::vector<int>> grid(grid_size_, std::vector<int>(grid_size_, 0));
@ -552,8 +521,10 @@ private:
if (filter_car_) if (filter_car_)
{ {
// 计算车体在栅格中的位置 // 计算车体在栅格中的位置
int car_min_x = static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution); int car_min_x =
int car_max_x = static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution); static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution);
int car_max_x =
static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution);
int car_min_y = static_cast<int>((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) / resolution); int car_min_y = static_cast<int>((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) / resolution);
int car_max_y = static_cast<int>((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) / resolution); int car_max_y = static_cast<int>((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) / resolution);
@ -574,7 +545,7 @@ private:
} }
// 标记点云障碍物 // 标记点云障碍物
for (const auto &point : cloud->points) for (const auto& point : cloud->points)
{ {
// X轴（前向）映射到行索引（从上到下为正X到负X） // X轴（前向）映射到行索引（从上到下为正X到负X）
int grid_x = static_cast<int>((grid_range_ / 2 - point.x) / resolution); int grid_x = static_cast<int>((grid_range_ / 2 - point.x) / resolution);
@ -596,7 +567,7 @@ private:
} }
/* ---------- 可视化栅格 - 终端打印 ---------- */ /* ---------- 可视化栅格 - 终端打印 ---------- */
void visualizeGridInTerminal(const std::vector<std::vector<int>> &grid) void visualizeGridInTerminal(const std::vector<std::vector<int>>& grid)
{ {
std::system("clear"); std::system("clear");
@ -634,7 +605,7 @@ private:
} }
/* ---------- 发布栅格 ---------- */ /* ---------- 发布栅格 ---------- */
void publishGrid(const std::vector<std::vector<int>> &grid) void publishGrid(const std::vector<std::vector<int>>& grid)
{ {
// 创建Int32MultiArray消息 // 创建Int32MultiArray消息
auto msg = std_msgs::msg::Int32MultiArray(); auto msg = std_msgs::msg::Int32MultiArray();
@ -651,7 +622,7 @@ private:
// 设置数据（按行优先展开） // 设置数据（按行优先展开）
msg.data.clear(); msg.data.clear();
for (const auto &row : grid) for (const auto& row : grid)
{ {
msg.data.insert(msg.data.end(), row.begin(), row.end()); msg.data.insert(msg.data.end(), row.begin(), row.end());
} }
@ -662,8 +633,7 @@ private:
/* ---------- 优化的匹配算法 ---------- */ /* ---------- 优化的匹配算法 ---------- */
std::pair<std::shared_ptr<CloudFrame>, std::shared_ptr<CloudFrame>> findBestMatchOptimized() std::pair<std::shared_ptr<CloudFrame>, std::shared_ptr<CloudFrame>> findBestMatchOptimized()
{ {
if (front_clouds_.empty() || rear_clouds_.empty()) if (front_clouds_.empty() || rear_clouds_.empty()) return {nullptr, nullptr};
return {nullptr, nullptr};
// 快速策略：优先使用最新的点云进行匹配 // 快速策略：优先使用最新的点云进行匹配
auto front_candidate = front_clouds_.front(); auto front_candidate = front_clouds_.front();
@ -708,8 +678,7 @@ private:
} }
/* ---------- 移除已使用的点云 ---------- */ /* ---------- 移除已使用的点云 ---------- */
void removeUsedClouds(std::shared_ptr<CloudFrame> used_front, void removeUsedClouds(std::shared_ptr<CloudFrame> used_front, std::shared_ptr<CloudFrame> used_rear)
std::shared_ptr<CloudFrame> used_rear)
{ {
// 从front_clouds_中移除 // 从front_clouds_中移除
auto front_it = std::find(front_clouds_.begin(), front_clouds_.end(), used_front); auto front_it = std::find(front_clouds_.begin(), front_clouds_.end(), used_front);
@ -777,13 +746,12 @@ private:
} }
/* ---------- 合并点云 ---------- */ /* ---------- 合并点云 ---------- */
std::shared_ptr<PointCloud2> mergeClouds(const CloudFrame &front, const CloudFrame &rear) std::shared_ptr<PointCloud2> mergeClouds(const CloudFrame& front, const CloudFrame& rear)
{ {
size_t front_points = front.cloud->data.size() / front.cloud->point_step; size_t front_points = front.cloud->data.size() / front.cloud->point_step;
size_t rear_points = rear.cloud->data.size() / rear.cloud->point_step; size_t rear_points = rear.cloud->data.size() / rear.cloud->point_step;
if (front.cloud->fields != rear.cloud->fields || if (front.cloud->fields != rear.cloud->fields || front.cloud->point_step != rear.cloud->point_step ||
front.cloud->point_step != rear.cloud->point_step ||
front.cloud->is_bigendian != rear.cloud->is_bigendian) front.cloud->is_bigendian != rear.cloud->is_bigendian)
{ {
RCLCPP_ERROR(get_logger(), "PointCloud2 formats do not match!"); RCLCPP_ERROR(get_logger(), "PointCloud2 formats do not match!");
@ -811,7 +779,7 @@ private:
} }
/* ---------- 坐标变换 - 简化版 ---------- */ /* ---------- 坐标变换 - 简化版 ---------- */
std::shared_ptr<PointCloud2> transformCloud(const PointCloud2 &in) std::shared_ptr<PointCloud2> transformCloud(const PointCloud2& in)
{ {
if (in.header.frame_id == target_frame_) if (in.header.frame_id == target_frame_)
{ {
@ -823,20 +791,16 @@ private:
try try
{ {
// 使用最新可用的变换，不等待 // 使用最新可用的变换，不等待
auto tf = tf_buffer_->lookupTransform( auto tf = tf_buffer_->lookupTransform(target_frame_, in.header.frame_id, tf2::TimePointZero);
target_frame_, in.header.frame_id,
tf2::TimePointZero);
auto out = cloud_pool_.acquire(); auto out = cloud_pool_.acquire();
tf2::doTransform(in, *out, tf); tf2::doTransform(in, *out, tf);
return out; return out;
} }
catch (const tf2::TransformException &e) catch (const tf2::TransformException& e)
{ {
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "TF failed (%s -> %s): %s",
"TF failed (%s -> %s): %s", in.header.frame_id.c_str(), target_frame_.c_str(), e.what());
in.header.frame_id.c_str(),
target_frame_.c_str(), e.what());
return nullptr; return nullptr;
} }
} }
@ -847,10 +811,8 @@ private:
auto current_time = now(); auto current_time = now();
if ((current_time - last_stats_time_).seconds() >= 5.0) // 每5秒输出一次 if ((current_time - last_stats_time_).seconds() >= 5.0) // 每5秒输出一次
{ {
RCLCPP_INFO(get_logger(), RCLCPP_INFO(get_logger(), "Stats: Front=%d, Rear=%d, Merged=%d, Cache(F:%zu,R:%zu)", front_count_,
"Stats: Front=%d, Rear=%d, Merged=%d, Cache(F:%zu,R:%zu)", rear_count_, merged_count_, front_clouds_.size(), rear_clouds_.size());
front_count_, rear_count_, merged_count_,
front_clouds_.size(), rear_clouds_.size());
last_stats_time_ = current_time; last_stats_time_ = current_time;
} }
@ -901,7 +863,7 @@ private:
int front_count_, rear_count_, merged_count_; int front_count_, rear_count_, merged_count_;
}; };
int main(int argc, char **argv) int main(int argc, char** argv)
{ {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);

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

@ -20,25 +20,25 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED) find_package(sweeper_interfaces REQUIRED)
find_package(logger REQUIRED)
#find_package(Boost 1.71.0 REQUIRED COMPONENTS thread atomic system regex) #find_package(Boost 1.71.0 REQUIRED COMPONENTS thread atomic system regex)
#if(Boost_FOUND) #if(Boost_FOUND)
#include_directories(${Boost_INCLUDE_DIRS}) #include_directories(${Boost_INCLUDE_DIRS})
#endif() #endif()
include_directories( include_directories(include/rtk ${catkin_INCLUDE_DIRS})
include/rtk
${catkin_INCLUDE_DIRS}
)
add_executable(rtk_node src/rtk_node.cpp src/serial_read.cpp) add_executable(rtk_node src/rtk_node.cpp src/serial_read.cpp)
target_link_libraries(rtk_node ${Boost_LIBRARIES}) target_link_libraries(rtk_node ${Boost_LIBRARIES})
ament_target_dependencies(rtk_node rclcpp std_msgs sweeper_interfaces) ament_target_dependencies(
install(TARGETS
rtk_node rtk_node
DESTINATION lib/${PROJECT_NAME} rclcpp
) std_msgs
sweeper_interfaces
logger)
install(TARGETS rtk_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
@ -52,5 +52,3 @@ if(BUILD_TESTING)
endif() endif()
ament_package() ament_package()

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

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

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

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

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

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

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

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