Files
VisionGuide/source/ObstaclePlanner.h
T
2026-10-26 10:56:56 +08:00

86 lines
2.1 KiB
C++

#pragma once
#include <QImage>
#include <QRect>
#include <QSize>
#include <QString>
#include <QVector>
struct SafetyPoint3D
{
double x = 0.0;
double y = 0.0;
double z = 0.0;
};
struct SafetyAabb
{
SafetyPoint3D minimum;
SafetyPoint3D maximum;
bool isValid() const;
SafetyAabb inflated(double clearanceMm) const;
bool contains(const SafetyPoint3D& point, double tolerance = 1e-9) const;
};
struct SafetyPathResult
{
bool valid = false;
bool obstacleDetected = false;
bool pathBlocked = false;
bool avoidanceRequired = false;
QString statusCode;
QString message;
double safeZMm = -1.0;
SafetyAabb obstacle;
SafetyAabb safeBox;
QVector<SafetyPoint3D> waypoints;
};
class ObstaclePlanner
{
public:
static bool detectRedObstacle(const QImage& image,
QRect& pixelBox,
double minimumAreaPx = 500.0);
static SafetyAabb pixelBoxToKnownCuboid(
const QRect& pixelBox,
const QSize& imageSize,
double workspaceXMinMm,
double workspaceXMaxMm,
double workspaceYMinMm,
double workspaceYMaxMm,
double obstacleSizeXmm,
double obstacleSizeYmm,
double obstacleSizeZmm,
double tableZmm = 0.0);
static bool segmentIntersectsAabb(const SafetyPoint3D& start,
const SafetyPoint3D& goal,
const SafetyAabb& box);
static SafetyPathResult planLiftOverPath(
const SafetyPoint3D& start,
const SafetyPoint3D& goal,
const SafetyAabb* obstacle,
double clearanceMm = 50.0,
double overheadMm = 80.0,
double maxWorkspaceZmm = 800.0);
static QImage createDemoScene(
bool obstaclePresent,
double obstacleCenterXmm,
double obstacleCenterYmm,
double workspaceXMinMm,
double workspaceXMaxMm,
double workspaceYMinMm,
double workspaceYMaxMm,
double obstacleSizeXmm,
double obstacleSizeYmm,
const QSize& imageSize = QSize(960, 600));
static bool selfTest(QString* report = nullptr);
};