86 lines
2.1 KiB
C++
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);
|
|
};
|
|
|