first Commit
This commit is contained in:
@@ -0,0 +1,85 @@
|
||||
#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);
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user