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.
64 lines
2.1 KiB
64 lines
2.1 KiB
#ifndef BUSBAR_PLANNER_HPP
|
|
#define BUSBAR_PLANNER_HPP
|
|
|
|
#include "segment_generator.hpp"
|
|
#include <unordered_map>
|
|
#include <vector>
|
|
#include <memory>
|
|
#include <unordered_set>
|
|
|
|
// 规划结果
|
|
struct BusbarPlanResult {
|
|
bool success = false;
|
|
std::vector<std::shared_ptr<BusbarSegment>> segments;
|
|
float total_cost = 0.0f;
|
|
std::string message;
|
|
int states_explored = 0; // 新增：实际探索的状态数
|
|
int states_generated = 0; // 新增：生成的总状态数
|
|
int states_pruned = 0; // 新增：被剪枝的状态数
|
|
};
|
|
|
|
class BusbarPlanner {
|
|
private:
|
|
const BusbarConfig& config_;
|
|
const SpaceAnalyzer& analyzer_;
|
|
SegmentGenerator generator_;
|
|
|
|
// 启发式函数
|
|
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 std::vector<std::shared_ptr<BusbarSegment>>& segments) const;
|
|
|
|
// 检查两段是否可以合并
|
|
bool canMergeSegments(const BusbarSegment& seg1,
|
|
const BusbarSegment& seg2) const;
|
|
|
|
// 生成初始直段候选
|
|
std::vector<SegmentCandidate> generateInitialStraightCandidates(
|
|
const Point& start_pos, const Vector3& start_normal,
|
|
const Vector3& start_dir) const;
|
|
|
|
// 检查是否可以用直段直达目标
|
|
std::shared_ptr<BusbarSegment> tryDirectConnection(
|
|
const SegmentEndState& current,
|
|
const Point& goal_pos, const Vector3& goal_normal) const;
|
|
|
|
public:
|
|
BusbarPlanner(const BusbarConfig& config, const SpaceAnalyzer& analyzer)
|
|
: config_(config), analyzer_(analyzer), generator_(config, analyzer) {}
|
|
|
|
BusbarPlanResult plan(const Point& start_pos, const Vector3& start_normal,
|
|
const Vector3& start_dir,
|
|
const Point& goal_pos, const Vector3& goal_normal) const;
|
|
};
|
|
|
|
#endif
|