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.
47 lines
1.3 KiB
47 lines
1.3 KiB
5 days ago
|
#ifndef COARSE_PLANNER_HPP
|
||
|
|
#define COARSE_PLANNER_HPP
|
||
|
|
|
||
|
|
#include "point.hpp"
|
||
|
|
#include "vector3.hpp"
|
||
|
|
#include "space_analyzer.hpp"
|
||
|
|
#include "busbar_config.hpp"
|
||
|
|
#include <vector>
|
||
|
|
#include <string>
|
||
|
|
|
||
|
|
// 粗规划结果
|
||
|
|
struct CoarsePlanResult {
|
||
|
|
bool success = false;
|
||
|
|
std::vector<Point> path; // 完整体素级路径
|
||
|
|
std::vector<Point> waypoints; // Douglas-Peucker简化航点
|
||
|
|
std::string message;
|
||
|
|
int voxels_explored = 0;
|
||
|
|
};
|
||
|
|
|
||
|
|
// 体素级粗规划器：1×1巴片（不考虑姿态），考虑防护间隙
|
||
|
|
class CoarsePlanner {
|
||
|
|
public:
|
||
|
|
CoarsePlanner(const BusbarConfig& config, const SpaceAnalyzer& analyzer);
|
||
|
|
|
||
|
|
CoarsePlanResult plan(const Point& start, const Point& goal);
|
||
|
|
|
||
|
|
private:
|
||
|
|
const BusbarConfig& config_;
|
||
|
|
const SpaceAnalyzer& analyzer_;
|
||
|
|
uint64_t xsize_, ysize_, zsize_;
|
||
|
|
|
||
|
|
inline size_t toIndex(int64_t x, int64_t y, int64_t z) const {
|
||
|
|
return static_cast<size_t>(x + y * xsize_ + z * xsize_ * ysize_);
|
||
|
|
}
|
||
|
|
|
||
|
|
inline bool isPassable(int64_t x, int64_t y, int64_t z) const {
|
||
|
|
return analyzer_.isInBounds(x, y, z) && !analyzer_.isObstacle(x, y, z);
|
||
|
|
}
|
||
|
|
|
||
|
|
static float heuristic(const Point& a, const Point& b);
|
||
|
|
|
||
|
|
// Douglas-Peucker路径简化
|
||
|
|
static std::vector<Point> simplifyPath(const std::vector<Point>& path, float epsilon);
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif
|