You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
189 lines
7.7 KiB
189 lines
7.7 KiB
#ifndef BUSBAR_PLANNER_HPP
|
|
#define BUSBAR_PLANNER_HPP
|
|
|
|
#include "segment_generator.hpp"
|
|
#include "coarse_planner.hpp"
|
|
#include <unordered_map>
|
|
#include <vector>
|
|
#include <memory>
|
|
#include <unordered_set>
|
|
#include <functional>
|
|
|
|
// 规划结果
|
|
struct BusbarPlanResult {
|
|
bool success = false;
|
|
std::vector<std::shared_ptr<BusbarSegment>> segments;
|
|
std::vector<std::shared_ptr<BusbarSegment>> merged_segments; // 合并后、简化前的路
|
|
std::vector<std::shared_ptr<BusbarSegment>> raw_segments; // 原始A*路径（合并前
|
|
float total_cost = 0.0f;
|
|
std::string message;
|
|
int states_explored = 0; // 实际探索的状态数
|
|
int states_generated = 0; // 生成的总状态数
|
|
int states_pruned = 0; // 被剪枝的状态数
|
|
std::vector<Point> coarse_path; // 粗规划完整体素路
|
|
std::vector<Point> coarse_waypoints; // 粗规划简化航
|
|
};
|
|
|
|
// 通道数据：与地图等大的bool网格，标记哪些体素在通道
|
|
struct CorridorData {
|
|
std::vector<bool> grid; // 与体素地图等
|
|
uint64_t xsize, ysize, zsize;
|
|
bool enabled = false; // 是否启用通道剪枝
|
|
|
|
CorridorData() : xsize(0), ysize(0), zsize(0), enabled(false) {}
|
|
|
|
// 检查体素是否在通道
|
|
inline bool isInCorridor(const Point& p) const {
|
|
if (!enabled) return true; // 未启用时全部通过
|
|
int64_t x = p.x(), y = p.y(), z = p.z();
|
|
if (x < 0 || y < 0 || z < 0 ||
|
|
static_cast<uint64_t>(x) >= xsize ||
|
|
static_cast<uint64_t>(y) >= ysize ||
|
|
static_cast<uint64_t>(z) >= zsize) {
|
|
return false;
|
|
}
|
|
return grid[static_cast<size_t>(x + y * xsize + z * xsize * ysize)];
|
|
}
|
|
};
|
|
|
|
class BusbarPlanner {
|
|
private:
|
|
const BusbarConfig& config_;
|
|
const SpaceAnalyzer& analyzer_;
|
|
float heuristic_weight_;
|
|
|
|
// 粗路径引导数据（mutable因为在const plan()中设置）
|
|
mutable std::vector<Point> coarse_waypoints_;
|
|
mutable std::vector<float> coarse_cumulative_dist_;
|
|
mutable bool coarse_path_available_ = false;
|
|
|
|
// 基于粗路径的启发式：返回沿粗路径的剩余距偏离距离
|
|
float coarsePathHeuristic(const Point& pos) const;
|
|
|
|
// 启发式函
|
|
float heuristic(const SegmentEndState& state,
|
|
const SegmentEndState& goal) const;
|
|
|
|
// 路径重建
|
|
std::vector<std::shared_ptr<BusbarSegment>> reconstructPath(
|
|
const std::unordered_map<SegmentEndState, SegmentEndState>& parents,
|
|
const std::unordered_map<SegmentEndState, std::shared_ptr<BusbarSegment>>& edges,
|
|
const SegmentEndState& goal) const;
|
|
|
|
// 合并连续直段
|
|
std::vector<std::shared_ptr<BusbarSegment>> mergeStraightSegments(
|
|
const SegmentGenerator& generator,
|
|
const std::vector<std::shared_ptr<BusbarSegment>>& segments) const;
|
|
|
|
// 检查两段是否可以合
|
|
bool canMergeSegments(const BusbarSegment& seg1,
|
|
const BusbarSegment& seg2) const;
|
|
|
|
// 后处理拉直：将连续的小弯+直段序列替换为单根长直段
|
|
std::vector<std::shared_ptr<BusbarSegment>> straightenPath(
|
|
const SegmentGenerator& generator,
|
|
const std::vector<std::shared_ptr<BusbarSegment>>& segments) const;
|
|
|
|
// 从原始路径做两节点拉直：同法向同高度时尝试替换为 直段+立弯+直段
|
|
std::vector<std::shared_ptr<BusbarSegment>> straightenRawPath(
|
|
const SegmentGenerator& generator,
|
|
const std::vector<std::shared_ptr<BusbarSegment>>& segments) const;
|
|
|
|
// 生成所有可能的初始方向
|
|
std::vector<SegmentCandidate> generateInitialCandidates(
|
|
const SegmentGenerator& generator,
|
|
const Point& start_pos, const Vector3& start_normal,
|
|
const Point& goal_pos,
|
|
const Vector3& start_tangent) const;
|
|
|
|
// 生成初始直段候
|
|
std::vector<SegmentCandidate> generateInitialStraightCandidates(
|
|
const SegmentGenerator& generator,
|
|
const Point& start_pos, const Vector3& start_normal,
|
|
const Vector3& start_dir) const;
|
|
|
|
// 检查是否可以用直段直达目标
|
|
std::shared_ptr<BusbarSegment> tryDirectConnection(
|
|
const SegmentGenerator& generator,
|
|
const SegmentEndState& current,
|
|
const Point& goal_pos, const Vector3& goal_normal,
|
|
const Vector3& goal_tangent) const;
|
|
|
|
// 尝试通过FLAT_BEND+直段到达终点(逆向推导)
|
|
std::pair<std::shared_ptr<BusbarSegment>, std::shared_ptr<BusbarSegment>> tryFlatBendPlusDirectConnection(
|
|
const SegmentGenerator& generator,
|
|
const SegmentEndState& current,
|
|
const Point& goal_pos, const Vector3& goal_normal,
|
|
const Vector3& goal_tangent) const;
|
|
|
|
// 尝试通过VERTICAL_BEND+直段到达终点(逆向推导，法向对齐但切向不对
|
|
std::pair<std::shared_ptr<BusbarSegment>, std::shared_ptr<BusbarSegment>> tryVerticalBendPlusDirectConnection(
|
|
const SegmentGenerator& generator,
|
|
const SegmentEndState& current,
|
|
const Point& goal_pos, const Vector3& goal_normal,
|
|
const Vector3& goal_tangent) const;
|
|
|
|
// 内部规划函数：规划两点之间的路径（用于分层规划）
|
|
BusbarPlanResult planSegment(
|
|
const Point& start_pos, const Vector3& start_normal, const Vector3& start_tangent,
|
|
const Point& goal_pos, const Vector3& goal_normal, const Vector3& goal_tangent,
|
|
float heuristic_weight, int max_iterations) const;
|
|
|
|
// 从体素路径构建通道
|
|
CorridorData buildCorridor(const std::vector<Point>& voxel_path, int corridor_radius) const;
|
|
|
|
std::function<void(const std::string&)> log_callback_;
|
|
|
|
public:
|
|
BusbarPlanner(const BusbarConfig& config, const SpaceAnalyzer& analyzer, float heuristic_weight = 3.0f)
|
|
: config_(config), analyzer_(analyzer), heuristic_weight_(heuristic_weight), log_callback_(nullptr) {}
|
|
|
|
void setLogCallback(std::function<void(const std::string&)> callback) {
|
|
log_callback_ = callback;
|
|
}
|
|
|
|
// 必经区域结构（AABB包围盒）
|
|
struct MandatoryRegion {
|
|
Point min_point; // 区域最小坐
|
|
Point max_point; // 区域最大坐
|
|
bool enabled; // 是否启用
|
|
|
|
MandatoryRegion() : min_point(0, 0, 0), max_point(0, 0, 0), enabled(false) {}
|
|
MandatoryRegion(const Point& min_p, const Point& max_p)
|
|
: min_point(min_p), max_point(max_p), enabled(true) {}
|
|
|
|
// 检查点是否在区域内
|
|
bool contains(const Point& p) const {
|
|
if (!enabled) return true; // 未启用时，视为所有点都在"区域
|
|
return p.x() >= min_point.x() && p.x() <= max_point.x() &&
|
|
p.y() >= min_point.y() && p.y() <= max_point.y() &&
|
|
p.z() >= min_point.z() && p.z() <= max_point.z();
|
|
}
|
|
|
|
// 获取区域中心
|
|
Point center() const {
|
|
return Point(
|
|
(min_point.x() + max_point.x()) / 2.0f,
|
|
(min_point.y() + max_point.y()) / 2.0f,
|
|
(min_point.z() + max_point.z()) / 2.0f
|
|
);
|
|
}
|
|
};
|
|
|
|
BusbarPlanResult plan(const Point& start_pos, const Vector3& start_normal,
|
|
const Point& goal_pos, const Vector3& goal_normal,
|
|
const Vector3& start_tangent = Vector3(0, 0, 0),
|
|
const Vector3& goal_tangent = Vector3(0, 0, 0),
|
|
const MandatoryRegion& mandatory_region = MandatoryRegion(),
|
|
float max_time_s = 0,
|
|
bool disable_coarse = false) const;
|
|
|
|
private:
|
|
void log(const std::string& msg) const {
|
|
if (log_callback_) {
|
|
log_callback_(msg);
|
|
}
|
|
}
|
|
};
|
|
|
|
#endif
|
|
|