1067 lines
41 KiB
C++
1067 lines
41 KiB
C++
#include <math.h>
|
||
#include <time.h>
|
||
|
||
#include <algorithm>
|
||
#include <iostream>
|
||
#include <memory>
|
||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||
#include <sensor_msgs/point_cloud2_iterator.hpp>
|
||
#include <sstream>
|
||
#include <std_msgs/msg/int32_multi_array.hpp>
|
||
#include <string>
|
||
#include <vector>
|
||
|
||
#include "logger/logger.h"
|
||
#include "rclcpp/rclcpp.hpp"
|
||
#include "sweeper_interfaces/msg/can_frame.hpp"
|
||
#include "sweeper_interfaces/msg/fu.hpp"
|
||
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
|
||
#include "sweeper_interfaces/msg/pl.hpp"
|
||
#include "sweeper_interfaces/msg/rtk.hpp"
|
||
#include "sweeper_interfaces/msg/sub.hpp"
|
||
|
||
using namespace std;
|
||
using namespace std::chrono;
|
||
|
||
// ================== RTK全局变量 ==================
|
||
system_clock::time_point last_rtk_time;
|
||
const duration<double> rtk_timeout = duration<double>(0.5);
|
||
|
||
struct struct_rtk
|
||
{
|
||
double lon;
|
||
double lat;
|
||
double direction;
|
||
double speed;
|
||
int reliability;
|
||
};
|
||
struct_rtk g_rtk;
|
||
|
||
// ================== GPS转相对坐标的常数 ==================
|
||
#define Lat_Origin 32.045062
|
||
#define Lon_Origin 118.845200366
|
||
#define deg_rad (0.01745329252)
|
||
#define R_LATI (6378137)
|
||
#define R_LONT (5407872)
|
||
|
||
// ================== 枚举定义 ==================
|
||
const int EMPTY = 0;
|
||
const int OBSTACLE = 1;
|
||
const int VEHICLE = 2;
|
||
int is_start = 0;
|
||
|
||
// ================== 路点结构体 ==================
|
||
struct Waypoint
|
||
{
|
||
double x, y;
|
||
int speed;
|
||
double lon, lat; // 路点的绝对坐标（GPS）
|
||
Waypoint() : x(0), y(0), speed(300), lon(0), lat(0) {}
|
||
Waypoint(double px, double py, int s = 300, double plon = 0, double plat = 0)
|
||
: x(px), y(py), speed(s), lon(plon), lat(plat)
|
||
{
|
||
}
|
||
};
|
||
|
||
// ================== 障碍物分析结构体 ==================
|
||
struct ObstacleAnalysis
|
||
{
|
||
bool has_front_critical = false;
|
||
bool has_front_area = false;
|
||
bool has_left_critical = false;
|
||
bool has_right_critical = false;
|
||
|
||
int obstacle_distance = -1;
|
||
int obstacle_width = 0;
|
||
int left_edge = -1, right_edge = -1;
|
||
int free_space_left = 0;
|
||
int free_space_right = 0;
|
||
|
||
int calculateSafeDistance(int speed) const { return std::max(3, speed / 400 + 2); }
|
||
bool needEmergencyBrake(int speed) const
|
||
{
|
||
if (!has_front_critical) return false;
|
||
return obstacle_distance <= calculateSafeDistance(speed);
|
||
}
|
||
};
|
||
|
||
// ================== GPS坐标转换函数 ==================
|
||
// 计算两点间距离(米)
|
||
double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
|
||
{
|
||
double x, y, length;
|
||
x = (R_LONT) * (lonti2 - lonti1) * deg_rad;
|
||
y = (R_LATI) * (lati2 - lati1) * deg_rad;
|
||
length = sqrt(x * x + y * y);
|
||
return length;
|
||
}
|
||
|
||
// GPS转相对坐标(米)
|
||
// Cur_navAngle: 当前航向角(度)
|
||
// Cur_lonti, Cur_lati: 当前位置GPS坐标
|
||
// Dest_lonti, Dest_lati: 目标位置GPS坐标
|
||
// x_diff, 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;
|
||
Navi_rad = Cur_navAngle * deg_rad;
|
||
|
||
x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT;
|
||
y = (Dest_lati - Cur_lati) * deg_rad * R_LATI;
|
||
|
||
k1 = cos(Navi_rad);
|
||
k2 = (-1) * sin(Navi_rad);
|
||
k3 = (-1) * k2;
|
||
k4 = k1;
|
||
|
||
*x_diff = x * k1 + y * k2;
|
||
*y_diff = x * k3 + y * k4;
|
||
|
||
return 0;
|
||
}
|
||
|
||
// ================== 状态机核心类 ==================
|
||
class StateMachine
|
||
{
|
||
public:
|
||
enum State
|
||
{
|
||
NORMAL,
|
||
WAITING,
|
||
LATERAL_SHIFT,
|
||
PARALLEL_MOVE
|
||
};
|
||
|
||
StateMachine() { current_state_ = NORMAL; }
|
||
|
||
void setState(State new_state)
|
||
{
|
||
if (new_state != current_state_)
|
||
{
|
||
exit(current_state_);
|
||
current_state_ = new_state;
|
||
enter(new_state);
|
||
}
|
||
}
|
||
|
||
void execute() {}
|
||
|
||
State getCurrentState() const { return current_state_; }
|
||
|
||
std::string getStateString() const
|
||
{
|
||
switch (current_state_)
|
||
{
|
||
case NORMAL:
|
||
return "正常行驶";
|
||
case WAITING:
|
||
return "等待决策";
|
||
case LATERAL_SHIFT:
|
||
return "横向偏移";
|
||
case PARALLEL_MOVE:
|
||
return "平行前进";
|
||
default:
|
||
return "未知状态";
|
||
}
|
||
}
|
||
|
||
ObstacleAnalysis obstacle_analysis;
|
||
int publish_speed = 0;
|
||
float publish_angle = 0.0f;
|
||
int counter = 0;
|
||
int direction = 0;
|
||
std::vector<float> angle_history;
|
||
rclcpp::Time start_time;
|
||
bool start_time_set = false;
|
||
|
||
private:
|
||
State current_state_;
|
||
|
||
void enter(State state)
|
||
{
|
||
switch (state)
|
||
{
|
||
case NORMAL:
|
||
counter = 0;
|
||
direction = 0;
|
||
break;
|
||
case WAITING:
|
||
publish_speed = 0;
|
||
publish_angle = 0.0f;
|
||
counter = 0;
|
||
break;
|
||
case LATERAL_SHIFT:
|
||
publish_speed = 300;
|
||
counter = 0;
|
||
break;
|
||
case PARALLEL_MOVE:
|
||
publish_speed = 300;
|
||
counter = 0;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
void exit(State state) {}
|
||
};
|
||
|
||
// ================== FU节点类 ==================
|
||
class fu_node : public rclcpp::Node
|
||
{
|
||
public:
|
||
fu_node(std::string name) : Node(name), state_machine_(std::make_unique<StateMachine>())
|
||
{
|
||
LOG_INFO("%s节点已经启动.", name.c_str());
|
||
// 初始化最后接收时间为启动时间（避免初始状态直接超时）
|
||
last_rtk_time = system_clock::now();
|
||
|
||
// ======== 参数声明 ========
|
||
this->declare_parameter("enable_obstacle_stop", true);
|
||
this->declare_parameter("enable_obstacle_avoid", true);
|
||
this->declare_parameter("enable_visualize_grid", true);
|
||
this->declare_parameter("FRONT_STOP_ZONE_ROWS", 3);
|
||
this->declare_parameter("REAR_STOP_ZONE_ROWS", 1);
|
||
this->declare_parameter("LEFT_STOP_ZONE_COLS", 3);
|
||
this->declare_parameter("RIGHT_STOP_ZONE_COLS", 3);
|
||
this->declare_parameter("front_area_extend", 7);
|
||
this->declare_parameter("left_area_extend", 2);
|
||
this->declare_parameter("right_area_extend", 2);
|
||
this->declare_parameter("waypoint_tolerance", 2.0);
|
||
this->declare_parameter("lateral_offset", 2);
|
||
this->declare_parameter("parallel_distance", 3);
|
||
this->declare_parameter("avoid_able_width", 6);
|
||
this->declare_parameter("min_free_space", 6);
|
||
|
||
// ======== 参数读取 ========
|
||
this->get_parameter("enable_obstacle_stop", enable_obstacle_stop_);
|
||
this->get_parameter("enable_obstacle_avoid", enable_obstacle_avoid_);
|
||
this->get_parameter("enable_visualize_grid", enable_visualize_grid_);
|
||
this->get_parameter("FRONT_STOP_ZONE_ROWS", FRONT_STOP_ZONE_ROWS_);
|
||
this->get_parameter("REAR_STOP_ZONE_ROWS", REAR_STOP_ZONE_ROWS_);
|
||
this->get_parameter("LEFT_STOP_ZONE_COLS", LEFT_STOP_ZONE_COLS_);
|
||
this->get_parameter("RIGHT_STOP_ZONE_COLS", RIGHT_STOP_ZONE_COLS_);
|
||
this->get_parameter("front_area_extend", front_area_extend_);
|
||
this->get_parameter("left_area_extend", left_area_extend_);
|
||
this->get_parameter("right_area_extend", right_area_extend_);
|
||
this->get_parameter("waypoint_tolerance", waypoint_tolerance_);
|
||
this->get_parameter("lateral_offset", lateral_offset_);
|
||
this->get_parameter("parallel_distance", parallel_distance_);
|
||
this->get_parameter("avoid_able_width", avoid_able_width_);
|
||
this->get_parameter("min_free_space", min_free_space_);
|
||
|
||
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_rtk = this->create_subscription<sweeper_interfaces::msg::Rtk>(
|
||
"rtk_message", 10, std::bind(&fu_node::msg_callback_rtk, this, std::placeholders::_1));
|
||
|
||
// ======== 发布者初始化 ========
|
||
pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("auto_mc_ctrl", 10);
|
||
|
||
// ======== 定时器初始化 ========
|
||
timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&fu_node::timer_callback, this));
|
||
|
||
// ======== 初始化 ========
|
||
avoiding_obstacle_ = false;
|
||
current_waypoint_idx_ = 0;
|
||
vehicle_min_x_ = -1;
|
||
vehicle_max_x_ = -1;
|
||
vehicle_min_y_ = -1;
|
||
vehicle_max_y_ = -1;
|
||
node_clock_ = this->get_clock();
|
||
|
||
front_stop_zone_ = {0, 0, 0, 0};
|
||
rear_stop_zone_ = {0, 0, 0, 0};
|
||
left_stop_zone_ = {0, 0, 0, 0};
|
||
right_stop_zone_ = {0, 0, 0, 0};
|
||
|
||
LOG_INFO(
|
||
"\n---------- FU节点参数配置 ----------\n 遇障停车: %s\n 绕障: %s\n 栅格可视化: %s\n"
|
||
" 路点容差: %.2f米\n 横向偏移: %d栅格\n 平行距离: %d栅格\n"
|
||
" 可绕宽度: %d栅格\n 最小空闲: %d栅格\n--------------------------------------------\n",
|
||
(enable_obstacle_stop_ ? "开启" : "关闭"), (enable_obstacle_avoid_ ? "开启" : "关闭"),
|
||
(enable_visualize_grid_ ? "是" : "否"), waypoint_tolerance_, lateral_offset_, parallel_distance_,
|
||
avoid_able_width_, min_free_space_);
|
||
}
|
||
|
||
private:
|
||
std::unique_ptr<StateMachine> state_machine_;
|
||
rclcpp::Clock::SharedPtr node_clock_;
|
||
|
||
// ======== 配置参数 ========
|
||
bool enable_obstacle_stop_, enable_obstacle_avoid_, enable_visualize_grid_;
|
||
int FRONT_STOP_ZONE_ROWS_, REAR_STOP_ZONE_ROWS_;
|
||
int LEFT_STOP_ZONE_COLS_, RIGHT_STOP_ZONE_COLS_;
|
||
int front_area_extend_, left_area_extend_, right_area_extend_;
|
||
double waypoint_tolerance_;
|
||
int lateral_offset_, parallel_distance_, avoid_able_width_, min_free_space_;
|
||
|
||
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
|
||
|
||
// ======== 规划层消息缓存 ========
|
||
double pl_x_ = 0.0, pl_y_ = 0.0;
|
||
int pl_speed_ = 0;
|
||
|
||
// ======== 栅格检测结果 ========
|
||
bool obstacle_in_front_ = false, obstacle_in_rear_ = false;
|
||
bool obstacle_in_left_ = false, obstacle_in_right_ = false;
|
||
bool obstacle_in_front_area_ = false;
|
||
int obstacle_left_edge_ = -1, obstacle_right_edge_ = -1;
|
||
int obstacle_max_x_ = 100;
|
||
int free_space_left_ = 0, free_space_right_ = 0;
|
||
|
||
// ======== 自车位置 ========
|
||
int vehicle_min_x_, vehicle_max_x_, vehicle_min_y_, vehicle_max_y_;
|
||
|
||
// ======== 安全区结构体 ========
|
||
struct SafetyZone
|
||
{
|
||
int min_x, max_x, min_y, max_y;
|
||
};
|
||
SafetyZone front_stop_zone_, rear_stop_zone_, left_stop_zone_, right_stop_zone_;
|
||
|
||
// ======== 避障相关 ========
|
||
bool avoiding_obstacle_;
|
||
std::vector<Waypoint> avoidance_waypoints_;
|
||
int current_waypoint_idx_;
|
||
std::vector<std::vector<int>> cached_grid_;
|
||
bool grid_data_valid_ = false;
|
||
rclcpp::Time last_grid_time_;
|
||
|
||
// ======== 发布者和订阅者 ========
|
||
rclcpp::Publisher<sweeper_interfaces::msg::McCtrl>::SharedPtr pub_mc;
|
||
rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscription_grid;
|
||
rclcpp::Subscription<sweeper_interfaces::msg::Pl>::SharedPtr msg_subscribe_pl;
|
||
rclcpp::Subscription<sweeper_interfaces::msg::Rtk>::SharedPtr msg_subscribe_rtk;
|
||
rclcpp::TimerBase::SharedPtr timer_;
|
||
|
||
// ======== 参数回调 ========
|
||
rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector<rclcpp::Parameter>& parameters)
|
||
{
|
||
rcl_interfaces::msg::SetParametersResult result;
|
||
result.successful = true;
|
||
for (const auto& param : parameters)
|
||
{
|
||
if (param.get_name() == "enable_obstacle_stop")
|
||
{
|
||
enable_obstacle_stop_ = param.as_bool();
|
||
LOG_INFO("[参数更新] 遇障停车: %s", enable_obstacle_stop_ ? "开启" : "关闭");
|
||
}
|
||
else if (param.get_name() == "enable_obstacle_avoid")
|
||
{
|
||
enable_obstacle_avoid_ = param.as_bool();
|
||
LOG_INFO("[参数更新] 自主绕障: %s", enable_obstacle_avoid_ ? "开启" : "关闭");
|
||
if (!enable_obstacle_avoid_)
|
||
{
|
||
state_machine_->setState(StateMachine::NORMAL);
|
||
avoiding_obstacle_ = false;
|
||
avoidance_waypoints_.clear();
|
||
}
|
||
}
|
||
else if (param.get_name() == "waypoint_tolerance")
|
||
waypoint_tolerance_ = param.as_double();
|
||
else if (param.get_name() == "lateral_offset")
|
||
lateral_offset_ = param.as_int();
|
||
else if (param.get_name() == "parallel_distance")
|
||
parallel_distance_ = param.as_int();
|
||
else if (param.get_name() == "avoid_able_width")
|
||
avoid_able_width_ = param.as_int();
|
||
else if (param.get_name() == "min_free_space")
|
||
min_free_space_ = param.as_int();
|
||
}
|
||
return result;
|
||
}
|
||
|
||
// ======== RTK回调 ========
|
||
void msg_callback_rtk(const sweeper_interfaces::msg::Rtk::SharedPtr msg)
|
||
{
|
||
last_rtk_time = system_clock::now();
|
||
|
||
g_rtk.reliability = (msg->p_quality == 4 && msg->h_quality == 4) ? 1 : 0;
|
||
g_rtk.lat = msg->lat;
|
||
g_rtk.lon = msg->lon;
|
||
g_rtk.direction = msg->head;
|
||
}
|
||
|
||
// ======== 角度计算 ========
|
||
float calculate_the_angle(double target_x, double target_y)
|
||
{
|
||
if (target_x == 0) return 90.0f;
|
||
float angle_f = atan2(target_y, target_x) * 180 / M_PI;
|
||
float steering_angle = (angle_f <= 90.0f) ? 90.0f - angle_f : -(angle_f - 90.0f);
|
||
// 计算阶段就限制角度范围
|
||
return std::clamp(steering_angle, -50.0f, 50.0f);
|
||
}
|
||
|
||
// ======== 自车位置检测 ========
|
||
void findVehicleRectangle(const std::vector<std::vector<int>>& grid)
|
||
{
|
||
vehicle_min_x_ = vehicle_max_x_ = vehicle_min_y_ = vehicle_max_y_ = -1;
|
||
for (size_t i = 0; i < grid.size(); ++i)
|
||
{
|
||
for (size_t j = 0; j < grid[i].size(); ++j)
|
||
{
|
||
if (grid[i][j] == VEHICLE)
|
||
{
|
||
if (vehicle_min_x_ == -1 || (int)i < vehicle_min_x_) vehicle_min_x_ = i;
|
||
if (vehicle_max_x_ == -1 || (int)i > vehicle_max_x_) vehicle_max_x_ = i;
|
||
if (vehicle_min_y_ == -1 || (int)j < vehicle_min_y_) vehicle_min_y_ = j;
|
||
if (vehicle_max_y_ == -1 || (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)
|
||
{
|
||
calculateSafetyZones(grid.size(), grid[0].size());
|
||
}
|
||
}
|
||
|
||
// ======== 安全区计算 ========
|
||
void calculateSafetyZones(int grid_height, int grid_width)
|
||
{
|
||
front_stop_zone_ = {max(0, vehicle_min_x_ - FRONT_STOP_ZONE_ROWS_), vehicle_min_x_ - 1, vehicle_min_y_,
|
||
vehicle_max_y_};
|
||
rear_stop_zone_ = {vehicle_max_x_ + 1, min(grid_height - 1, vehicle_max_x_ + REAR_STOP_ZONE_ROWS_),
|
||
vehicle_min_y_, vehicle_max_y_};
|
||
left_stop_zone_ = {vehicle_min_x_, vehicle_max_x_, max(0, vehicle_min_y_ - LEFT_STOP_ZONE_COLS_),
|
||
vehicle_min_y_ - 1};
|
||
right_stop_zone_ = {vehicle_min_x_, vehicle_max_x_, vehicle_max_y_ + 1,
|
||
min(grid_width - 1, vehicle_max_y_ + RIGHT_STOP_ZONE_COLS_)};
|
||
}
|
||
|
||
// ======== 栅格回调 ========
|
||
void gridCallback(const std_msgs::msg::Int32MultiArray::SharedPtr msg)
|
||
{
|
||
last_grid_time_ = node_clock_->now();
|
||
|
||
if (msg->layout.dim.size() != 2)
|
||
{
|
||
LOG_ERROR("栅格地图消息维度无效！");
|
||
return;
|
||
}
|
||
|
||
size_t height = msg->layout.dim[0].size;
|
||
size_t width = msg->layout.dim[1].size;
|
||
|
||
if (msg->data.size() != height * width)
|
||
{
|
||
LOG_ERROR("栅格地图数据长度不匹配！期望%zu，实际%zu", height * width, msg->data.size());
|
||
return;
|
||
}
|
||
|
||
std::vector<std::vector<int>> grid(height, std::vector<int>(width));
|
||
for (size_t i = 0; i < height; ++i)
|
||
for (size_t j = 0; j < width; ++j) grid[i][j] = msg->data[i * width + j];
|
||
|
||
findVehicleRectangle(grid);
|
||
analyzeObstacles(grid);
|
||
if (enable_visualize_grid_) visualizeGridInTerminal(grid);
|
||
cacheGridData(grid);
|
||
|
||
// 立即执行紧急停车检查（高优先级）
|
||
updateObstacleAnalysis();
|
||
if (enable_obstacle_stop_ && is_start)
|
||
{
|
||
if (state_machine_->obstacle_analysis.has_front_critical)
|
||
{
|
||
state_machine_->publish_speed = 0;
|
||
state_machine_->publish_angle = 0.0f;
|
||
state_machine_->setState(StateMachine::WAITING);
|
||
publishControlCommand(true); // 立即发布带刹车的停车指令
|
||
LOG_WARN("[紧急停车] 栅格回调中检测到前方障碍物，立即停车！");
|
||
}
|
||
}
|
||
}
|
||
|
||
// ======== 栅格缓存 ========
|
||
void cacheGridData(const std::vector<std::vector<int>>& grid)
|
||
{
|
||
cached_grid_ = grid;
|
||
grid_data_valid_ = true;
|
||
}
|
||
|
||
// ======== 障碍物分析 ========
|
||
void analyzeObstacles(const std::vector<std::vector<int>>& grid)
|
||
{
|
||
obstacle_in_front_ = obstacle_in_rear_ = obstacle_in_left_ = obstacle_in_right_ = false;
|
||
obstacle_in_front_area_ = false;
|
||
obstacle_left_edge_ = obstacle_right_edge_ = -1;
|
||
free_space_left_ = free_space_right_ = 0;
|
||
obstacle_max_x_ = 100;
|
||
if (vehicle_min_x_ == -1) return;
|
||
|
||
// 检测前方必停区障碍物
|
||
bool found_front_obstacle = false;
|
||
for (int i = front_stop_zone_.max_x; i >= front_stop_zone_.min_x && !found_front_obstacle; --i)
|
||
for (int j = front_stop_zone_.min_y; j <= front_stop_zone_.max_y; ++j)
|
||
if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE)
|
||
{
|
||
obstacle_in_front_ = true;
|
||
found_front_obstacle = true;
|
||
break;
|
||
}
|
||
|
||
// 检测后方必停区障碍物
|
||
for (int i = rear_stop_zone_.min_x; i <= rear_stop_zone_.max_x; ++i)
|
||
for (int j = rear_stop_zone_.min_y; j <= rear_stop_zone_.max_y; ++j)
|
||
if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE)
|
||
{
|
||
obstacle_in_rear_ = true;
|
||
break;
|
||
}
|
||
|
||
// 检测左侧必停区障碍物
|
||
for (int i = left_stop_zone_.min_x; i <= left_stop_zone_.max_x; ++i)
|
||
for (int j = left_stop_zone_.min_y; j <= left_stop_zone_.max_y; ++j)
|
||
if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE)
|
||
{
|
||
obstacle_in_left_ = true;
|
||
break;
|
||
}
|
||
|
||
// 检测右侧必停区障碍物
|
||
for (int i = right_stop_zone_.min_x; i <= right_stop_zone_.max_x; ++i)
|
||
for (int j = right_stop_zone_.min_y; j <= right_stop_zone_.max_y; ++j)
|
||
if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE)
|
||
{
|
||
obstacle_in_right_ = true;
|
||
break;
|
||
}
|
||
|
||
// 检测前方扩展区域障碍物
|
||
int front_area_min_x = max(0, vehicle_min_x_ - front_area_extend_);
|
||
int front_area_max_x = vehicle_min_x_ - 1;
|
||
int front_area_min_y = max(0, vehicle_min_y_ - left_area_extend_);
|
||
int front_area_max_y = min((int)grid[0].size() - 1, vehicle_max_y_ + right_area_extend_);
|
||
int leftmost_obstacle = -1, rightmost_obstacle = -1;
|
||
|
||
for (size_t i = front_area_min_x; i <= (size_t)front_area_max_x; ++i)
|
||
{
|
||
for (size_t j = front_area_min_y; j <= (size_t)front_area_max_y; ++j)
|
||
{
|
||
if (grid[i][j] == OBSTACLE)
|
||
{
|
||
obstacle_in_front_area_ = true;
|
||
if (leftmost_obstacle == -1 || (int)j < leftmost_obstacle) leftmost_obstacle = j;
|
||
if (rightmost_obstacle == -1 || (int)j > rightmost_obstacle) rightmost_obstacle = j;
|
||
int distance = vehicle_min_x_ - (int)i;
|
||
if (distance < obstacle_max_x_) obstacle_max_x_ = distance;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 计算绕障空间
|
||
calculateAvoidanceSpace(grid, leftmost_obstacle, rightmost_obstacle);
|
||
}
|
||
|
||
// ======== 绕障空间计算 ========
|
||
void calculateAvoidanceSpace(const std::vector<std::vector<int>>& grid, int core_left_obstacle,
|
||
int core_right_obstacle)
|
||
{
|
||
if (!obstacle_in_front_area_)
|
||
{
|
||
free_space_left_ = left_area_extend_ + (vehicle_max_y_ - vehicle_min_y_ + 1);
|
||
free_space_right_ = right_area_extend_ + (vehicle_max_y_ - vehicle_min_y_ + 1);
|
||
obstacle_left_edge_ = obstacle_right_edge_ = -1;
|
||
return;
|
||
}
|
||
|
||
obstacle_left_edge_ = core_left_obstacle;
|
||
obstacle_right_edge_ = core_right_obstacle;
|
||
size_t avoidance_min_x = (size_t)max(0, vehicle_min_x_ - front_area_extend_);
|
||
size_t avoidance_max_x = (size_t)vehicle_min_x_ - 1;
|
||
|
||
// 计算左侧空闲空间
|
||
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);
|
||
|
||
// 计算右侧空闲空间
|
||
int right_check_start = obstacle_right_edge_ + 1;
|
||
int right_check_end = min((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);
|
||
}
|
||
|
||
// ======== 方向空闲空间计算 ========
|
||
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;
|
||
int available_space = 0;
|
||
for (int j = check_start_y; j <= check_end_y; ++j)
|
||
{
|
||
if (j < 0 || j >= (int)grid[0].size()) break;
|
||
bool column_clear = true;
|
||
for (size_t i = min_x; i <= max_x; ++i)
|
||
{
|
||
if (i >= grid.size() || grid[i][j] == OBSTACLE)
|
||
{
|
||
column_clear = false;
|
||
break;
|
||
}
|
||
}
|
||
if (column_clear)
|
||
available_space++;
|
||
else
|
||
break;
|
||
}
|
||
return available_space;
|
||
}
|
||
|
||
// ======== 栅格可视化 ========
|
||
void visualizeGridInTerminal(const std::vector<std::vector<int>>& grid)
|
||
{
|
||
std::stringstream ss;
|
||
ss << " ----------栅格地图可视化---------- " << endl;
|
||
ss << " " << string(grid[0].size(), '-') << endl;
|
||
ss << " ";
|
||
for (size_t i = 0; i < grid[0].size(); i++) ss << (i % 10);
|
||
ss << endl;
|
||
for (size_t i = 0; i < grid.size(); i++)
|
||
{
|
||
ss << (i < 10 ? " " : "") << i << "|";
|
||
for (size_t j = 0; j < grid[i].size(); j++)
|
||
{
|
||
bool in_front = (i >= front_stop_zone_.min_x && i <= front_stop_zone_.max_x &&
|
||
j >= front_stop_zone_.min_y && j <= front_stop_zone_.max_y);
|
||
bool in_left = (i >= left_stop_zone_.min_x && i <= left_stop_zone_.max_x &&
|
||
j >= left_stop_zone_.min_y && j <= left_stop_zone_.max_y);
|
||
bool in_right = (i >= right_stop_zone_.min_x && i <= right_stop_zone_.max_x &&
|
||
j >= right_stop_zone_.min_y && j <= right_stop_zone_.max_y);
|
||
|
||
if (grid[i][j] == OBSTACLE)
|
||
ss << "@";
|
||
else if (grid[i][j] == VEHICLE)
|
||
ss << "V";
|
||
else if (in_front)
|
||
ss << "F";
|
||
else if (in_left)
|
||
ss << "L";
|
||
else if (in_right)
|
||
ss << "R";
|
||
else
|
||
ss << ".";
|
||
}
|
||
ss << "|" << endl;
|
||
}
|
||
ss << " " << string(grid[0].size(), '-') << endl;
|
||
LOG_INFO("%s", ss.str().c_str());
|
||
}
|
||
|
||
// ======== 规划层消息回调 ========
|
||
void msg_callback_pl(const sweeper_interfaces::msg::Pl::SharedPtr msg)
|
||
{
|
||
pl_x_ = msg->x;
|
||
pl_y_ = msg->y;
|
||
pl_speed_ = (int)msg->speed;
|
||
is_start = msg->is_start;
|
||
|
||
LOG_INFO("[规划层消息回调] 目标点位置： x -- %.2f , y -- %.2f", pl_x_, pl_y_);
|
||
}
|
||
|
||
// ======== 绕障路点规划 ========
|
||
void planAvoidanceWaypoints(int avoid_direction)
|
||
{
|
||
avoidance_waypoints_.clear();
|
||
|
||
const double GRID_RESOLUTION = 0.3;
|
||
const int SAFE_GRID_GAP = 1;
|
||
const int AVOID_SPEED = 300;
|
||
|
||
Waypoint point_lateral_shift;
|
||
Waypoint point_parallel_forward;
|
||
double actual_lateral_offset_m = 0.0;
|
||
int lateral_offset_grid_num = 0;
|
||
|
||
const int stage1_forward_grid = state_machine_->obstacle_analysis.obstacle_distance;
|
||
const int stage2_forward_grid = parallel_distance_;
|
||
const double stage1_forward_m = stage1_forward_grid * GRID_RESOLUTION;
|
||
const double stage2_forward_m = stage2_forward_grid * GRID_RESOLUTION;
|
||
|
||
// 计算横向偏移量
|
||
if (avoid_direction == -1)
|
||
{
|
||
lateral_offset_grid_num = (vehicle_max_y_ - state_machine_->obstacle_analysis.left_edge) + SAFE_GRID_GAP;
|
||
actual_lateral_offset_m = -1 * lateral_offset_grid_num * GRID_RESOLUTION;
|
||
}
|
||
else if (avoid_direction == 1)
|
||
{
|
||
lateral_offset_grid_num = (state_machine_->obstacle_analysis.right_edge - vehicle_min_y_) + SAFE_GRID_GAP;
|
||
actual_lateral_offset_m = 1 * lateral_offset_grid_num * GRID_RESOLUTION;
|
||
}
|
||
|
||
// 相对坐标
|
||
double relative_x_1 = actual_lateral_offset_m;
|
||
double relative_y_1 = stage1_forward_m;
|
||
double relative_x_2 = actual_lateral_offset_m;
|
||
double relative_y_2 = stage2_forward_m;
|
||
|
||
// 转换为绝对GPS坐标
|
||
double abs_lon_1, abs_lat_1, abs_lon_2, abs_lat_2;
|
||
rtk_relative_to_gps(relative_x_1, relative_y_1, g_rtk.direction, g_rtk.lon, g_rtk.lat, &abs_lon_1, &abs_lat_1);
|
||
rtk_relative_to_gps(relative_x_2, relative_y_2, g_rtk.direction, g_rtk.lon, g_rtk.lat, &abs_lon_2, &abs_lat_2);
|
||
|
||
// 构建路点
|
||
point_lateral_shift = Waypoint(relative_x_1, relative_y_1, AVOID_SPEED, abs_lon_1, abs_lat_1);
|
||
point_parallel_forward = Waypoint(relative_x_2, relative_y_2, AVOID_SPEED, abs_lon_2, abs_lat_2);
|
||
|
||
avoidance_waypoints_.push_back(point_lateral_shift);
|
||
avoidance_waypoints_.push_back(point_parallel_forward);
|
||
|
||
current_waypoint_idx_ = 0;
|
||
avoiding_obstacle_ = true;
|
||
state_machine_->setState(StateMachine::LATERAL_SHIFT);
|
||
|
||
LOG_INFO("[绕障规划] 方向：%s | 路点1 GPS(%.6f,%.6f) | 路点2 GPS(%.6f,%.6f)",
|
||
avoid_direction == -1 ? "左" : "右", abs_lon_1, abs_lat_1, abs_lon_2, abs_lat_2);
|
||
}
|
||
|
||
// ======== 相对坐标转GPS绝对坐标 ========
|
||
void rtk_relative_to_gps(double rel_x, double rel_y, double cur_heading, double cur_lon, double cur_lat,
|
||
double* dest_lon, double* dest_lat)
|
||
{
|
||
double Navi_rad = cur_heading * deg_rad;
|
||
|
||
// 逆向旋转矩阵：从车体坐标系转回地理坐标系
|
||
float k1 = cos(Navi_rad);
|
||
float k2 = sin(Navi_rad);
|
||
float k3 = -sin(Navi_rad);
|
||
float k4 = cos(Navi_rad);
|
||
|
||
// 地理坐标增量
|
||
double delta_x = rel_x * k1 + rel_y * k3; // 东向增量
|
||
double delta_y = rel_x * k2 + rel_y * k4; // 北向增量
|
||
|
||
// 转换为GPS坐标增量
|
||
double delta_lon = delta_x / (deg_rad * R_LONT);
|
||
double delta_lat = delta_y / (deg_rad * R_LATI);
|
||
|
||
*dest_lon = cur_lon + delta_lon;
|
||
*dest_lat = cur_lat + delta_lat;
|
||
|
||
LOG_INFO("[相对转GPS] 相对(%.2f,%.2f) -> GPS增量(%.8f,%.8f) -> 目标(%.6f,%.6f)", rel_x, rel_y, delta_lon,
|
||
delta_lat, *dest_lon, *dest_lat);
|
||
}
|
||
|
||
// ======== 路点到达判断 ========
|
||
// 返回值：1=到达 0=未到达 -1=RTK信号丢失(需要停车)
|
||
int waypointReached(const Waypoint& target)
|
||
{
|
||
// 验证RTK信号质量
|
||
auto now = system_clock::now();
|
||
duration<double> time_since_last = now - last_rtk_time;
|
||
|
||
if (g_rtk.reliability != 0 && time_since_last <= rtk_timeout)
|
||
{
|
||
// RTK信号良好：使用绝对坐标判断
|
||
double distance = ntzx_GPS_length(g_rtk.lon, g_rtk.lat, target.lon, target.lat);
|
||
|
||
LOG_INFO("[路点判断] RTK信号良好 | 当前GPS(%.6f,%.6f) | 目标GPS(%.6f,%.6f) | 距离: %.2f m | 容差: %.2f m",
|
||
g_rtk.lon, g_rtk.lat, target.lon, target.lat, distance, waypoint_tolerance_);
|
||
|
||
return distance < waypoint_tolerance_ ? 1 : 0;
|
||
}
|
||
else
|
||
{
|
||
// RTK信号丢失：无法安全判断
|
||
if (time_since_last > rtk_timeout)
|
||
{
|
||
LOG_ERROR("[路点判断] RTK超时(%.2fs)，信号丢失，停车退出绕障！", time_since_last.count());
|
||
}
|
||
else
|
||
{
|
||
LOG_ERROR("[路点判断] RTK信号差(可靠性=%d)，停车退出绕障！", g_rtk.reliability);
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
// ======== 路点跟踪执行 ========
|
||
void executeWaypointTracking()
|
||
{
|
||
if (current_waypoint_idx_ >= (int)avoidance_waypoints_.size())
|
||
{
|
||
avoiding_obstacle_ = false;
|
||
current_waypoint_idx_ = 0;
|
||
avoidance_waypoints_.clear();
|
||
state_machine_->setState(StateMachine::NORMAL);
|
||
LOG_INFO("[绕障完成] 所有路点跟踪完毕，恢复全局规划控制");
|
||
return;
|
||
}
|
||
|
||
Waypoint& target = avoidance_waypoints_[current_waypoint_idx_];
|
||
|
||
// 计算目标方向角：
|
||
float target_angle = calculate_the_angle(target.x, target.y);
|
||
state_machine_->publish_angle = target_angle;
|
||
state_machine_->publish_speed = target.speed;
|
||
|
||
LOG_INFO("[路点跟踪] 路点%d/%d | 相对坐标(%.2f,%.2f) | 目标方向: %.2f度", current_waypoint_idx_ + 1,
|
||
(int)avoidance_waypoints_.size(), target.x, target.y, target_angle);
|
||
|
||
// 检查路点到达情况
|
||
int reached = waypointReached(target);
|
||
|
||
if (reached == -1)
|
||
{
|
||
// RTK信号丢失，立即停车并退出绕障
|
||
state_machine_->publish_speed = 0;
|
||
state_machine_->publish_angle = 0.0f;
|
||
state_machine_->setState(StateMachine::WAITING);
|
||
|
||
avoiding_obstacle_ = false;
|
||
current_waypoint_idx_ = 0;
|
||
avoidance_waypoints_.clear();
|
||
|
||
LOG_ERROR("[绕障中止] RTK信号丢失，停车并退出绕障！");
|
||
return;
|
||
}
|
||
else if (reached == 1)
|
||
{
|
||
// 已到达路点，切换到下一个
|
||
current_waypoint_idx_++;
|
||
|
||
if (current_waypoint_idx_ < (int)avoidance_waypoints_.size())
|
||
{
|
||
state_machine_->setState(StateMachine::PARALLEL_MOVE);
|
||
LOG_INFO("[路点跟踪] 已到达路点%d，切换到路点%d", current_waypoint_idx_, current_waypoint_idx_ + 1);
|
||
}
|
||
else
|
||
{
|
||
avoiding_obstacle_ = false;
|
||
current_waypoint_idx_ = 0;
|
||
avoidance_waypoints_.clear();
|
||
state_machine_->setState(StateMachine::NORMAL);
|
||
LOG_INFO("[绕障完成] 所有路点跟踪完毕，恢复全局规划控制");
|
||
return;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 未到达路点，继续前往
|
||
state_machine_->setState(StateMachine::LATERAL_SHIFT);
|
||
}
|
||
|
||
// 安全检查：前方必停区有障碍物则立即停车
|
||
if (state_machine_->obstacle_analysis.has_front_critical)
|
||
{
|
||
state_machine_->publish_speed = 0;
|
||
state_machine_->publish_angle = 0.0f;
|
||
state_machine_->setState(StateMachine::WAITING);
|
||
LOG_WARN("[绕障安全] 路点跟踪中检测到前方必停区障碍物，立即停车");
|
||
|
||
// 立即发布停车指令（带刹车）
|
||
publishControlCommand(true);
|
||
avoiding_obstacle_ = false;
|
||
current_waypoint_idx_ = 0;
|
||
avoidance_waypoints_.clear();
|
||
return;
|
||
}
|
||
}
|
||
|
||
// ======== 可绕障判断 ========
|
||
bool canAvoidObstacle() const
|
||
{
|
||
if (state_machine_->obstacle_analysis.obstacle_width <= 0 ||
|
||
state_machine_->obstacle_analysis.obstacle_width > avoid_able_width_)
|
||
{
|
||
LOG_INFO("[绕障判断] 障碍物宽度%d栅格，超过可绕最大宽度%d栅格，不可绕",
|
||
state_machine_->obstacle_analysis.obstacle_width, avoid_able_width_);
|
||
return false;
|
||
}
|
||
bool left_ok = state_machine_->obstacle_analysis.free_space_left >= min_free_space_;
|
||
bool right_ok = state_machine_->obstacle_analysis.free_space_right >= min_free_space_;
|
||
LOG_INFO("[绕障判断] 左侧空闲%d栅格，右侧空闲%d栅格，最小需求%d栅格 | 左可绕：%s，右可绕：%s",
|
||
state_machine_->obstacle_analysis.free_space_left, state_machine_->obstacle_analysis.free_space_right,
|
||
min_free_space_, left_ok ? "是" : "否", right_ok ? "是" : "否");
|
||
return left_ok || right_ok;
|
||
}
|
||
|
||
// ======== 绕障方向选择 ========
|
||
int selectOptimalAvoidDirection() const
|
||
{
|
||
if (state_machine_->obstacle_analysis.free_space_left > state_machine_->obstacle_analysis.free_space_right)
|
||
return -1;
|
||
else if (state_machine_->obstacle_analysis.free_space_right > state_machine_->obstacle_analysis.free_space_left)
|
||
return 1;
|
||
else
|
||
return (pl_x_ > 0) ? 1 : -1;
|
||
}
|
||
|
||
// ======== 更新障碍物分析 ========
|
||
void updateObstacleAnalysis()
|
||
{
|
||
state_machine_->obstacle_analysis.has_front_critical = obstacle_in_front_;
|
||
state_machine_->obstacle_analysis.has_front_area = obstacle_in_front_area_;
|
||
state_machine_->obstacle_analysis.has_left_critical = obstacle_in_left_;
|
||
state_machine_->obstacle_analysis.has_right_critical = obstacle_in_right_;
|
||
state_machine_->obstacle_analysis.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1;
|
||
state_machine_->obstacle_analysis.left_edge = obstacle_left_edge_;
|
||
state_machine_->obstacle_analysis.right_edge = obstacle_right_edge_;
|
||
state_machine_->obstacle_analysis.free_space_left = free_space_left_;
|
||
state_machine_->obstacle_analysis.free_space_right = free_space_right_;
|
||
|
||
if (obstacle_left_edge_ != -1 && obstacle_right_edge_ != -1)
|
||
{
|
||
state_machine_->obstacle_analysis.obstacle_width = obstacle_right_edge_ - obstacle_left_edge_ + 1;
|
||
}
|
||
|
||
LOG_INFO("[障碍物分析] 距离：%d栅格 | 宽度：%d栅格 | 左边缘：%d | 右边缘：%d",
|
||
state_machine_->obstacle_analysis.obstacle_distance, state_machine_->obstacle_analysis.obstacle_width,
|
||
state_machine_->obstacle_analysis.left_edge, state_machine_->obstacle_analysis.right_edge);
|
||
}
|
||
|
||
// ======== 紧急停车执行 ========
|
||
bool executeEmergencyStop()
|
||
{
|
||
if (!enable_obstacle_stop_) return false;
|
||
|
||
// 检查栅格数据新鲜度（超过500ms认为数据过期）
|
||
auto now = node_clock_->now();
|
||
auto grid_age = (now - last_grid_time_).seconds();
|
||
if (grid_age > 0.5)
|
||
{
|
||
LOG_WARN("[紧急停车] 栅格数据过期(%.3fs)，执行安全停车！", grid_age);
|
||
state_machine_->publish_speed = 0;
|
||
state_machine_->publish_angle = 0.0f;
|
||
state_machine_->setState(StateMachine::WAITING);
|
||
return true;
|
||
}
|
||
|
||
if (state_machine_->obstacle_analysis.has_front_critical)
|
||
{
|
||
state_machine_->publish_speed = 0;
|
||
state_machine_->publish_angle = 0.0f;
|
||
state_machine_->setState(StateMachine::WAITING);
|
||
LOG_WARN("[紧急停车] 前方必停区检测到障碍物，执行紧急停车");
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
// ======== 定时器回调（主逻辑） ========
|
||
void timer_callback()
|
||
{
|
||
// 更新障碍物分析
|
||
updateObstacleAnalysis();
|
||
|
||
// 开始清扫任务
|
||
if (is_start)
|
||
{
|
||
// 紧急停车优先级最高
|
||
if (executeEmergencyStop())
|
||
{
|
||
publishControlCommand(true); // 紧急停车使用刹车
|
||
return;
|
||
}
|
||
|
||
// PL速度为0，停车
|
||
if (pl_speed_ == 0)
|
||
{
|
||
state_machine_->publish_speed = 0;
|
||
state_machine_->publish_angle = 0;
|
||
LOG_INFO("PL速度为0，已停车");
|
||
publishControlCommand();
|
||
return;
|
||
}
|
||
|
||
// 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态
|
||
auto now = system_clock::now();
|
||
duration<double> time_since_last_rtk = now - last_rtk_time;
|
||
bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout);
|
||
|
||
if (state_machine_->getCurrentState() == StateMachine::WAITING &&
|
||
!state_machine_->obstacle_analysis.has_front_critical &&
|
||
rtk_signal_good)
|
||
{
|
||
state_machine_->setState(StateMachine::NORMAL);
|
||
LOG_INFO("[状态恢复] 障碍物已消失且RTK信号良好，恢复正常行驶状态");
|
||
}
|
||
|
||
// 绕障逻辑
|
||
if (enable_obstacle_avoid_)
|
||
{
|
||
if (avoiding_obstacle_)
|
||
{
|
||
// 正在绕障，执行路点跟踪
|
||
executeWaypointTracking();
|
||
}
|
||
else if (state_machine_->obstacle_analysis.has_front_area && canAvoidObstacle())
|
||
{
|
||
// 需要开始绕障
|
||
int avoid_dir = selectOptimalAvoidDirection();
|
||
planAvoidanceWaypoints(avoid_dir);
|
||
LOG_INFO("[启动绕障] 开始%s侧绕障", avoid_dir == -1 ? "左" : "右");
|
||
}
|
||
else if (state_machine_->getCurrentState() != StateMachine::NORMAL)
|
||
{
|
||
// 无需绕障，恢复正常状态
|
||
state_machine_->setState(StateMachine::NORMAL);
|
||
}
|
||
}
|
||
|
||
// 正常行驶逻辑
|
||
if (state_machine_->getCurrentState() == StateMachine::NORMAL)
|
||
{
|
||
state_machine_->publish_speed = 800;
|
||
state_machine_->publish_angle = calculate_the_angle(pl_x_, pl_y_);
|
||
}
|
||
|
||
// 发布控制指令
|
||
publishControlCommand();
|
||
}
|
||
}
|
||
|
||
// ======== 发布控制指令 ========
|
||
void publishControlCommand(bool emergency_brake = false)
|
||
{
|
||
auto message = sweeper_interfaces::msg::McCtrl();
|
||
message.sweep = true;
|
||
message.brake = emergency_brake ? 1 : 0; // 紧急停车时启用刹车
|
||
message.gear = 2;
|
||
message.angle_speed = 800;
|
||
|
||
// 转速限制
|
||
message.rpm = std::clamp(state_machine_->publish_speed, 0, 800);
|
||
message.angle = std::clamp(state_machine_->publish_angle, -50.0f, 50.0f);
|
||
|
||
pub_mc->publish(message);
|
||
|
||
if (emergency_brake)
|
||
{
|
||
LOG_WARN("[控制发布] 紧急刹车！状态：%s | 转速：%d | 转向角度：%.1f° | 刹车：%d",
|
||
state_machine_->getStateString().c_str(), message.rpm, message.angle, message.brake);
|
||
}
|
||
else
|
||
{
|
||
LOG_INFO("[控制发布] 状态：%s | 转速：%d | 转向角度：%.1f°", state_machine_->getStateString().c_str(),
|
||
message.rpm, message.angle);
|
||
}
|
||
}
|
||
};
|
||
|
||
// ======== 主函数 ========
|
||
int main(int argc, char** argv)
|
||
{
|
||
logger::Logger::Init("fu", "./nodes_log");
|
||
|
||
rclcpp::init(argc, argv);
|
||
auto node = std::make_shared<fu_node>("fu_node");
|
||
rclcpp::spin(node);
|
||
rclcpp::shutdown();
|
||
|
||
logger::Logger::Shutdown();
|
||
|
||
return 0;
|
||
} |