PathFinding3D25D.h

Go to the documentation of this file.

#pragma once
 
#include <CscCommon.h>
#include <Axiomes/CscPoint3dIterable.h>
 
using namespace conscience_core::axiomes;
 
namespace conscience_core::ai::algorithms::path_finding {
 
struct Plan3D25DDebugExport {
    bool exportTopViewPng = false;
    bool exportColoredPly = false;
 
    fs::path topViewPngPath;
    fs::path coloredPlyPath;
 
    int pngScale = 2;
};
 
struct Plan3D25DParams {
    double resolutionCm = 5.0;
 
    double robotWidthCm = 40.0;   // Z axis
    double robotLengthCm = 80.0;  // X axis
    double robotHeightCm = 50.0; // Y axis
    double desiredClearanceToObstacleCm = 20.0;
    double robotHardSafetyMarginCm = 6.0;
 
    double mapMarginCm = 150.0;
 
    double maxStepHeightCm = 8.0;
    double maxSlopeRads = 0.45;
    double maxRoughnessCm = 12.0;
 
    double weightedAStarHeuristicWeight = 1.45;
 
    double slopeCostWeight = 4.0;
    double roughnessCostWeight = 0.15;
    double climbCostWeight = 0.35;
    double clearanceCostWeight = 10.0;
    double unknownCellCost = 3.0;
 
    bool allowUnknownCells = true;
    bool smoothingPathResult = true;
 
    std::optional<double> distanceForAddIntermediatePointsCm = 10.0;
 
    int nearestValidSearchRadiusCells = 40;
 
    unsigned minPointsPerCell = 4;
 
    optional<double> globalMinY = 0;
 
    Plan3D25DDebugExport exportConfig;
};
 
std::vector<CscPoint3dOriented *> planPath3D25DWeightedAStar(
    const CscPoint3dIterable &cartographyWorldCoordinates,
    const CscPoint3dIterable &realtimeScanWorldCoordinates,
    const CscPoint3d &startPosition,
    const CscQuaternion &entityStartOrientationQuaternion,
    const CscPoint3d &targetPosition,
    const Plan3D25DParams &params,
    CscPoint3d **outClosestValidPosition = nullptr);
}