diff --git a/src/autonomy/fu/CMakeLists.txt b/src/autonomy/fu/CMakeLists.txt index 35c120b..a06e86d 100644 --- a/src/autonomy/fu/CMakeLists.txt +++ b/src/autonomy/fu/CMakeLists.txt @@ -16,13 +16,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - add_compile_options(-w) # 禁用所有警告 + add_compile_options(-w) # 禁用所有警告 endif() -include_directories( - include/fu - ${catkin_INCLUDE_DIRS} -) +include_directories(include/fu ${catkin_INCLUDE_DIRS}) # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -30,19 +27,23 @@ find_package(std_msgs REQUIRED) find_package(sweeper_interfaces REQUIRED) find_package(sensor_msgs REQUIRED) find_package(mc REQUIRED) +find_package(logger REQUIRED) include_directories(include) add_executable(fu_node src/fu_node.cpp src/jsoncpp.cpp) -ament_target_dependencies(fu_node rclcpp std_msgs sweeper_interfaces sensor_msgs mc) - -install(TARGETS +ament_target_dependencies( fu_node - DESTINATION lib/${PROJECT_NAME} -) + rclcpp + std_msgs + sweeper_interfaces + sensor_msgs + mc + logger) -install(DIRECTORY launch config - DESTINATION share/${PROJECT_NAME}) +install(TARGETS fu_node DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -53,6 +54,6 @@ if(BUILD_TESTING) # uncomment the line when this package is not in a git repo #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() -endif() +endif() ament_package() diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index 5faeb65..c50d136 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -1,27 +1,29 @@ -#include "rclcpp/rclcpp.hpp" -#include "sweeper_interfaces/msg/pl.hpp" -#include "sweeper_interfaces/msg/fu.hpp" -#include "sweeper_interfaces/msg/detect_line.hpp" -#include "sweeper_interfaces/msg/sub.hpp" +#include +#include + +#include +#include #include #include -#include "sweeper_interfaces/msg/can_frame.hpp" -#include "sweeper_interfaces/msg/mc_ctrl.hpp" - +#include #include -#include #include -#include +#include -#include -#include -#include +#include "logger/logger.h" +#include "rclcpp/rclcpp.hpp" +#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; // 栅格值定义 -const int EMPTY = 0; // 空区域 -const int OBSTACLE = 1; // 障碍物 -const int VEHICLE = 2; // 车体 +const int EMPTY = 0; // 空区域 +const int OBSTACLE = 1; // 障碍物 +const int VEHICLE = 2; // 车体 int located = 0; bool detected_line = false; @@ -31,22 +33,22 @@ int is_start = 0; class fu_node : public rclcpp::Node { -public: + public: 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_avoid", true); // 是否开启绕障 - this->declare_parameter("enable_visualize_grid", true); // 是否可视化栅格 - this->declare_parameter("FRONT_STOP_ZONE_ROWS", 3); // 车前必停区行数 - this->declare_parameter("REAR_STOP_ZONE_ROWS", 1); // 车后必停区行数 - this->declare_parameter("LEFT_STOP_ZONE_COLS", 3); // 车左必停区列数 - this->declare_parameter("RIGHT_STOP_ZONE_COLS", 3); // 车右必停区列数 - this->declare_parameter("front_area_extend", 7); // 前方扩展检测区域 - this->declare_parameter("left_area_extend", 2); // 左侧扩展检测区域 - this->declare_parameter("right_area_extend", 2); // 右侧扩展检测区域 + this->declare_parameter("enable_obstacle_stop", true); // 是否是否开启遇障停车 + this->declare_parameter("enable_obstacle_avoid", true); // 是否开启绕障 + this->declare_parameter("enable_visualize_grid", true); // 是否可视化栅格 + this->declare_parameter("FRONT_STOP_ZONE_ROWS", 3); // 车前必停区行数 + this->declare_parameter("REAR_STOP_ZONE_ROWS", 1); // 车后必停区行数 + this->declare_parameter("LEFT_STOP_ZONE_COLS", 3); // 车左必停区列数 + this->declare_parameter("RIGHT_STOP_ZONE_COLS", 3); // 车右必停区列数 + this->declare_parameter("front_area_extend", 7); // 前方扩展检测区域 + this->declare_parameter("left_area_extend", 2); // 左侧扩展检测区域 + this->declare_parameter("right_area_extend", 2); // 右侧扩展检测区域 this->get_parameter("enable_obstacle_stop", enable_obstacle_stop_); this->get_parameter("enable_obstacle_avoid", enable_obstacle_avoid_); @@ -60,24 +62,23 @@ public: this->get_parameter("right_area_extend", right_area_extend_); // 创建参数回调，支持动态修改 - parameter_callback_handle_ = this->add_on_set_parameters_callback( - std::bind(&fu_node::parameter_callback, this, std::placeholders::_1)); + parameter_callback_handle_ = + this->add_on_set_parameters_callback(std::bind(&fu_node::parameter_callback, this, std::placeholders::_1)); // 创建订阅器 subscription_grid = this->create_subscription( "grid_raw", 10, std::bind(&fu_node::gridCallback, this, std::placeholders::_1)); - msg_subscribe_pl = this->create_subscription("pl_message", 10, - std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1)); - msg_subscribe_DetectLine = this->create_subscription("airy_message", 10, - std::bind(&fu_node::msg_callback_DetectLine, this, std::placeholders::_1)); + msg_subscribe_pl = this->create_subscription( + "pl_message", 10, std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1)); + msg_subscribe_DetectLine = this->create_subscription( + "airy_message", 10, std::bind(&fu_node::msg_callback_DetectLine, this, std::placeholders::_1)); // 发布控制指令 pub_mc = this->create_publisher("auto_mc_ctrl", 10); // 创建定时器，100ms为周期 - timer_ = this->create_wall_timer(std::chrono::milliseconds(100), - std::bind(&fu_node::timer_callback, this)); + timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&fu_node::timer_callback, this)); // 避障相关参数初始化 avoid_counter_ = 0; @@ -94,38 +95,32 @@ public: node_clock_ = this->get_clock(); // 打印所有参数值（添加到构造函数末尾） - RCLCPP_INFO_STREAM(this->get_logger(), - "\n\n---------- FU节点参数配置 ----------" - << "\n 遇障停车功能: " << (enable_obstacle_stop_ ? "开启" : "关闭") - << "\n 绕障功能: " << (enable_obstacle_avoid_ ? "开启" : "关闭") - << "\n 可视化栅格: " << (enable_visualize_grid_ ? "是" : "否") - << "\n 车前必停区行数: " << FRONT_STOP_ZONE_ROWS_ - << "\n 车后必停区行数: " << REAR_STOP_ZONE_ROWS_ - << "\n 车左必停区列数: " << LEFT_STOP_ZONE_COLS_ - << "\n 车右必停区列数: " << RIGHT_STOP_ZONE_COLS_ - << "\n 前方扩展检测区域: " << front_area_extend_ - << "\n 左侧扩展检测区域: " << left_area_extend_ - << "\n 右侧扩展检测区域: " << right_area_extend_ - << "\n--------------------------------------------\n"); + LOG_INFO( + "\n\n---------- FU节点参数配置 ----------\n 遇障停车功能: %s\n 绕障功能: %s\n 可视化栅格: %s\n " + "车前必停区行数: %d\n 车后必停区行数: %d\n 车左必停区列数: %d\n 车右必停区列数: %d\n 前方扩展检测区域: " + "%d\n 左侧扩展检测区域: %d\n 右侧扩展检测区域: %d\n--------------------------------------------\n", + (enable_obstacle_stop_ ? "开启" : "关闭"), (enable_obstacle_avoid_ ? "开启" : "关闭"), + (enable_visualize_grid_ ? "是" : "否"), FRONT_STOP_ZONE_ROWS_, REAR_STOP_ZONE_ROWS_, LEFT_STOP_ZONE_COLS_, + RIGHT_STOP_ZONE_COLS_, front_area_extend_, left_area_extend_, right_area_extend_); } -private: + private: rclcpp::Clock::SharedPtr node_clock_; enum AvoidState { - AVOID_NONE, // 无绕障 - AVOID_WAITING, // 等待决策 - AVOID_LATERAL_SHIFT, // 阶段1: 横向偏移 - AVOID_PARALLEL_MOVE, // 阶段2: 平行前进 - AVOID_RETURN_TO_PATH // 阶段3: 回归轨迹 + AVOID_NONE, // 无绕障 + AVOID_WAITING, // 等待决策 + AVOID_LATERAL_SHIFT, // 阶段1: 横向偏移 + AVOID_PARALLEL_MOVE, // 阶段2: 平行前进 + AVOID_RETURN_TO_PATH // 阶段3: 回归轨迹 }; // 避障状态管理 struct AvoidanceContext { AvoidState state = AVOID_NONE; - int direction = 0; // -1=左, 1=右 + int direction = 0; // -1=左, 1=右 int counter = 0; bool start_time_set = false; @@ -135,22 +130,22 @@ private: // 路径记录 std::vector angle_history; - int original_angle = 0; // 原始角度 + int original_angle = 0; // 原始角度 // 阶段1参数 - int lateral_shift_distance = 0; // 需要横向偏移的距离(栅格数) - int lateral_shift_achieved = 0; // 已完成的横向偏移 + int lateral_shift_distance = 0; // 需要横向偏移的距离(栅格数) + int lateral_shift_achieved = 0; // 已完成的横向偏移 // 阶段2参数 - double parallel_start_obstacle_x = -1; // 开始平行前进时障碍物的X坐标 + double parallel_start_obstacle_x = -1; // 开始平行前进时障碍物的X坐标 // 阶段3参数 - bool obstacle_passed = false; // 障碍物是否已通过 + bool obstacle_passed = false; // 障碍物是否已通过 // 动态参数 int max_avoid_angle = 25; - double min_lateral_time = 2.0; // 最小横向偏移时间 - double min_parallel_time = 1.5; // 最小平行前进时间 + double min_lateral_time = 2.0; // 最小横向偏移时间 + double min_parallel_time = 1.5; // 最小平行前进时间 void reset() { @@ -176,14 +171,14 @@ private: } } - double getElapsedTime(const rclcpp::Time &start, const rclcpp::Time ¤t) const + double getElapsedTime(const rclcpp::Time& start, const rclcpp::Time& current) const { try { auto duration = current - start; return duration.seconds(); } - catch (const std::runtime_error &e) + catch (const std::runtime_error& e) { return 0.0; } @@ -195,36 +190,34 @@ private: // 障碍物分析结果结构体 struct ObstacleAnalysis { - bool has_front_critical = false; // 前方必停区 - bool has_front_area = false; // 前方扩展区域 - bool has_left_critical = false; // 左方必停区 - bool has_right_critical = false; // 右方必停区 + bool has_front_critical = false; // 前方必停区 + bool has_front_area = false; // 前方扩展区域 + bool has_left_critical = false; // 左方必停区 + bool has_right_critical = false; // 右方必停区 - int obstacle_distance = -1; // 最近障碍物距离 - int obstacle_width = 0; // 障碍物宽度 - int left_edge = -1, right_edge = -1; // 障碍物边缘 - int free_space_left = 0; // 左侧可用空间 - int free_space_right = 0; // 右侧可用空间 + int obstacle_distance = -1; // 最近障碍物距离 + int obstacle_width = 0; // 障碍物宽度 + int left_edge = -1, right_edge = -1; // 障碍物边缘 + int free_space_left = 0; // 左侧可用空间 + int free_space_right = 0; // 右侧可用空间 // 动态安全距离计算 int calculateSafeDistance(int speed) const { // 基于速度计算安全距离：速度越快，需要的距离越远 - return std::max(3, speed / 400 + 2); // 最少3格，速度1500时为5格 + return std::max(3, speed / 400 + 2); // 最少3格，速度1500时为5格 } // 评估绕障可行性 bool canAvoid() const { - return obstacle_width > 0 && obstacle_width < 10 && - (free_space_left >= 3 || free_space_right >= 3); + return obstacle_width > 0 && obstacle_width < 10 && (free_space_left >= 3 || free_space_right >= 3); } // 评估是否需要紧急制动 bool needEmergencyBrake(int speed) const { - if (!has_front_critical) - return false; + if (!has_front_critical) return false; int safe_distance = calculateSafeDistance(speed); return obstacle_distance <= safe_distance; } @@ -233,67 +226,67 @@ private: ObstacleAnalysis obstacle_analysis_; // 新增：控制参数 - bool enable_obstacle_stop_; // 遇障停车使能 - bool enable_obstacle_avoid_; // 绕障功能使能 - bool enable_visualize_grid_; // 可视化栅格使能 + bool enable_obstacle_stop_; // 遇障停车使能 + bool enable_obstacle_avoid_; // 绕障功能使能 + bool enable_visualize_grid_; // 可视化栅格使能 // 必停区范围参数 - int FRONT_STOP_ZONE_ROWS_; // 车前必停区行数 - int REAR_STOP_ZONE_ROWS_; // 车后必停区行数 - int LEFT_STOP_ZONE_COLS_; // 车左必停区列数 - int RIGHT_STOP_ZONE_COLS_; // 车右必停区列数 + int FRONT_STOP_ZONE_ROWS_; // 车前必停区行数 + int REAR_STOP_ZONE_ROWS_; // 车后必停区行数 + int LEFT_STOP_ZONE_COLS_; // 车左必停区列数 + int RIGHT_STOP_ZONE_COLS_; // 车右必停区列数 // 分析前方更宽区域的障碍物 - int front_area_extend_; // 前方扩展检测区域 - int left_area_extend_; // 左侧扩展检测区域 - int right_area_extend_; // 右侧扩展检测区域 + int front_area_extend_; // 前方扩展检测区域 + int left_area_extend_; // 左侧扩展检测区域 + int right_area_extend_; // 右侧扩展检测区域 rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; // 接收PL消息的参数 double x = 0.0; double y = 0.0; - int speed = 0; // 从PL消息获取的速度 - int reliability = 0; // RTK信号质量 1行 0不行 + int speed = 0; // 从PL消息获取的速度 + int reliability = 0; // RTK信号质量 1行 0不行 // 发布消息的参数 - int publish_speed = 0; // 转速占空比 0-1500 - int publish_angle = 0; // [-40.0,40.0]，负数表示左转，正数表示右转 + int publish_speed = 0; // 转速占空比 0-1500 + int publish_angle = 0; // [-40.0,40.0]，负数表示左转，正数表示右转 // 车体矩形边界（栅格坐标） - int vehicle_min_x_; // 车体最小行坐标（顶部） - int vehicle_max_x_; // 车体最大行坐标（底部） - int vehicle_min_y_; // 车体最小列坐标（左侧） - int vehicle_max_y_; // 车体最大列坐标（右侧） + int vehicle_min_x_; // 车体最小行坐标（顶部） + int vehicle_max_x_; // 车体最大行坐标（底部） + int vehicle_min_y_; // 车体最小列坐标（左侧） + int vehicle_max_y_; // 车体最大列坐标（右侧） // 必停区定义（栅格坐标）- 动态计算 struct SafetyZone { - int min_x, max_x; // X轴范围（行） - int min_y, max_y; // Y轴范围（列） + int min_x, max_x; // X轴范围（行） + int min_y, max_y; // Y轴范围（列） }; - SafetyZone front_stop_zone_; // 前方必停区 - SafetyZone rear_stop_zone_; // 后方必停区 - SafetyZone left_stop_zone_; // 左方必停区 - SafetyZone right_stop_zone_; // 右方必停区 + SafetyZone front_stop_zone_; // 前方必停区 + SafetyZone rear_stop_zone_; // 后方必停区 + SafetyZone left_stop_zone_; // 左方必停区 + SafetyZone right_stop_zone_; // 右方必停区 - AvoidState avoid_state_; // 当前避障状态 - int avoid_counter_; // 避障计数器 - int avoid_direction_; // 避障方向（-1=左，1=右） - int avoid_angle_; // 避障角度 - rclcpp::Time last_avoid_time_; // 上次避障时间 + AvoidState avoid_state_; // 当前避障状态 + int avoid_counter_; // 避障计数器 + int avoid_direction_; // 避障方向（-1=左，1=右） + int avoid_angle_; // 避障角度 + rclcpp::Time last_avoid_time_; // 上次避障时间 // 障碍物检测结果 - bool obstacle_in_front_ = false; // 前方必停区有障碍物 - bool obstacle_in_rear_ = false; // 后方必停区有障碍物 - bool obstacle_in_left_ = false; // 左方必停区有障碍物 - bool obstacle_in_right_ = false; // 右方必停区有障碍物 - bool obstacle_in_front_area_ = false; // 前方区域有障碍物 - int obstacle_left_edge_ = -1; // 障碍物左边缘 - int obstacle_right_edge_ = -1; // 障碍物右边缘 - int obstacle_max_x_ = 100; // 障碍物距离车体最近的X坐标 - int free_space_left_ = 0; // 左侧可用空间 - int free_space_right_ = 0; // 右侧可用空间 + bool obstacle_in_front_ = false; // 前方必停区有障碍物 + bool obstacle_in_rear_ = false; // 后方必停区有障碍物 + bool obstacle_in_left_ = false; // 左方必停区有障碍物 + bool obstacle_in_right_ = false; // 右方必停区有障碍物 + bool obstacle_in_front_area_ = false; // 前方区域有障碍物 + int obstacle_left_edge_ = -1; // 障碍物左边缘 + int obstacle_right_edge_ = -1; // 障碍物右边缘 + int obstacle_max_x_ = 100; // 障碍物距离车体最近的X坐标 + int free_space_left_ = 0; // 左侧可用空间 + int free_space_right_ = 0; // 右侧可用空间 // 声明订阅者和发布者 rclcpp::Subscription::SharedPtr subscription_grid; @@ -305,22 +298,22 @@ private: rclcpp::TimerBase::SharedPtr timer_; // 参数动态更新回调 - rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector ¶meters) + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector& parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; - for (const auto ¶m : parameters) + for (const auto& param : parameters) { if (param.get_name() == "enable_obstacle_stop") { 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") { 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_) { @@ -360,7 +353,7 @@ private: } // 查找车体在栅格中的矩形边界 - void findVehicleRectangle(const std::vector> &grid) + void findVehicleRectangle(const std::vector>& grid) { // 重置车体边界 vehicle_min_x_ = -1; @@ -376,23 +369,18 @@ private: if (grid[i][j] == VEHICLE) { // 更新最小和最大行坐标 - if (vehicle_min_x_ == -1 || static_cast(i) < vehicle_min_x_) - vehicle_min_x_ = i; - if (vehicle_max_x_ == -1 || static_cast(i) > vehicle_max_x_) - vehicle_max_x_ = i; + if (vehicle_min_x_ == -1 || static_cast(i) < vehicle_min_x_) vehicle_min_x_ = i; + if (vehicle_max_x_ == -1 || static_cast(i) > vehicle_max_x_) vehicle_max_x_ = i; // 更新最小和最大列坐标 - if (vehicle_min_y_ == -1 || static_cast(j) < vehicle_min_y_) - vehicle_min_y_ = j; - if (vehicle_max_y_ == -1 || static_cast(j) > vehicle_max_y_) - vehicle_max_y_ = j; + if (vehicle_min_y_ == -1 || static_cast(j) < vehicle_min_y_) vehicle_min_y_ = j; + if (vehicle_max_y_ == -1 || static_cast(j) > vehicle_max_y_) vehicle_max_y_ = j; } } } // 如果找到车体，计算必停区域 - if (vehicle_min_x_ != -1 && vehicle_max_x_ != -1 && - vehicle_min_y_ != -1 && vehicle_max_y_ != -1) + if (vehicle_min_x_ != -1 && vehicle_max_x_ != -1 && vehicle_min_y_ != -1 && vehicle_max_y_ != -1) { calculateSafetyZones(grid.size(), grid[0].size()); } @@ -403,12 +391,12 @@ private: { // 前方必停区：车体前方FRONT_STOP_ZONE_ROWS行，与车体同宽 front_stop_zone_.min_x = max(0, vehicle_min_x_ - FRONT_STOP_ZONE_ROWS_); - front_stop_zone_.max_x = vehicle_min_x_ - 1; // 紧接车体前方 + front_stop_zone_.max_x = vehicle_min_x_ - 1; // 紧接车体前方 front_stop_zone_.min_y = vehicle_min_y_; front_stop_zone_.max_y = vehicle_max_y_; // 后方必停区：车体后方REAR_STOP_ZONE_ROWS行，与车体同宽 - rear_stop_zone_.min_x = vehicle_max_x_ + 1; // 紧接车体后方 + rear_stop_zone_.min_x = vehicle_max_x_ + 1; // 紧接车体后方 rear_stop_zone_.max_x = min(grid_height - 1, vehicle_max_x_ + REAR_STOP_ZONE_ROWS_); rear_stop_zone_.min_y = vehicle_min_y_; rear_stop_zone_.max_y = vehicle_max_y_; @@ -417,12 +405,12 @@ private: left_stop_zone_.min_x = vehicle_min_x_; left_stop_zone_.max_x = vehicle_max_x_; left_stop_zone_.min_y = max(0, vehicle_min_y_ - LEFT_STOP_ZONE_COLS_); - left_stop_zone_.max_y = vehicle_min_y_ - 1; // 紧接车体左侧 + left_stop_zone_.max_y = vehicle_min_y_ - 1; // 紧接车体左侧 // 右方必停区：车体右侧RIGHT_STOP_ZONE_COLS列，与车体同高 right_stop_zone_.min_x = vehicle_min_x_; right_stop_zone_.max_x = vehicle_max_x_; - right_stop_zone_.min_y = vehicle_max_y_ + 1; // 紧接车体右侧 + right_stop_zone_.min_y = vehicle_max_y_ + 1; // 紧接车体右侧 right_stop_zone_.max_y = min(grid_width - 1, vehicle_max_y_ + RIGHT_STOP_ZONE_COLS_); } @@ -431,7 +419,7 @@ private: // 检查消息是否包含维度信息 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; } @@ -442,8 +430,7 @@ private: // 检查数据长度是否匹配 if (msg->data.size() != height * width) { - RCLCPP_ERROR(this->get_logger(), "Grid data size mismatch! Expected %zu, got %zu", - height * width, msg->data.size()); + LOG_ERROR("Grid data size mismatch! Expected %zu, got %zu", height * width, msg->data.size()); return; } @@ -466,23 +453,22 @@ private: analyzeObstacles(grid); // 可视化栅格 - if (enable_visualize_grid_) - visualizeGridInTerminal(grid); + if (enable_visualize_grid_) visualizeGridInTerminal(grid); - cacheGridData(grid); // 缓存栅格数据 + cacheGridData(grid); // 缓存栅格数据 } std::vector> cached_grid_; bool grid_data_valid_ = false; - void cacheGridData(const std::vector> &grid) + void cacheGridData(const std::vector>& grid) { cached_grid_ = grid; grid_data_valid_ = true; } // 分析障碍物分布 - void analyzeObstacles(const std::vector> &grid) + void analyzeObstacles(const std::vector>& grid) { // 重置检测结果 obstacle_in_front_ = false; @@ -497,9 +483,7 @@ private: obstacle_max_x_ = 100; // 如果未找到车体位置，不进行检测 - if (vehicle_min_x_ == -1 || vehicle_max_x_ == -1 || - vehicle_min_y_ == -1 || vehicle_max_y_ == -1) - return; + if (vehicle_min_x_ == -1 || vehicle_max_x_ == -1 || vehicle_min_y_ == -1 || vehicle_max_y_ == -1) return; // 检查前方必停区 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) { - if (static_cast(i) < grid.size() && - static_cast(j) < grid[0].size() && + if (static_cast(i) < grid.size() && static_cast(j) < grid[0].size() && grid[i][j] == OBSTACLE) { obstacle_in_front_ = true; @@ -533,7 +516,7 @@ private: } // 检查其他必停区 - void checkOtherStopZones(const std::vector> &grid) + void checkOtherStopZones(const std::vector>& grid) { // 检查后方必停区 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> &grid) + void analyzeFrontExtendedArea(const std::vector>& grid) { size_t front_area_min_x = static_cast(max(0, vehicle_min_x_ - front_area_extend_)); size_t front_area_max_x = static_cast(vehicle_min_x_ - 1); @@ -621,7 +604,8 @@ private: } // 计算绕障可用空间 - 在更大范围内分析左右通道 - void calculateAvoidanceSpace(const std::vector> &grid, int core_left_obstacle, int core_right_obstacle) + void calculateAvoidanceSpace(const std::vector>& grid, int core_left_obstacle, + int core_right_obstacle) { 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_end = obstacle_left_edge_ - 1; - free_space_left_ = calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, - left_check_start, left_check_end); + free_space_left_ = + 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_end = min(static_cast(grid[0].size() - 1), vehicle_max_y_ + right_area_extend_); - free_space_right_ = calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, - right_check_start, right_check_end); + free_space_right_ = + calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, right_check_start, right_check_end); } else { @@ -655,26 +639,23 @@ private: } // 计算指定方向的可用空间 - int calculateSpaceInDirection(const std::vector> &grid, - size_t min_x, size_t max_x, int check_start_y, int check_end_y) + int calculateSpaceInDirection(const std::vector>& grid, size_t min_x, size_t max_x, + int check_start_y, int check_end_y) { - if (check_start_y > check_end_y) - return 0; + if (check_start_y > check_end_y) return 0; int available_space = 0; // 从起始位置开始，逐列检查直到遇到障碍物 for (int j = check_start_y; j <= check_end_y; ++j) { - if (j < 0 || j >= static_cast(grid[0].size())) - break; + if (j < 0 || j >= static_cast(grid[0].size())) break; bool column_clear = true; // 检查该列在检测范围内是否有障碍物 for (size_t i = min_x; i <= max_x; ++i) { - if (i >= grid.size()) - break; + if (i >= grid.size()) break; if (grid[i][j] == OBSTACLE) { column_clear = false; @@ -688,35 +669,35 @@ private: } else { - break; // 遇到障碍物，停止计算 + break; // 遇到障碍物，停止计算 } } return available_space; } - void visualizeGridInTerminal(const std::vector> &grid) + void visualizeGridInTerminal(const std::vector>& 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） - std::cout << " "; + ss << " "; 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++) { // 行号显示（两位数，右对齐） - std::cout << (i < 10 ? " " : "") << i << "|"; + ss << (i < 10 ? " " : "") << i << "|"; // 栅格内容 for (size_t j = 0; j < grid[i].size(); j++) @@ -739,31 +720,33 @@ private: // 根据栅格值和必停区状态选择显示符号 if (grid[i][j] == OBSTACLE) - std::cout << "@"; + ss << "@"; else if (grid[i][j] == VEHICLE) - std::cout << "V"; + ss << "V"; else if (in_front_zone) - std::cout << "F"; // 前方必停区 + ss << "F"; // 前方必停区 else if (in_left_zone) - std::cout << "L"; // 左方必停区 + ss << "L"; // 左方必停区 else if (in_right_zone) - std::cout << "R"; // 右方必停区 + ss << "R"; // 右方必停区 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) { x = msg->x; y = msg->y; - speed = (int)msg->speed; // 获取PL消息中的速度 + speed = (int)msg->speed; // 获取PL消息中的速度 reliability = msg->reliability; located = msg->enter_range; is_start = msg->is_start; @@ -781,7 +764,7 @@ private: { publish_speed = 0; publish_angle = 0; - RCLCPP_INFO(this->get_logger(), "PL速度为0，已停车"); + LOG_INFO("PL速度为0，已停车"); } else if (is_start) { @@ -791,18 +774,18 @@ private: // 发布控制指令 sweeper_interfaces::msg::McCtrl message; message.sweep = true; - message.brake = 0; // 0开;1关 - message.gear = 2; // 0空挡;1后退;2前进 + message.brake = 0; // 0开;1关 + message.gear = 2; // 0空挡;1后退;2前进 if (reliability == 1) message.rpm = publish_speed; else { message.rpm = 0; - RCLCPP_ERROR(this->get_logger(), "rtk信号差,停车！！！"); + LOG_ERROR("rtk信号差,停车！！！"); } - message.angle = publish_angle; // 负数表示左转，正数表示右转 + message.angle = publish_angle; // 负数表示左转，正数表示右转 message.angle_speed = 800; pub_mc->publish(message); @@ -811,30 +794,27 @@ private: std::string state_str; switch (avoid_ctx_.state) { - case AVOID_NONE: - state_str = "正常行驶"; - break; - case AVOID_WAITING: - state_str = "等待决策"; - break; - case AVOID_LATERAL_SHIFT: - state_str = "横向偏移"; - break; - case AVOID_PARALLEL_MOVE: - state_str = "平行前进"; - break; - case AVOID_RETURN_TO_PATH: - state_str = "回归轨迹"; - break; - default: - state_str = "未知状态"; + case AVOID_NONE: + state_str = "正常行驶"; + break; + case AVOID_WAITING: + state_str = "等待决策"; + break; + case AVOID_LATERAL_SHIFT: + state_str = "横向偏移"; + break; + case AVOID_PARALLEL_MOVE: + state_str = "平行前进"; + break; + case AVOID_RETURN_TO_PATH: + state_str = "回归轨迹"; + break; + default: + state_str = "未知状态"; } - RCLCPP_INFO_STREAM(this->get_logger(), - "控制: 速度=" << publish_speed - << ", 角度=" << publish_angle - << ", 状态=" << state_str - << ", 前方" << (obstacle_in_front_ ? "有" : "无") << "障碍物"); + LOG_INFO("控制: 速度=%d, 角度=%d, 状态=%s, 前方%s障碍物", publish_speed, publish_angle, state_str.c_str(), + (obstacle_in_front_ ? "有" : "无")); } else { @@ -860,7 +840,7 @@ private: publish_speed = 0; publish_angle = 0; avoid_ctx_.reset(); - RCLCPP_WARN(this->get_logger(), "未检测到车体位置，安全停车"); + LOG_WARN("未检测到车体位置，安全停车"); return; } @@ -882,7 +862,7 @@ private: void updateObstacleAnalysis() { - obstacle_analysis_ = ObstacleAnalysis{}; // 重置 + obstacle_analysis_ = ObstacleAnalysis{}; // 重置 obstacle_analysis_.has_front_critical = obstacle_in_front_; obstacle_analysis_.has_front_area = obstacle_in_front_area_; @@ -892,9 +872,7 @@ private: obstacle_analysis_.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1; obstacle_analysis_.left_edge = obstacle_left_edge_; obstacle_analysis_.right_edge = obstacle_right_edge_; - RCLCPP_INFO(this->get_logger(), "障碍物左侧=%d，障碍物右侧=%d", - obstacle_analysis_.left_edge, - obstacle_analysis_.right_edge); + LOG_INFO("障碍物左侧=%d，障碍物右侧=%d", obstacle_analysis_.left_edge, obstacle_analysis_.right_edge); obstacle_analysis_.free_space_left = free_space_left_; obstacle_analysis_.free_space_right = free_space_right_; @@ -907,8 +885,7 @@ private: // 第一优先级：紧急停车策略 bool executeEmergencyStop() { - if (!enable_obstacle_stop_) - return false; + if (!enable_obstacle_stop_) return false; // 前方必停区有障碍物，立即停车 if (obstacle_analysis_.has_front_critical) @@ -921,8 +898,7 @@ private: // 每2秒打印一次状态 if (avoid_ctx_.counter % 20 == 0) { - RCLCPP_WARN(this->get_logger(), "紧急停车：前方必停区有障碍物，距离=%d格", - obstacle_analysis_.obstacle_distance); + LOG_WARN("紧急停车：前方必停区有障碍物，距离=%d格", obstacle_analysis_.obstacle_distance); } return true; } @@ -933,8 +909,7 @@ private: publish_speed = 0; publish_angle = 0; avoid_ctx_.state = AVOID_WAITING; - RCLCPP_WARN(this->get_logger(), "动态紧急制动：障碍物距离%d格，速度%d", - obstacle_analysis_.obstacle_distance, publish_speed); + LOG_WARN("动态紧急制动：障碍物距离%d格，速度%d", obstacle_analysis_.obstacle_distance, publish_speed); return true; } @@ -944,15 +919,14 @@ private: // 第二优先级：转向保护策略 bool executeTurnProtection() { - if (!enable_obstacle_stop_) - return false; + if (!enable_obstacle_stop_) return false; // 检查转向方向的安全性 if (publish_angle < -10 && obstacle_analysis_.has_left_critical) { publish_speed = 0; publish_angle = 0; - RCLCPP_WARN(this->get_logger(), "转向保护：左转时左侧有障碍物"); + LOG_WARN("转向保护：左转时左侧有障碍物"); return true; } @@ -960,7 +934,7 @@ private: { publish_speed = 0; publish_angle = 0; - RCLCPP_WARN(this->get_logger(), "转向保护：右转时右侧有障碍物"); + LOG_WARN("转向保护：右转时右侧有障碍物"); return true; } @@ -980,17 +954,15 @@ private: avoid_ctx_.counter++; if (avoid_ctx_.counter % 30 == 0) - { // 每3秒打印一次 - RCLCPP_INFO(this->get_logger(), "障碍物无法绕行(宽度=%d格)，等待通过...", - obstacle_analysis_.obstacle_width); + { // 每3秒打印一次 + LOG_INFO("障碍物无法绕行(宽度=%d格)，等待通过...", obstacle_analysis_.obstacle_width); } } else { // 距离足够，可以减速接近 publish_speed = std::max(600, publish_speed / 2); - RCLCPP_INFO(this->get_logger(), "障碍物较宽，减速接近中，距离=%d格", - obstacle_analysis_.obstacle_distance); + LOG_INFO("障碍物较宽，减速接近中，距离=%d格", obstacle_analysis_.obstacle_distance); } } @@ -1018,13 +990,9 @@ private: avoid_ctx_.parallel_start_obstacle_x = -1; avoid_ctx_.obstacle_passed = false; - RCLCPP_INFO(this->get_logger(), - "启动绕障: 方向=%s, 需要偏移=%d格, 障碍物宽度=%d, 左侧空间=%d, 右侧空间=%d", - (direction == -1) ? "左" : "右", - required_shift, - obstacle_analysis_.obstacle_width, - obstacle_analysis_.free_space_left, - obstacle_analysis_.free_space_right); + LOG_INFO("启动绕障: 方向=%s, 需要偏移=%d格, 障碍物宽度=%d, 左侧空间=%d, 右侧空间=%d", + (direction == -1) ? "左" : "右", 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; // 2. 安全性评分(关键因素) - if (obstacle_analysis_.has_left_critical) - left_score -= 1000; - if (obstacle_analysis_.has_right_critical) - right_score -= 1000; + if (obstacle_analysis_.has_left_critical) left_score -= 1000; + if (obstacle_analysis_.has_right_critical) right_score -= 1000; // 3. 障碍物位置分析 - 基于车辆边界而非中心 if (obstacle_analysis_.left_edge > -1 && obstacle_analysis_.right_edge > -1) @@ -1054,18 +1020,16 @@ private: // 右侧可用空间 = 障碍物右边界到车辆右边界的距离 int right_clearance = vehicle_max_y_ - obstacle_analysis_.right_edge; - RCLCPP_INFO(this->get_logger(), - "位置分析: 左侧间隙=%d, 右侧间隙=%d, 车宽=%d", - left_clearance, right_clearance, vehicle_width); + LOG_INFO("位置分析: 左侧间隙=%d, 右侧间隙=%d, 车宽=%d", left_clearance, right_clearance, vehicle_width); // 基于实际间隙大小评分 if (left_clearance > right_clearance + 2) { - left_score += 25; // 左侧明显更宽松 + left_score += 25; // 左侧明显更宽松 } else if (right_clearance > left_clearance + 2) { - right_score += 25; // 右侧明显更宽松 + right_score += 25; // 右侧明显更宽松 } // 如果障碍物严重偏向一侧,反方向绕行 @@ -1073,50 +1037,45 @@ private: { // 障碍物完全在车辆左侧,应该右绕 right_score += 30; - RCLCPP_INFO(this->get_logger(), "障碍物在左侧,倾向右绕"); + LOG_INFO("障碍物在左侧,倾向右绕"); } else if (obstacle_analysis_.left_edge > vehicle_max_y_) { // 障碍物完全在车辆右侧,应该左绕 left_score += 30; - RCLCPP_INFO(this->get_logger(), "障碍物在右侧,倾向左绕"); + LOG_INFO("障碍物在右侧,倾向左绕"); } } // 4. 目标方向偏好 - 降低权重,避免过度影响 float target_angle = calculate_the_angle(x, y); - if (target_angle < -15) // 阈值从10提高到15 - left_score += 15; // 权重从20降低到15 - if (target_angle > 15) - right_score += 15; + if (target_angle < -15) // 阈值从10提高到15 + left_score += 15; // 权重从20降低到15 + if (target_angle > 15) right_score += 15; // 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 { int recent_avg = 0; int count = std::min(5, static_cast(avoid_ctx_.angle_history.size())); - for (int i = avoid_ctx_.angle_history.size() - count; - i < static_cast(avoid_ctx_.angle_history.size()); i++) + for (int i = avoid_ctx_.angle_history.size() - count; i < static_cast(avoid_ctx_.angle_history.size()); + i++) { recent_avg += avoid_ctx_.angle_history[i]; } recent_avg /= count; - if (recent_avg < -8) // 阈值从5提高到8 - left_score += 15; // 权重从10提高到15 - if (recent_avg > 8) - right_score += 15; + if (recent_avg < -8) // 阈值从5提高到8 + left_score += 15; // 权重从10提高到15 + if (recent_avg > 8) right_score += 15; } int selected_direction = (left_score > right_score) ? -1 : 1; - RCLCPP_INFO(this->get_logger(), - "方向选择 - 左评分:%d 右评分:%d → %s (左空间:%d 右空间:%d)", - left_score, right_score, - (selected_direction == -1) ? "左绕" : "右绕", - obstacle_analysis_.free_space_left, - obstacle_analysis_.free_space_right); + LOG_INFO("方向选择 - 左评分:%d 右评分:%d → %s (左空间:%d 右空间:%d)", left_score, right_score, + (selected_direction == -1) ? "左绕" : "右绕", obstacle_analysis_.free_space_left, + obstacle_analysis_.free_space_right); return selected_direction; } @@ -1126,18 +1085,18 @@ private: { if (obstacle_analysis_.left_edge == -1 || obstacle_analysis_.right_edge == -1) { - return 3; // 默认偏移3格 + return 3; // 默认偏移3格 } - int safety_margin = 2; // 安全余量 + int safety_margin = 2; // 安全余量 int required_shift = 0; - if (direction == -1) // 左绕 + if (direction == -1) // 左绕 { // 车体右边界需要绕过障碍物右边界 required_shift = vehicle_max_y_ - obstacle_analysis_.right_edge + safety_margin; } - else // 右绕 + else // 右绕 { // 车体左边界需要绕过障碍物左边界 required_shift = obstacle_analysis_.left_edge - vehicle_min_y_ + safety_margin; @@ -1158,7 +1117,7 @@ private: if (avoid_ctx_.state != AVOID_NONE) { avoid_ctx_.reset(); - RCLCPP_INFO(this->get_logger(), "前方无障碍物，重置绕障状态"); + LOG_INFO("前方无障碍物，重置绕障状态"); } return; } @@ -1166,7 +1125,7 @@ private: // 评估是否需要绕障 if (!obstacle_analysis_.canAvoid()) { - RCLCPP_INFO(this->get_logger(), "障碍物无法绕行，减速靠近"); + LOG_INFO("障碍物无法绕行，减速靠近"); handleUnpassableObstacle(); return; } @@ -1174,22 +1133,22 @@ private: // 三阶段状态机 switch (avoid_ctx_.state) { - case AVOID_NONE: - case AVOID_WAITING: - initiateAvoidance(); - break; + case AVOID_NONE: + case AVOID_WAITING: + initiateAvoidance(); + break; - case AVOID_LATERAL_SHIFT: - executeLateralShift(); - break; + case AVOID_LATERAL_SHIFT: + executeLateralShift(); + break; - case AVOID_PARALLEL_MOVE: - executeParallelMove(); - break; + case AVOID_PARALLEL_MOVE: + executeParallelMove(); + break; - case AVOID_RETURN_TO_PATH: - executeReturnToPath(); - break; + case AVOID_RETURN_TO_PATH: + executeReturnToPath(); + break; } } @@ -1229,8 +1188,8 @@ private: bool safe_distance_achieved = verifySafeDistance(); // 所有条件都满足才进入下一阶段 - bool shift_complete = time_sufficient && distance_sufficient && - lateral_clearance_achieved && safe_distance_achieved; + bool shift_complete = + time_sufficient && distance_sufficient && lateral_clearance_achieved && safe_distance_achieved; if (shift_complete) { @@ -1239,20 +1198,17 @@ private: avoid_ctx_.last_state_change_time_set = true; avoid_ctx_.parallel_start_obstacle_x = obstacle_max_x_; - RCLCPP_INFO(this->get_logger(), - "阶段1完成: 横向偏移完成(%.1fs, %d格), 进入阶段2: 平行前进", - shift_time, avoid_ctx_.lateral_shift_achieved); + LOG_INFO("阶段1完成: 横向偏移完成(%.1fs, %d格), 进入阶段2: 平行前进", shift_time, + avoid_ctx_.lateral_shift_achieved); } else { if (avoid_ctx_.counter % 10 == 0) { - RCLCPP_INFO(this->get_logger(), - "阶段1进行中: 时间%.1fs/%s, 距离%d/%d格, 间隙%s, 安全距离%s", - shift_time, time_sufficient ? "✓" : "✗", - avoid_ctx_.lateral_shift_achieved, avoid_ctx_.lateral_shift_distance, - lateral_clearance_achieved ? "✓" : "✗", - safe_distance_achieved ? "✓" : "✗"); + LOG_INFO("阶段1进行中: 时间%.1fs/%s, 距离%d/%d格, 间隙%s, 安全距离%s", shift_time, + time_sufficient ? "✓" : "✗", avoid_ctx_.lateral_shift_achieved, + avoid_ctx_.lateral_shift_distance, lateral_clearance_achieved ? "✓" : "✗", + safe_distance_achieved ? "✓" : "✗"); } } @@ -1265,43 +1221,40 @@ private: // 检查当前是否已经没有前方障碍物警告 if (obstacle_analysis_.has_front_critical || obstacle_analysis_.has_front_area) { - RCLCPP_DEBUG(this->get_logger(), "前方仍有障碍区域"); + LOG_DEBUG("前方仍有障碍区域"); return false; } // 检查绕障方向的侧方是否有障碍物 if (avoid_ctx_.direction == -1 && obstacle_analysis_.has_left_critical) { - RCLCPP_DEBUG(this->get_logger(), "左绕时左侧仍有障碍"); + LOG_DEBUG("左绕时左侧仍有障碍"); return false; } if (avoid_ctx_.direction == 1 && obstacle_analysis_.has_right_critical) { - RCLCPP_DEBUG(this->get_logger(), "右绕时右侧仍有障碍"); + LOG_DEBUG("右绕时右侧仍有障碍"); return false; } // 检查障碍物是否已经移到侧后方 if (obstacle_analysis_.left_edge > -1 && obstacle_analysis_.right_edge > -1) { - if (avoid_ctx_.direction == -1) // 左绕,障碍物应该在右侧 + if (avoid_ctx_.direction == -1) // 左绕,障碍物应该在右侧 { if (obstacle_analysis_.left_edge <= vehicle_max_y_) { - RCLCPP_DEBUG(this->get_logger(), - "左绕时障碍物左边界%d仍在车右边界%d范围内", - obstacle_analysis_.left_edge, vehicle_max_y_); + LOG_DEBUG("左绕时障碍物左边界%d仍在车右边界%d范围内", obstacle_analysis_.left_edge, vehicle_max_y_); return false; } } - else // 右绕,障碍物应该在左侧 + else // 右绕,障碍物应该在左侧 { if (obstacle_analysis_.right_edge >= vehicle_min_y_) { - RCLCPP_DEBUG(this->get_logger(), - "右绕时障碍物右边界%d仍在车左边界%d范围内", - obstacle_analysis_.right_edge, vehicle_min_y_); + LOG_DEBUG("右绕时障碍物右边界%d仍在车左边界%d范围内", obstacle_analysis_.right_edge, + vehicle_min_y_); return false; } } @@ -1314,13 +1267,14 @@ private: 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; // 基础角度 if (available_space >= 4) { - base_angle = 25; // 空间充足，可以大角度 + base_angle = 25; // 空间充足，可以大角度 } else if (available_space >= 3) { @@ -1328,7 +1282,7 @@ private: } else { - base_angle = 15; // 空间紧张，小角度谨慎 + base_angle = 15; // 空间紧张，小角度谨慎 } // 距离修正：障碍物越近，角度越小 @@ -1343,43 +1297,36 @@ private: // 检查横向间隙是否足够 bool checkLateralClearance() { - if (!grid_data_valid_ || cached_grid_.empty()) - return false; + if (!grid_data_valid_ || cached_grid_.empty()) 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) { - RCLCPP_DEBUG(this->get_logger(), - "横移距离不足: %d < %d", - avoid_ctx_.lateral_shift_achieved, MIN_SHIFT_THRESHOLD); + LOG_DEBUG("横移距离不足: %d < %d", avoid_ctx_.lateral_shift_achieved, MIN_SHIFT_THRESHOLD); return false; } // 安全余量 - const int SAFE_MARGIN = 3; // 3格 + const int SAFE_MARGIN = 3; // 3格 // ---------------------------- // 1) 基于障碍物边界判断横向绕出 // ---------------------------- - if (avoid_ctx_.direction == -1) // 左绕 + if (avoid_ctx_.direction == -1) // 左绕 { 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(), - "左绕未完成: 车右边界%d > 目标位置%d", - vehicle_max_y_, required_position); + LOG_DEBUG("左绕未完成: 车右边界%d > 目标位置%d", vehicle_max_y_, required_position); return false; } } - else // 右绕 + else // 右绕 { int required_position = obstacle_analysis_.right_edge + SAFE_MARGIN; if (vehicle_min_y_ < required_position) { - RCLCPP_DEBUG(this->get_logger(), - "右绕未完成: 车左边界%d < 目标位置%d", - vehicle_min_y_, required_position); + LOG_DEBUG("右绕未完成: 车左边界%d < 目标位置%d", vehicle_min_y_, required_position); return false; } } @@ -1387,24 +1334,20 @@ private: // ---------------------------- // 2) 检查前方更深的区域是否清空 // ---------------------------- - const int CHECK_DEPTH = 10; // 10行 + const int CHECK_DEPTH = 10; // 10行 for (int depth = 1; depth <= CHECK_DEPTH; depth++) { int check_row = vehicle_min_x_ - depth; - if (check_row < 0) - break; + if (check_row < 0) break; // 检查整个车宽范围 for (int col = vehicle_min_y_; col <= vehicle_max_y_; col++) { - if (col < 0 || col >= static_cast(cached_grid_[0].size())) - continue; + if (col < 0 || col >= static_cast(cached_grid_[0].size())) continue; if (cached_grid_[check_row][col] == OBSTACLE) { - RCLCPP_DEBUG(this->get_logger(), - "前方第%d行仍有障碍,位置[%d,%d]", - depth, check_row, col); + LOG_DEBUG("前方第%d行仍有障碍,位置[%d,%d]", depth, check_row, col); return false; } } @@ -1413,61 +1356,51 @@ private: // ---------------------------- // 3) 【新增】检查侧方是否真正清空 // ---------------------------- - if (avoid_ctx_.direction == -1) // 左绕,检查右侧 + if (avoid_ctx_.direction == -1) // 左绕,检查右侧 { for (int depth = 1; depth <= 5; depth++) { int check_row = vehicle_min_x_ - depth; - if (check_row < 0) - break; + if (check_row < 0) break; // 检查车辆右侧若干列 for (int offset = 1; offset <= 3; offset++) { int check_col = vehicle_max_y_ + offset; - if (check_col >= static_cast(cached_grid_[0].size())) - break; + if (check_col >= static_cast(cached_grid_[0].size())) break; if (cached_grid_[check_row][check_col] == OBSTACLE) { - RCLCPP_DEBUG(this->get_logger(), - "右侧仍有障碍[%d,%d],左绕未完全清空", - check_row, check_col); + LOG_DEBUG("右侧仍有障碍[%d,%d],左绕未完全清空", check_row, check_col); return false; } } } } - else // 右绕,检查左侧 + else // 右绕,检查左侧 { for (int depth = 1; depth <= 5; depth++) { int check_row = vehicle_min_x_ - depth; - if (check_row < 0) - break; + if (check_row < 0) break; // 检查车辆左侧若干列 for (int offset = 1; offset <= 3; offset++) { int check_col = vehicle_min_y_ - offset; - if (check_col < 0) - break; + if (check_col < 0) break; if (cached_grid_[check_row][check_col] == OBSTACLE) { - RCLCPP_DEBUG(this->get_logger(), - "左侧仍有障碍[%d,%d],右绕未完全清空", - check_row, check_col); + LOG_DEBUG("左侧仍有障碍[%d,%d],右绕未完全清空", check_row, check_col); return false; } } } } - RCLCPP_INFO(this->get_logger(), - "横向间隙检查通过: 方向=%s, 已移动=%d格", - (avoid_ctx_.direction == -1) ? "左" : "右", - avoid_ctx_.lateral_shift_achieved); + LOG_INFO("横向间隙检查通过: 方向=%s, 已移动=%d格", (avoid_ctx_.direction == -1) ? "左" : "右", + avoid_ctx_.lateral_shift_achieved); return true; } @@ -1492,7 +1425,7 @@ private: // 安全检查 if (checkParallelMoveSafety()) { - return; // 检测到危险 + return; // 检测到危险 } // 判断障碍物是否已经与车头持平或更后 @@ -1508,18 +1441,14 @@ private: avoid_ctx_.last_state_change_time_set = true; avoid_ctx_.obstacle_passed = true; - RCLCPP_INFO(this->get_logger(), - "阶段2完成: 障碍物已在车身后方(%.1fs), 进入阶段3: 回归轨迹", - parallel_time); + LOG_INFO("阶段2完成: 障碍物已在车身后方(%.1fs), 进入阶段3: 回归轨迹", parallel_time); } else { if (avoid_ctx_.counter % 10 == 0) { - RCLCPP_INFO(this->get_logger(), - "阶段2进行中: 平行前进 %.1fs, 障碍物位置检查=%s", - parallel_time, - obstacle_aligned_or_behind ? "已通过" : "未通过"); + LOG_INFO("阶段2进行中: 平行前进 %.1fs, 障碍物位置检查=%s", parallel_time, + obstacle_aligned_or_behind ? "已通过" : "未通过"); } } @@ -1530,24 +1459,21 @@ private: bool checkObstaclePosition() { // 方法1: 检查前方核心区域是否清空 - bool front_core_clear = !obstacle_analysis_.has_front_area && - !obstacle_analysis_.has_front_critical; + bool front_core_clear = !obstacle_analysis_.has_front_area && !obstacle_analysis_.has_front_critical; // 方法2: 检查障碍物是否不再在绕障侧的检测范围内 bool lateral_clear = false; - if (avoid_ctx_.direction == -1) // 左绕 + if (avoid_ctx_.direction == -1) // 左绕 { // 检查右侧区域是否清空（障碍物应该在左后方） lateral_clear = !obstacle_analysis_.has_right_critical && - (obstacle_analysis_.right_edge == -1 || - obstacle_analysis_.right_edge < vehicle_min_y_); + (obstacle_analysis_.right_edge == -1 || obstacle_analysis_.right_edge < vehicle_min_y_); } - else // 右绕 + else // 右绕 { // 检查左侧区域是否清空（障碍物应该在右后方） lateral_clear = !obstacle_analysis_.has_left_critical && - (obstacle_analysis_.left_edge == -1 || - obstacle_analysis_.left_edge > vehicle_max_y_); + (obstacle_analysis_.left_edge == -1 || obstacle_analysis_.left_edge > vehicle_max_y_); } // 综合判断 @@ -1555,11 +1481,8 @@ private: if (avoid_ctx_.counter % 10 == 0) { - RCLCPP_DEBUG(this->get_logger(), - "障碍物位置检查: 前方清空=%s, 侧方清空=%s => %s", - front_core_clear ? "是" : "否", - lateral_clear ? "是" : "否", - obstacle_passed ? "已通过" : "未通过"); + LOG_DEBUG("障碍物位置检查: 前方清空=%s, 侧方清空=%s => %s", front_core_clear ? "是" : "否", + lateral_clear ? "是" : "否", obstacle_passed ? "已通过" : "未通过"); } return obstacle_passed; @@ -1573,7 +1496,7 @@ private: { publish_speed = 0; avoid_ctx_.state = AVOID_WAITING; - RCLCPP_ERROR(this->get_logger(), "平行前进时前方出现障碍物！"); + LOG_ERROR("平行前进时前方出现障碍物！"); return true; } @@ -1582,7 +1505,7 @@ private: { publish_speed = 0; avoid_ctx_.state = AVOID_WAITING; - RCLCPP_ERROR(this->get_logger(), "平行前进时左侧出现障碍物！"); + LOG_ERROR("平行前进时左侧出现障碍物！"); return true; } @@ -1590,7 +1513,7 @@ private: { publish_speed = 0; avoid_ctx_.state = AVOID_WAITING; - RCLCPP_ERROR(this->get_logger(), "平行前进时右侧出现障碍物！"); + LOG_ERROR("平行前进时右侧出现障碍物！"); return true; } @@ -1620,8 +1543,8 @@ private: // 2. 缓慢回正 int angle_diff = target_angle_int - publish_angle; - const int ANGLE_TOLERANCE = 3; // ±3度容差 - const int RETURN_RATE = 1; // 固定回正速率 + const int ANGLE_TOLERANCE = 3; // ±3度容差 + const int RETURN_RATE = 1; // 固定回正速率 if (std::abs(angle_diff) <= ANGLE_TOLERANCE) { @@ -1630,9 +1553,7 @@ private: publish_speed = 800; avoid_ctx_.reset(); - RCLCPP_INFO(this->get_logger(), - "✓ 回正完成(%.1fs), 绕障结束! 目标角度=%d°", - return_time, target_angle_int); + LOG_INFO("✓ 回正完成(%.1fs), 绕障结束! 目标角度=%d°", return_time, target_angle_int); } else { @@ -1646,13 +1567,11 @@ private: publish_angle -= std::min(RETURN_RATE, -angle_diff); } - publish_speed = 700; // 回正时保持低速 + publish_speed = 700; // 回正时保持低速 if (avoid_ctx_.counter % 10 == 0) { - RCLCPP_INFO(this->get_logger(), - "回正中: 当前角度=%d°, 目标角度=%d°, 差值=%d°", - publish_angle, target_angle_int, angle_diff); + LOG_INFO("回正中: 当前角度=%d°, 目标角度=%d°, 差值=%d°", publish_angle, target_angle_int, angle_diff); } } @@ -1667,7 +1586,7 @@ private: { publish_speed = 0; publish_angle = 0; - RCLCPP_WARN(this->get_logger(), "回正暂停: 需要左转但左侧有障碍"); + LOG_WARN("回正暂停: 需要左转但左侧有障碍"); return true; } @@ -1675,7 +1594,7 @@ private: { publish_speed = 0; publish_angle = 0; - RCLCPP_WARN(this->get_logger(), "回正暂停: 需要右转但右侧有障碍"); + LOG_WARN("回正暂停: 需要右转但右侧有障碍"); return true; } @@ -1684,7 +1603,7 @@ private: { publish_speed = 0; publish_angle = 0; - RCLCPP_WARN(this->get_logger(), "回正暂停: 前方出现障碍"); + LOG_WARN("回正暂停: 前方出现障碍"); avoid_ctx_.state = AVOID_WAITING; return true; } @@ -1700,7 +1619,7 @@ private: { publish_speed = 0; avoid_ctx_.state = AVOID_WAITING; - RCLCPP_ERROR(this->get_logger(), "左绕过程中左侧出现障碍物！"); + LOG_ERROR("左绕过程中左侧出现障碍物！"); return true; } @@ -1708,7 +1627,7 @@ private: { publish_speed = 0; avoid_ctx_.state = AVOID_WAITING; - RCLCPP_ERROR(this->get_logger(), "右绕过程中右侧出现障碍物！"); + LOG_ERROR("右绕过程中右侧出现障碍物！"); 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); auto node = std::make_shared("fu_node"); rclcpp::spin(node); rclcpp::shutdown(); + // 关闭日志系统 + logger::Logger::Shutdown(); return 0; } \ No newline at end of file diff --git a/src/autonomy/pl/CMakeLists.txt b/src/autonomy/pl/CMakeLists.txt index f4e3038..99cf29f 100644 --- a/src/autonomy/pl/CMakeLists.txt +++ b/src/autonomy/pl/CMakeLists.txt @@ -21,19 +21,19 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sweeper_interfaces REQUIRED) # find_package(mc REQUIRED) +find_package(logger REQUIRED) -include_directories( - include/pl - ${catkin_INCLUDE_DIRS} -) +include_directories(include/pl ${catkin_INCLUDE_DIRS}) add_executable(pl_node src/pl_node.cpp src/pl.cpp src/jsoncpp.cpp) -ament_target_dependencies(pl_node rclcpp std_msgs sweeper_interfaces) - -install(TARGETS +ament_target_dependencies( pl_node - DESTINATION lib/${PROJECT_NAME} -) + rclcpp + std_msgs + sweeper_interfaces + logger) + +install(TARGETS pl_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -44,6 +44,6 @@ if(BUILD_TESTING) # uncomment the line when this package is not in a git repo #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() -endif() +endif() ament_package() diff --git a/src/autonomy/pl/src/pl.cpp b/src/autonomy/pl/src/pl.cpp index 2b2b3cd..f6d9c66 100644 --- a/src/autonomy/pl/src/pl.cpp +++ b/src/autonomy/pl/src/pl.cpp @@ -1,21 +1,24 @@ #include "pl.hpp" -#include -#include -#include #include +#include +#include + +#include + +#include "logger/logger.h" using namespace std; #define WRC_MAX_POINTS_IN_PATH 2000 -#define Lat_Origin 32.045062 // 地面平面坐标系原点的纬度 -#define Lon_Origin 118.845200366 // 地面平面坐标系原点的经度 +#define Lat_Origin 32.045062 // 地面平面坐标系原点的纬度 +#define Lon_Origin 118.845200366 // 地面平面坐标系原点的经度 #define Re 6378137 #define Rn 6356755 -#define deg_rad (0.01745329252) // Transfer from angle degree to rad +#define deg_rad (0.01745329252) // Transfer from angle degree to rad #define R_LATI (6378137) -#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度) +#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度) float nl_radius; int nl_within_radius; @@ -39,18 +42,19 @@ TaskStatus task_status = TaskStatus::PENDING; double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2) { double x, y, length; - x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式 + x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式 y = (R_LATI) * (lati2 - lati1) * deg_rad; length = sqrt(x * x + y * y); return length; } // Cur_navAngle指定y轴正方向.Cur_lonti，Cur_lati为原点；x_diff,y_diff是Dest_lonti,Dest_lati在该坐标系的直角坐标:m -int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati, double *x_diff, double *y_diff) +int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati, + double* x_diff, double* y_diff) { double Navi_rad, x, y; - float k1, k2, k3, k4; // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta); - Navi_rad = Cur_navAngle * deg_rad; // 角度转化为弧度 + float k1, k2, k3, k4; // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta); + Navi_rad = Cur_navAngle * deg_rad; // 角度转化为弧度 // 以当前位置为原点，以正北为y正轴，以正东为x正轴； x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT; y = (Dest_lati - Cur_lati) * deg_rad * R_LATI; @@ -66,31 +70,30 @@ int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, doubl } // Find_Nearest_Point -int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info) +int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point* Road_info) { if (start_index < 0) { return -1; } - int Nearest_point_index = start_index; // 初始化为起始索引 + int Nearest_point_index = start_index; // 初始化为起始索引 double min_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, - Road_info[start_index].LongitudeInfo, - Road_info[start_index].LatitudeInfo); + Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo); // 从 start_index + 1 开始搜索 for (int i = 1; i < GpsPointNum; i++) { - int current_index = (start_index + i) % GpsPointNum; // 环绕访问 + int current_index = (start_index + i) % GpsPointNum; // 环绕访问 - double current_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, - Road_info[current_index].LongitudeInfo, - Road_info[current_index].LatitudeInfo); + double current_distance = + ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, + Road_info[current_index].LongitudeInfo, Road_info[current_index].LatitudeInfo); if (current_distance < min_distance) { min_distance = current_distance; - Nearest_point_index = current_index; // 更新最近点的索引 + Nearest_point_index = current_index; // 更新最近点的索引 // 如果距离小于 0.8 m，提前退出 if (min_distance < 0.8) @@ -100,7 +103,7 @@ int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_in } } - return Nearest_point_index; // 返回最近点的索引 + return Nearest_point_index; // 返回最近点的索引 } /* int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info) @@ -112,13 +115,15 @@ int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_in } int Nearest_point_index = 0; double aft_distance = 0, pre_distance = 0; - pre_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo); + pre_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, +Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo); int start = start_index + 1; for (int i = start; i < GpsPointNum; i++) { - aft_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo); + aft_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree, +Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo); // printf("i: %d , aft_distance: %lf \t",i,aft_distance); if (aft_distance < pre_distance) { @@ -152,7 +157,7 @@ int Road_Planning_Find_Aim_Point(int start_index, int dest_num) return (dest_index); } -int star_find_nearest_point(double lon, double lat, WRC_GPS_Point *Road_info) +int star_find_nearest_point(double lon, double lat, WRC_GPS_Point* Road_info) { double dis = 10000; int point = 0; @@ -164,7 +169,7 @@ int star_find_nearest_point(double lon, double lat, WRC_GPS_Point *Road_info) dis = dis_temp; point = i; } - printf("i: %d , dis_temp: %lf", i, dis_temp); + LOG_DEBUG("i: %d , dis_temp: %lf", i, dis_temp); } return point; } @@ -174,8 +179,7 @@ Direction straight_or_turn(double cur_direction, double des_direction, int thres { double diff = std::fmod(std::fabs(cur_direction - des_direction), 360.0); - if (diff > 180) - diff = 360 - diff; + if (diff > 180) diff = 360 - diff; // std::cout << "diff : " << diff << std::endl; @@ -211,8 +215,8 @@ void PL_ProcThread() // double head_raw = g_NavInfo.Yaw_rad; road_pos = Road_Planning_Find_Nearest_Point(des_pos - 3, g_NavInfo, GPSPointQueue); - des_pos = Road_Planning_Find_Aim_Point(road_pos, 3); // 找到当前点后第 3 个点，作为目标点 - direction_pos = Road_Planning_Find_Aim_Point(road_pos, 6); // 找到当前点后第 6 个点，判断直行/转弯 + des_pos = Road_Planning_Find_Aim_Point(road_pos, 3); // 找到当前点后第 3 个点，作为目标点 + direction_pos = Road_Planning_Find_Aim_Point(road_pos, 6); // 找到当前点后第 6 个点，判断直行/转弯 // printf("当前位置: %d\n", road_pos); // printf("目标位置: %d\n", des_pos); @@ -238,16 +242,15 @@ void PL_ProcThread() // cout << " * * * * * * * * * * * * * * * * * * * * *" << endl; // cout << endl; - // cout << "--- -target_head " << target_head << "--- current_head " << current_head << " " << abs(target_head - current_head) << endl; - // cout << endl; - // cout << "--- -distance " << distance << "- -nl_radius " << nl_radius << endl; + // cout << "--- -target_head " << target_head << "--- current_head " << current_head << " " << + // abs(target_head - current_head) << endl; cout << endl; cout << "--- -distance " << distance << "- -nl_radius + // " << nl_radius << endl; // cout << endl; // cout << " * * * * * * * * * * * * * * * * * * * * *" << endl; if (road_pos >= (GpsPointNum - 1)) { - x = 0.0; y = 1.0; pl_speed = 0; @@ -260,10 +263,8 @@ void PL_ProcThread() double x_cm_tmp = 0; double y_cm_tmp = 0; - ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat, - GPSPointQueue[des_pos].LongitudeInfo, - GPSPointQueue[des_pos].LatitudeInfo, - &x_cm_tmp, &y_cm_tmp); + ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat, GPSPointQueue[des_pos].LongitudeInfo, + GPSPointQueue[des_pos].LatitudeInfo, &x_cm_tmp, &y_cm_tmp); x = x_cm_tmp; y = y_cm_tmp; @@ -277,20 +278,20 @@ void PL_ProcThread() return; } -void *pl_thread(void *args) +void* pl_thread(void* args) { (void)args; sleep(3); memset(GPSPointQueue, 0, sizeof(GPSPointQueue)); - FILE *fp_gps = fopen("gps_load_now.txt", "r"); + FILE* fp_gps = fopen("gps_load_now.txt", "r"); if (NULL == fp_gps) { - printf("open route file error\n"); + LOG_ERROR("open route file error"); return NULL; } - std::cout << "have opened " << "gps_load_now.txt" << std::endl; + LOG_INFO("have opened gps_load_now.txt"); char mystring[200]; memset(mystring, '\0', 200); @@ -318,11 +319,10 @@ void *pl_thread(void *args) } GpsPointNum = i / 4; - printf("point_num:%d\n", GpsPointNum); + LOG_INFO("point_num:%d", GpsPointNum); fclose(fp_gps); - std::cout << std::endl; - printf("#################### auto drive on! ####################\n"); + LOG_INFO("#################### auto drive on! ####################"); PL_ProcThread(); return NULL; diff --git a/src/autonomy/pl/src/pl_node.cpp b/src/autonomy/pl/src/pl_node.cpp index b9a93f4..03866f1 100644 --- a/src/autonomy/pl/src/pl_node.cpp +++ b/src/autonomy/pl/src/pl_node.cpp @@ -1,22 +1,24 @@ +#include +#include +#include +#include + +#include // 新增：用于时间戳处理 +#include +#include +#include + +#include "json.h" +#include "logger/logger.h" +#include "pl.hpp" #include "rclcpp/rclcpp.hpp" -#include "sweeper_interfaces/msg/rtk.hpp" #include "sweeper_interfaces/msg/pl.hpp" #include "sweeper_interfaces/msg/route.hpp" +#include "sweeper_interfaces/msg/rtk.hpp" #include "sweeper_interfaces/msg/task.hpp" -#include "json.h" -#include -#include -#include -#include -#include "pl.hpp" -#include -#include -#include -#include -#include // 新增：用于时间戳处理 using namespace std; -using namespace std::chrono; // 新增：时间戳命名空间 +using namespace std::chrono; // 新增：时间戳命名空间 pthread_t pl_thread_t; int is_start = 0; @@ -29,34 +31,39 @@ const duration rtk_timeout = duration(0.5); class pl_node : public rclcpp::Node { -public: + public: pl_node(std::string name) : Node(name) { - RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); + LOG_INFO("%s节点已经启动.", name.c_str()); // 初始化最后接收时间为当前时间（避免初始状态直接超时） last_rtk_time = system_clock::now(); // 创建订阅者订阅话题 - msg_subscribe_ = this->create_subscription("rtk_message", 10, std::bind(&pl_node::msg_callback, this, std::placeholders::_1)); - task_subscribe_ = this->create_subscription("task_message/download", 10, std::bind(&pl_node::task_callback, this, std::placeholders::_1)); + msg_subscribe_ = this->create_subscription( + "rtk_message", 10, std::bind(&pl_node::msg_callback, this, std::placeholders::_1)); + task_subscribe_ = this->create_subscription( + "task_message/download", 10, std::bind(&pl_node::task_callback, this, std::placeholders::_1)); // 创建发布者 msg_publisher_ = this->create_publisher("pl_message", 10); task_publisher_ = this->create_publisher("task_message/upload", 10); // 创建定时器，定时发布 - timer_pl = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&pl_node::timer_callback_pl, this)); - timer_task = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&pl_node::timer_callback_task, this)); + timer_pl = + this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&pl_node::timer_callback_pl, this)); + timer_task = + this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&pl_node::timer_callback_task, this)); } -private: + private: // 收到话题数据的回调函数 void msg_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg) { // 新增：更新最后接收时间戳 last_rtk_time = system_clock::now(); - // RCLCPP_INFO(this->get_logger(), "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", + // LOG_INFO( + // "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", // msg->lat, msg->lon, msg->head, msg->speed, msg->p_quality, msg->h_quality); g_rtk.reliability = 1; @@ -75,19 +82,19 @@ private: if (is_start == 0 && msg->task_status == 1) { pthread_create(&pl_thread_t, NULL, pl_thread, NULL); - cout << " started" << endl; + LOG_INFO("started"); is_start = 1; task_status = TaskStatus::PENDING; } else if (is_start == 1 && msg->task_status == 0) { pthread_cancel(pl_thread_t); - cout << "pl_thread_t is canceled " << endl; + LOG_INFO("pl_thread_t is canceled"); is_start = 0; task_status = TaskStatus::COMPLETED; } - // RCLCPP_INFO(this->get_logger(), " ==================================== is_start : %d", is_start); - // RCLCPP_INFO(this->get_logger(), " ==================================== task_status : %d", msg->task_status); + // LOG_INFO(" ==================================== is_start : %d", is_start); + // LOG_INFO(" ==================================== task_status : %d", msg->task_status); } void timer_callback_pl() @@ -98,12 +105,12 @@ private: if (is_start == 1) { - if (sock < 0) // 首次连接 + if (sock < 0) // 首次连接 { struct sockaddr_in serv_addr; if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { - RCLCPP_ERROR(this->get_logger(), "Socket creation error"); + LOG_ERROR("Socket creation error"); return; } @@ -112,31 +119,31 @@ private: if (inet_pton(AF_INET, "192.168.5.1", &serv_addr.sin_addr) <= 0) { - RCLCPP_ERROR(this->get_logger(), "Invalid address/ Address not supported"); + LOG_ERROR("Invalid address/ Address not supported"); close(sock); return; } - if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) + if (connect(sock, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) { - RCLCPP_ERROR(this->get_logger(), "Connection Failed"); + LOG_ERROR("Connection Failed"); close(sock); sock = -1; return; } } - const char *message = "Status: RUNNING"; - if (send(sock, message, strlen(message), 0) < 0) // 发送失败则重建连接 + const char* message = "Status: RUNNING"; + if (send(sock, message, strlen(message), 0) < 0) // 发送失败则重建连接 { close(sock); sock = -1; - return timer_callback_pl(); // 重试 + return timer_callback_pl(); // 重试 } } else if (sock >= 0) { - close(sock); // 停止时关闭连接 + close(sock); // 停止时关闭连接 sock = -1; } @@ -151,11 +158,11 @@ private: message.speed = 0; if (time_since_last > rtk_timeout) { - RCLCPP_ERROR(this->get_logger(), "RTK数据超时(%.2fs)，停车！！！", time_since_last.count()); + LOG_ERROR("RTK数据超时(%.2fs)，停车！！！", time_since_last.count()); } else { - RCLCPP_ERROR(this->get_logger(), "RTK信号差，停车！！！"); + LOG_ERROR("RTK信号差，停车！！！"); } } else @@ -163,17 +170,18 @@ private: message.reliability = g_rtk.reliability; // message.drive_mode = drive_mode; - if (drive_mode == Direction::STRAIGHT_D) // 直行 + if (drive_mode == Direction::STRAIGHT_D) // 直行 message.drive_mode = 0; - else if (drive_mode == Direction::LEFT_TURN || drive_mode == Direction::RIGHT_TURN) // 转弯 + else if (drive_mode == Direction::LEFT_TURN || drive_mode == Direction::RIGHT_TURN) // 转弯 message.drive_mode = 1; message.enter_range = nl_within_radius; message.is_start = is_start; - // RCLCPP_INFO(this->get_logger(), " ==================================== message.is_start : %d", message.is_start); + // LOG_INFO(" ==================================== message.is_start : %d", + // message.is_start); // 日志打印 - // RCLCPP_INFO(this->get_logger(), "x:'%lf',y:'%lf',speed:'%lf',drive_mode:'%d'", + // LOG_INFO("x:'%lf',y:'%lf',speed:'%lf',drive_mode:'%d'", // message.x, message.y, message.speed, message.drive_mode); // 发布消息 msg_publisher_->publish(message); @@ -207,24 +215,28 @@ void initConfig() std::ifstream in("config.json", std::ios::binary); if (!in.is_open()) { - std::cout << "read config file error" << std::endl; + LOG_ERROR("read config file error"); return; } if (reader.parse(in, root)) { nl_radius = root["detect_line_tolerance"].as(); } - in.close(); // 关闭文件流 + in.close(); // 关闭文件流 } -int main(int argc, char **argv) +int main(int argc, char** argv) { initConfig(); rclcpp::init(argc, argv); + // 初始化日志系统 + logger::Logger::Init("pl", "./log"); /*创建对应节点的共享指针对象*/ auto node = std::make_shared("pl_node"); /* 运行节点，并检测退出信号*/ rclcpp::spin(node); rclcpp::shutdown(); + // 关闭日志系统 + logger::Logger::Shutdown(); // pthread_join(pl_thread_t, NULL); return 0; diff --git a/src/autonomy/route/CMakeLists.txt b/src/autonomy/route/CMakeLists.txt index c253540..ee5ff04 100644 --- a/src/autonomy/route/CMakeLists.txt +++ b/src/autonomy/route/CMakeLists.txt @@ -21,23 +21,20 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sweeper_interfaces REQUIRED) find_package(CURL REQUIRED) +find_package(logger REQUIRED) +include_directories(include/route ${catkin_INCLUDE_DIRS}) -include_directories( - include/route - ${catkin_INCLUDE_DIRS} -) - -add_executable(route_node - src/route_node.cpp - src/md5.cpp - src/jsoncpp.cpp) -ament_target_dependencies(route_node rclcpp std_msgs sweeper_interfaces CURL) - -install(TARGETS +add_executable(route_node src/route_node.cpp src/md5.cpp src/jsoncpp.cpp) +ament_target_dependencies( route_node - DESTINATION lib/${PROJECT_NAME} -) + rclcpp + std_msgs + sweeper_interfaces + CURL + logger) + +install(TARGETS route_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -48,5 +45,5 @@ if(BUILD_TESTING) # uncomment the line when this package is not in a git repo #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() -endif() +endif() ament_package() diff --git a/src/autonomy/route/package.xml b/src/autonomy/route/package.xml index 430bac2..b5f9625 100644 --- a/src/autonomy/route/package.xml +++ b/src/autonomy/route/package.xml @@ -12,6 +12,7 @@ rclcpp std_msgs sweeper_interfaces + logger ament_lint_auto ament_lint_common diff --git a/src/autonomy/route/src/route_node.cpp b/src/autonomy/route/src/route_node.cpp index 23e7b13..6d03528 100644 --- a/src/autonomy/route/src/route_node.cpp +++ b/src/autonomy/route/src/route_node.cpp @@ -1,25 +1,27 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "json.h" +#include "logger/logger.h" +#include "md5.h" #include "rclcpp/rclcpp.hpp" #include "sweeper_interfaces/msg/rtk.hpp" #include "sweeper_interfaces/msg/sub.hpp" -#include -#include "md5.h" -#include "json.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include +std::mutex collect_mutex; // 互斥锁，用于保护 isCollecting 变量 -std::mutex collect_mutex; // 互斥锁，用于保护 isCollecting 变量 - -#define deg_rad (0.01745329252) // Transfer from angle degree to rad +#define deg_rad (0.01745329252) // Transfer from angle degree to rad #define R_LATI (6378137) -#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度) +#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度) using namespace std; /* 存储接收RTK的GPS点信息 */ struct struct_rtk @@ -40,83 +42,87 @@ struct_rtk last_g_rtk = {}; string vid; string upload_URL; std::string destinationFilePath1 = "./gps_load_now.txt"; -void *route_thread(void *args); +void* route_thread(void* args); string calculate_md5(string filename) { MD5 md5; ifstream file; file.open(filename, ios::binary); md5.update(file); - cout << md5.toString() << endl; + LOG_INFO("%s", md5.toString().c_str()); return md5.toString(); } bool upload_file(string filename) { - CURL *curl; + CURL* curl; CURLcode ret; curl = curl_easy_init(); - struct curl_httppost *post = NULL; - struct curl_httppost *last = NULL; - curl_formadd(&post, &last, CURLFORM_PTRNAME, "vid", CURLFORM_PTRCONTENTS, vid.c_str(), CURLFORM_END); // form-data key(path) 和 value(device_cover) + struct curl_httppost* post = NULL; + struct curl_httppost* last = NULL; + curl_formadd(&post, &last, CURLFORM_PTRNAME, "vid", CURLFORM_PTRCONTENTS, vid.c_str(), + CURLFORM_END); // form-data key(path) 和 value(device_cover) curl_formadd(&post, &last, CURLFORM_COPYNAME, "file", CURLFORM_FILE, filename.c_str(), CURLFORM_END); - curl_formadd(&post, &last, CURLFORM_COPYNAME, "md5", CURLFORM_COPYCONTENTS, calculate_md5(filename).c_str(), CURLFORM_END); + curl_formadd(&post, &last, CURLFORM_COPYNAME, "md5", CURLFORM_COPYCONTENTS, calculate_md5(filename).c_str(), + CURLFORM_END); curl_easy_setopt(curl, CURLOPT_URL, upload_URL.c_str()); curl_easy_setopt(curl, CURLOPT_HTTPPOST, post); ret = curl_easy_perform(curl); if (ret != CURLE_OK) { - fprintf(stderr, "curl_easy_perform() failed: %s\n", curl_easy_strerror(ret)); + LOG_ERROR("curl_easy_perform() failed: %s", curl_easy_strerror(ret)); return false; } curl_formfree(post); curl_easy_cleanup(curl); - cout << "上传成功" << endl; + LOG_INFO("上传成功"); return true; } -void copyFileAndOverwrite(const std::string &sourceFilePath, const std::string &destinationFilePath) +void copyFileAndOverwrite(const std::string& sourceFilePath, const std::string& destinationFilePath) { std::ifstream src(sourceFilePath, std::ios::binary); std::ofstream dst(destinationFilePath, std::ios::binary); if (!src) { - std::cerr << "无法打开源文件: " << sourceFilePath << std::endl; + LOG_ERROR("无法打开源文件: %s", sourceFilePath.c_str()); return; } if (!dst) { - std::cerr << "无法打开目标文件: " << destinationFilePath << std::endl; + LOG_ERROR("无法打开目标文件: %s", destinationFilePath.c_str()); return; } - dst << src.rdbuf(); // 将源文件内容复制到目标文件 + dst << src.rdbuf(); // 将源文件内容复制到目标文件 if (!dst) { - std::cerr << "复制文件时出错" << std::endl; + LOG_ERROR("复制文件时出错"); } } class route_node : public rclcpp::Node { -public: + public: route_node(std::string name) : Node(name) { - RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); + LOG_INFO("%s节点已经启动.", name.c_str()); // 创建一个订阅者订阅话题 - sub_subscribe_ = this->create_subscription("gather", 10, std::bind(&route_node::sub_callback, this, std::placeholders::_1)); + sub_subscribe_ = this->create_subscription( + "gather", 10, std::bind(&route_node::sub_callback, this, std::placeholders::_1)); - msg_subscribe_ = this->create_subscription("rtk_message", 10, std::bind(&route_node::msg_callback, this, std::placeholders::_1)); + msg_subscribe_ = this->create_subscription( + "rtk_message", 10, std::bind(&route_node::msg_callback, this, std::placeholders::_1)); } -private: + private: // 收到话题数据的回调函数 void msg_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg) { if (msg->p_quality < 4 || msg->h_quality < 4) { g_rtk.reliability = 0; - RCLCPP_INFO(this->get_logger(), "rtk信号差!"); + LOG_INFO("rtk信号差!"); } if (first_flag) { @@ -133,40 +139,34 @@ private: if (!isCollecting) { - // cout << "等待采集...." << endl; - RCLCPP_INFO(this->get_logger(), "等待采集....."); + LOG_INFO("等待采集....."); } else { - // cout << "平台采集中" << endl; - RCLCPP_INFO(this->get_logger(), "平台采集中....."); + LOG_INFO("平台采集中....."); } } void sub_callback(const sweeper_interfaces::msg::Sub::SharedPtr msg) { if (msg->get_route && !isCollecting) { - std::lock_guard lock(collect_mutex); // 加锁 + std::lock_guard lock(collect_mutex); // 加锁 isCollecting = true; - // cout << "平台开始采集" << endl; - RCLCPP_INFO(this->get_logger(), "平台开始采集"); + LOG_INFO("平台开始采集"); pthread_create(&route_thread_t, NULL, route_thread, NULL); } else if (!msg->get_route && isCollecting) { isCollecting = false; - // cout << "平台结束采集" << endl; - RCLCPP_INFO(this->get_logger(), "平台结束采集"); + LOG_INFO("平台结束采集"); pthread_cancel(route_thread_t); if (upload_file(filename)) { - // cout << "上传成功" << endl; - RCLCPP_INFO(this->get_logger(), "上传成功"); + LOG_INFO("上传成功"); } else { - // cout << "上传失败" << endl; - RCLCPP_INFO(this->get_logger(), "上传失败"); + LOG_INFO("上传失败"); } } } @@ -178,13 +178,13 @@ private: double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2) { double x, y, length; - x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式 + x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式 y = (R_LATI) * (lati2 - lati1) * deg_rad; length = sqrt(x * x + y * y); return length; } -void *route_thread(void *args) +void* route_thread(void* args) { (void)args; filename = ""; @@ -199,12 +199,12 @@ void *route_thread(void *args) oss << "routes/gps_load_" << timestamp << ".txt"; filename = oss.str(); - FILE *fp_routes = fopen(filename.c_str(), "w"); + FILE* fp_routes = fopen(filename.c_str(), "w"); usleep(5000); while (1) { double distance = ntzx_GPS_length(last_g_rtk.lon, last_g_rtk.lat, g_rtk.lon, g_rtk.lat); - if (distance >= 1.0) // 每隔1米采样一次 + if (distance >= 1.0) // 每隔1米采样一次 { fprintf(fp_routes, "%.10lf\n%.11lf\n%lf\n%lf\n", g_rtk.lat, g_rtk.lon, g_rtk.head, 0.0); fflush(fp_routes); @@ -212,7 +212,7 @@ void *route_thread(void *args) last_g_rtk.lat = g_rtk.lat; last_g_rtk.lon = g_rtk.lon; last_g_rtk.head = g_rtk.head; - printf("hit!\n"); + LOG_DEBUG("hit!"); } } } @@ -224,27 +224,31 @@ void init_main() std::ifstream in("config.json", std::ios::binary); if (!in.is_open()) { - std::cout << "read config file error" << std::endl; + LOG_ERROR("read config file error"); return; } if (reader.parse(in, root)) { vid = root["mqtt"]["vid"].asString(); upload_URL = root["mqtt"]["upload_url"].asString(); - cout << "vid:" << vid << endl; + LOG_INFO("vid:%s", vid.c_str()); } - in.close(); // 关闭文件流 + in.close(); // 关闭文件流 } -int main(int argc, char **argv) +int main(int argc, char** argv) { init_main(); rclcpp::init(argc, argv); + /*初始化日志系统*/ + logger::Logger::Init("route", "./log"); /*创建对应节点的共享指针对象*/ auto node = std::make_shared("route_node"); /* 运行节点，并检测退出信号*/ rclcpp::spin(node); rclcpp::shutdown(); + /*关闭日志系统*/ + logger::Logger::Shutdown(); // pthread_join(route_thread_t, NULL); return 0; } diff --git a/src/base/mc/CMakeLists.txt b/src/base/mc/CMakeLists.txt index 7c3bd63..c0d73af 100644 --- a/src/base/mc/CMakeLists.txt +++ b/src/base/mc/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(ament_index_cpp REQUIRED) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(logger REQUIRED) # 搜索 src 目录下所有 .cpp 文件 file(GLOB SRC_FILES src/*.cpp) @@ -18,15 +19,19 @@ file(GLOB SRC_FILES src/*.cpp) # 创建可执行文件 add_executable(mc_node ${SRC_FILES}) -ament_target_dependencies(mc_node ament_index_cpp rclcpp std_msgs sweeper_interfaces) +ament_target_dependencies( + mc_node + ament_index_cpp + rclcpp + std_msgs + sweeper_interfaces + logger) # Set include directories for the target -target_include_directories( - mc_node - PUBLIC $ - $) +target_include_directories(mc_node PUBLIC $ + $) -target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17) +target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17) # Install target install(TARGETS mc_node DESTINATION lib/${PROJECT_NAME}) diff --git a/src/base/mc/include/mc/can_struct.h b/src/base/mc/include/mc/can_struct.h index 1089fea..2eceb30 100644 --- a/src/base/mc/include/mc/can_struct.h +++ b/src/base/mc/include/mc/can_struct.h @@ -175,12 +175,12 @@ struct can_MCU_cmd frame.ext = EXT_FLAG; frame.rtr = RTR_FLAG; - // std::cout << "MCU frame : "; + // LOG_INFO("MCU frame : "); // for (int i = 0; i < 8; ++i) // { - // printf("0X%x ", frame.data[i]); + // LOG_INFO("0X%x ", frame.data[i]); // } - // printf("\n"); + // LOG_INFO(""); return frame; } diff --git a/src/base/mc/package.xml b/src/base/mc/package.xml index 1fd862f..600731a 100644 --- a/src/base/mc/package.xml +++ b/src/base/mc/package.xml @@ -11,6 +11,7 @@ sweeper_interfaces rclcpp ament_index_cpp + logger rosidl_default_runtime diff --git a/src/base/mc/src/can_driver.cpp b/src/base/mc/src/can_driver.cpp index 71b27a8..3b64fd1 100644 --- a/src/base/mc/src/can_driver.cpp +++ b/src/base/mc/src/can_driver.cpp @@ -1,27 +1,25 @@ #include "mc/can_driver.h" -#include -#include -#include -#include + #include -#include -#include -#include -#include #include #include +#include +#include +#include +#include + +#include +#include +#include +#include CANDriver::CANDriver() = default; -CANDriver::~CANDriver() -{ - close(); -} +CANDriver::~CANDriver() { close(); } -bool CANDriver::open(const std::string &interface) +bool CANDriver::open(const std::string& interface) { - if (running) - return false; + if (running) return false; sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW); if (sockfd < 0) @@ -45,7 +43,7 @@ bool CANDriver::open(const std::string &interface) addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; - if (bind(sockfd, reinterpret_cast(&addr), sizeof(addr)) < 0) + if (bind(sockfd, reinterpret_cast(&addr), sizeof(addr)) < 0) { perror("bind"); ::close(sockfd); @@ -63,12 +61,10 @@ bool CANDriver::open(const std::string &interface) void CANDriver::close() { - if (!running) - return; + if (!running) return; running = false; - if (receiveThread.joinable()) - receiveThread.join(); + if (receiveThread.joinable()) receiveThread.join(); if (sockfd >= 0) { @@ -77,17 +73,14 @@ void CANDriver::close() } } -bool CANDriver::sendFrame(const CANFrame &frame) +bool CANDriver::sendFrame(const CANFrame& frame) { - if (!running) - return false; + if (!running) return false; struct can_frame raw_frame{}; raw_frame.can_id = frame.id; - if (frame.ext) - raw_frame.can_id |= CAN_EFF_FLAG; - if (frame.rtr) - raw_frame.can_id |= CAN_RTR_FLAG; + if (frame.ext) raw_frame.can_id |= CAN_EFF_FLAG; + if (frame.rtr) raw_frame.can_id |= CAN_RTR_FLAG; raw_frame.can_dlc = frame.dlc; std::memcpy(raw_frame.data, frame.data, frame.dlc); @@ -100,28 +93,27 @@ bool CANDriver::sendFrame(const CANFrame &frame) return true; } -void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData) +void CANDriver::setReceiveCallback(ReceiveCallback callback, void* userData) { this->callback = callback; this->userData = userData; } -bool CANDriver::setFilter(const std::vector &filters) +bool CANDriver::setFilter(const std::vector& filters) { - if (!running) - return false; + if (!running) return false; filters_ = filters; return applyFilters(); } -bool CANDriver::addFilter(const can_filter &filter) +bool CANDriver::addFilter(const can_filter& filter) { filters_.push_back(filter); return applyFilters(); } -bool CANDriver::addFilters(const std::vector &filters) +bool CANDriver::addFilters(const std::vector& filters) { filters_.insert(filters_.end(), filters.begin(), filters.end()); return applyFilters(); @@ -129,8 +121,7 @@ bool CANDriver::addFilters(const std::vector &filters) bool CANDriver::applyFilters() { - if (!running) - return false; + if (!running) return false; if (filters_.empty()) { @@ -138,8 +129,7 @@ bool CANDriver::applyFilters() return true; } - if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, - filters_.data(), filters_.size() * sizeof(can_filter)) < 0) + if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, filters_.data(), filters_.size() * sizeof(can_filter)) < 0) { perror("setsockopt"); return false; @@ -157,7 +147,7 @@ void CANDriver::receiveThreadFunc() while (running) { - int ret = poll(&fds, 1, 100); // 等待最多100ms + int ret = poll(&fds, 1, 100); // 等待最多100ms if (ret < 0) { perror("poll"); diff --git a/src/base/mc/src/can_utils.cpp b/src/base/mc/src/can_utils.cpp index 12ff860..ac4ea4b 100644 --- a/src/base/mc/src/can_utils.cpp +++ b/src/base/mc/src/can_utils.cpp @@ -1,12 +1,15 @@ #include "mc/can_utils.hpp" -#include + #include +#include + +#include "logger/logger.h" bool g_can_print_enable = false; -void receiveHandler(const CANFrame &frame, void *userData) +void receiveHandler(const CANFrame& frame, void* userData) { - auto *context = static_cast(userData); + auto* context = static_cast(userData); auto node = context->node; auto pub = context->publisher; auto now = node->now(); @@ -20,10 +23,10 @@ void receiveHandler(const CANFrame &frame, void *userData) if (g_can_print_enable) { std::stringstream ss; - ss << "CAN ID: " << std::hex << std::uppercase - << std::setw((frame.id > 0x7FF) ? 8 : 5) << std::setfill(' ') << frame.id << " Data: "; + ss << "CAN ID: " << std::hex << std::uppercase << std::setw((frame.id > 0x7FF) ? 8 : 5) << std::setfill(' ') + << frame.id << " Data: "; for (int i = 0; i < frame.dlc; ++i) ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " "; - RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str()); + LOG_INFO("%s", ss.str().c_str()); } } \ No newline at end of file diff --git a/src/base/mc/src/control_cache.cpp b/src/base/mc/src/control_cache.cpp index d138d37..c5fec19 100644 --- a/src/base/mc/src/control_cache.cpp +++ b/src/base/mc/src/control_cache.cpp @@ -2,7 +2,7 @@ ControlCache control_cache; -void ControlCache::update(const sweeper_interfaces::msg::McCtrl &msg) +void ControlCache::update(const sweeper_interfaces::msg::McCtrl& msg) { std::lock_guard lock(mutex_); latest_msg_ = msg; @@ -10,15 +10,13 @@ void ControlCache::update(const sweeper_interfaces::msg::McCtrl &msg) has_data_ = true; } -bool ControlCache::get(sweeper_interfaces::msg::McCtrl &msg) +bool ControlCache::get(sweeper_interfaces::msg::McCtrl& msg) { std::lock_guard lock(mutex_); - if (!has_data_) - return false; + if (!has_data_) return false; auto now = std::chrono::steady_clock::now(); - if (std::chrono::duration_cast(now - last_update_time_).count() > 100) - return false; + if (std::chrono::duration_cast(now - last_update_time_).count() > 100) return false; msg = latest_msg_; return true; diff --git a/src/base/mc/src/get_config.cpp b/src/base/mc/src/get_config.cpp index 1e1f41d..c267a1e 100644 --- a/src/base/mc/src/get_config.cpp +++ b/src/base/mc/src/get_config.cpp @@ -1,6 +1,6 @@ #include "mc/get_config.h" -bool load_config(Config &config) +bool load_config(Config& config) { try { @@ -20,7 +20,7 @@ bool load_config(Config &config) return true; } - catch (const std::exception &e) + catch (const std::exception& e) { std::cerr << "Error parsing MQTT config: " << e.what() << std::endl; return false; diff --git a/src/base/mc/src/mc.cpp b/src/base/mc/src/mc.cpp index 49ae59c..aea5216 100644 --- a/src/base/mc/src/mc.cpp +++ b/src/base/mc/src/mc.cpp @@ -1,73 +1,78 @@ -#include "rclcpp/rclcpp.hpp" -#include "mc/get_config.h" +#include + +#include "logger/logger.h" #include "mc/can_driver.h" #include "mc/can_utils.hpp" #include "mc/control_cache.hpp" +#include "mc/get_config.h" #include "mc/timer_tasks.hpp" +#include "rclcpp/rclcpp.hpp" #include "sweeper_interfaces/msg/can_frame.hpp" #include "sweeper_interfaces/msg/mc_ctrl.hpp" -#include - namespace sweeperMsg = sweeper_interfaces::msg; CANDriver canctl; void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg) { - // std::cout << "\n 刹车: " << (msg->brake ? "已刹车" : "未刹车") - // << "\n 挡位: "; - // switch (msg->gear) - // { - // case 0: - // std::cout << "空挡"; - // break; - // case 2: - // std::cout << "前进挡"; - // break; - // case 1: - // std::cout << "后退挡"; - // break; - // default: - // std::cout << "未知挡位(" << static_cast(msg->gear) << ")"; - // break; - // } - // std::cout << "\n 行走电机转速: " << static_cast(msg->rpm) << " RPM" - // << "\n 轮端转向角度: " << msg->angle << "°" - // << "\n 清扫状态: " << (msg->sweep ? "正在清扫" : "未清扫") - // << std::endl; + // LOG_INFO("\n 刹车: %s", (msg->brake ? "已刹车" : "未刹车")); + // LOG_INFO(" 挡位: "); + // switch (msg->gear) + // { + // case 0: + // LOG_INFO("空挡"); + // break; + // case 2: + // LOG_INFO("前进挡"); + // break; + // case 1: + // LOG_INFO("后退挡"); + // break; + // default: + // LOG_INFO("未知挡位(%d)", static_cast(msg->gear)); + // break; + // } + // LOG_INFO(" 行走电机转速: %d RPM", static_cast(msg->rpm)); + // LOG_INFO(" 轮端转向角度: %.1f°", msg->angle); + // LOG_INFO(" 清扫状态: %s", (msg->sweep ? "正在清扫" : "未清扫")); - control_cache.update(*msg); + control_cache.update(*msg); } -int main(int argc, char **argv) +int main(int argc, char** argv) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("can_driver_node"); - RCLCPP_INFO(node->get_logger(), "Starting mc package..."); + rclcpp::init(argc, argv); + /*初始化日志系统*/ + logger::Logger::Init("mc", "./log"); - auto pub = node->create_publisher("can_data", 10); - auto sub = node->create_subscription("mc_ctrl", 10, mcCtrlCallback); + auto node = rclcpp::Node::make_shared("can_driver_node"); + LOG_INFO("Starting mc package..."); - Config mc_config; - load_config(mc_config); + auto pub = node->create_publisher("can_data", 10); + auto sub = node->create_subscription("mc_ctrl", 10, mcCtrlCallback); - if (!canctl.open(mc_config.can_dev)) - { - RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str()); - return -1; - } + Config mc_config; + load_config(mc_config); - auto context = std::make_shared(); - context->node = node; - context->publisher = pub; - canctl.setReceiveCallback(receiveHandler, context.get()); + if (!canctl.open(mc_config.can_dev)) + { + LOG_ERROR("Failed to open CAN interface: %s", mc_config.can_dev.c_str()); + logger::Logger::Shutdown(); + return -1; + } - setupTimers(node, canctl); + auto context = std::make_shared(); + context->node = node; + context->publisher = pub; + canctl.setReceiveCallback(receiveHandler, context.get()); - rclcpp::on_shutdown([&]() - { canctl.close(); }); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; + setupTimers(node, canctl); + + rclcpp::on_shutdown([&]() { canctl.close(); }); + rclcpp::spin(node); + rclcpp::shutdown(); + /*关闭日志系统*/ + logger::Logger::Shutdown(); + return 0; } diff --git a/src/base/mc/src/timer_tasks.cpp b/src/base/mc/src/timer_tasks.cpp index 34728e7..b86b9b0 100644 --- a/src/base/mc/src/timer_tasks.cpp +++ b/src/base/mc/src/timer_tasks.cpp @@ -1,134 +1,127 @@ #include "mc/timer_tasks.hpp" -#include "mc/control_cache.hpp" + +#include "logger/logger.h" #include "mc/can_struct.h" +#include "mc/control_cache.hpp" // 定时器回调：MCU 控制 -void mcuTimerTask(CANDriver &canctl) +void mcuTimerTask(CANDriver& canctl) { - auto msg = get_safe_control(); - mcu_cmd.setEnabled(true); - mcu_cmd.setGear(msg.gear); - mcu_cmd.setRPM(msg.rpm); - mcu_cmd.setBrake(msg.brake); - canctl.sendFrame(mcu_cmd.toFrame()); - // std::cout << "mcuTimerTask" << std::endl; - // std::cout << "msg.gear " << msg.gear << std::endl; - // std::cout << "msg.brake " << msg.brake << std::endl; - // std::cout << "msg.rpm " << msg.rpm << std::endl; + auto msg = get_safe_control(); + mcu_cmd.setEnabled(true); + mcu_cmd.setGear(msg.gear); + mcu_cmd.setRPM(msg.rpm); + mcu_cmd.setBrake(msg.brake); + canctl.sendFrame(mcu_cmd.toFrame()); + // LOG_INFO("mcuTimerTask"); + // LOG_INFO("msg.gear %d", msg.gear); + // LOG_INFO("msg.brake %d", msg.brake); + // LOG_INFO("msg.rpm %d", msg.rpm); } // 定时器回调：EPS 控制 -void epsTimerTask(CANDriver &canctl) +void epsTimerTask(CANDriver& canctl) { - auto msg = get_safe_control(); - eps_cmd.setCenterCmd(0); - eps_cmd.setAngle(msg.angle); - eps_cmd.setAngularSpeed(msg.angle_speed); - eps_cmd.pack(); - canctl.sendFrame(eps_cmd.toFrame()); - // std::cout << "epsTimerTask" << std::endl; + auto msg = get_safe_control(); + eps_cmd.setCenterCmd(0); + eps_cmd.setAngle(msg.angle); + eps_cmd.setAngularSpeed(msg.angle_speed); + eps_cmd.pack(); + canctl.sendFrame(eps_cmd.toFrame()); + // LOG_INFO("epsTimerTask"); } // 定时器回调：VCU 扫地控制 // 修改timer_tasks.cpp中的vcuTimerTask函数 -void vcuTimerTask(CANDriver &canctl) +void vcuTimerTask(CANDriver& canctl) { - static bool vcu_initialized = false; - static bool last_sweep_state = false; // 记录上一次清扫状态 + static bool vcu_initialized = false; + static bool last_sweep_state = false; // 记录上一次清扫状态 - auto msg = get_safe_control(); + auto msg = get_safe_control(); - // 首次运行时初始化VCU控制 - if (!vcu_initialized) - { - // 发送CAN使能指令 - vcu_enable_cmd.setCANEnable(true); - canctl.sendFrame(vcu_enable_cmd.toFrame()); - // std::cout << "mcuTimerTask" << std::endl; - vcu_initialized = true; - RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled"); - } + // 首次运行时初始化VCU控制 + if (!vcu_initialized) + { + // 发送CAN使能指令 + vcu_enable_cmd.setCANEnable(true); + canctl.sendFrame(vcu_enable_cmd.toFrame()); + // LOG_INFO("mcuTimerTask"); + vcu_initialized = true; + LOG_INFO("[VCU] CAN control enabled"); + } - // 检查清扫状态是否变化 - bool sweep_state_changed = (msg.sweep != last_sweep_state); + // 检查清扫状态是否变化 + bool sweep_state_changed = (msg.sweep != last_sweep_state); - if (sweep_state_changed) - { - // 根据清扫状态设置所有部件 - int8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止 - int8_t target_value2 = msg.sweep ? -100 : 100; // -100表示下沉，100表示抬升 + if (sweep_state_changed) + { + // 根据清扫状态设置所有部件 + int8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止 + int8_t target_value2 = msg.sweep ? -100 : 100; // -100表示下沉，100表示抬升 - // ===== 控制0x211报文 (电机M1-M7) ===== - vcu_motor1_cmd.setByte0(msg.sweep ? 1 : 0); // 1表示正向运行，0表示停止 - vcu_motor1_cmd.setByte1(target_value); - vcu_motor1_cmd.setByte2(target_value); - vcu_motor1_cmd.setByte3(target_value); - vcu_motor1_cmd.setByte4(target_value2); - vcu_motor1_cmd.setByte5(target_value2); - vcu_motor1_cmd.setByte6(target_value); - vcu_motor1_cmd.setByte7(target_value); - canctl.sendFrame(vcu_motor1_cmd.toFrame()); - // std::cout << "vcuTimerTask1" << std::endl; + // ===== 控制0x211报文 (电机M1-M7) ===== + vcu_motor1_cmd.setByte0(msg.sweep ? 1 : 0); // 1表示正向运行，0表示停止 + vcu_motor1_cmd.setByte1(target_value); + vcu_motor1_cmd.setByte2(target_value); + vcu_motor1_cmd.setByte3(target_value); + vcu_motor1_cmd.setByte4(target_value2); + vcu_motor1_cmd.setByte5(target_value2); + vcu_motor1_cmd.setByte6(target_value); + vcu_motor1_cmd.setByte7(target_value); + canctl.sendFrame(vcu_motor1_cmd.toFrame()); + // LOG_INFO("vcuTimerTask1"); - // ===== 控制0x212报文 (电机M8和LED输出) ===== - vcu_motor2_cmd.setByte0(target_value); - vcu_motor2_cmd.setByte1(target_value); - vcu_motor2_cmd.setByte2(target_value); - vcu_motor2_cmd.setByte3(target_value); - vcu_motor2_cmd.setByte4(target_value); - vcu_motor2_cmd.setByte5(target_value); - vcu_motor2_cmd.setByte6(target_value); - vcu_motor2_cmd.setByte7(target_value); - canctl.sendFrame(vcu_motor2_cmd.toFrame()); - // std::cout << "vcuTimerTask2" << std::endl; + // ===== 控制0x212报文 (电机M8和LED输出) ===== + vcu_motor2_cmd.setByte0(target_value); + vcu_motor2_cmd.setByte1(target_value); + vcu_motor2_cmd.setByte2(target_value); + vcu_motor2_cmd.setByte3(target_value); + vcu_motor2_cmd.setByte4(target_value); + vcu_motor2_cmd.setByte5(target_value); + vcu_motor2_cmd.setByte6(target_value); + vcu_motor2_cmd.setByte7(target_value); + canctl.sendFrame(vcu_motor2_cmd.toFrame()); + // LOG_INFO("vcuTimerTask2"); - // RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), - // "[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated"); + // LOG_INFO("[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated"); - // 更新状态记录 - last_sweep_state = msg.sweep; - } + // 更新状态记录 + last_sweep_state = msg.sweep; + } } // 定时器回调：BMS 查询任务 -void bmsTimerTask(CANDriver &canctl) +void bmsTimerTask(CANDriver& canctl) { - static bool bms_initialized = false; + static bool bms_initialized = false; - // 首次运行时初始化日志 - if (!bms_initialized) - { - RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[BMS] Query task started"); - bms_initialized = true; - } + // 首次运行时初始化日志 + if (!bms_initialized) + { + LOG_INFO("[BMS] Query task started"); + bms_initialized = true; + } - // 周期性发送 - canctl.sendFrame(bms_query_0x100.toFrame()); - canctl.sendFrame(bms_query_0x101.toFrame()); + // 周期性发送 + canctl.sendFrame(bms_query_0x100.toFrame()); + canctl.sendFrame(bms_query_0x101.toFrame()); } // 注册所有定时器任务 -void setupTimers(rclcpp::Node::SharedPtr node, CANDriver &canctl) +void setupTimers(rclcpp::Node::SharedPtr node, CANDriver& canctl) { - static auto timer_mcu = node->create_wall_timer( - std::chrono::milliseconds(50), - [&canctl]() - { mcuTimerTask(canctl); }); + static auto timer_mcu = + node->create_wall_timer(std::chrono::milliseconds(50), [&canctl]() { mcuTimerTask(canctl); }); - static auto timer_eps = node->create_wall_timer( - std::chrono::milliseconds(50), - [&canctl]() - { epsTimerTask(canctl); }); + static auto timer_eps = + node->create_wall_timer(std::chrono::milliseconds(50), [&canctl]() { epsTimerTask(canctl); }); - static auto timer_vcu = node->create_wall_timer( - std::chrono::milliseconds(100), - [&canctl]() - { vcuTimerTask(canctl); }); + static auto timer_vcu = + node->create_wall_timer(std::chrono::milliseconds(100), [&canctl]() { vcuTimerTask(canctl); }); - static auto timer_bms = node->create_wall_timer( - std::chrono::milliseconds(200), - [&canctl]() - { bmsTimerTask(canctl); }); + static auto timer_bms = + node->create_wall_timer(std::chrono::milliseconds(200), [&canctl]() { bmsTimerTask(canctl); }); - RCLCPP_INFO(node->get_logger(), "[TIMER] All timers setup completed"); + LOG_INFO("[TIMER] All timers setup completed"); } \ No newline at end of file diff --git a/src/base/vehicle_params/CMakeLists.txt b/src/base/vehicle_params/CMakeLists.txt index bf1ae33..06da073 100644 --- a/src/base/vehicle_params/CMakeLists.txt +++ b/src/base/vehicle_params/CMakeLists.txt @@ -13,40 +13,30 @@ endif() # ========================= find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(sweeper_interfaces REQUIRED) # ⭐ 新增 +find_package(sweeper_interfaces REQUIRED) +find_package(logger REQUIRED) # ========================= # include 路径 # ========================= -include_directories( - include -) +include_directories(include) # ========================= # 可执行文件 # ========================= -add_executable(vehicle_params_node - src/vehicle_params_node.cpp -) +add_executable(vehicle_params_node src/vehicle_params_node.cpp) # ========================= # 链接依赖 # ========================= -ament_target_dependencies(vehicle_params_node - rclcpp - sweeper_interfaces # ⭐ 新增 -) +ament_target_dependencies(vehicle_params_node rclcpp sweeper_interfaces logger) # ========================= # 安装 # ========================= -install(TARGETS vehicle_params_node - DESTINATION lib/${PROJECT_NAME} -) +install(TARGETS vehicle_params_node DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY include/ - DESTINATION include/ -) +install(DIRECTORY include/ DESTINATION include/) # ========================= # 导出 diff --git a/src/base/vehicle_params/package.xml b/src/base/vehicle_params/package.xml index fdce56f..bc972a8 100644 --- a/src/base/vehicle_params/package.xml +++ b/src/base/vehicle_params/package.xml @@ -6,10 +6,8 @@ vehicle_params 0.1.0 - - Central vehicle identity provider node. - Fetches IMEI / VID from B-side service and publishes them as ROS2 messages. - + Central vehicle identity provider node. Fetches IMEI / VID from B-side service and + publishes them as ROS2 messages. nvidia Apache-2.0 @@ -19,7 +17,8 @@ rclcpp - sweeper_interfaces + sweeper_interfaces + logger ament_lint_auto @@ -28,4 +27,4 @@ ament_cmake - + \ No newline at end of file diff --git a/src/base/vehicle_params/src/vehicle_params_node.cpp b/src/base/vehicle_params/src/vehicle_params_node.cpp index 2c134c7..be715fc 100644 --- a/src/base/vehicle_params/src/vehicle_params_node.cpp +++ b/src/base/vehicle_params/src/vehicle_params_node.cpp @@ -1,46 +1,42 @@ -#include -#include - #include #include +#include +#include #include #include -#include "vehicle_params/httplib.h" +#include "logger/logger.h" #include "sweeper_interfaces/msg/vehicle_identity.hpp" +#include "vehicle_params/httplib.h" using sweeper_interfaces::msg::VehicleIdentity; class VehicleParamsNode : public rclcpp::Node { -public: - VehicleParamsNode() - : Node("vehicle_params_node") + public: + VehicleParamsNode() : Node("vehicle_params_node") { - RCLCPP_INFO(this->get_logger(), "vehicle_params_node starting..."); + LOG_INFO("vehicle_params_node starting..."); // ---------- neardi ---------- this->declare_parameter("neardi.host", "192.168.5.1"); this->declare_parameter("neardi.port", 8080); // ---------- publisher ---------- - identity_pub_ = this->create_publisher( - "/vehicle/identity", - rclcpp::QoS(1).transient_local().reliable()); + identity_pub_ = + this->create_publisher("/vehicle/identity", rclcpp::QoS(1).transient_local().reliable()); // ---------- worker ---------- - worker_ = std::thread([this]() - { fetch_from_neardi_loop(); }); + worker_ = std::thread([this]() { fetch_from_neardi_loop(); }); } ~VehicleParamsNode() { running_.store(false); - if (worker_.joinable()) - worker_.join(); + if (worker_.joinable()) worker_.join(); } -private: + private: std::atomic running_{true}; std::thread worker_; rclcpp::Publisher::SharedPtr identity_pub_; @@ -58,15 +54,13 @@ private: while (rclcpp::ok() && running_.load()) { - RCLCPP_INFO(this->get_logger(), - "Requesting vehicle identity from neardi (%s:%d)...", - host.c_str(), port); + LOG_INFO("Requesting vehicle identity from neardi (%s:%d)...", host.c_str(), port); auto res = cli.Post("/api/v1/device/register"); if (!res || res->status != 200) { - RCLCPP_WARN(this->get_logger(), "neardi request failed, retrying..."); + LOG_WARN("neardi request failed, retrying..."); retry_sleep(); continue; } @@ -97,11 +91,9 @@ private: identity_pub_->publish(msg); - RCLCPP_INFO(this->get_logger(), - "Vehicle identity published: IMEI=%s, VID=%s", - imei.c_str(), vid.c_str()); + LOG_INFO("Vehicle identity published: IMEI=%s, VID=%s", imei.c_str(), vid.c_str()); - return; // 成功一次即可 + return; // 成功一次即可 } catch (...) { @@ -110,16 +102,19 @@ private: } } - void retry_sleep() - { - std::this_thread::sleep_for(std::chrono::seconds(2)); - } + void retry_sleep() { std::this_thread::sleep_for(std::chrono::seconds(2)); } }; -int main(int argc, char **argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); + /*初始化日志系统*/ + logger::Logger::Init("vehicle_params", "./log"); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); + + /*关闭日志系统*/ + logger::Logger::Shutdown(); return 0; } diff --git a/src/communication/mqtt_report/CMakeLists.txt b/src/communication/mqtt_report/CMakeLists.txt index 5f242cd..5a49801 100644 --- a/src/communication/mqtt_report/CMakeLists.txt +++ b/src/communication/mqtt_report/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(sweeper_interfaces REQUIRED) find_package(radio_ctrl REQUIRED) find_package(std_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(logger REQUIRED) include_directories(include ${catkin_INCLUDE_DIRS}) @@ -44,7 +45,8 @@ ament_target_dependencies( ament_index_cpp rclcpp sweeper_interfaces - std_msgs) + std_msgs + logger) # 包含 radio_ctrl 头文件 target_include_directories(mqtt_report_node PRIVATE ${radio_ctrl_INCLUDE_DIRS}) diff --git a/src/communication/mqtt_report/include/mqtt_report/fault_codes.h b/src/communication/mqtt_report/include/mqtt_report/fault_codes.h index 86b39c1..407d40a 100644 --- a/src/communication/mqtt_report/include/mqtt_report/fault_codes.h +++ b/src/communication/mqtt_report/include/mqtt_report/fault_codes.h @@ -27,12 +27,11 @@ class ErrorCodeSet // 打印所有当前错误码 void printErrors() const { - // std::cout << "Current error codes: "; - // for (int code : errors) - // { - // std::cout << code << " "; + // LOG_INFO("Current error codes: "); + // for (int code : getAllErrorCodes()) { + // LOG_INFO("%d ", code); // } - // std::cout << std::endl; + // LOG_INFO(""); } // 获取所有当前错误码 diff --git a/src/communication/mqtt_report/package.xml b/src/communication/mqtt_report/package.xml index 16d6f79..70e129d 100644 --- a/src/communication/mqtt_report/package.xml +++ b/src/communication/mqtt_report/package.xml @@ -11,6 +11,7 @@ sweeper_interfaces radio_ctrl ament_index_cpp + logger ament_cmake @@ -20,4 +21,4 @@ ament_cmake - + \ No newline at end of file diff --git a/src/communication/mqtt_report/src/can/can_decoder.cpp b/src/communication/mqtt_report/src/can/can_decoder.cpp index 5a85bd5..0c07211 100644 --- a/src/communication/mqtt_report/src/can/can_decoder.cpp +++ b/src/communication/mqtt_report/src/can/can_decoder.cpp @@ -5,6 +5,7 @@ #include #include +#include "logger/logger.h" #include "mqtt_report/fault_codes.h" // ===================== ctor ===================== @@ -49,11 +50,8 @@ void CanDecoder::handle_bms_100(const sweeperMsg::CanFrame& msg) ctx_.info.chargeStatus = (current >= 0); } - std::cout << std::fixed << std::setprecision(2); - std::cout << "[0x100] 总电压: " << voltage << " V, " - << "电流: " << current << " A, " - << "剩余容量: " << capacity << " Ah, " - << "是否在充电: " << ((current >= 0) ? "是" : "否") << std::endl; + LOG_INFO("[0x100] 总电压: %.2f V, 电流: %.2f A, 剩余容量: %.2f Ah, 是否在充电: %s", voltage, current, capacity, + ((current >= 0) ? "是" : "否")); } void CanDecoder::handle_bms_101(const sweeperMsg::CanFrame& msg) @@ -71,10 +69,8 @@ void CanDecoder::handle_bms_101(const sweeperMsg::CanFrame& msg) ctx_.info.power = rsoc; } - std::cout << std::fixed << std::setprecision(2); - std::cout << "[0x101] 充满容量: " << full_capacity << " Ah, " - << "循环次数: " << cycle_count << " 次, " - << "剩余容量百分比(RSOC): " << rsoc << " %" << std::endl; + LOG_INFO("[0x101] 充满容量: %.2f Ah, 循环次数: %d 次, 剩余容量百分比(RSOC): %.1f %", full_capacity, cycle_count, + rsoc); } // ===================== MCU ===================== diff --git a/src/communication/mqtt_report/src/core/input_health_monitor.cpp b/src/communication/mqtt_report/src/core/input_health_monitor.cpp index 4f946ab..5894056 100644 --- a/src/communication/mqtt_report/src/core/input_health_monitor.cpp +++ b/src/communication/mqtt_report/src/core/input_health_monitor.cpp @@ -2,6 +2,8 @@ #include +#include "logger/logger.h" + static constexpr int64_t WARN_INTERVAL_MS = 10 * 1000; static constexpr int64_t IDENTITY_TIMEOUT_MS = 15 * 1000; static constexpr int64_t GPS_TIMEOUT_MS = 5 * 1000; @@ -31,7 +33,7 @@ void InputHealthMonitor::check(rclcpp::Logger logger, int64_t now, bool mqtt_con { if (!mqtt_connected) { - RCLCPP_WARN(logger, "MQTT connection lost."); + LOG_WARN("MQTT connection lost."); } check_input("vehicle identity (/vehicle/identity)", identity_, now, IDENTITY_TIMEOUT_MS); @@ -46,15 +48,14 @@ void InputHealthMonitor::check_input(const char* name, InputMonitor& mon, int64_ if (mon.ok && now - mon.last_rx_ts > timeout_ms) { mon.ok = false; - RCLCPP_WARN(rclcpp::get_logger("input_monitor"), "%s timeout (%ld ms without data).", name, - now - mon.last_rx_ts); + LOG_WARN("%s timeout (%ld ms without data).", name, now - mon.last_rx_ts); mon.last_warn_ts = now; return; } if (!mon.ok && now - mon.last_warn_ts > WARN_INTERVAL_MS) { - RCLCPP_WARN(rclcpp::get_logger("input_monitor"), "Waiting for %s...", name); + LOG_WARN("Waiting for %s...", name); mon.last_warn_ts = now; } } diff --git a/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp b/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp index e1dbd49..aac66db 100644 --- a/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp +++ b/src/communication/mqtt_report/src/node/mqtt_vehicle_node.cpp @@ -5,6 +5,7 @@ #include #include +#include "logger/logger.h" #include "mqtt_report/can/can_decoder.h" #include "mqtt_report/core/input_health_monitor.h" #include "mqtt_report/core/vehicle_context.h" @@ -75,7 +76,7 @@ class VehicleReportNode : public rclcpp::Node ctx_.vid = msg->vid; ctx_.ready = msg->ready; - RCLCPP_INFO(get_logger(), "Vehicle identity ready: IMEI=%s, VID=%s", msg->imei.c_str(), msg->vid.c_str()); + LOG_INFO("Vehicle identity ready: IMEI=%s, VID=%s", msg->imei.c_str(), msg->vid.c_str()); int64_t now = getCurrentTimestampMs(); @@ -168,28 +169,31 @@ int main(int argc, char** argv) } rclcpp::init(argc, argv); + /*初始化日志系统*/ + logger::Logger::Init("mqtt_report", "./log"); Config config; - auto logger = rclcpp::get_logger("main"); - if (!load_config(config_path, config)) { - RCLCPP_ERROR(logger, "Failed to load config: %s", config_path.c_str()); + LOG_ERROR("Failed to load config: %s", config_path.c_str()); + logger::Logger::Shutdown(); return 1; } if (mqtt_reconnect_interval <= 0) mqtt_reconnect_interval = 5000; - RCLCPP_INFO(logger, "Config loaded: broker=%s:%d, user=%s", config.mqtt_ip.c_str(), config.mqtt_port, - config.mqtt_username.empty() ? "" : config.mqtt_username.c_str()); + LOG_INFO("Config loaded: broker=%s:%d, user=%s", config.mqtt_ip.c_str(), config.mqtt_port, + config.mqtt_username.empty() ? "" : config.mqtt_username.c_str()); - RCLCPP_INFO(logger, "Topic templates: gps='%s', info='%s', fault='%s'", config.gps_topic_template.c_str(), - config.info_topic_template.c_str(), config.fault_topic_template.c_str()); + LOG_INFO("Topic templates: gps='%s', info='%s', fault='%s'", config.gps_topic_template.c_str(), + config.info_topic_template.c_str(), config.fault_topic_template.c_str()); auto node = std::make_shared(config); rclcpp::spin(node); rclcpp::shutdown(); + /*关闭日志系统*/ + logger::Logger::Shutdown(); return 0; } diff --git a/src/communication/sub/CMakeLists.txt b/src/communication/sub/CMakeLists.txt index 08b6b9d..76f3584 100644 --- a/src/communication/sub/CMakeLists.txt +++ b/src/communication/sub/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sweeper_interfaces REQUIRED) find_package(CURL REQUIRED) +find_package(logger REQUIRED) add_executable( sub_node @@ -43,7 +44,8 @@ ament_target_dependencies( rclcpp std_msgs sweeper_interfaces - CURL) + CURL + logger) if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64) target_link_libraries(sub_node ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c-static.a) diff --git a/src/communication/sub/package.xml b/src/communication/sub/package.xml index 7e92f33..1ba39a5 100644 --- a/src/communication/sub/package.xml +++ b/src/communication/sub/package.xml @@ -12,6 +12,7 @@ rclcpp std_msgs sweeper_interfaces + logger ament_lint_auto ament_lint_common diff --git a/src/communication/sub/src/main.cpp b/src/communication/sub/src/main.cpp index 29f0ef8..d207a5b 100644 --- a/src/communication/sub/src/main.cpp +++ b/src/communication/sub/src/main.cpp @@ -3,6 +3,7 @@ #include #include +#include "logger/logger.h" #include "sub/control_state.hpp" #include "sub/json.h" #include "sub/mqtt_receiver.hpp" @@ -66,9 +67,6 @@ int main(int argc, char** argv) return 1; } - std::cout << "MQTT address: " << cfg.address << "\n"; - std::cout << "MQTT topic template: " << cfg.remote_topic_template << "\n"; - // 2) shared state ControlState state; { @@ -82,10 +80,19 @@ int main(int argc, char** argv) // 4) ROS2 spin rclcpp::init(argc, argv); + /*初始化日志系统*/ + logger::Logger::Init("sub", "./log"); + + LOG_INFO("MQTT address: %s", cfg.address.c_str()); + LOG_INFO("MQTT topic template: %s", cfg.remote_topic_template.c_str()); + auto node = std::make_shared(state, "sub_node"); rclcpp::spin(node); rclcpp::shutdown(); + /*关闭日志系统*/ + logger::Logger::Shutdown(); + // 5) stop mqtt mqtt.stop(); return 0; diff --git a/src/communication/sub/src/mqtt_receiver.cpp b/src/communication/sub/src/mqtt_receiver.cpp index 2ca2e50..1a16c62 100644 --- a/src/communication/sub/src/mqtt_receiver.cpp +++ b/src/communication/sub/src/mqtt_receiver.cpp @@ -9,6 +9,7 @@ #include #include +#include "logger/logger.h" #include "sub/json.h" namespace sub_node_pkg @@ -287,7 +288,7 @@ void MqttReceiver::runLoop() } else { - std::cout << "[MQTT] Connected to broker\n"; + LOG_INFO("[MQTT] Connected to broker"); } std::string topic = get_sub_topic(); @@ -306,7 +307,7 @@ void MqttReceiver::runLoop() } state_.mqtt_connected = true; - std::cout << "[MQTT] Connected & subscribed: " << topic << "\n"; + LOG_INFO("[MQTT] Connected & subscribed: %s", topic.c_str()); return true; }; @@ -333,7 +334,7 @@ void MqttReceiver::runLoop() // 2) identity 从 false->true（或 vid 变化）时，主动订阅一次 if (ready && (!last_ready || topic != last_topic)) { - std::cout << "[MQTT] Identity ready/topic changed, subscribing: " << topic << "\n"; + LOG_INFO("[MQTT] Identity ready/topic changed, subscribing: %s", topic.c_str()); // 已连接就直接 subscribe；未连接则走 do_connect_and_sub if (MQTTClient_isConnected(client_) != 0) @@ -342,7 +343,7 @@ void MqttReceiver::runLoop() if (rc_sub == MQTTCLIENT_SUCCESS) { state_.mqtt_connected = true; - std::cout << "[MQTT] Subscribed: " << topic << "\n"; + LOG_INFO("[MQTT] Subscribed: %s", topic.c_str()); last_topic = topic; } else diff --git a/src/communication/sub/src/sub_node.cpp b/src/communication/sub/src/sub_node.cpp index b1bc3c1..cf272a5 100644 --- a/src/communication/sub/src/sub_node.cpp +++ b/src/communication/sub/src/sub_node.cpp @@ -2,6 +2,7 @@ #include +#include "logger/logger.h" #include "sub/control_mapper.hpp" namespace sub_node_pkg @@ -9,7 +10,7 @@ namespace sub_node_pkg SubNode::SubNode(ControlState& state, const std::string& name) : Node(name), state_(state) { - RCLCPP_INFO(this->get_logger(), "%s node started.", name.c_str()); + LOG_INFO("%s node started.", name.c_str()); pub_mc_ = this->create_publisher("remote_mc_ctrl", 10); pub_gather_ = this->create_publisher("gather", 10); @@ -70,7 +71,7 @@ void SubNode::identityCallback(const sweeper_interfaces::msg::VehicleIdentity::S state_.vid = msg->vid; state_.identity_ready = msg->ready; - RCLCPP_INFO(get_logger(), "Identity: VID=%s ready=%d", msg->vid.c_str(), msg->ready); + LOG_INFO("Identity: VID=%s ready=%d", msg->vid.c_str(), msg->ready); } } // namespace sub_node_pkg diff --git a/src/communication/task_manager/CMakeLists.txt b/src/communication/task_manager/CMakeLists.txt index b960838..03db341 100644 --- a/src/communication/task_manager/CMakeLists.txt +++ b/src/communication/task_manager/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sweeper_interfaces REQUIRED) find_package(CURL REQUIRED) +find_package(logger REQUIRED) include_directories(include/task_manager include/paho_mqtt_3c ${catkin_INCLUDE_DIRS}) @@ -41,7 +42,8 @@ ament_target_dependencies( rclcpp std_msgs sweeper_interfaces - CURL) + CURL + logger) if(CMAKE_SYSTEM_PROCESSOR MATCHES aarch64) target_link_libraries(task_manager_node ${PROJECT_SOURCE_DIR}/lib/libpaho-mqtt3c-static.a) diff --git a/src/communication/task_manager/package.xml b/src/communication/task_manager/package.xml index a7e2c48..0217770 100644 --- a/src/communication/task_manager/package.xml +++ b/src/communication/task_manager/package.xml @@ -12,6 +12,7 @@ rclcpp std_msgs sweeper_interfaces + logger ament_lint_auto ament_lint_common diff --git a/src/communication/task_manager/src/mqtt_manager.cpp b/src/communication/task_manager/src/mqtt_manager.cpp index 2bbe002..9099e14 100644 --- a/src/communication/task_manager/src/mqtt_manager.cpp +++ b/src/communication/task_manager/src/mqtt_manager.cpp @@ -4,6 +4,8 @@ #include #include +#include "logger/logger.h" + MQTTManager::MQTTManager() : client_(nullptr), is_running_(false), @@ -58,7 +60,7 @@ bool MQTTManager::reconnect() { std::lock_guard lock(client_mutex_); - std::cout << "Attempting to reconnect to MQTT server..." << std::endl; + LOG_INFO("Attempting to reconnect to MQTT server."); int rc = MQTTClient_connect(client_, &conn_opts_); if (rc != MQTTCLIENT_SUCCESS) @@ -74,7 +76,7 @@ bool MQTTManager::reconnect() return false; } - std::cout << "Successfully reconnected to MQTT server." << std::endl; + LOG_INFO("Successfully reconnected to MQTT server."); reconnect_attempts_ = 0; is_heartbeat_lost_ = false; last_message_time_ = std::chrono::steady_clock::now(); @@ -105,7 +107,7 @@ bool MQTTManager::subscribe(const string& topic, int qos) return false; } - std::cout << "Successfully subscribed to topic: " << topic << std::endl; + LOG_INFO("Successfully subscribed to topic: %s", topic.c_str()); return true; } @@ -148,7 +150,7 @@ void MQTTManager::start() { if (is_running_) { - std::cout << "MQTT manager is already running." << std::endl; + LOG_INFO("MQTT manager is already running."); return; } @@ -161,7 +163,7 @@ void MQTTManager::start() is_running_ = true; mqtt_thread_ = std::thread(&MQTTManager::mqttLoop, this); - std::cout << "MQTT manager started." << std::endl; + LOG_INFO("MQTT manager started."); } void MQTTManager::stop() @@ -182,7 +184,7 @@ void MQTTManager::stop() } MQTTClient_destroy(&client_); - std::cout << "MQTT manager stopped." << std::endl; + LOG_INFO("MQTT manager stopped."); } void MQTTManager::mqttLoop() @@ -215,7 +217,7 @@ void MQTTManager::mqttLoop() if (duration > heartbeat_timeout_ && !is_heartbeat_lost_) { is_heartbeat_lost_ = true; - std::cout << "Heartbeat timeout: No message received in " << heartbeat_timeout_ << "ms." << std::endl; + LOG_WARN("Heartbeat timeout: No message received in %dms.", heartbeat_timeout_); if (conn_lost_callback_) { conn_lost_callback_((char*)"Heartbeat timeout"); @@ -235,7 +237,7 @@ int MQTTManager::onMessageArrived(void* context, char* topicName, int topicLen, pThis->last_message_time_ = std::chrono::steady_clock::now(); pThis->is_heartbeat_lost_ = false; - std::cout << "Message arrived on topic: " << topicName << std::endl; + LOG_INFO("Message arrived on topic: %s", topicName); if (pThis->message_callback_) { @@ -248,7 +250,7 @@ int MQTTManager::onMessageArrived(void* context, char* topicName, int topicLen, void MQTTManager::onConnectionLost(void* context, char* cause) { MQTTManager* pThis = static_cast(context); - std::cout << "Connection lost. Cause: " << (cause ? cause : "Unknown") << std::endl; + LOG_WARN("Connection lost. Cause: %s", (cause ? cause : "Unknown")); if (pThis->conn_lost_callback_) { @@ -259,7 +261,7 @@ void MQTTManager::onConnectionLost(void* context, char* cause) void MQTTManager::onDelivered(void* context, MQTTClient_deliveryToken dt) { MQTTManager* pThis = static_cast(context); - std::cout << "Message with token " << dt << " delivered." << std::endl; + LOG_INFO("Message with token %d delivered.", dt); if (pThis->delivered_callback_) { diff --git a/src/communication/task_manager/src/task_manager.cpp b/src/communication/task_manager/src/task_manager.cpp index ae127c5..4830816 100644 --- a/src/communication/task_manager/src/task_manager.cpp +++ b/src/communication/task_manager/src/task_manager.cpp @@ -1,40 +1,38 @@ #include "task_manager.hpp" -#include "md5.h" -#include -#include -#include + #include -TaskManager::TaskManager() - : is_running_(false), task_status_(0), - destination_file_path_("./gps_load_now.txt") +#include +#include +#include + +#include "logger/logger.h" +#include "md5.h" + +TaskManager::TaskManager() : is_running_(false), task_status_(0), destination_file_path_("./gps_load_now.txt") { current_task_.id = 0; current_task_.status = TaskStatus::PENDING; } -TaskManager::~TaskManager() -{ - stop(); -} +TaskManager::~TaskManager() { stop(); } void TaskManager::start() { if (is_running_) { - std::cout << "TaskManager is already running." << std::endl; + LOG_WARN("TaskManager is already running."); return; } is_running_ = true; task_thread_ = std::thread(&TaskManager::processTasksLoop, this); - std::cout << "TaskManager started." << std::endl; + LOG_INFO("TaskManager started."); } void TaskManager::stop() { - if (!is_running_) - return; + if (!is_running_) return; { std::lock_guard lock(queue_mutex_); @@ -47,10 +45,10 @@ void TaskManager::stop() task_thread_.join(); } - std::cout << "TaskManager stopped." << std::endl; + LOG_INFO("TaskManager stopped."); } -void TaskManager::addTask(const MQTT_JSON &mqtt_json) +void TaskManager::addTask(const MQTT_JSON& mqtt_json) { { std::lock_guard lock(queue_mutex_); @@ -83,39 +81,38 @@ void TaskManager::updateCurrentTask(int taskId, TaskStatus status) current_task_.id = taskId; current_task_.status = status; current_task_.lastUpdateTime = std::chrono::steady_clock::now(); - std::cout << "Task updated: ID=" << taskId << ", Status=" << status << std::endl; + LOG_INFO("Task updated: ID=%d, Status=%d", taskId, status); } -string TaskManager::calculate_md5(const string &filename) +string TaskManager::calculate_md5(const string& filename) { MD5 md5; std::ifstream file; file.open(filename, std::ios::binary); if (!file) { - std::cerr << "Failed to open file for MD5 calculation: " << filename << std::endl; + LOG_ERROR("Failed to open file for MD5 calculation: %s", filename.c_str()); return ""; } md5.update(file); return md5.toString(); } -bool TaskManager::downloadFile(const string &url, const string &expected_md5, - const string &filename) +bool TaskManager::downloadFile(const string& url, const string& expected_md5, const string& filename) { if (url.empty()) { - std::cerr << "Download URL is empty." << std::endl; + LOG_ERROR("Download URL is empty."); return false; } std::string command = "wget -O routes/" + filename + " \"" + url + "\""; - std::cout << "Executing command: " << command << std::endl; + LOG_INFO("Executing command: %s", command.c_str()); int result = system(command.c_str()); if (result != 0) { - std::cerr << "Download failed: " << url << std::endl; + LOG_ERROR("Download failed: %s", url.c_str()); return false; } @@ -124,31 +121,29 @@ bool TaskManager::downloadFile(const string &url, const string &expected_md5, std::string actual_md5 = calculate_md5(filepath); if (actual_md5 != expected_md5) { - std::cerr << "MD5 verification failed. Expected: " << expected_md5 - << ", Actual: " << actual_md5 << std::endl; + LOG_ERROR("MD5 verification failed. Expected: %s, Actual: %s", expected_md5.c_str(), actual_md5.c_str()); remove(filepath.c_str()); return false; } - std::cout << "File downloaded and verified: " << filepath << std::endl; + LOG_INFO("File downloaded and verified: %s", filepath.c_str()); return true; } -void TaskManager::copyFileAndOverwrite(const string &sourceFilePath, - const string &destinationFilePath) +void TaskManager::copyFileAndOverwrite(const string& sourceFilePath, const string& destinationFilePath) { std::ifstream src(sourceFilePath, std::ios::binary); std::ofstream dst(destinationFilePath, std::ios::binary); if (!src) { - std::cerr << "Failed to open source file: " << sourceFilePath << std::endl; + LOG_ERROR("Failed to open source file: %s", sourceFilePath.c_str()); return; } if (!dst) { - std::cerr << "Failed to open destination file: " << destinationFilePath << std::endl; + LOG_ERROR("Failed to open destination file: %s", destinationFilePath.c_str()); return; } @@ -156,15 +151,15 @@ void TaskManager::copyFileAndOverwrite(const string &sourceFilePath, if (!dst) { - std::cerr << "Error while copying file." << std::endl; + LOG_ERROR("Error while copying file."); } } -bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo) +bool TaskManager::processStartTask(const JSON_DATA& json_data, long seqNo) { - std::cout << "Processing start task ID: " << json_data.value.id << std::endl; - std::cout << "URL: " << json_data.value.routeInfo.url << std::endl; - std::cout << "MD5: " << json_data.value.routeInfo.md5 << std::endl; + LOG_INFO("Processing start task ID: %d", json_data.value.id); + LOG_INFO("URL: %s", json_data.value.routeInfo.url.c_str()); + LOG_INFO("MD5: %s", json_data.value.routeInfo.md5.c_str()); // 更新当前任务ID updateCurrentTask(json_data.value.id, TaskStatus::RUNNING); @@ -175,11 +170,10 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo) // 检查文件是否已存在 if (access(filepath.c_str(), F_OK) == -1) { - std::cout << "File not found, downloading: " << downFileName << std::endl; - if (!downloadFile(json_data.value.routeInfo.url, - json_data.value.routeInfo.md5, downFileName)) + LOG_INFO("File not found, downloading: %s", downFileName.c_str()); + if (!downloadFile(json_data.value.routeInfo.url, json_data.value.routeInfo.md5, downFileName)) { - std::cout << "Download failed, skipping task." << std::endl; + LOG_WARN("Download failed, skipping task."); updateCurrentTask(json_data.value.id, TaskStatus::FAILED); if (send_response_callback_) { @@ -191,7 +185,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo) } else { - std::cout << "File already exists, skipping download: " << filepath << std::endl; + LOG_INFO("File already exists, skipping download: %s", filepath.c_str()); } // 复制文件 @@ -200,7 +194,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo) // 启动任务 task_status_ = 1; updateCurrentTask(json_data.value.id, TaskStatus::RUNNING); - std::cout << "Task started successfully: " << json_data.value.id << std::endl; + LOG_INFO("Task started successfully: %d", json_data.value.id); // 发送响应（响应主题由调用方决定） if (send_response_callback_) @@ -213,7 +207,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo) bool TaskManager::processStopTask(long taskId, long seqNo) { - std::cout << "Processing stop task ID: " << taskId << std::endl; + LOG_INFO("Processing stop task ID: %d", taskId); task_status_ = 0; updateCurrentTask(taskId, TaskStatus::COMPLETED); @@ -232,8 +226,7 @@ void TaskManager::processTasksLoop() std::unique_lock lock(queue_mutex_); // 等待队列中有任务或线程需要停止 - queue_cv_.wait(lock, [this] - { return !task_queue_.empty() || !is_running_; }); + queue_cv_.wait(lock, [this] { return !task_queue_.empty() || !is_running_; }); // 检查是否需要退出 if (!is_running_ && task_queue_.empty()) @@ -263,9 +256,9 @@ void TaskManager::processTasksLoop() processStopTask(taskId, mqtt_json.seqNo); } } - catch (const boost::bad_any_cast &e) + catch (const boost::bad_any_cast& e) { - std::cerr << "Bad any_cast in task processing: " << e.what() << std::endl; + LOG_ERROR("Bad any_cast in task processing: %s", e.what()); } } } diff --git a/src/communication/task_manager/src/task_manager_main.cpp b/src/communication/task_manager/src/task_manager_main.cpp index 17a7fda..e26e776 100644 --- a/src/communication/task_manager/src/task_manager_main.cpp +++ b/src/communication/task_manager/src/task_manager_main.cpp @@ -6,6 +6,7 @@ #include #include "json.h" +#include "logger/logger.h" #include "mqtt_manager.hpp" #include "rclcpp/rclcpp.hpp" #include "sweeper_interfaces/msg/task.hpp" @@ -52,12 +53,12 @@ string generate_mqtt_client_id(); // 信号处理函数 void signalHandler(int signum) { - std::cout << "Interrupt signal (" << signum << ") received." << std::endl; + LOG_INFO("Interrupt signal (%d) received.", signum); signal_received = signum; if (signum == SIGINT && rclcpp::ok()) { - std::cout << "Shutting down ROS2 node..." << std::endl; + LOG_INFO("Shutting down ROS2 node."); rclcpp::shutdown(); } } @@ -87,26 +88,26 @@ ROUTE_INFO get_route_info(const Json::Value& root) if (root.isMember("routeName") && root["routeName"].isString()) route_info.routeName = root["routeName"].asString(); else - std::cerr << "Missing or invalid 'routeName' field" << std::endl; + LOG_ERROR("Missing or invalid 'routeName' field"); if (root.isMember("fileName") && root["fileName"].isString()) route_info.fileName = root["fileName"].asString(); else - std::cerr << "Missing or invalid 'fileName' field" << std::endl; + LOG_ERROR("Missing or invalid 'fileName' field"); if (root.isMember("url") && root["url"].isString()) route_info.url = root["url"].asString(); else - std::cerr << "Missing or invalid 'url' field" << std::endl; + LOG_ERROR("Missing or invalid 'url' field"); if (root.isMember("md5") && root["md5"].isString()) route_info.md5 = root["md5"].asString(); else - std::cerr << "Missing or invalid 'md5' field" << std::endl; + LOG_ERROR("Missing or invalid 'md5' field"); } else { - std::cerr << "routeInfo is not a valid JSON object" << std::endl; + LOG_ERROR("routeInfo is not a valid JSON object"); } return route_info; @@ -122,31 +123,31 @@ TASK_VALUE get_task_value(const Json::Value& root) if (root.isMember("id") && root["id"].isInt()) task_value.id = root["id"].asInt(); else - std::cerr << "Missing or invalid 'id' field" << std::endl; + LOG_ERROR("Missing or invalid 'id' field"); if (root.isMember("name") && root["name"].isString()) task_value.name = root["name"].asString(); else - std::cerr << "Missing or invalid 'name' field" << std::endl; + LOG_ERROR("Missing or invalid 'name' field"); if (root.isMember("mode") && root["mode"].isInt()) task_value.mode = root["mode"].asInt(); else - std::cerr << "Missing or invalid 'mode' field" << std::endl; + LOG_ERROR("Missing or invalid 'mode' field"); if (root.isMember("count") && root["count"].isInt()) task_value.count = root["count"].asInt(); else - std::cerr << "Missing or invalid 'count' field" << std::endl; + LOG_ERROR("Missing or invalid 'count' field"); if (root.isMember("routeInfo") && root["routeInfo"].isObject()) task_value.routeInfo = get_route_info(root["routeInfo"]); else - std::cerr << "Missing or invalid 'routeInfo' field" << std::endl; + LOG_ERROR("Missing or invalid 'routeInfo' field"); } else { - std::cerr << "task value is not a valid JSON object" << std::endl; + LOG_ERROR("task value is not a valid JSON object"); } return task_value; @@ -161,13 +162,13 @@ bool loadConfig(const string& config_file) if (!in.is_open()) { - std::cout << "Failed to read config file: " << config_file << std::endl; + LOG_ERROR("Failed to read config file: %s", config_file.c_str()); return false; } if (!reader.parse(in, root)) { - std::cout << "Failed to parse config file." << std::endl; + LOG_ERROR("Failed to parse config file."); return false; } @@ -182,10 +183,10 @@ bool loadConfig(const string& config_file) in.close(); - std::cout << "Config loaded successfully." << std::endl; - std::cout << "MQTT Address: " << config.mqtt_address << std::endl; - std::cout << "Topic Sub template: " << config.mqtt_topic_sub_task_tmpl << std::endl; - std::cout << "Topic Pub template: " << config.mqtt_topic_push_status_tmpl << std::endl; + LOG_INFO("Config loaded successfully."); + LOG_INFO("MQTT Address: %s", config.mqtt_address.c_str()); + LOG_INFO("Topic Sub template: %s", config.mqtt_topic_sub_task_tmpl.c_str()); + LOG_INFO("Topic Pub template: %s", config.mqtt_topic_push_status_tmpl.c_str()); return true; } @@ -206,11 +207,11 @@ void sendGeneralResponse(const string& topic, long seqNo, int code, const string Json::FastWriter writer; string responseJson = writer.write(responseData); - std::cout << "[General Response] seqNo=" << seqNo << ", code=" << code << ", msg=" << msg << std::endl; - std::cout << "Response JSON: " << responseJson << std::endl; + LOG_INFO("[General Response] seqNo=%d, code=%d, msg=%s", seqNo, code, msg.c_str()); + LOG_DEBUG("Response JSON: %s", responseJson.c_str()); bool success = mqtt_manager.publish(topic, responseJson, 0); - std::cout << "General response publish to [" << topic << "] " << (success ? "[OK]" : "[FAILED]") << std::endl; + LOG_INFO("General response publish to [%s] %s", topic.c_str(), (success ? "[OK]" : "[FAILED]")); } // 发送任务专用应答（包含任务信息） @@ -235,19 +236,18 @@ void sendTaskResponse(long seqNo, int code, const string& msg) Json::FastWriter writer; string responseJson = writer.write(responseData); - std::cout << "[Task Response] seqNo=" << seqNo << ", code=" << code << ", taskId=" << current_task_id - << ", taskStatus=" << (int)current_status << std::endl; - std::cout << "Response JSON: " << responseJson << std::endl; + LOG_INFO("[Task Response] seqNo=%ld, code=%d, taskId=%d, taskStatus=%d", seqNo, code, current_task_id, + (int)current_status); + LOG_DEBUG("Response JSON: %s", responseJson.c_str()); if (mqtt_topic_sub_task.empty()) { - std::cout << "[TASK] response topic not ready, drop response. seqNo=" << seqNo << std::endl; + LOG_WARN("[TASK] response topic not ready, drop response. seqNo=%ld", seqNo); return; } bool success = mqtt_manager.publish(mqtt_topic_sub_task, responseJson, 0); - std::cout << "Task response publish to [" << mqtt_topic_sub_task << "] " << (success ? "[OK]" : "[FAILED]") - << std::endl; + LOG_INFO("Task response publish to [%s] %s", mqtt_topic_sub_task.c_str(), (success ? "[OK]" : "[FAILED]")); } // MQTT消息回调 @@ -265,12 +265,12 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message if (!reader.parse(buffer, root)) { - std::cout << "Failed to parse JSON from MQTT message." << std::endl; + LOG_ERROR("Failed to parse JSON from MQTT message."); delete[] buffer; return 1; } - std::cout << "MQTT message received on topic: " << topicName << std::endl; + LOG_INFO("MQTT message received on topic: %s", topicName); if (root.isMember("type") && root["type"].asString() == "request") { @@ -301,12 +301,12 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message mqtt_json.seqNo = seqNo; mqtt_json.data = json_data; - std::cout << "Start task: " << json_data.value.id << std::endl; + LOG_INFO("Start task: %d", json_data.value.id); task_manager.addTask(mqtt_json); } catch (const std::exception& e) { - std::cerr << "Error processing start command: " << e.what() << std::endl; + LOG_ERROR("Error processing start command: %s", e.what()); sendTaskResponse(seqNo, 400, "Failed to process start command"); } } @@ -321,14 +321,14 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message mqtt_json.seqNo = seqNo; mqtt_json.data = stop_id; - std::cout << "Stop task: " << stop_id << std::endl; + LOG_INFO("Stop task: %d", stop_id); task_manager.addTask(mqtt_json); sendTaskResponse(seqNo, 200, "Task stop command received"); } catch (const std::exception& e) { - std::cerr << "Error processing stop command: " << e.what() << std::endl; + LOG_ERROR("Error processing stop command: %s", e.what()); sendTaskResponse(seqNo, 400, "Failed to process stop command"); } } @@ -344,11 +344,11 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message } else if (root.isMember("type") && root["type"].asString() == "noReply") { - std::cout << "NoReply message received" << std::endl; + LOG_INFO("NoReply message received"); } else { - std::cout << "Unknown message type" << std::endl; + LOG_INFO("Unknown message type"); } delete[] buffer; @@ -364,14 +364,14 @@ void try_subscribe_task_topic() if (subscribed.exchange(true)) return; // 已经订阅过了 - std::cout << "[TASK] subscribe MQTT topic: " << mqtt_topic_sub_task << std::endl; + LOG_INFO("[TASK] subscribe MQTT topic: %s", mqtt_topic_sub_task.c_str()); mqtt_manager.subscribe(mqtt_topic_sub_task, 0); } // 周期性上报任务状态到MQTT（200ms间隔持续上报） void reportTaskStatusToMqtt() { - std::cout << "DEBUG: Status report thread started" << std::endl; + LOG_DEBUG("Status report thread started"); while (rclcpp::ok() && !signal_received) { @@ -396,7 +396,7 @@ void reportTaskStatusToMqtt() } } - std::cout << "DEBUG: Status report thread exiting" << std::endl; + LOG_DEBUG("Status report thread exiting"); } // ROS2节点类 @@ -405,7 +405,7 @@ class TaskManagerNode : public rclcpp::Node public: TaskManagerNode(string name) : Node(name) { - RCLCPP_INFO(this->get_logger(), "Node %s started.", name.c_str()); + LOG_INFO("Node %s started.", name.c_str()); msg_publisher_ = this->create_publisher("task_message/download", 10); task_subscribe_ = this->create_subscription( "task_message/upload", 10, std::bind(&TaskManagerNode::task_callback, this, std::placeholders::_1)); @@ -432,8 +432,8 @@ class TaskManagerNode : public rclcpp::Node pos = mqtt_topic_push_status.find("{vid}"); if (pos != std::string::npos) mqtt_topic_push_status.replace(pos, 5, vid); - RCLCPP_INFO(this->get_logger(), "[TASK] identity ready VID=%s => sub=%s pub=%s", vid.c_str(), - mqtt_topic_sub_task.c_str(), mqtt_topic_push_status.c_str()); + LOG_INFO("[TASK] identity ready VID=%s => sub=%s pub=%s", vid.c_str(), mqtt_topic_sub_task.c_str(), + mqtt_topic_push_status.c_str()); try_subscribe_task_topic(); }); @@ -448,7 +448,7 @@ class TaskManagerNode : public rclcpp::Node if (msg.task_status != last_status) { msg_publisher_->publish(msg); - RCLCPP_INFO(this->get_logger(), "Task status published: %d", msg.task_status); + LOG_INFO("Task status published: %d", msg.task_status); } last_status = msg.task_status; @@ -462,7 +462,7 @@ class TaskManagerNode : public rclcpp::Node } else { - RCLCPP_WARN(this->get_logger(), "Invalid task status: %d", msg->task_status); + LOG_WARN("Invalid task status: %d", msg->task_status); } } @@ -477,6 +477,9 @@ int main(int argc, char** argv) { signal(SIGINT, signalHandler); + // 初始化日志系统 + logger::Logger::Init("task_manager", "./log"); + // 默认配置路径 std::string config_path = "config.json"; @@ -491,11 +494,11 @@ int main(int argc, char** argv) } } - std::cout << "[TASK] Using config: " << config_path << std::endl; + LOG_INFO("[TASK] Using config: %s", config_path.c_str()); if (!loadConfig(config_path)) { - std::cerr << "Failed to load configuration." << std::endl; + LOG_ERROR("Failed to load configuration."); return 1; } @@ -503,7 +506,7 @@ int main(int argc, char** argv) string client_id = generate_mqtt_client_id(); if (!mqtt_manager.init(config.mqtt_address, client_id, config.mqtt_username, config.mqtt_password)) { - std::cerr << "Failed to initialize MQTT manager." << std::endl; + LOG_ERROR("Failed to initialize MQTT manager."); return 1; } @@ -516,7 +519,7 @@ int main(int argc, char** argv) mqtt_manager.setConnRestoredCallback( [](const char* cause) { - std::cout << "[TASK] MQTT reconnected: " << cause << std::endl; + LOG_INFO("[TASK] MQTT reconnected: %s", cause); subscribed.store(false); try_subscribe_task_topic(); }); @@ -530,7 +533,7 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = std::make_shared("task_manager_node"); - std::cout << "Starting status report thread..." << std::endl; + LOG_INFO("Starting status report thread..."); status_report_thread = std::thread(reportTaskStatusToMqtt); rclcpp::executors::SingleThreadedExecutor executor; @@ -542,7 +545,7 @@ int main(int argc, char** argv) std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - std::cout << "Shutting down components..." << std::endl; + LOG_INFO("Shutting down components..."); task_manager.stop(); mqtt_manager.stop(); @@ -551,5 +554,8 @@ int main(int argc, char** argv) status_report_thread.join(); } + // 关闭日志系统 + logger::Logger::Shutdown(); + return 0; } \ No newline at end of file diff --git a/src/control/ctrl_arbiter/CMakeLists.txt b/src/control/ctrl_arbiter/CMakeLists.txt index 64faf2e..34311c5 100644 --- a/src/control/ctrl_arbiter/CMakeLists.txt +++ b/src/control/ctrl_arbiter/CMakeLists.txt @@ -10,21 +10,20 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sweeper_interfaces REQUIRED) find_package(std_msgs REQUIRED) +find_package(logger REQUIRED) file(GLOB SRC_FILES "src/*.cpp") add_executable(ctrl_arbiter_node ${SRC_FILES}) -ament_target_dependencies(ctrl_arbiter_node +ament_target_dependencies( + ctrl_arbiter_node rclcpp sweeper_interfaces std_msgs -) + logger) -install(TARGETS - ctrl_arbiter_node - DESTINATION lib/${PROJECT_NAME} -) +install(TARGETS ctrl_arbiter_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp b/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp index 970a057..c5ae01c 100644 --- a/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp +++ b/src/control/ctrl_arbiter/src/ctrl_arbiter.cpp @@ -1,57 +1,54 @@ -#include "rclcpp/rclcpp.hpp" -#include "sweeper_interfaces/msg/mc_ctrl.hpp" #include #include +#include "logger/logger.h" +#include "rclcpp/rclcpp.hpp" +#include "sweeper_interfaces/msg/mc_ctrl.hpp" + using namespace std::chrono_literals; namespace sweeperMsg = sweeper_interfaces::msg; class ArbitrationNode : public rclcpp::Node { -public: - ArbitrationNode() - : Node("control_arbitrator") + public: + ArbitrationNode() : Node("control_arbitrator") { - timeout_ms_ = 200; // 200毫秒超时阈值，可调整 + timeout_ms_ = 200; // 200毫秒超时阈值，可调整 publisher_ = this->create_publisher("/mc_ctrl", 10); - sub_radio_ = this->create_subscription( - "/radio_mc_ctrl", 10, - [this](const sweeperMsg::McCtrl::SharedPtr msg) - { - std::lock_guard lock(mutex_); - radio_msg_ = *msg; - radio_last_time_ = this->now(); - radio_valid_ = true; - }); + sub_radio_ = this->create_subscription("/radio_mc_ctrl", 10, + [this](const sweeperMsg::McCtrl::SharedPtr msg) + { + std::lock_guard lock(mutex_); + radio_msg_ = *msg; + radio_last_time_ = this->now(); + radio_valid_ = true; + }); - sub_remote_ = this->create_subscription( - "/remote_mc_ctrl", 10, - [this](const sweeperMsg::McCtrl::SharedPtr msg) - { - std::lock_guard lock(mutex_); - remote_msg_ = *msg; - remote_last_time_ = this->now(); - remote_valid_ = true; - }); + sub_remote_ = this->create_subscription("/remote_mc_ctrl", 10, + [this](const sweeperMsg::McCtrl::SharedPtr msg) + { + std::lock_guard lock(mutex_); + remote_msg_ = *msg; + remote_last_time_ = this->now(); + remote_valid_ = true; + }); - sub_auto_ = this->create_subscription( - "/auto_mc_ctrl", 10, - [this](const sweeperMsg::McCtrl::SharedPtr msg) - { - std::lock_guard lock(mutex_); - auto_msg_ = *msg; - auto_last_time_ = this->now(); - auto_valid_ = true; - }); + sub_auto_ = this->create_subscription("/auto_mc_ctrl", 10, + [this](const sweeperMsg::McCtrl::SharedPtr msg) + { + std::lock_guard lock(mutex_); + auto_msg_ = *msg; + auto_last_time_ = this->now(); + auto_valid_ = true; + }); - timer_ = this->create_wall_timer(20ms, [this]() - { this->arbitrateAndPublish(); }); - RCLCPP_INFO(this->get_logger(), "ArbitrationNode started, waiting for control sources..."); + timer_ = this->create_wall_timer(20ms, [this]() { this->arbitrateAndPublish(); }); + LOG_INFO("ArbitrationNode started, waiting for control sources..."); } -private: + private: void arbitrateAndPublish() { std::lock_guard lock(mutex_); @@ -61,21 +58,21 @@ private: if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000) { publisher_->publish(radio_msg_); - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using RADIO control"); + LOG_INFO_THROTTLE(2000, "[ARBITER] Using RADIO control"); return; } if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000) { publisher_->publish(remote_msg_); - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using REMOTE control"); + LOG_INFO_THROTTLE(2000, "[ARBITER] Using REMOTE control"); return; } if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000) { publisher_->publish(auto_msg_); - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using AUTO control"); + LOG_INFO_THROTTLE(2000, "[ARBITER] Using AUTO control"); return; } @@ -90,8 +87,7 @@ private: safe_msg.sweep = false; publisher_->publish(safe_msg); - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "[ARBITER] All sources timeout, publishing FAILSAFE control"); + LOG_WARN_THROTTLE(1000, "[ARBITER] All sources timeout, publishing FAILSAFE control"); } rclcpp::Publisher::SharedPtr publisher_; @@ -109,11 +105,17 @@ private: int timeout_ms_; }; -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { + // 初始化日志系统 + logger::Logger::Init("ctrl_arbiter", "./log"); + rclcpp::init(argc, argv); auto node = std::make_shared(); rclcpp::spin(node); + rclcpp::shutdown(); + // 关闭日志系统 + logger::Logger::Shutdown(); return 0; } \ No newline at end of file diff --git a/src/control/radio_ctrl/CMakeLists.txt b/src/control/radio_ctrl/CMakeLists.txt index 1dae9f4..5bc5ee2 100644 --- a/src/control/radio_ctrl/CMakeLists.txt +++ b/src/control/radio_ctrl/CMakeLists.txt @@ -14,13 +14,19 @@ find_package(rclcpp REQUIRED) find_package(sweeper_interfaces REQUIRED) find_package(std_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(logger REQUIRED) add_executable(radio_ctrl_node src/radio_ctrl.cpp src/uart_handler.cpp) -target_include_directories( - radio_ctrl_node PUBLIC $) +target_include_directories(radio_ctrl_node PUBLIC $) -ament_target_dependencies(radio_ctrl_node ament_index_cpp rclcpp std_msgs sweeper_interfaces) +ament_target_dependencies( + radio_ctrl_node + ament_index_cpp + rclcpp + std_msgs + sweeper_interfaces + logger) install(TARGETS radio_ctrl_node DESTINATION lib/${PROJECT_NAME}) @@ -28,10 +34,7 @@ install(TARGETS radio_ctrl_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) # 安装头文件到正确路径 -install( - DIRECTORY include/ - DESTINATION include -) +install(DIRECTORY include/ DESTINATION include) # 导出头文件路径，使其他包能发现 ament_export_include_directories(include) diff --git a/src/control/radio_ctrl/include/radio_ctrl/get_config.h b/src/control/radio_ctrl/include/radio_ctrl/get_config.h index 57904f7..f95b048 100644 --- a/src/control/radio_ctrl/include/radio_ctrl/get_config.h +++ b/src/control/radio_ctrl/include/radio_ctrl/get_config.h @@ -1,8 +1,9 @@ #pragma once #include -#include #include +#include + #include "ament_index_cpp/get_package_share_directory.hpp" #include "nlohmann/json.hpp" @@ -16,7 +17,7 @@ struct RmoCtlConfig double eps_angle_max; }; -bool load_config(RmoCtlConfig &config) +bool load_config(RmoCtlConfig& config) { try { @@ -39,7 +40,7 @@ bool load_config(RmoCtlConfig &config) return true; } - catch (const std::exception &e) + catch (const std::exception& e) { std::cerr << "Error parsing config: " << e.what() << std::endl; return false; diff --git a/src/control/radio_ctrl/include/radio_ctrl/uart_handler.h b/src/control/radio_ctrl/include/radio_ctrl/uart_handler.h index 41de259..140e98d 100644 --- a/src/control/radio_ctrl/include/radio_ctrl/uart_handler.h +++ b/src/control/radio_ctrl/include/radio_ctrl/uart_handler.h @@ -1,31 +1,32 @@ #ifndef UART_HANDLER_H #define UART_HANDLER_H -#include +#include +#include +#include +#include + +#include #include -#include #include #include -#include -#include -#include -#include +#include +#include #include -#include class UartHandler { -public: - UartHandler(const std::string &device, int baudrate = 100000); + public: + UartHandler(const std::string& device, int baudrate = 100000); ~UartHandler(); - bool open_serial(); // 打开串口 - void start_reading(); // 启动读取数据的线程 - void stop_reading(); // 停止读取数据的线程 - int get_channel_value(int channel); // 获取指定通道的数据 - bool get_data_safe(); // 获取数据安全性 + bool open_serial(); // 打开串口 + void start_reading(); // 启动读取数据的线程 + void stop_reading(); // 停止读取数据的线程 + int get_channel_value(int channel); // 获取指定通道的数据 + bool get_data_safe(); // 获取数据安全性 -private: + private: std::string serial_device; int baudrate; int fd; @@ -44,13 +45,13 @@ private: SBUS_SIGNAL_OK }; - std::atomic reading; // 控制读取线程的状态 - std::thread read_thread; // 读取数据的线程 + std::atomic reading; // 控制读取线程的状态 + std::thread read_thread; // 读取数据的线程 - void read_loop(); // 持续读取串口数据 - void parse_data(std::vector &buffer); // 解析数据 - int sbus_parse(); // SBUS 数据解析 - void print_hex(uint8_t *buf, int len); // 打印数据（调试用） + void read_loop(); // 持续读取串口数据 + void parse_data(std::vector& buffer); // 解析数据 + int sbus_parse(); // SBUS 数据解析 + void print_hex(uint8_t* buf, int len); // 打印数据（调试用） }; -#endif // UART_HANDLER_H +#endif // UART_HANDLER_H diff --git a/src/control/radio_ctrl/package.xml b/src/control/radio_ctrl/package.xml index 0930508..3d37f97 100644 --- a/src/control/radio_ctrl/package.xml +++ b/src/control/radio_ctrl/package.xml @@ -13,6 +13,7 @@ std_msgs ament_index_cpp sweeper_interfaces + logger ament_cmake ament_cmake @@ -23,4 +24,4 @@ ament_cmake - + \ No newline at end of file diff --git a/src/control/radio_ctrl/src/radio_ctrl.cpp b/src/control/radio_ctrl/src/radio_ctrl.cpp index baf7ac7..2fab2ad 100644 --- a/src/control/radio_ctrl/src/radio_ctrl.cpp +++ b/src/control/radio_ctrl/src/radio_ctrl.cpp @@ -1,29 +1,31 @@ -#include "radio_ctrl/uart_handler.h" -#include "radio_ctrl/get_config.h" -#include #include +#include #include -#include "sweeper_interfaces/msg/mc_ctrl.hpp" + +#include "logger/logger.h" +#include "radio_ctrl/get_config.h" +#include "radio_ctrl/uart_handler.h" #include "sweeper_interfaces/msg/can_frame.hpp" +#include "sweeper_interfaces/msg/mc_ctrl.hpp" namespace sweeperMsg = sweeper_interfaces::msg; constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f; constexpr float GEAR_RATIO = 16.5f; -constexpr float DELTA_T = 0.02f; // 20ms -constexpr int PRINT_INTERVAL = 25; // 打印间隔：每25次回调打印一次（25*20ms=500ms，即2Hz） +constexpr float DELTA_T = 0.02f; // 20ms +constexpr int PRINT_INTERVAL = 25; // 打印间隔：每25次回调打印一次（25*20ms=500ms，即2Hz） class SBUSNode : public rclcpp::Node { -public: - SBUSNode() : Node("sbus_node"), current_feedback_angle(0.0f), print_counter(0) // 初始化计数器 + public: + SBUSNode() : Node("sbus_node"), current_feedback_angle(0.0f), print_counter(0) // 初始化计数器 { if (load_config(config)) { - std::cout << "Serial Port: " << config.serial_port << "\n"; - std::cout << "Baudrate: " << config.baudrate << "\n"; - std::cout << "MCU RPM Max: " << config.mcu_rpm_max << "\n"; - std::cout << "EPS Angle Max: " << config.eps_angle_max << "\n"; + LOG_INFO("Serial Port: %s", config.serial_port.c_str()); + LOG_INFO("Baudrate: %d", config.baudrate); + LOG_INFO("MCU RPM Max: %d", config.mcu_rpm_max); + LOG_INFO("EPS Angle Max: %f", config.eps_angle_max); } // 发布控制指令消息的发布器 @@ -31,22 +33,19 @@ public: // 订阅CAN反馈的回调函数 can_sub_ = this->create_subscription( - "can_data", 10, - std::bind(&SBUSNode::can_callback, this, std::placeholders::_1)); + "can_data", 10, std::bind(&SBUSNode::can_callback, this, std::placeholders::_1)); // 初始化串口读取（读取遥控器数据） uart_handler_ = std::make_shared(config.serial_port, config.baudrate); if (!uart_handler_->open_serial()) { - RCLCPP_ERROR(this->get_logger(), "Failed to open serial port!"); + LOG_ERROR("Failed to open serial port!"); return; } - uart_handler_->start_reading(); // 启动串口后台读线程 + uart_handler_->start_reading(); // 启动串口后台读线程 // 创建周期定时器（每20ms回调一次控制逻辑，50Hz） - timer_ = this->create_wall_timer( - std::chrono::milliseconds(20), - std::bind(&SBUSNode::timer_callback, this)); + timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&SBUSNode::timer_callback, this)); } ~SBUSNode() @@ -57,44 +56,44 @@ public: } } -private: + private: // 定时器回调函数，用于周期性发布控制指令 void timer_callback() { static int MCU_RPM_MAX = config.mcu_rpm_max; static float EPS_ANGLE_MAX = config.eps_angle_max; - bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性 + bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性 - auto msg = sweeperMsg::McCtrl(); // 控制消息对象 - uint16_t ch_data[10]; // 各通道遥控数据 + auto msg = sweeperMsg::McCtrl(); // 控制消息对象 + uint16_t ch_data[10]; // 各通道遥控数据 - if (data_safe) // 数据安全，进行数据解析并发布 + if (data_safe) // 数据安全，进行数据解析并发布 { // 赋值与打印（注释掉原有的高频打印） for (int i = 0; i < 10; ++i) { ch_data[i] = uart_handler_->get_channel_value(i); - // printf("ch[%d]:%d ", i, ch_data[i]); // 高频打印已注释 + // LOG_DEBUG("ch[%d]:%d ", i, ch_data[i]); // 高频打印已注释 } - // printf("\n"); // 高频打印已注释 + // LOG_DEBUG("\n"); // 高频打印已注释 - uint16_t gear = ch_data[7]; // 挡位 最下1792，中间992，最上192 - uint16_t sweep = ch_data[6]; // 清扫 最上192，最下1792 - int16_t speed = ch_data[2] - 992; // 左边摇杆，最下192，中间992，最上1792 -> [-800,800] + uint16_t gear = ch_data[7]; // 挡位 最下1792，中间992，最上192 + uint16_t sweep = ch_data[6]; // 清扫 最上192，最下1792 + int16_t speed = ch_data[2] - 992; // 左边摇杆，最下192，中间992，最上1792 -> [-800,800] // 挡位选择 if (gear == 192) { - msg.gear = 2; // D挡 + msg.gear = 2; // D挡 } else if (gear == 992) { - msg.gear = 0; // N挡 + msg.gear = 0; // N挡 } else if (gear == 1792) { - msg.gear = 1; // R挡 + msg.gear = 1; // R挡 } // 油门 / 刹车逻辑 @@ -130,10 +129,8 @@ private: float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO; // 限制电机转速到 [120, 1500] 范围，防止过小/过大 - uint16_t can_speed = std::clamp( - static_cast(motor_rpm), - static_cast(120), - static_cast(1500)); + uint16_t can_speed = + std::clamp(static_cast(motor_rpm), static_cast(120), static_cast(1500)); msg.angle = target_angle; msg.angle_speed = can_speed; @@ -144,25 +141,25 @@ private: // 降低打印频率：每 PRINT_INTERVAL 次回调打印一次 // if (++print_counter >= PRINT_INTERVAL) // { - // std::cout << "\n=====================================" + // LOG_INFO("\n=====================================") // << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车") // << "\n 挡位: "; // switch (msg.gear) // { // case 0: - // std::cout << "空挡"; + // LOG_INFO("空挡"); // break; // case 2: - // std::cout << "前进挡"; + // LOG_INFO("前进挡"); // break; // case 1: - // std::cout << "后退挡"; + // LOG_INFO("后退挡"); // break; // default: - // std::cout << "未知挡位(" << static_cast(msg.gear) << ")"; + // LOG_INFO("未知挡位(%d)", static_cast(msg.gear)); // break; // } - // std::cout << "\n 行走电机转速: " << static_cast(msg.rpm) << " RPM" + // LOG_INFO("\n 行走电机转速: %d RPM", static_cast(msg.rpm)); // << "\n 轮端转向角度: " << msg.angle << "°" // << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫") // << "\n=====================================" << std::endl; @@ -174,7 +171,7 @@ private: // 低频率打印等待信息（每2次回调打印一次，避免刷屏） if (++print_counter >= PRINT_INTERVAL / 2) { - RCLCPP_INFO(this->get_logger(), "Waiting for radio control data..."); + LOG_INFO("Waiting for radio control data..."); print_counter = 0; } return; @@ -201,15 +198,21 @@ private: rclcpp::TimerBase::SharedPtr timer_; std::shared_ptr uart_handler_; - float current_feedback_angle; // 当前反馈角度（从CAN中读取） + float current_feedback_angle; // 当前反馈角度（从CAN中读取） RmoCtlConfig config; - int print_counter; // 打印计数器（新增） + int print_counter; // 打印计数器（新增） }; -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { + // 初始化日志系统 + logger::Logger::Init("radio_ctrl", "./log"); + rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); // 启动节点 + rclcpp::spin(std::make_shared()); // 启动节点 rclcpp::shutdown(); + + // 关闭日志系统 + logger::Logger::Shutdown(); return 0; } \ No newline at end of file diff --git a/src/control/radio_ctrl/src/uart_handler.cpp b/src/control/radio_ctrl/src/uart_handler.cpp index cc6b543..0228131 100644 --- a/src/control/radio_ctrl/src/uart_handler.cpp +++ b/src/control/radio_ctrl/src/uart_handler.cpp @@ -1,14 +1,18 @@ #include "radio_ctrl/uart_handler.h" + +#include #include -#include +#include +#include +#include #include #include -#include -#include -#include -#include +#include + #include +#include "logger/logger.h" + #ifndef BOTHER #define BOTHER 0010000 #endif @@ -29,10 +33,10 @@ struct termios2 #define TCSETS2 _IOW('T', 0x2B, struct termios2) // 构造函数 -UartHandler::UartHandler(const std::string &device, int baudrate) +UartHandler::UartHandler(const std::string& device, int baudrate) : serial_device(device), baudrate(baudrate), fd(-1), failsafe_status(SBUS_SIGNAL_LOST) { - std::fill(std::begin(sbus_channels), std::end(sbus_channels), 0); // 初始化通道数据 + std::fill(std::begin(sbus_channels), std::end(sbus_channels), 0); // 初始化通道数据 } // 析构函数，关闭串口 @@ -63,8 +67,7 @@ bool UartHandler::open_serial() return false; } - tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | - IGNCR | ICRNL | IXON); + tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); tio.c_iflag |= (INPCK | IGNPAR); tio.c_oflag &= ~OPOST; tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); @@ -84,7 +87,7 @@ bool UartHandler::open_serial() return false; } - // std::cout << "Serial port " << serial_device << " opened and configured.\n"; + // LOG_INFO("Serial port %s opened and configured.", serial_device.c_str()); return true; } @@ -130,16 +133,16 @@ void UartHandler::read_loop() } } -void UartHandler::print_hex(uint8_t *buf, int len) +void UartHandler::print_hex(uint8_t* buf, int len) { for (int i = 0; i < len; ++i) { - printf("%02X ", buf[i]); + LOG_DEBUG("%02X ", buf[i]); } - printf("\n"); + LOG_DEBUG("\n"); } -void UartHandler::parse_data(std::vector &buffer) +void UartHandler::parse_data(std::vector& buffer) { while (buffer.size() >= kSbusFrameLength) { @@ -147,7 +150,7 @@ void UartHandler::parse_data(std::vector &buffer) auto it = std::find(buffer.begin(), buffer.end(), 0x0F); if (it == buffer.end()) { - buffer.clear(); // 没找到帧头，清空 buffer + buffer.clear(); // 没找到帧头，清空 buffer failsafe_status = SBUS_SIGNAL_LOST; return; } @@ -159,8 +162,7 @@ void UartHandler::parse_data(std::vector &buffer) if (buffer.size() - index < kSbusFrameLength) { // 数据不够，等下次再处理 - if (index > 0) - buffer.erase(buffer.begin(), buffer.begin() + index); + if (index > 0) buffer.erase(buffer.begin(), buffer.begin() + index); return; } @@ -215,9 +217,9 @@ int UartHandler::get_channel_value(int channel) { if (channel >= 0 && channel < 8) { - return sbus_channels[channel]; // 返回指定通道的值 + return sbus_channels[channel]; // 返回指定通道的值 } - return 0; // 如果无效的通道，返回默认值 + return 0; // 如果无效的通道，返回默认值 } bool UartHandler::get_data_safe() diff --git a/src/control/remote_ctrl/CMakeLists.txt b/src/control/remote_ctrl/CMakeLists.txt index fb8bc33..fe2f36f 100644 --- a/src/control/remote_ctrl/CMakeLists.txt +++ b/src/control/remote_ctrl/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sweeper_interfaces REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(logger REQUIRED) # ======== MQTT libs ======== if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64") @@ -41,7 +42,12 @@ target_link_libraries( ssl crypto) -ament_target_dependencies(remote_ctrl_node rclcpp sweeper_interfaces ament_index_cpp) +ament_target_dependencies( + remote_ctrl_node + rclcpp + sweeper_interfaces + ament_index_cpp + logger) # ===== install executable ===== install(TARGETS remote_ctrl_node DESTINATION lib/${PROJECT_NAME}) diff --git a/src/control/remote_ctrl/include/remote_ctrl/mqtt_client.hpp b/src/control/remote_ctrl/include/remote_ctrl/mqtt_client.hpp index e0ee9b0..00bea1a 100644 --- a/src/control/remote_ctrl/include/remote_ctrl/mqtt_client.hpp +++ b/src/control/remote_ctrl/include/remote_ctrl/mqtt_client.hpp @@ -1,27 +1,23 @@ #pragma once -#include "paho_mqtt_3c/MQTTClient.h" -#include #include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include + +#include "paho_mqtt_3c/MQTTClient.h" class MQTTClientWrapper { -public: - using MessageCallback = - std::function; - using ConnectedCallback = - std::function; + public: + using MessageCallback = std::function; + using ConnectedCallback = std::function; - MQTTClientWrapper(const std::string &server_uri, - const std::string &client_id, - const std::string &username = "", - const std::string &password = "", - int reconnect_ms = 3000) + MQTTClientWrapper(const std::string& server_uri, const std::string& client_id, const std::string& username = "", + const std::string& password = "", int reconnect_ms = 3000) : server_uri_(server_uri), client_id_(client_id), username_(username), @@ -30,11 +26,8 @@ public: connected_(false), stop_flag_(false) { - int rc = MQTTClient_create(&client_, - server_uri_.c_str(), - client_id_.c_str(), - MQTTCLIENT_PERSISTENCE_NONE, - nullptr); + int rc = + MQTTClient_create(&client_, server_uri_.c_str(), client_id_.c_str(), MQTTCLIENT_PERSISTENCE_NONE, nullptr); if (rc != MQTTCLIENT_SUCCESS) { @@ -44,32 +37,26 @@ public: } MQTTClient_setCallbacks(client_, this, - connLostCB, // 连接丢失回调 - msgArrivedCB, // 消息回调 + connLostCB, // 连接丢失回调 + msgArrivedCB, // 消息回调 nullptr); // 启动网络循环线程 - loop_thread_ = std::thread([this]() - { loopFunc(); }); + loop_thread_ = std::thread([this]() { loopFunc(); }); } - ~MQTTClientWrapper() - { - stop(); - } + ~MQTTClientWrapper() { stop(); } void stop() { stop_flag_ = true; - if (loop_thread_.joinable()) - loop_thread_.join(); + if (loop_thread_.joinable()) loop_thread_.join(); std::lock_guard lk(mtx_); if (client_) { - if (connected_) - MQTTClient_disconnect(client_, 2000); + if (connected_) MQTTClient_disconnect(client_, 2000); MQTTClient_destroy(&client_); } @@ -88,13 +75,11 @@ public: connected_cb_ = std::move(cb); } - bool subscribe(const std::string &topic, int qos = 0) + bool subscribe(const std::string& topic, int qos = 0) { std::lock_guard lk(mtx_); - if (!client_ || - !connected_.load(std::memory_order_acquire) || - !ready_.load(std::memory_order_acquire)) + if (!client_ || !connected_.load(std::memory_order_acquire) || !ready_.load(std::memory_order_acquire)) return false; int rc = MQTTClient_subscribe(client_, topic.c_str(), qos); @@ -108,19 +93,15 @@ public: return true; } - bool publish(const std::string &topic, - const std::string &payload, - int qos = 0) + bool publish(const std::string& topic, const std::string& payload, int qos = 0) { std::lock_guard lk(mtx_); - if (!client_ || - !connected_.load(std::memory_order_acquire) || - !ready_.load(std::memory_order_acquire)) + if (!client_ || !connected_.load(std::memory_order_acquire) || !ready_.load(std::memory_order_acquire)) return false; MQTTClient_message msg = MQTTClient_message_initializer; - msg.payload = (void *)payload.data(); + msg.payload = (void*)payload.data(); msg.payloadlen = payload.size(); msg.qos = qos; @@ -137,16 +118,12 @@ public: return true; } - bool connect() - { - return doConnect(); - } + bool connect() { return doConnect(); } -private: + private: bool doConnect() { - if (!client_) - return false; + if (!client_) return false; MQTTClient_connectOptions opts = MQTTClient_connectOptions_initializer; opts.keepAliveInterval = 15; @@ -175,24 +152,22 @@ private: return false; } - static void connLostCB(void *ctx, char *cause) + static void connLostCB(void* ctx, char* cause) { - auto self = static_cast(ctx); + auto self = static_cast(ctx); self->connected_.store(false, std::memory_order_release); self->ready_.store(false, std::memory_order_release); - std::cerr << "[MQTT] lost connection: " - << (cause ? cause : "unknown") << std::endl; + std::cerr << "[MQTT] lost connection: " << (cause ? cause : "unknown") << std::endl; } - static int msgArrivedCB(void *ctx, char *topic, - int topicLen, MQTTClient_message *message) + static int msgArrivedCB(void* ctx, char* topic, int topicLen, MQTTClient_message* message) { - auto self = static_cast(ctx); + auto self = static_cast(ctx); std::string t(topic, topicLen > 0 ? topicLen : strlen(topic)); - std::string payload((char *)message->payload, message->payloadlen); + std::string payload((char*)message->payload, message->payloadlen); if (self->msg_cb_) { @@ -235,8 +210,7 @@ private: // ⭐ 在 ready 状态触发 connected callback if (need_callback_.exchange(false)) { - if (connected_cb_) - connected_cb_(); + if (connected_cb_) connected_cb_(); } } } @@ -246,7 +220,7 @@ private: } } -private: + private: MQTTClient client_{nullptr}; std::string server_uri_, client_id_; std::string username_, password_; @@ -261,6 +235,6 @@ private: MessageCallback msg_cb_; ConnectedCallback connected_cb_; - std::atomic ready_{false}; // 是否真正进入可用状态 + std::atomic ready_{false}; // 是否真正进入可用状态 std::atomic need_callback_{false}; }; diff --git a/src/control/remote_ctrl/package.xml b/src/control/remote_ctrl/package.xml index 7181f41..06d814e 100644 --- a/src/control/remote_ctrl/package.xml +++ b/src/control/remote_ctrl/package.xml @@ -4,9 +4,7 @@ remote_ctrl 0.1.0 - - Remote control node using MQTT to receive vehicle control commands. - + Remote control node using MQTT to receive vehicle control commands. cxh @@ -20,6 +18,7 @@ rclcpp sweeper_interfaces ament_index_cpp + logger ament_lint_auto @@ -28,4 +27,4 @@ ament_cmake - + \ No newline at end of file diff --git a/src/control/remote_ctrl/src/config.cpp b/src/control/remote_ctrl/src/config.cpp index e9fd926..82e102d 100644 --- a/src/control/remote_ctrl/src/config.cpp +++ b/src/control/remote_ctrl/src/config.cpp @@ -4,6 +4,8 @@ #include #include +#include "logger/logger.h" + using json = nlohmann::json; bool config::load(const std::string& path, Config& cfg) @@ -13,7 +15,7 @@ bool config::load(const std::string& path, Config& cfg) std::ifstream ifs(path); if (!ifs.is_open()) { - std::cerr << "Failed to open config file: " << path << std::endl; + LOG_ERROR("Failed to open config file: %s", path.c_str()); return false; } @@ -34,7 +36,7 @@ bool config::load(const std::string& path, Config& cfg) } catch (const std::exception& e) { - std::cerr << "Error parsing remote_ctrl config: " << e.what() << std::endl; + LOG_ERROR("Error parsing remote_ctrl config: %s", e.what()); return false; } } diff --git a/src/control/remote_ctrl/src/remote_ctrl_node.cpp b/src/control/remote_ctrl/src/remote_ctrl_node.cpp index 516be44..c3c96b7 100644 --- a/src/control/remote_ctrl/src/remote_ctrl_node.cpp +++ b/src/control/remote_ctrl/src/remote_ctrl_node.cpp @@ -7,6 +7,7 @@ #include #include +#include "logger/logger.h" #include "nlohmann/json.hpp" #include "remote_ctrl/config.hpp" #include "remote_ctrl/mqtt_client.hpp" @@ -83,7 +84,7 @@ class RemoteCtrlNode : public rclcpp::Node if (!identity_ready_.load()) { - RCLCPP_WARN(this->get_logger(), "[REMOTE] MQTT connected but identity not ready"); + LOG_WARN("[REMOTE] MQTT connected but identity not ready"); return; } @@ -100,14 +101,14 @@ class RemoteCtrlNode : public rclcpp::Node vid_ = msg->vid; identity_ready_.store(!vid_.empty()); - RCLCPP_INFO(this->get_logger(), "[REMOTE] Identity ready: VID=%s", vid_.c_str()); + LOG_INFO("[REMOTE] Identity ready: VID=%s", vid_.c_str()); ctrl_topic_ = cfg_.remote_topic_template; if (ctrl_topic_.find("{vid}") == std::string::npos) { - RCLCPP_WARN(this->get_logger(), "[REMOTE] remote_topic_template missing '{vid}', template=%s", - cfg_.remote_topic_template.c_str()); + LOG_WARN("[REMOTE] remote_topic_template missing '{vid}', template=%s", + cfg_.remote_topic_template.c_str()); } auto pos = ctrl_topic_.find("{vid}"); @@ -116,8 +117,7 @@ class RemoteCtrlNode : public rclcpp::Node try_subscribe_ctrl_topic(); }); - RCLCPP_INFO(this->get_logger(), "RemoteCtrlNode started mqtt=%s client_id=%s", uri.str().c_str(), - client_id.c_str()); + LOG_INFO("RemoteCtrlNode started mqtt=%s client_id=%s", uri.str().c_str(), client_id.c_str()); // ===== 看门狗 & 定时发布 ===== last_msg_time_ = std::chrono::steady_clock::now(); @@ -137,8 +137,7 @@ class RemoteCtrlNode : public rclcpp::Node // 只在第一次进入“失活”时打印日志 if (remote_alive_.exchange(false)) { - RCLCPP_WARN(this->get_logger(), - "[REMOTE] control timeout, enter safe-stop"); + LOG_WARN("[REMOTE] control timeout, enter safe-stop"); } } }); @@ -213,7 +212,7 @@ class RemoteCtrlNode : public rclcpp::Node if (already) return; // 已经订阅过，退出 - RCLCPP_INFO(this->get_logger(), "[REMOTE] subscribe ctrl topic: %s", ctrl_topic_.c_str()); + LOG_INFO("[REMOTE] subscribe ctrl topic: %s", ctrl_topic_.c_str()); mqtt_->subscribe(ctrl_topic_, 1); } @@ -227,12 +226,12 @@ class RemoteCtrlNode : public rclcpp::Node bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed); if (was_dead) { - RCLCPP_INFO(this->get_logger(), "[REMOTE] control recovered"); + LOG_INFO("[REMOTE] control recovered"); } last_msg_time_ = std::chrono::steady_clock::now(); - RCLCPP_DEBUG(this->get_logger(), "[REMOTE] recv MQTT payload: %s", payload.c_str()); + LOG_DEBUG("[REMOTE] recv MQTT payload: %s", payload.c_str()); try { @@ -258,7 +257,7 @@ class RemoteCtrlNode : public rclcpp::Node remote_authorized_.store(true, std::memory_order_release); remote_alive_.store(true, std::memory_order_relaxed); - RCLCPP_INFO(this->get_logger(), "[REMOTE] authorized (mode=3)"); + LOG_INFO("[REMOTE] authorized (mode=3)"); } else if (mode == 0) { @@ -269,7 +268,7 @@ class RemoteCtrlNode : public rclcpp::Node // 明确清空目标状态 desired_ = {}; - RCLCPP_INFO(this->get_logger(), "[REMOTE] deauthorized (mode=0)"); + LOG_INFO("[REMOTE] deauthorized (mode=0)"); } // mode 指令处理完直接返回 @@ -357,7 +356,7 @@ class RemoteCtrlNode : public rclcpp::Node } catch (const std::exception& e) { - RCLCPP_WARN(this->get_logger(), "[REMOTE] JSON parse error: %s", e.what()); + LOG_WARN("[REMOTE] JSON parse error: %s", e.what()); } } @@ -392,6 +391,9 @@ class RemoteCtrlNode : public rclcpp::Node int main(int argc, char* argv[]) { + // 初始化日志系统 + logger::Logger::Init("remote_ctrl", "./log"); + // =============================== // 1. 默认配置路径 + 手动解析 --config // =============================== @@ -414,12 +416,12 @@ int main(int argc, char* argv[]) Config cfg; if (!config::load(config_path, cfg)) { - RCLCPP_ERROR(rclcpp::get_logger("main"), "Failed to load config file: %s", config_path.c_str()); + LOG_ERROR("Failed to load config file: %s", config_path.c_str()); return 1; } - RCLCPP_INFO(rclcpp::get_logger("main"), "Config loaded: broker=%s:%d remote_topic_template=%s", cfg.mqtt_ip.c_str(), - cfg.mqtt_port, cfg.remote_topic_template.c_str()); + LOG_INFO("Config loaded: broker=%s:%d remote_topic_template=%s", cfg.mqtt_ip.c_str(), cfg.mqtt_port, + cfg.remote_topic_template.c_str()); // =============================== // 3. 把配置传入 Node 构造函数 @@ -430,5 +432,8 @@ int main(int argc, char* argv[]) node.reset(); rclcpp::shutdown(); + + // 关闭日志系统 + logger::Logger::Shutdown(); return 0; } diff --git a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp index d4b80d8..84fba5a 100644 --- a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp +++ b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp @@ -1,24 +1,23 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - +#include +#include +#include #include #include -#include -#include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include +#include using sensor_msgs::msg::PointCloud2; using std::placeholders::_1; @@ -28,14 +27,14 @@ struct CloudFrame { std::shared_ptr cloud; rclcpp::Time stamp; - rclcpp::Time received_time; // 添加接收时间用于延时分析 + rclcpp::Time received_time; // 添加接收时间用于延时分析 std::string source; }; // 内存池实现 class PointCloud2Pool { -public: + public: std::shared_ptr acquire() { std::lock_guard lock(mutex_); @@ -53,8 +52,7 @@ public: void release(std::shared_ptr cloud) { - if (!cloud) - return; + if (!cloud) return; cloud->data.clear(); cloud->width = 0; @@ -62,20 +60,20 @@ public: cloud->row_step = 0; std::lock_guard lock(mutex_); - if (pool_.size() < 10) // 限制池大小 + if (pool_.size() < 10) // 限制池大小 { pool_.push_back(cloud); } } -private: + private: std::vector> pool_; std::mutex mutex_; }; class LidarMerger : public rclcpp::Node { -public: + public: LidarMerger() : Node("lidar_merger") { /* ---------- 参数 ---------- */ @@ -83,11 +81,11 @@ public: rear_topic_ = declare_parameter("rear_topic", "/rslidar_points/rear_lidar"); output_topic_ = declare_parameter("output_topic", "/rslidar_points"); target_frame_ = declare_parameter("frame_id", "rslidar"); - queue_size_ = declare_parameter("queue_size", 3); // 减小队列大小 - cache_size_ = declare_parameter("cache_size", 10); // 增加缓存以应对延时 - time_tolerance_ = declare_parameter("time_tolerance", 0.1); // 放宽时间容差 - max_wait_time_ = declare_parameter("max_wait_time", 1.0); // 增加到1.0s以适应实际延时 - enable_debug_ = declare_parameter("enable_debug", false); // 默认关闭调试减少日志开销 + queue_size_ = declare_parameter("queue_size", 3); // 减小队列大小 + cache_size_ = declare_parameter("cache_size", 10); // 增加缓存以应对延时 + time_tolerance_ = declare_parameter("time_tolerance", 0.1); // 放宽时间容差 + max_wait_time_ = declare_parameter("max_wait_time", 1.0); // 增加到1.0s以适应实际延时 + enable_debug_ = declare_parameter("enable_debug", false); // 默认关闭调试减少日志开销 // 点云处理参数 filter_min_x_ = declare_parameter("filter_min_x", -10.0f); @@ -96,71 +94,57 @@ public: filter_max_y_ = declare_parameter("filter_max_y", 10.0f); filter_min_z_ = declare_parameter("filter_min_z", -2.0f); filter_max_z_ = declare_parameter("filter_max_z", 2.0f); - voxel_size_ = declare_parameter("voxel_size", 0.1f); // 降采样体素大小 - stat_mean_k_ = declare_parameter("stat_mean_k", 30); // 统计离群点去除的k值 - stat_std_thresh_ = declare_parameter("stat_std_thresh", 1.0f); // 统计离群点去除的标准差阈值 - grid_size_ = declare_parameter("grid_size", 100); // 栅格大小 - grid_range_ = declare_parameter("grid_range", 20.0f); // 栅格范围 + voxel_size_ = declare_parameter("voxel_size", 0.1f); // 降采样体素大小 + stat_mean_k_ = declare_parameter("stat_mean_k", 30); // 统计离群点去除的k值 + stat_std_thresh_ = declare_parameter("stat_std_thresh", 1.0f); // 统计离群点去除的标准差阈值 + grid_size_ = declare_parameter("grid_size", 100); // 栅格大小 + grid_range_ = declare_parameter("grid_range", 20.0f); // 栅格范围 enable_print_ = declare_parameter("enable_print", true); // 车身过滤参数 - filter_car_ = declare_parameter("filter_car", true); // 是否启用车身过滤 - car_length_ = declare_parameter("car_length", 2.3f); // 车长（米） - car_width_ = declare_parameter("car_width", 1.32f); // 车宽（米） - car_lidar_offset_x_ = declare_parameter("car_lidar_offset_x", 0.0f); // LiDAR在x轴的安装偏移 - car_lidar_offset_y_ = declare_parameter("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移 + filter_car_ = declare_parameter("filter_car", true); // 是否启用车身过滤 + car_length_ = declare_parameter("car_length", 2.3f); // 车长（米） + car_width_ = declare_parameter("car_width", 1.32f); // 车宽（米） + car_lidar_offset_x_ = declare_parameter("car_lidar_offset_x", 0.0f); // LiDAR在x轴的安装偏移 + car_lidar_offset_y_ = declare_parameter("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移 // 打印所有参数值（添加到构造函数末尾） - RCLCPP_INFO_STREAM(this->get_logger(), - "\n\n---------- 点云融合节点参数配置 ----------" - << "\n [输入输出]" - << "\n 前雷达话题: " << front_topic_ - << "\n 后雷达话题: " << rear_topic_ - << "\n 输出话题: " << output_topic_ - << "\n 目标坐标系: " << target_frame_ - << "\n [同步参数]" - << "\n 队列大小: " << queue_size_ - << "\n 缓存大小: " << cache_size_ - << "\n 时间容差(s): " << time_tolerance_ - << "\n 最大等待时间(s): " << max_wait_time_ - << "\n [调试选项]" - << "\n 调试模式: " << (enable_debug_ ? "开启" : "关闭") - << "\n [点云处理]" - << "\n 空间过滤范围(m): [" << filter_min_x_ << ", " << filter_max_x_ - << "] x [" << filter_min_y_ << ", " << filter_max_y_ - << "] x [" << filter_min_z_ << ", " << filter_max_z_ << "]" - << "\n 体素大小(m): " << voxel_size_ - << "\n 离群点k值: " << stat_mean_k_ - << "\n 离群点标准差阈值: " << stat_std_thresh_ - << "\n 栅格大小: " << grid_size_ << "x" << grid_size_ - << "\n 栅格范围(m): " << grid_range_ - << "\n 打印栅格: " << (enable_print_ ? "开启" : "关闭") - << "\n [车身过滤]" - << "\n 启用车身过滤: " << (filter_car_ ? "是" : "否") - << "\n 车身尺寸(m): " << car_length_ << " x " << car_width_ - << "\n LiDAR偏移(m): (" << car_lidar_offset_x_ << ", " << car_lidar_offset_y_ << ")" - << "\n--------------------------------------------\n"); + RCLCPP_INFO_STREAM( + this->get_logger(), + "\n\n---------- 点云融合节点参数配置 ----------" + << "\n [输入输出]" + << "\n 前雷达话题: " << front_topic_ << "\n 后雷达话题: " << rear_topic_ + << "\n 输出话题: " << output_topic_ << "\n 目标坐标系: " << target_frame_ << "\n [同步参数]" + << "\n 队列大小: " << queue_size_ << "\n 缓存大小: " << cache_size_ << "\n 时间容差(s): " + << time_tolerance_ << "\n 最大等待时间(s): " << max_wait_time_ << "\n [调试选项]" + << "\n 调试模式: " << (enable_debug_ ? "开启" : "关闭") << "\n [点云处理]" + << "\n 空间过滤范围(m): [" << filter_min_x_ << ", " << filter_max_x_ << "] x [" << filter_min_y_ + << ", " << filter_max_y_ << "] x [" << filter_min_z_ << ", " << filter_max_z_ << "]" + << "\n 体素大小(m): " << voxel_size_ << "\n 离群点k值: " << stat_mean_k_ + << "\n 离群点标准差阈值: " << stat_std_thresh_ << "\n 栅格大小: " << grid_size_ << "x" + << grid_size_ << "\n 栅格范围(m): " << grid_range_ + << "\n 打印栅格: " << (enable_print_ ? "开启" : "关闭") << "\n [车身过滤]" + << "\n 启用车身过滤: " << (filter_car_ ? "是" : "否") << "\n 车身尺寸(m): " << car_length_ + << " x " << car_width_ << "\n LiDAR偏移(m): (" << car_lidar_offset_x_ << ", " << car_lidar_offset_y_ + << ")" + << "\n--------------------------------------------\n"); /* ---------- TF ---------- */ tf_buffer_ = std::make_shared(get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); /* ---------- QoS - 优化为低延时 ---------- */ - auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)) - .best_effort() - .durability_volatile(); + auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)).best_effort().durability_volatile(); // 移除deadline约束，避免因延时导致的消息丢弃 /* ---------- 订阅 ---------- */ - sub_front_ = create_subscription( - front_topic_, qos, std::bind(&LidarMerger::frontCB, this, std::placeholders::_1)); - sub_rear_ = create_subscription( - rear_topic_, qos, std::bind(&LidarMerger::rearCB, this, std::placeholders::_1)); + sub_front_ = create_subscription(front_topic_, qos, + std::bind(&LidarMerger::frontCB, this, std::placeholders::_1)); + sub_rear_ = create_subscription(rear_topic_, qos, + std::bind(&LidarMerger::rearCB, this, std::placeholders::_1)); /* ---------- 发布 ---------- */ - auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)) - .reliable() - .durability_volatile(); + auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)).reliable().durability_volatile(); pub_ = create_publisher(output_topic_, pub_qos); // 创建栅格发布者 @@ -176,7 +160,7 @@ public: cache_size_, time_tolerance_, max_wait_time_); } -private: + private: /* ---------- 回调函数 - 同步处理 ---------- */ void frontCB(const PointCloud2::SharedPtr msg) { @@ -207,8 +191,7 @@ private: } /* ---------- 同步处理点云 ---------- */ - void processCloudSync(const PointCloud2::SharedPtr msg, const std::string &source, - const rclcpp::Time &receive_time) + void processCloudSync(const PointCloud2::SharedPtr msg, const std::string& source, const rclcpp::Time& receive_time) { // 立即进行坐标变换 auto transformed_cloud = transformCloud(*msg); @@ -219,11 +202,8 @@ private: } // 创建CloudFrame - auto cloud_frame = std::make_shared(CloudFrame{ - transformed_cloud, - rclcpp::Time(msg->header.stamp), - receive_time, - source}); + auto cloud_frame = std::make_shared( + CloudFrame{transformed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source}); // 添加到缓存并立即尝试合并 { @@ -241,7 +221,7 @@ private: /* ---------- 添加到缓存 ---------- */ void addToCache(std::shared_ptr frame) { - auto &cache = (frame->source == "front") ? front_clouds_ : rear_clouds_; + auto& cache = (frame->source == "front") ? front_clouds_ : rear_clouds_; // 保持时间排序（最新的在前） auto it = cache.begin(); @@ -262,14 +242,12 @@ private: /* ---------- 尝试合并 - 触发式 ---------- */ void tryMerge() { - if (front_clouds_.empty() || rear_clouds_.empty()) - return; + if (front_clouds_.empty() || rear_clouds_.empty()) return; // 查找最佳匹配 auto [front_frame, rear_frame] = findBestMatchOptimized(); - if (!front_frame || !rear_frame) - return; + if (!front_frame || !rear_frame) return; // 自适应延时检查 - 基于实际网络条件调整 auto now_time = now(); @@ -279,7 +257,7 @@ private: // 动态调整max_wait_time基于观察到的延时 static double observed_max_delay = 0.0; double current_max_delay = std::max(front_age, rear_age); - observed_max_delay = std::max(observed_max_delay * 0.95, current_max_delay); // 指数衰减 + observed_max_delay = std::max(observed_max_delay * 0.95, current_max_delay); // 指数衰减 // 使用观察到的延时来决定是否处理，而不是固定阈值 double adaptive_threshold = std::min(max_wait_time_, observed_max_delay + 0.1); @@ -289,8 +267,8 @@ private: if (enable_debug_) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, - "Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs", - front_age, rear_age, adaptive_threshold); + "Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs", front_age, + rear_age, adaptive_threshold); } // 不直接返回，而是清理最旧的数据后重试 @@ -314,8 +292,7 @@ private: auto merged = mergeClouds(*front_frame, *rear_frame); auto merge_end = std::chrono::high_resolution_clock::now(); - if (!merged) - return; + if (!merged) return; // 设置时间戳和frame_id auto front_time = front_frame->stamp.nanoseconds(); @@ -333,8 +310,7 @@ private: // ========================= 点云 ========================= auto processed_pcl = processPointCloud(merged_pcl); - if (!processed_pcl) - return; + if (!processed_pcl) return; // 将处理后的PCL点云转回ROS消息 auto processed = std::make_shared(); @@ -346,28 +322,27 @@ private: // ========================= 点云 ========================= // ========================= 栅格 ========================= auto grid = processPointCloud_grid(merged_pcl); - if (grid.empty()) - return; + if (grid.empty()) return; // 可视化栅格 - if (enable_print_) - visualizeGridInTerminal(grid); + if (enable_print_) visualizeGridInTerminal(grid); // 发布栅格到ROS话题 publishGrid(grid); // ========================= 栅格 ========================= // 延时分析（降低频率） - if (enable_debug_ && merged_count_ % 10 == 0) // 每10次输出一次 + if (enable_debug_ && merged_count_ % 10 == 0) // 每10次输出一次 { auto total_delay = (now_time - processed->header.stamp).seconds(); - auto process_time = std::chrono::duration(std::chrono::high_resolution_clock::now() - merge_start).count(); + auto process_time = + std::chrono::duration(std::chrono::high_resolution_clock::now() - merge_start).count(); - RCLCPP_INFO(get_logger(), - "Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs", - merged_count_, merged->data.size() / merged->point_step, - processed->data.size() / processed->point_step, - total_delay, process_time * 1000, adaptive_threshold); + RCLCPP_INFO( + get_logger(), + "Merged #%d: points=%zu → %zu, total_delay=%.3fs, process_time=%.3fms, adaptive_threshold=%.3fs", + merged_count_, merged->data.size() / merged->point_step, processed->data.size() / processed->point_step, + total_delay, process_time * 1000, adaptive_threshold); } // 清理已使用的点云 @@ -375,17 +350,16 @@ private: } /* ---------- 点云处理流程 ---------- */ - pcl::PointCloud::Ptr processPointCloud(const pcl::PointCloud::Ptr &cloud) + pcl::PointCloud::Ptr processPointCloud(const pcl::PointCloud::Ptr& cloud) { - if (!cloud || cloud->empty()) - return nullptr; + if (!cloud || cloud->empty()) return nullptr; // 1. 条件过滤 auto filtered = applyConditionalFiltering(cloud); if (filtered->empty()) { RCLCPP_WARN(get_logger(), "[processPointCloud] Filtered cloud is empty!"); - return cloud; // 返回原始云而不是nullptr + return cloud; // 返回原始云而不是nullptr } // 2. 降采样 @@ -406,10 +380,9 @@ private: return outlier_removed; } - std::vector> processPointCloud_grid(const pcl::PointCloud::Ptr &cloud) + std::vector> processPointCloud_grid(const pcl::PointCloud::Ptr& cloud) { - if (!cloud || cloud->empty()) - return {}; // 返回空栅格 + if (!cloud || cloud->empty()) return {}; // 返回空栅格 // 1. 条件过滤 auto filtered = applyConditionalFiltering(cloud); @@ -440,16 +413,15 @@ private: } /* ---------- 条件过滤 ---------- */ - pcl::PointCloud::Ptr applyConditionalFiltering(const pcl::PointCloud::Ptr &cloud) + pcl::PointCloud::Ptr applyConditionalFiltering(const pcl::PointCloud::Ptr& cloud) { pcl::PointCloud::Ptr filtered(new pcl::PointCloud); - for (const auto &point : cloud->points) + for (const auto& point : cloud->points) { // 自定义过滤条件：保留特定区域内的点 - if (point.x > filter_min_x_ && point.x < filter_max_x_ && - point.y > filter_min_y_ && point.y < filter_max_y_ && - point.z > filter_min_z_ && point.z < filter_max_z_) + if (point.x > filter_min_x_ && point.x < filter_max_x_ && point.y > filter_min_y_ && + point.y < filter_max_y_ && point.z > filter_min_z_ && point.z < filter_max_z_) { filtered->points.push_back(point); } @@ -460,8 +432,7 @@ private: filtered->is_dense = true; // 不启用车身过滤，直接返回 - if (!filter_car_) - return filtered; + if (!filter_car_) return filtered; // RCLCPP_INFO(get_logger(), "启用车身过滤"); // 使用CropBox移除车身区域 @@ -469,17 +440,15 @@ private: crop.setInputCloud(filtered); // 设置车身区域（最小点和最大点） - Eigen::Vector4f min_point(car_lidar_offset_x_ - car_length_ / 2.0f, - car_lidar_offset_y_ - car_width_ / 2.0f, + Eigen::Vector4f min_point(car_lidar_offset_x_ - car_length_ / 2.0f, car_lidar_offset_y_ - car_width_ / 2.0f, filter_min_z_, 1.0); - Eigen::Vector4f max_point(car_lidar_offset_x_ + car_length_ / 2.0f, - car_lidar_offset_y_ + car_width_ / 2.0f, + Eigen::Vector4f max_point(car_lidar_offset_x_ + car_length_ / 2.0f, car_lidar_offset_y_ + car_width_ / 2.0f, filter_max_z_, 1.0); crop.setMin(min_point); crop.setMax(max_point); - crop.setNegative(true); // true表示保留外部点，false表示保留内部点 + crop.setNegative(true); // true表示保留外部点，false表示保留内部点 pcl::PointCloud::Ptr output(new pcl::PointCloud); crop.filter(*output); @@ -489,11 +458,11 @@ private: // 右侧主刷 (x:0.85~1.25, y:0.6~0.96, z:1.2~1.65) pcl::CropBox crop1; crop1.setInputCloud(output); - Eigen::Vector4f min1(0.85f, 0.6f, 1.2f, 1.0); // 右侧主刷最小点 - Eigen::Vector4f max1(1.25f, 0.96f, 1.65f, 1.0); // 右侧主刷最大点 + Eigen::Vector4f min1(0.85f, 0.6f, 1.2f, 1.0); // 右侧主刷最小点 + Eigen::Vector4f max1(1.25f, 0.96f, 1.65f, 1.0); // 右侧主刷最大点 crop1.setMin(min1); crop1.setMax(max1); - crop1.setNegative(true); // 剔除右侧主刷内的点 + crop1.setNegative(true); // 剔除右侧主刷内的点 pcl::PointCloud::Ptr output1(new pcl::PointCloud); crop1.filter(*output1); @@ -502,20 +471,20 @@ private: pcl::CropBox crop2; crop2.setInputCloud(output1); // X轴对称：将y坐标取相反数，保持x和z坐标不变 - Eigen::Vector4f min2(0.85f, -0.96f, 1.2f, 1.0); // 左侧主刷最小点（y取反后注意交换大小） - Eigen::Vector4f max2(1.25f, -0.6f, 1.65f, 1.0); // 左侧主刷最大点（y取反后注意交换大小） + Eigen::Vector4f min2(0.85f, -0.96f, 1.2f, 1.0); // 左侧主刷最小点（y取反后注意交换大小） + Eigen::Vector4f max2(1.25f, -0.6f, 1.65f, 1.0); // 左侧主刷最大点（y取反后注意交换大小） crop2.setMin(min2); crop2.setMax(max2); - crop2.setNegative(true); // 剔除左侧主刷内的点 + crop2.setNegative(true); // 剔除左侧主刷内的点 pcl::PointCloud::Ptr final_output(new pcl::PointCloud); crop2.filter(*final_output); - return final_output; // 剔除两侧主刷 + return final_output; // 剔除两侧主刷 } /* ---------- 降采样（体素网格过滤） ---------- */ - pcl::PointCloud::Ptr applyVoxelGridFiltering(const pcl::PointCloud::Ptr &cloud) + pcl::PointCloud::Ptr applyVoxelGridFiltering(const pcl::PointCloud::Ptr& cloud) { pcl::PointCloud::Ptr downsampled(new pcl::PointCloud); pcl::VoxelGrid vg; @@ -528,21 +497,21 @@ private: } /* ---------- 离群点去除 ---------- */ - pcl::PointCloud::Ptr applyStatisticalOutlierRemoval(const pcl::PointCloud::Ptr &cloud) + pcl::PointCloud::Ptr applyStatisticalOutlierRemoval(const pcl::PointCloud::Ptr& cloud) { pcl::PointCloud::Ptr filtered(new pcl::PointCloud); pcl::StatisticalOutlierRemoval sor; sor.setInputCloud(cloud); - sor.setMeanK(stat_mean_k_); // 考虑每个点的k个最近邻 - sor.setStddevMulThresh(stat_std_thresh_); // 标准差倍数阈值 + sor.setMeanK(stat_mean_k_); // 考虑每个点的k个最近邻 + sor.setStddevMulThresh(stat_std_thresh_); // 标准差倍数阈值 sor.filter(*filtered); return filtered; } /* ---------- 栅格化处理 ---------- */ - std::vector> generateOccupancyGrid(const pcl::PointCloud::Ptr &cloud) + std::vector> generateOccupancyGrid(const pcl::PointCloud::Ptr& cloud) { // 0:空 1:障碍物 2:车体 std::vector> grid(grid_size_, std::vector(grid_size_, 0)); @@ -552,8 +521,10 @@ private: if (filter_car_) { // 计算车体在栅格中的位置 - int car_min_x = static_cast((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution); - int car_max_x = static_cast((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution); + int car_min_x = + static_cast((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution); + int car_max_x = + static_cast((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution); int car_min_y = static_cast((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) / resolution); int car_max_y = static_cast((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) / resolution); @@ -574,7 +545,7 @@ private: } // 标记点云障碍物 - for (const auto &point : cloud->points) + for (const auto& point : cloud->points) { // X轴（前向）映射到行索引（从上到下为正X到负X） int grid_x = static_cast((grid_range_ / 2 - point.x) / resolution); @@ -596,7 +567,7 @@ private: } /* ---------- 可视化栅格 - 终端打印 ---------- */ - void visualizeGridInTerminal(const std::vector> &grid) + void visualizeGridInTerminal(const std::vector>& grid) { std::system("clear"); @@ -634,24 +605,24 @@ private: } /* ---------- 发布栅格 ---------- */ - void publishGrid(const std::vector> &grid) + void publishGrid(const std::vector>& grid) { // 创建Int32MultiArray消息 auto msg = std_msgs::msg::Int32MultiArray(); // 设置维度信息 msg.layout.dim.resize(2); - msg.layout.dim[0].label = "height"; // 行（对应X轴） + msg.layout.dim[0].label = "height"; // 行（对应X轴） msg.layout.dim[0].size = grid.size(); msg.layout.dim[0].stride = grid.size() * grid[0].size(); - msg.layout.dim[1].label = "width"; // 列（对应Y轴） + msg.layout.dim[1].label = "width"; // 列（对应Y轴） msg.layout.dim[1].size = grid[0].size(); msg.layout.dim[1].stride = grid[0].size(); // 设置数据（按行优先展开） msg.data.clear(); - for (const auto &row : grid) + for (const auto& row : grid) { msg.data.insert(msg.data.end(), row.begin(), row.end()); } @@ -662,8 +633,7 @@ private: /* ---------- 优化的匹配算法 ---------- */ std::pair, std::shared_ptr> findBestMatchOptimized() { - if (front_clouds_.empty() || rear_clouds_.empty()) - return {nullptr, nullptr}; + if (front_clouds_.empty() || rear_clouds_.empty()) return {nullptr, nullptr}; // 快速策略：优先使用最新的点云进行匹配 auto front_candidate = front_clouds_.front(); @@ -708,8 +678,7 @@ private: } /* ---------- 移除已使用的点云 ---------- */ - void removeUsedClouds(std::shared_ptr used_front, - std::shared_ptr used_rear) + void removeUsedClouds(std::shared_ptr used_front, std::shared_ptr used_rear) { // 从front_clouds_中移除 auto front_it = std::find(front_clouds_.begin(), front_clouds_.end(), used_front); @@ -735,7 +704,7 @@ private: void cleanupExpiredClouds() { auto now_time = now(); - auto cleanup_threshold = max_wait_time_; // 使用配置的等待时间 + auto cleanup_threshold = max_wait_time_; // 使用配置的等待时间 // 清理front_clouds_中过期的数据 while (!front_clouds_.empty()) @@ -777,13 +746,12 @@ private: } /* ---------- 合并点云 ---------- */ - std::shared_ptr mergeClouds(const CloudFrame &front, const CloudFrame &rear) + std::shared_ptr mergeClouds(const CloudFrame& front, const CloudFrame& rear) { size_t front_points = front.cloud->data.size() / front.cloud->point_step; size_t rear_points = rear.cloud->data.size() / rear.cloud->point_step; - if (front.cloud->fields != rear.cloud->fields || - front.cloud->point_step != rear.cloud->point_step || + if (front.cloud->fields != rear.cloud->fields || front.cloud->point_step != rear.cloud->point_step || front.cloud->is_bigendian != rear.cloud->is_bigendian) { RCLCPP_ERROR(get_logger(), "PointCloud2 formats do not match!"); @@ -811,7 +779,7 @@ private: } /* ---------- 坐标变换 - 简化版 ---------- */ - std::shared_ptr transformCloud(const PointCloud2 &in) + std::shared_ptr transformCloud(const PointCloud2& in) { if (in.header.frame_id == target_frame_) { @@ -823,20 +791,16 @@ private: try { // 使用最新可用的变换，不等待 - auto tf = tf_buffer_->lookupTransform( - target_frame_, in.header.frame_id, - tf2::TimePointZero); + auto tf = tf_buffer_->lookupTransform(target_frame_, in.header.frame_id, tf2::TimePointZero); auto out = cloud_pool_.acquire(); tf2::doTransform(in, *out, tf); return out; } - catch (const tf2::TransformException &e) + catch (const tf2::TransformException& e) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, - "TF failed (%s -> %s): %s", - in.header.frame_id.c_str(), - target_frame_.c_str(), e.what()); + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "TF failed (%s -> %s): %s", + in.header.frame_id.c_str(), target_frame_.c_str(), e.what()); return nullptr; } } @@ -845,12 +809,10 @@ private: void printStats() { auto current_time = now(); - if ((current_time - last_stats_time_).seconds() >= 5.0) // 每5秒输出一次 + if ((current_time - last_stats_time_).seconds() >= 5.0) // 每5秒输出一次 { - RCLCPP_INFO(get_logger(), - "Stats: Front=%d, Rear=%d, Merged=%d, Cache(F:%zu,R:%zu)", - front_count_, rear_count_, merged_count_, - front_clouds_.size(), rear_clouds_.size()); + RCLCPP_INFO(get_logger(), "Stats: Front=%d, Rear=%d, Merged=%d, Cache(F:%zu,R:%zu)", front_count_, + rear_count_, merged_count_, front_clouds_.size(), rear_clouds_.size()); last_stats_time_ = current_time; } @@ -866,11 +828,11 @@ private: float filter_min_x_, filter_max_x_; float filter_min_y_, filter_max_y_; float filter_min_z_, filter_max_z_; - float voxel_size_; // 降采样体素大小 - int stat_mean_k_; // 统计离群点去除的k值 - float stat_std_thresh_; // 统计离群点去除的标准差阈值 - int grid_size_; // 栅格大小 - float grid_range_; // 栅格范围 + float voxel_size_; // 降采样体素大小 + int stat_mean_k_; // 统计离群点去除的k值 + float stat_std_thresh_; // 统计离群点去除的标准差阈值 + int grid_size_; // 栅格大小 + float grid_range_; // 栅格范围 bool enable_print_; // 车身过滤参数 @@ -901,7 +863,7 @@ private: int front_count_, rear_count_, merged_count_; }; -int main(int argc, char **argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); diff --git a/src/perception/rtk/CMakeLists.txt b/src/perception/rtk/CMakeLists.txt index 30c40f3..092e378 100644 --- a/src/perception/rtk/CMakeLists.txt +++ b/src/perception/rtk/CMakeLists.txt @@ -20,25 +20,25 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sweeper_interfaces REQUIRED) +find_package(logger REQUIRED) #find_package(Boost 1.71.0 REQUIRED COMPONENTS thread atomic system regex) #if(Boost_FOUND) - #include_directories(${Boost_INCLUDE_DIRS}) +#include_directories(${Boost_INCLUDE_DIRS}) #endif() -include_directories( - include/rtk - ${catkin_INCLUDE_DIRS} -) +include_directories(include/rtk ${catkin_INCLUDE_DIRS}) add_executable(rtk_node src/rtk_node.cpp src/serial_read.cpp) target_link_libraries(rtk_node ${Boost_LIBRARIES}) -ament_target_dependencies(rtk_node rclcpp std_msgs sweeper_interfaces) - -install(TARGETS +ament_target_dependencies( rtk_node - DESTINATION lib/${PROJECT_NAME} -) + rclcpp + std_msgs + sweeper_interfaces + logger) + +install(TARGETS rtk_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -49,8 +49,6 @@ if(BUILD_TESTING) # uncomment the line when this package is not in a git repo #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() -endif() +endif() ament_package() - - diff --git a/src/perception/rtk/include/rtk/serial_read.hpp b/src/perception/rtk/include/rtk/serial_read.hpp index 4ae4712..58de184 100644 --- a/src/perception/rtk/include/rtk/serial_read.hpp +++ b/src/perception/rtk/include/rtk/serial_read.hpp @@ -1,24 +1,23 @@ #ifndef __UART_READ_H__ #define __UART_READ_H__ -#include #include +#include using namespace std; using namespace boost::asio; class Boost_serial { -public: + public: Boost_serial(); ~Boost_serial(); void init(const string port_name); - int serial_read(char buf[],int size); + int serial_read(char buf[], int size); -private: + private: io_service service; - serial_port *sp; + serial_port* sp; }; - #endif \ No newline at end of file diff --git a/src/perception/rtk/package.xml b/src/perception/rtk/package.xml index f727ab9..8286f2d 100644 --- a/src/perception/rtk/package.xml +++ b/src/perception/rtk/package.xml @@ -12,6 +12,7 @@ rclcpp std_msgs sweeper_interfaces + logger ament_lint_auto ament_lint_common diff --git a/src/perception/rtk/src/rtk_node.cpp b/src/perception/rtk/src/rtk_node.cpp index 8fcfe06..1956f96 100644 --- a/src/perception/rtk/src/rtk_node.cpp +++ b/src/perception/rtk/src/rtk_node.cpp @@ -1,18 +1,20 @@ -#include "rclcpp/rclcpp.hpp" -#include "sweeper_interfaces/msg/rtk.hpp" -#include "serial_read.hpp" - -#include #include +#include + #include +#include "logger/logger.h" +#include "rclcpp/rclcpp.hpp" +#include "serial_read.hpp" +#include "sweeper_interfaces/msg/rtk.hpp" + class rtk_node : public rclcpp::Node { -public: + public: // 构造函数,有一个参数为节点名称 rtk_node(std::string name) : Node(name) { - RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); + LOG_INFO("%s节点已经启动.", name.c_str()); lat = 0.0; lon = 0.0; @@ -29,17 +31,17 @@ public: timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&rtk_node::timer_callback, this)); } -private: + private: void GPYBM_to_gps(char serial_buf[]) { position = 0; line_string.clear(); - char *p_gpybm = strstr(serial_buf, "GPYBM"); + char* p_gpybm = strstr(serial_buf, "GPYBM"); if (p_gpybm == NULL) { - RCLCPP_INFO(this->get_logger(), "未检测到GPYBM字符串!"); + LOG_INFO("未检测到GPYBM字符串!"); return; } @@ -103,13 +105,13 @@ private: // 读取串口传来的定位信息 memset(serial_buf, 0, sizeof(serial_buf)); int num = boost_serial->serial_read(serial_buf, 200); - // printf("num:%d \n",num); - // printf("serial_buf: "); + // LOG_DEBUG("num:%d \n",num); + // LOG_DEBUG("serial_buf: "); // for (int i = 0; i < 300; i++) // { - // printf("%c", serial_buf[i]); + // LOG_DEBUG("%c", serial_buf[i]); // } - // printf("\n"); + // LOG_DEBUG("\n"); if (c_queue.size() >= 400) { @@ -132,12 +134,12 @@ private: } // 解析定位信息 - // printf("gps_buf: "); + // LOG_DEBUG("gps_buf: "); // for (int i = 0; i < 300; i++) // { - // printf("%c", gps_buf[i]); + // LOG_DEBUG("%c", gps_buf[i]); // } - // printf("\n"); + // LOG_DEBUG("\n"); GPYBM_to_gps(gps_buf); @@ -152,8 +154,8 @@ private: message.h_quality = h_quality; // 日志打印 - RCLCPP_INFO(this->get_logger(), "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", - message.lat, message.lon, message.head, message.speed, message.p_quality, message.h_quality); + LOG_INFO("lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", message.lat, + message.lon, message.head, message.speed, message.p_quality, message.h_quality); // 发布消息 command_publisher_->publish(message); } @@ -170,7 +172,7 @@ private: rclcpp::Publisher::SharedPtr command_publisher_; // 串口读取类指针 - Boost_serial *boost_serial; + Boost_serial* boost_serial; // 串口读取buffer char serial_buf[300]; @@ -180,8 +182,8 @@ private: double lon; double head; double speed; - int p_quality; // 定位解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解 - int h_quality; // 定向解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解 + int p_quality; // 定位解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解 + int h_quality; // 定向解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解 std::queue c_queue; // 解析定位信息用到的中间变量 @@ -190,15 +192,21 @@ private: string line_string; }; -int main(int argc, char **argv) +int main(int argc, char** argv) { - unsigned int a = -1; - printf("%u\n", a); + // 初始化日志系统 + logger::Logger::Init("rtk", "./log"); + + // unsigned int a = -1; + // LOG_DEBUG("%u\n", a); rclcpp::init(argc, argv); /*创建对应节点的共享指针对象*/ auto node = std::make_shared("rtk_node"); /* 运行节点，并检测退出信号*/ rclcpp::spin(node); rclcpp::shutdown(); + + // 关闭日志系统 + logger::Logger::Shutdown(); return 0; } diff --git a/src/perception/rtk/src/serial_read.cpp b/src/perception/rtk/src/serial_read.cpp index a68c546..3a90853 100644 --- a/src/perception/rtk/src/serial_read.cpp +++ b/src/perception/rtk/src/serial_read.cpp @@ -1,9 +1,8 @@ #include "serial_read.hpp" -Boost_serial::Boost_serial() -{ - ; -} +#include "logger/logger.h" + +Boost_serial::Boost_serial() { ; } Boost_serial::~Boost_serial() { @@ -26,11 +25,11 @@ void Boost_serial::init(const string port_name) sp->set_option(serial_port::parity(serial_port::parity::none)); sp->set_option(serial_port::stop_bits(serial_port::stop_bits::one)); sp->set_option(serial_port::character_size(8)); - cout << "打开串口成功！" << endl; + LOG_INFO("打开串口成功！"); } else { - cout << "打开串口失败！" << endl; + LOG_ERROR("打开串口失败！"); } } @@ -39,8 +38,7 @@ int Boost_serial::serial_read(char buf[], int size) int num = read(*sp, buffer(buf, size)); if (num <= 0) { - cout << "没有读到数据！" << endl; + LOG_WARN("没有读到数据！"); } return num; } -