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

持久化日志并格式化代码

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

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

@ -19,10 +19,7 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang
add_compile_options(-w) # 禁用所有警告
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)

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

@ -1,21 +1,23 @@
#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 <math.h>
#include <time.h>
#include <algorithm>
#include <iostream>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include <sstream>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <vector>
#include <string>
#include <algorithm>
#include <vector>
#include <math.h>
#include <iostream>
#include <time.h>
#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;
// 栅格值定义
@ -34,7 +36,7 @@ class fu_node : public rclcpp::Node
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); // 是否是否开启遇障停车
@ -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<std_msgs::msg::Int32MultiArray>(
"grid_raw", 10, std::bind(&fu_node::gridCallback, this, std::placeholders::_1));
msg_subscribe_pl = this->create_subscription<sweeper_interfaces::msg::Pl>("pl_message", 10,
std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1));
msg_subscribe_DetectLine = this->create_subscription<sweeper_interfaces::msg::DetectLine>("airy_message", 10,
std::bind(&fu_node::msg_callback_DetectLine, this, std::placeholders::_1));
msg_subscribe_pl = this->create_subscription<sweeper_interfaces::msg::Pl>(
"pl_message", 10, std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1));
msg_subscribe_DetectLine = this->create_subscription<sweeper_interfaces::msg::DetectLine>(
"airy_message", 10, std::bind(&fu_node::msg_callback_DetectLine, this, std::placeholders::_1));
// 发布控制指令
pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("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,19 +95,13 @@ 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:
@ -216,15 +211,13 @@ private:
// 评估绕障可行性
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;
}
@ -315,12 +308,12 @@ private:
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_)
{
@ -376,23 +369,18 @@ private:
if (grid[i][j] == VEHICLE)
{
// 更新最小和最大行坐标
if (vehicle_min_x_ == -1 || static_cast<int>(i) < vehicle_min_x_)
vehicle_min_x_ = i;
if (vehicle_max_x_ == -1 || static_cast<int>(i) > vehicle_max_x_)
vehicle_max_x_ = i;
if (vehicle_min_x_ == -1 || static_cast<int>(i) < vehicle_min_x_) vehicle_min_x_ = i;
if (vehicle_max_x_ == -1 || static_cast<int>(i) > vehicle_max_x_) vehicle_max_x_ = i;
// 更新最小和最大列坐标
if (vehicle_min_y_ == -1 || static_cast<int>(j) < vehicle_min_y_)
vehicle_min_y_ = j;
if (vehicle_max_y_ == -1 || static_cast<int>(j) > vehicle_max_y_)
vehicle_max_y_ = j;
if (vehicle_min_y_ == -1 || static_cast<int>(j) < vehicle_min_y_) vehicle_min_y_ = j;
if (vehicle_max_y_ == -1 || static_cast<int>(j) > vehicle_max_y_) vehicle_max_y_ = j;
}
}
}
// 如果找到车体，计算必停区域
if (vehicle_min_x_ != -1 && vehicle_max_x_ != -1 &&
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());
}
@ -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,8 +453,7 @@ private:
analyzeObstacles(grid);
// 可视化栅格
if (enable_visualize_grid_)
visualizeGridInTerminal(grid);
if (enable_visualize_grid_) visualizeGridInTerminal(grid);
cacheGridData(grid); // 缓存栅格数据
}
@ -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<size_t>(i) < grid.size() &&
static_cast<size_t>(j) < grid[0].size() &&
if (static_cast<size_t>(i) < grid.size() && static_cast<size_t>(j) < grid[0].size() &&
grid[i][j] == OBSTACLE)
{
obstacle_in_front_ = true;
@ -621,7 +604,8 @@ private:
}
// 计算绕障可用空间 - 在更大范围内分析左右通道
void calculateAvoidanceSpace(const std::vector<std::vector<int>> &grid, int core_left_obstacle, int core_right_obstacle)
void calculateAvoidanceSpace(const std::vector<std::vector<int>>& grid, int core_left_obstacle,
int core_right_obstacle)
{
if (obstacle_in_front_area_)
{
@ -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<int>(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<std::vector<int>> &grid,
size_t min_x, size_t max_x, int check_start_y, int check_end_y)
int calculateSpaceInDirection(const std::vector<std::vector<int>>& 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<int>(grid[0].size()))
break;
if (j < 0 || j >= static_cast<int>(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;
@ -697,26 +678,26 @@ private:
void visualizeGridInTerminal(const std::vector<std::vector<int>>& grid)
{
std::system("clear");
std::stringstream ss;
std::cout << " --------------------障碍物矩阵-------------------- " << std::endl;
ss << " --------------------障碍物矩阵-------------------- " << std::endl;
// 打印顶部边框
std::cout << " " << std::string(grid[0].size(), '-') << std::endl;
ss << " " << std::string(grid[0].size(), '-') << std::endl;
// 打印列号（仅0-9）
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,24 +720,26 @@ 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)
@ -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)
{
@ -799,7 +782,7 @@ private:
else
{
message.rpm = 0;
RCLCPP_ERROR(this->get_logger(), "rtk信号差,停车！！！");
LOG_ERROR("rtk信号差,停车！！！");
}
message.angle = publish_angle; // 负数表示左转，正数表示右转
@ -830,11 +813,8 @@ private:
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;
}
@ -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;
}
@ -981,16 +955,14 @@ private:
if (avoid_ctx_.counter % 30 == 0)
{ // 每3秒打印一次
RCLCPP_INFO(this->get_logger(), "障碍物无法绕行(宽度=%d格)，等待通过...",
obstacle_analysis_.obstacle_width);
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,9 +1020,7 @@ 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)
@ -1073,13 +1037,13 @@ 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("障碍物在右侧,倾向左绕");
}
}
@ -1087,8 +1051,7 @@ private:
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) right_score += 15;
// 5. 历史趋势稳定性(避免频繁切换) - 权重提高
if (!avoid_ctx_.angle_history.empty() && avoid_ctx_.angle_history.size() >= 5) // 从3提高到5
@ -1096,8 +1059,8 @@ private:
int recent_avg = 0;
int count = std::min(5, static_cast<int>(avoid_ctx_.angle_history.size()));
for (int i = avoid_ctx_.angle_history.size() - count;
i < static_cast<int>(avoid_ctx_.angle_history.size()); i++)
for (int i = avoid_ctx_.angle_history.size() - count; i < static_cast<int>(avoid_ctx_.angle_history.size());
i++)
{
recent_avg += avoid_ctx_.angle_history[i];
}
@ -1105,17 +1068,13 @@ private:
if (recent_avg < -8) // 阈值从5提高到8
left_score += 15; // 权重从10提高到15
if (recent_avg > 8)
right_score += 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,
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;
@ -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;
}
@ -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,19 +1198,16 @@ 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 ? "✓" : "✗",
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,20 +1221,20 @@ 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;
}
@ -1289,9 +1245,7 @@ private:
{
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;
}
}
@ -1299,9 +1253,8 @@ private:
{
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,7 +1267,8 @@ 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; // 基础角度
@ -1343,16 +1297,13 @@ 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格
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;
}
@ -1366,9 +1317,7 @@ private:
int required_position = obstacle_analysis_.left_edge - SAFE_MARGIN;
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;
}
}
@ -1377,9 +1326,7 @@ private:
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;
}
}
@ -1391,20 +1338,16 @@ private:
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<int>(cached_grid_[0].size()))
continue;
if (col < 0 || col >= static_cast<int>(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;
}
}
@ -1418,21 +1361,17 @@ private:
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<int>(cached_grid_[0].size()))
break;
if (check_col >= static_cast<int>(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;
}
}
@ -1443,30 +1382,24 @@ private:
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) ? "左" : "右",
LOG_INFO("横向间隙检查通过: 方向=%s, 已移动=%d格", (avoid_ctx_.direction == -1) ? "左" : "右",
avoid_ctx_.lateral_shift_achieved);
return true;
@ -1508,17 +1441,13 @@ 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,
LOG_INFO("阶段2进行中: 平行前进 %.1fs, 障碍物位置检查=%s", parallel_time,
obstacle_aligned_or_behind ? "已通过" : "未通过");
}
}
@ -1530,8 +1459,7 @@ 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;
@ -1539,15 +1467,13 @@ private:
{
// 检查右侧区域是否清空（障碍物应该在左后方）
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 // 右绕
{
// 检查左侧区域是否清空（障碍物应该在右后方）
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;
}
@ -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
{
@ -1650,9 +1571,7 @@ private:
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;
}
@ -1718,9 +1637,14 @@ private:
int main(int argc, char** argv)
{
// 初始化日志系统
logger::Logger::Init("fu", "./log");
rclcpp::init(argc, argv);
auto node = std::make_shared<fu_node>("fu_node");
rclcpp::spin(node);
rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
return 0;
}

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

@ -21,19 +21,19 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(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)

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

@ -1,9 +1,12 @@
#include "pl.hpp"
#include <iostream>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include <iostream>
#include "logger/logger.h"
using namespace std;
@ -46,7 +49,8 @@ double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
}
// Cur_navAngle指定y轴正方向.Cur_lonti，Cur_lati为原点；x_diff,y_diff是Dest_lonti,Dest_lati在该坐标系的直角坐标:m
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);
@ -75,17 +79,16 @@ int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_in
int Nearest_point_index = start_index; // 初始化为起始索引
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; // 环绕访问
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)
{
@ -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)
{
@ -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;
@ -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;
@ -287,10 +288,10 @@ void *pl_thread(void *args)
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;

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

@ -1,19 +1,21 @@
#include <arpa/inet.h>
#include <pthread.h>
#include <sys/socket.h>
#include <unistd.h>
#include <chrono> // 新增：用于时间戳处理
#include <cstring>
#include <fstream>
#include <iostream>
#include "json.h"
#include "logger/logger.h"
#include "pl.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/rtk.hpp"
#include "sweeper_interfaces/msg/pl.hpp"
#include "sweeper_interfaces/msg/route.hpp"
#include "sweeper_interfaces/msg/rtk.hpp"
#include "sweeper_interfaces/msg/task.hpp"
#include "json.h"
#include <pthread.h>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include "pl.hpp"
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <cstring>
#include <chrono> // 新增：用于时间戳处理
using namespace std;
using namespace std::chrono; // 新增：时间戳命名空间
@ -32,21 +34,25 @@ class pl_node : public rclcpp::Node
public:
pl_node(std::string name) : Node(name)
{
RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
LOG_INFO("%s节点已经启动.", name.c_str());
// 初始化最后接收时间为当前时间（避免初始状态直接超时）
last_rtk_time = system_clock::now();
// 创建订阅者订阅话题
msg_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>("rtk_message", 10, std::bind(&pl_node::msg_callback, this, std::placeholders::_1));
task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>("task_message/download", 10, std::bind(&pl_node::task_callback, this, std::placeholders::_1));
msg_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>(
"rtk_message", 10, std::bind(&pl_node::msg_callback, this, std::placeholders::_1));
task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>(
"task_message/download", 10, std::bind(&pl_node::task_callback, this, std::placeholders::_1));
// 创建发布者
msg_publisher_ = this->create_publisher<sweeper_interfaces::msg::Pl>("pl_message", 10);
task_publisher_ = this->create_publisher<sweeper_interfaces::msg::Task>("task_message/upload", 10);
// 创建定时器，定时发布
timer_pl = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&pl_node::timer_callback_pl, this));
timer_task = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&pl_node::timer_callback_task, this));
timer_pl =
this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&pl_node::timer_callback_pl, this));
timer_task =
this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&pl_node::timer_callback_task, this));
}
private:
@ -56,7 +62,8 @@ private:
// 新增：更新最后接收时间戳
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()
@ -103,7 +110,7 @@ private:
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,14 +119,14 @@ 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)
{
RCLCPP_ERROR(this->get_logger(), "Connection Failed");
LOG_ERROR("Connection Failed");
close(sock);
sock = -1;
return;
@ -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
@ -170,10 +177,11 @@ private:
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,7 +215,7 @@ 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))
@ -220,11 +228,15 @@ int main(int argc, char **argv)
{
initConfig();
rclcpp::init(argc, argv);
// 初始化日志系统
logger::Logger::Init("pl", "./log");
/*创建对应节点的共享指针对象*/
auto node = std::make_shared<pl_node>("pl_node");
/* 运行节点，并检测退出信号*/
rclcpp::spin(node);
rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
// pthread_join(pl_thread_t, NULL);
return 0;

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

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

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

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

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

@ -1,19 +1,21 @@
#include <curl/curl.h>
#include <pthread.h>
#include <unistd.h>
#include <chrono>
#include <cstdio>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <mutex>
#include <sstream>
#include "json.h"
#include "logger/logger.h"
#include "md5.h"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/rtk.hpp"
#include "sweeper_interfaces/msg/sub.hpp"
#include <curl/curl.h>
#include "md5.h"
#include "json.h"
#include <pthread.h>
#include <iostream>
#include <unistd.h>
#include <cstdio>
#include <chrono>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <mutex>
std::mutex collect_mutex; // 互斥锁，用于保护 isCollecting 变量
@ -47,7 +49,7 @@ string calculate_md5(string filename)
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)
@ -57,20 +59,22 @@ bool upload_file(string filename)
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)
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)
@ -80,13 +84,13 @@ void copyFileAndOverwrite(const std::string &sourceFilePath, const std::string &
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;
}
@ -94,7 +98,7 @@ void copyFileAndOverwrite(const std::string &sourceFilePath, const std::string &
if (!dst)
{
std::cerr << "复制文件时出错" << std::endl;
LOG_ERROR("复制文件时出错");
}
}
class route_node : public rclcpp::Node
@ -102,11 +106,13 @@ class route_node : public rclcpp::Node
public:
route_node(std::string name) : Node(name)
{
RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
LOG_INFO("%s节点已经启动.", name.c_str());
// 创建一个订阅者订阅话题
sub_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Sub>("gather", 10, std::bind(&route_node::sub_callback, this, std::placeholders::_1));
sub_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Sub>(
"gather", 10, std::bind(&route_node::sub_callback, this, std::placeholders::_1));
msg_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>("rtk_message", 10, std::bind(&route_node::msg_callback, this, std::placeholders::_1));
msg_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Rtk>(
"rtk_message", 10, std::bind(&route_node::msg_callback, this, std::placeholders::_1));
}
private:
@ -116,7 +122,7 @@ private:
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,13 +139,11 @@ 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)
@ -148,25 +152,21 @@ private:
{
std::lock_guard<std::mutex> lock(collect_mutex); // 加锁
isCollecting = true;
// cout << "平台开始采集" << endl;
RCLCPP_INFO(this->get_logger(), "平台开始采集");
LOG_INFO("平台开始采集");
pthread_create(&route_thread_t, NULL, route_thread, NULL);
}
else if (!msg->get_route && isCollecting)
{
isCollecting = false;
// cout << "平台结束采集" << endl;
RCLCPP_INFO(this->get_logger(), "平台结束采集");
LOG_INFO("平台结束采集");
pthread_cancel(route_thread_t);
if (upload_file(filename))
{
// cout << "上传成功" << endl;
RCLCPP_INFO(this->get_logger(), "上传成功");
LOG_INFO("上传成功");
}
else
{
// cout << "上传失败" << endl;
RCLCPP_INFO(this->get_logger(), "上传失败");
LOG_INFO("上传失败");
}
}
}
@ -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,14 +224,14 @@ 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(); // 关闭文件流
}
@ -240,11 +240,15 @@ int main(int argc, char **argv)
{
init_main();
rclcpp::init(argc, argv);
/*初始化日志系统*/
logger::Logger::Init("route", "./log");
/*创建对应节点的共享指针对象*/
auto node = std::make_shared<route_node>("route_node");
/* 运行节点，并检测退出信号*/
rclcpp::spin(node);
rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
// pthread_join(route_thread_t, NULL);
return 0;
}

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

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

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

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

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

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

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

@ -1,27 +1,25 @@
#include "mc/can_driver.h"
#include <cstring>
#include <stdexcept>
#include <iostream>
#include <thread>
#include <fcntl.h>
#include <unistd.h>
#include <poll.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <poll.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <cstring>
#include <iostream>
#include <stdexcept>
#include <thread>
CANDriver::CANDriver() = default;
CANDriver::~CANDriver()
{
close();
}
CANDriver::~CANDriver() { close(); }
bool CANDriver::open(const std::string& interface)
{
if (running)
return false;
if (running) return false;
sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sockfd < 0)
@ -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)
{
@ -79,15 +75,12 @@ void CANDriver::close()
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);
@ -108,8 +101,7 @@ void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData)
bool CANDriver::setFilter(const std::vector<can_filter>& filters)
{
if (!running)
return false;
if (!running) return false;
filters_ = filters;
return applyFilters();
@ -129,8 +121,7 @@ bool CANDriver::addFilters(const std::vector<can_filter> &filters)
bool CANDriver::applyFilters()
{
if (!running)
return false;
if (!running) return false;
if (filters_.empty())
{
@ -138,8 +129,7 @@ bool CANDriver::applyFilters()
return true;
}
if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER,
filters_.data(), filters_.size() * sizeof(can_filter)) < 0)
if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, filters_.data(), filters_.size() * sizeof(can_filter)) < 0)
{
perror("setsockopt");
return false;

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

@ -1,6 +1,9 @@
#include "mc/can_utils.hpp"
#include <sstream>
#include <iomanip>
#include <sstream>
#include "logger/logger.h"
bool g_can_print_enable = false;
@ -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());
}
}

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

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

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

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

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

@ -1,6 +1,8 @@
#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)
@ -11,10 +13,10 @@ void mcuTimerTask(CANDriver &canctl)
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;
// 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 控制
@ -26,7 +28,7 @@ void epsTimerTask(CANDriver &canctl)
eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame());
// std::cout << "epsTimerTask" << std::endl;
// LOG_INFO("epsTimerTask");
}
// 定时器回调：VCU 扫地控制
@ -44,9 +46,9 @@ void vcuTimerTask(CANDriver &canctl)
// 发送CAN使能指令
vcu_enable_cmd.setCANEnable(true);
canctl.sendFrame(vcu_enable_cmd.toFrame());
// std::cout << "mcuTimerTask" << std::endl;
// LOG_INFO("mcuTimerTask");
vcu_initialized = true;
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled");
LOG_INFO("[VCU] CAN control enabled");
}
// 检查清扫状态是否变化
@ -68,7 +70,7 @@ void vcuTimerTask(CANDriver &canctl)
vcu_motor1_cmd.setByte6(target_value);
vcu_motor1_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor1_cmd.toFrame());
// std::cout << "vcuTimerTask1" << std::endl;
// LOG_INFO("vcuTimerTask1");
// ===== 控制0x212报文 (电机M8和LED输出) =====
vcu_motor2_cmd.setByte0(target_value);
@ -80,10 +82,9 @@ void vcuTimerTask(CANDriver &canctl)
vcu_motor2_cmd.setByte6(target_value);
vcu_motor2_cmd.setByte7(target_value);
canctl.sendFrame(vcu_motor2_cmd.toFrame());
// std::cout << "vcuTimerTask2" << std::endl;
// 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;
@ -98,7 +99,7 @@ void bmsTimerTask(CANDriver &canctl)
// 首次运行时初始化日志
if (!bms_initialized)
{
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[BMS] Query task started");
LOG_INFO("[BMS] Query task started");
bms_initialized = true;
}
@ -110,25 +111,17 @@ void bmsTimerTask(CANDriver &canctl)
// 注册所有定时器任务
void setupTimers(rclcpp::Node::SharedPtr node, CANDriver& canctl)
{
static auto timer_mcu = node->create_wall_timer(
std::chrono::milliseconds(50),
[&canctl]()
{ mcuTimerTask(canctl); });
static auto timer_mcu =
node->create_wall_timer(std::chrono::milliseconds(50), [&canctl]() { mcuTimerTask(canctl); });
static auto timer_eps = node->create_wall_timer(
std::chrono::milliseconds(50),
[&canctl]()
{ epsTimerTask(canctl); });
static auto timer_eps =
node->create_wall_timer(std::chrono::milliseconds(50), [&canctl]() { epsTimerTask(canctl); });
static auto timer_vcu = node->create_wall_timer(
std::chrono::milliseconds(100),
[&canctl]()
{ vcuTimerTask(canctl); });
static auto timer_vcu =
node->create_wall_timer(std::chrono::milliseconds(100), [&canctl]() { vcuTimerTask(canctl); });
static auto timer_bms = node->create_wall_timer(
std::chrono::milliseconds(200),
[&canctl]()
{ bmsTimerTask(canctl); });
static auto timer_bms =
node->create_wall_timer(std::chrono::milliseconds(200), [&canctl]() { bmsTimerTask(canctl); });
RCLCPP_INFO(node->get_logger(), "[TIMER] All timers setup completed");
LOG_INFO("[TIMER] All timers setup completed");
}

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

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

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

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

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

@ -1,43 +1,39 @@
#include <rclcpp/rclcpp.hpp>
#include <nlohmann/json.hpp>
#include <atomic>
#include <chrono>
#include <nlohmann/json.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <thread>
#include "vehicle_params/httplib.h"
#include "logger/logger.h"
#include "sweeper_interfaces/msg/vehicle_identity.hpp"
#include "vehicle_params/httplib.h"
using sweeper_interfaces::msg::VehicleIdentity;
class VehicleParamsNode : public rclcpp::Node
{
public:
VehicleParamsNode()
: Node("vehicle_params_node")
VehicleParamsNode() : Node("vehicle_params_node")
{
RCLCPP_INFO(this->get_logger(), "vehicle_params_node starting...");
LOG_INFO("vehicle_params_node starting...");
// ---------- neardi ----------
this->declare_parameter<std::string>("neardi.host", "192.168.5.1");
this->declare_parameter<int>("neardi.port", 8080);
// ---------- publisher ----------
identity_pub_ = this->create_publisher<VehicleIdentity>(
"/vehicle/identity",
rclcpp::QoS(1).transient_local().reliable());
identity_pub_ =
this->create_publisher<VehicleIdentity>("/vehicle/identity", rclcpp::QoS(1).transient_local().reliable());
// ---------- worker ----------
worker_ = std::thread([this]()
{ fetch_from_neardi_loop(); });
worker_ = std::thread([this]() { fetch_from_neardi_loop(); });
}
~VehicleParamsNode()
{
running_.store(false);
if (worker_.joinable())
worker_.join();
if (worker_.joinable()) worker_.join();
}
private:
@ -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,9 +91,7 @@ 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; // 成功一次即可
}
@ -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)
{
rclcpp::init(argc, argv);
/*初始化日志系统*/
logger::Logger::Init("vehicle_params", "./log");
rclcpp::spin(std::make_shared<VehicleParamsNode>());
rclcpp::shutdown();
/*关闭日志系统*/
logger::Logger::Shutdown();
return 0;
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -1,40 +1,38 @@
#include "task_manager.hpp"
#include "md5.h"
#include <iostream>
#include <fstream>
#include <cstdlib>
#include <unistd.h>
TaskManager::TaskManager()
: is_running_(false), task_status_(0),
destination_file_path_("./gps_load_now.txt")
#include <cstdlib>
#include <fstream>
#include <iostream>
#include "logger/logger.h"
#include "md5.h"
TaskManager::TaskManager() : is_running_(false), task_status_(0), destination_file_path_("./gps_load_now.txt")
{
current_task_.id = 0;
current_task_.status = TaskStatus::PENDING;
}
TaskManager::~TaskManager()
{
stop();
}
TaskManager::~TaskManager() { stop(); }
void TaskManager::start()
{
if (is_running_)
{
std::cout << "TaskManager is already running." << std::endl;
LOG_WARN("TaskManager is already running.");
return;
}
is_running_ = true;
task_thread_ = std::thread(&TaskManager::processTasksLoop, this);
std::cout << "TaskManager started." << std::endl;
LOG_INFO("TaskManager started.");
}
void TaskManager::stop()
{
if (!is_running_)
return;
if (!is_running_) return;
{
std::lock_guard<std::mutex> lock(queue_mutex_);
@ -47,7 +45,7 @@ void TaskManager::stop()
task_thread_.join();
}
std::cout << "TaskManager stopped." << std::endl;
LOG_INFO("TaskManager stopped.");
}
void TaskManager::addTask(const MQTT_JSON& mqtt_json)
@ -83,7 +81,7 @@ 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)
@ -93,29 +91,28 @@ string TaskManager::calculate_md5(const string &filename)
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)
{
std::cout << "Processing start task ID: " << json_data.value.id << std::endl;
std::cout << "URL: " << json_data.value.routeInfo.url << std::endl;
std::cout << "MD5: " << json_data.value.routeInfo.md5 << std::endl;
LOG_INFO("Processing start task ID: %d", json_data.value.id);
LOG_INFO("URL: %s", json_data.value.routeInfo.url.c_str());
LOG_INFO("MD5: %s", json_data.value.routeInfo.md5.c_str());
// 更新当前任务ID
updateCurrentTask(json_data.value.id, TaskStatus::RUNNING);
@ -175,11 +170,10 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo)
// 检查文件是否已存在
if (access(filepath.c_str(), F_OK) == -1)
{
std::cout << "File not found, downloading: " << downFileName << std::endl;
if (!downloadFile(json_data.value.routeInfo.url,
json_data.value.routeInfo.md5, downFileName))
LOG_INFO("File not found, downloading: %s", downFileName.c_str());
if (!downloadFile(json_data.value.routeInfo.url, json_data.value.routeInfo.md5, downFileName))
{
std::cout << "Download failed, skipping task." << std::endl;
LOG_WARN("Download failed, skipping task.");
updateCurrentTask(json_data.value.id, TaskStatus::FAILED);
if (send_response_callback_)
{
@ -191,7 +185,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo)
}
else
{
std::cout << "File already exists, skipping download: " << filepath << std::endl;
LOG_INFO("File already exists, skipping download: %s", filepath.c_str());
}
// 复制文件
@ -200,7 +194,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo)
// 启动任务
task_status_ = 1;
updateCurrentTask(json_data.value.id, TaskStatus::RUNNING);
std::cout << "Task started successfully: " << json_data.value.id << std::endl;
LOG_INFO("Task started successfully: %d", json_data.value.id);
// 发送响应（响应主题由调用方决定）
if (send_response_callback_)
@ -213,7 +207,7 @@ bool TaskManager::processStartTask(const JSON_DATA &json_data, long seqNo)
bool TaskManager::processStopTask(long taskId, long seqNo)
{
std::cout << "Processing stop task ID: " << taskId << std::endl;
LOG_INFO("Processing stop task ID: %d", taskId);
task_status_ = 0;
updateCurrentTask(taskId, TaskStatus::COMPLETED);
@ -232,8 +226,7 @@ void TaskManager::processTasksLoop()
std::unique_lock<std::mutex> lock(queue_mutex_);
// 等待队列中有任务或线程需要停止
queue_cv_.wait(lock, [this]
{ return !task_queue_.empty() || !is_running_; });
queue_cv_.wait(lock, [this] { return !task_queue_.empty() || !is_running_; });
// 检查是否需要退出
if (!is_running_ && task_queue_.empty())
@ -265,7 +258,7 @@ void TaskManager::processTasksLoop()
}
catch (const boost::bad_any_cast& e)
{
std::cerr << "Bad any_cast in task processing: " << e.what() << std::endl;
LOG_ERROR("Bad any_cast in task processing: %s", e.what());
}
}
}

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

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

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

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

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

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

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

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

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

@ -1,8 +1,9 @@
#pragma once
#include <fstream>
#include <string>
#include <iostream>
#include <string>
#include "ament_index_cpp/get_package_share_directory.hpp"
#include "nlohmann/json.hpp"

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

@ -1,17 +1,18 @@
#ifndef UART_HANDLER_H
#define UART_HANDLER_H
#include <thread>
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <algorithm>
#include <atomic>
#include <string>
#include <cstring>
#include <iostream>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <termios.h>
#include <string>
#include <thread>
#include <vector>
#include <algorithm>
class UartHandler
{

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

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

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

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

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

@ -1,14 +1,18 @@
#include "radio_ctrl/uart_handler.h"
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include <stdint.h>
#include <unistd.h>
#include <iostream>
#include "logger/logger.h"
#ifndef BOTHER
#define BOTHER 0010000
#endif
@ -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;
}
@ -134,9 +137,9 @@ void UartHandler::print_hex(uint8_t *buf, int len)
{
for (int i = 0; i < len; ++i)
{
printf("%02X ", buf[i]);
LOG_DEBUG("%02X ", buf[i]);
}
printf("\n");
LOG_DEBUG("\n");
}
void UartHandler::parse_data(std::vector<uint8_t>& buffer)
@ -159,8 +162,7 @@ void UartHandler::parse_data(std::vector<uint8_t> &buffer)
if (buffer.size() - index < kSbusFrameLength)
{
// 数据不够，等下次再处理
if (index > 0)
buffer.erase(buffer.begin(), buffer.begin() + index);
if (index > 0) buffer.erase(buffer.begin(), buffer.begin() + index);
return;
}

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

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

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

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

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

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

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

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

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

@ -7,6 +7,7 @@
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#include "logger/logger.h"
#include "nlohmann/json.hpp"
#include "remote_ctrl/config.hpp"
#include "remote_ctrl/mqtt_client.hpp"
@ -83,7 +84,7 @@ class RemoteCtrlNode : public rclcpp::Node
if (!identity_ready_.load())
{
RCLCPP_WARN(this->get_logger(), "[REMOTE] MQTT connected but identity not ready");
LOG_WARN("[REMOTE] MQTT connected but identity not ready");
return;
}
@ -100,13 +101,13 @@ 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",
LOG_WARN("[REMOTE] remote_topic_template missing '{vid}', template=%s",
cfg_.remote_topic_template.c_str());
}
@ -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;
}

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

@ -1,24 +1,23 @@
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <deque>
#include <unordered_map>
#include <mutex>
#include <memory>
#include <chrono>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <chrono>
#include <deque>
#include <functional>
#include <memory>
#include <mutex>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/multi_array_dimension.hpp>
#include <functional>
#include <unordered_map>
using sensor_msgs::msg::PointCloud2;
using std::placeholders::_1;
@ -53,8 +52,7 @@ public:
void release(std::shared_ptr<PointCloud2> cloud)
{
if (!cloud)
return;
if (!cloud) return;
cloud->data.clear();
cloud->width = 0;
@ -111,34 +109,24 @@ public:
car_lidar_offset_y_ = declare_parameter<float>("car_lidar_offset_y", 0.0f); // LiDAR在y轴的安装偏移
// 打印所有参数值（添加到构造函数末尾）
RCLCPP_INFO_STREAM(this->get_logger(),
RCLCPP_INFO_STREAM(
this->get_logger(),
"\n\n---------- 点云融合节点参数配置 ----------"
<< "\n [输入输出]"
<< "\n 前雷达话题: " << 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 前雷达话题: " << 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 ---------- */
@ -146,21 +134,17 @@ public:
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
/* ---------- QoS - 优化为低延时 ---------- */
auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_))
.best_effort()
.durability_volatile();
auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)).best_effort().durability_volatile();
// 移除deadline约束，避免因延时导致的消息丢弃
/* ---------- 订阅 ---------- */
sub_front_ = create_subscription<PointCloud2>(
front_topic_, qos, std::bind(&LidarMerger::frontCB, this, std::placeholders::_1));
sub_rear_ = create_subscription<PointCloud2>(
rear_topic_, qos, std::bind(&LidarMerger::rearCB, this, std::placeholders::_1));
sub_front_ = create_subscription<PointCloud2>(front_topic_, qos,
std::bind(&LidarMerger::frontCB, this, std::placeholders::_1));
sub_rear_ = create_subscription<PointCloud2>(rear_topic_, qos,
std::bind(&LidarMerger::rearCB, this, std::placeholders::_1));
/* ---------- 发布 ---------- */
auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_))
.reliable()
.durability_volatile();
auto pub_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size_)).reliable().durability_volatile();
pub_ = create_publisher<PointCloud2>(output_topic_, pub_qos);
// 创建栅格发布者
@ -207,8 +191,7 @@ private:
}
/* ---------- 同步处理点云 ---------- */
void processCloudSync(const PointCloud2::SharedPtr msg, const std::string &source,
const rclcpp::Time &receive_time)
void processCloudSync(const PointCloud2::SharedPtr msg, const std::string& source, const rclcpp::Time& receive_time)
{
// 立即进行坐标变换
auto transformed_cloud = transformCloud(*msg);
@ -219,11 +202,8 @@ private:
}
// 创建CloudFrame
auto cloud_frame = std::make_shared<CloudFrame>(CloudFrame{
transformed_cloud,
rclcpp::Time(msg->header.stamp),
receive_time,
source});
auto cloud_frame = std::make_shared<CloudFrame>(
CloudFrame{transformed_cloud, rclcpp::Time(msg->header.stamp), receive_time, source});
// 添加到缓存并立即尝试合并
{
@ -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();
@ -289,8 +267,8 @@ private:
if (enable_debug_)
{
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000,
"Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs",
front_age, rear_age, adaptive_threshold);
"Data age check: front_age=%.3fs, rear_age=%.3fs, threshold=%.3fs", front_age,
rear_age, adaptive_threshold);
}
// 不直接返回，而是清理最旧的数据后重试
@ -314,8 +292,7 @@ private:
auto merged = mergeClouds(*front_frame, *rear_frame);
auto merge_end = std::chrono::high_resolution_clock::now();
if (!merged)
return;
if (!merged) return;
// 设置时间戳和frame_id
auto front_time = front_frame->stamp.nanoseconds();
@ -333,8 +310,7 @@ private:
// ========================= 点云 =========================
auto processed_pcl = processPointCloud(merged_pcl);
if (!processed_pcl)
return;
if (!processed_pcl) return;
// 将处理后的PCL点云转回ROS消息
auto processed = std::make_shared<sensor_msgs::msg::PointCloud2>();
@ -346,12 +322,10 @@ 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);
@ -361,12 +335,13 @@ private:
if (enable_debug_ && merged_count_ % 10 == 0) // 每10次输出一次
{
auto total_delay = (now_time - processed->header.stamp).seconds();
auto process_time = std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - merge_start).count();
auto process_time =
std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - merge_start).count();
RCLCPP_INFO(get_logger(),
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,
merged_count_, merged->data.size() / merged->point_step, processed->data.size() / processed->point_step,
total_delay, process_time * 1000, adaptive_threshold);
}
@ -377,8 +352,7 @@ private:
/* ---------- 点云处理流程 ---------- */
pcl::PointCloud<pcl::PointXYZ>::Ptr processPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
if (!cloud || cloud->empty())
return nullptr;
if (!cloud || cloud->empty()) return nullptr;
// 1. 条件过滤
auto filtered = applyConditionalFiltering(cloud);
@ -408,8 +382,7 @@ private:
}
std::vector<std::vector<int>> processPointCloud_grid(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
if (!cloud || cloud->empty())
return {}; // 返回空栅格
if (!cloud || cloud->empty()) return {}; // 返回空栅格
// 1. 条件过滤
auto filtered = applyConditionalFiltering(cloud);
@ -447,9 +420,8 @@ private:
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,12 +440,10 @@ 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);
@ -552,8 +521,10 @@ private:
if (filter_car_)
{
// 计算车体在栅格中的位置
int car_min_x = static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution);
int car_max_x = static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution);
int car_min_x =
static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ + car_length_ / 2.0f)) / resolution);
int car_max_x =
static_cast<int>((grid_range_ / 2 - (car_lidar_offset_x_ - car_length_ / 2.0f)) / resolution);
int car_min_y = static_cast<int>((-car_lidar_offset_y_ - car_width_ / 2.0f + grid_range_ / 2) / resolution);
int car_max_y = static_cast<int>((-car_lidar_offset_y_ + car_width_ / 2.0f + grid_range_ / 2) / resolution);
@ -662,8 +633,7 @@ private:
/* ---------- 优化的匹配算法 ---------- */
std::pair<std::shared_ptr<CloudFrame>, std::shared_ptr<CloudFrame>> findBestMatchOptimized()
{
if (front_clouds_.empty() || rear_clouds_.empty())
return {nullptr, nullptr};
if (front_clouds_.empty() || rear_clouds_.empty()) return {nullptr, nullptr};
// 快速策略：优先使用最新的点云进行匹配
auto front_candidate = front_clouds_.front();
@ -708,8 +678,7 @@ private:
}
/* ---------- 移除已使用的点云 ---------- */
void removeUsedClouds(std::shared_ptr<CloudFrame> used_front,
std::shared_ptr<CloudFrame> used_rear)
void removeUsedClouds(std::shared_ptr<CloudFrame> used_front, std::shared_ptr<CloudFrame> used_rear)
{
// 从front_clouds_中移除
auto front_it = std::find(front_clouds_.begin(), front_clouds_.end(), used_front);
@ -782,8 +751,7 @@ private:
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!");
@ -823,9 +791,7 @@ 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);
@ -833,10 +799,8 @@ private:
}
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;
}
}
@ -847,10 +811,8 @@ private:
auto current_time = now();
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;
}

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

@ -20,25 +20,25 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sweeper_interfaces REQUIRED)
find_package(logger REQUIRED)
#find_package(Boost 1.71.0 REQUIRED COMPONENTS thread atomic system regex)
#if(Boost_FOUND)
#include_directories(${Boost_INCLUDE_DIRS})
#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)
@ -52,5 +52,3 @@ if(BUILD_TESTING)
endif()
ament_package()

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

@ -1,8 +1,8 @@
#ifndef __UART_READ_H__
#define __UART_READ_H__
#include <iostream>
#include <boost/asio.hpp>
#include <iostream>
using namespace std;
using namespace boost::asio;
@ -20,5 +20,4 @@ private:
serial_port* sp;
};
#endif

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

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

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

@ -1,18 +1,20 @@
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/rtk.hpp"
#include "serial_read.hpp"
#include <string.h>
#include <stdlib.h>
#include <string.h>
#include <queue>
#include "logger/logger.h"
#include "rclcpp/rclcpp.hpp"
#include "serial_read.hpp"
#include "sweeper_interfaces/msg/rtk.hpp"
class rtk_node : public rclcpp::Node
{
public:
// 构造函数,有一个参数为节点名称
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;
@ -39,7 +41,7 @@ private:
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);
}
@ -192,13 +194,19 @@ private:
int main(int argc, char** argv)
{
unsigned int a = -1;
printf("%u\n", a);
// 初始化日志系统
logger::Logger::Init("rtk", "./log");
// unsigned int a = -1;
// LOG_DEBUG("%u\n", a);
rclcpp::init(argc, argv);
/*创建对应节点的共享指针对象*/
auto node = std::make_shared<rtk_node>("rtk_node");
/* 运行节点，并检测退出信号*/
rclcpp::spin(node);
rclcpp::shutdown();
// 关闭日志系统
logger::Logger::Shutdown();
return 0;
}

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

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