first Commit
This commit is contained in:
@@ -0,0 +1,331 @@
|
||||
#ifndef NOMINMAX
|
||||
#define NOMINMAX
|
||||
#endif
|
||||
|
||||
#include "ObstaclePlanner.h"
|
||||
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include <QPainter>
|
||||
#include <QStringList>
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <random>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
std::array<double, 3> pointArray(const SafetyPoint3D& point)
|
||||
{
|
||||
return {point.x, point.y, point.z};
|
||||
}
|
||||
|
||||
SafetyPathResult makeResult(bool valid,
|
||||
bool obstacleDetected,
|
||||
bool pathBlocked,
|
||||
bool avoidanceRequired,
|
||||
const QString& statusCode,
|
||||
const QString& message)
|
||||
{
|
||||
SafetyPathResult result;
|
||||
result.valid = valid;
|
||||
result.obstacleDetected = obstacleDetected;
|
||||
result.pathBlocked = pathBlocked;
|
||||
result.avoidanceRequired = avoidanceRequired;
|
||||
result.statusCode = statusCode;
|
||||
result.message = message;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
bool SafetyAabb::isValid() const
|
||||
{
|
||||
return std::isfinite(minimum.x) && std::isfinite(minimum.y) && std::isfinite(minimum.z) &&
|
||||
std::isfinite(maximum.x) && std::isfinite(maximum.y) && std::isfinite(maximum.z) &&
|
||||
minimum.x <= maximum.x && minimum.y <= maximum.y && minimum.z <= maximum.z;
|
||||
}
|
||||
|
||||
SafetyAabb SafetyAabb::inflated(double clearanceMm) const
|
||||
{
|
||||
return {{minimum.x - clearanceMm, minimum.y - clearanceMm, minimum.z - clearanceMm},
|
||||
{maximum.x + clearanceMm, maximum.y + clearanceMm, maximum.z + clearanceMm}};
|
||||
}
|
||||
|
||||
bool SafetyAabb::contains(const SafetyPoint3D& point, double tolerance) const
|
||||
{
|
||||
return point.x >= minimum.x - tolerance && point.x <= maximum.x + tolerance &&
|
||||
point.y >= minimum.y - tolerance && point.y <= maximum.y + tolerance &&
|
||||
point.z >= minimum.z - tolerance && point.z <= maximum.z + tolerance;
|
||||
}
|
||||
|
||||
bool ObstaclePlanner::detectRedObstacle(const QImage& image,
|
||||
QRect& pixelBox,
|
||||
double minimumAreaPx)
|
||||
{
|
||||
pixelBox = QRect();
|
||||
if (image.isNull()) return false;
|
||||
const QImage rgb = image.convertToFormat(QImage::Format_RGB888);
|
||||
cv::Mat rgbView(rgb.height(), rgb.width(), CV_8UC3,
|
||||
const_cast<uchar*>(rgb.constBits()),
|
||||
static_cast<size_t>(rgb.bytesPerLine()));
|
||||
cv::Mat hsv;
|
||||
cv::cvtColor(rgbView, hsv, cv::COLOR_RGB2HSV);
|
||||
cv::Mat lowerRed, upperRed, mask;
|
||||
cv::inRange(hsv, cv::Scalar(0, 80, 70), cv::Scalar(12, 255, 255), lowerRed);
|
||||
cv::inRange(hsv, cv::Scalar(168, 80, 70), cv::Scalar(180, 255, 255), upperRed);
|
||||
cv::bitwise_or(lowerRed, upperRed, mask);
|
||||
const cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
|
||||
cv::morphologyEx(mask, mask, cv::MORPH_OPEN, kernel);
|
||||
cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, kernel);
|
||||
std::vector<std::vector<cv::Point>> contours;
|
||||
cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
|
||||
if (contours.empty()) return false;
|
||||
const auto best = std::max_element(contours.begin(), contours.end(),
|
||||
[](const std::vector<cv::Point>& lhs, const std::vector<cv::Point>& rhs) {
|
||||
return cv::contourArea(lhs) < cv::contourArea(rhs);
|
||||
});
|
||||
if (best == contours.end() || cv::contourArea(*best) < minimumAreaPx) return false;
|
||||
const cv::Rect rect = cv::boundingRect(*best);
|
||||
pixelBox = QRect(rect.x, rect.y, rect.width, rect.height);
|
||||
return pixelBox.isValid();
|
||||
}
|
||||
|
||||
SafetyAabb ObstaclePlanner::pixelBoxToKnownCuboid(
|
||||
const QRect& pixelBox,
|
||||
const QSize& imageSize,
|
||||
double workspaceXMinMm,
|
||||
double workspaceXMaxMm,
|
||||
double workspaceYMinMm,
|
||||
double workspaceYMaxMm,
|
||||
double obstacleSizeXmm,
|
||||
double obstacleSizeYmm,
|
||||
double obstacleSizeZmm,
|
||||
double tableZmm)
|
||||
{
|
||||
if (!pixelBox.isValid() || imageSize.width() < 2 || imageSize.height() < 2)
|
||||
return {};
|
||||
const double centerU = pixelBox.x() + (pixelBox.width() - 1) * 0.5;
|
||||
const double centerV = pixelBox.y() + (pixelBox.height() - 1) * 0.5;
|
||||
const double centerX = workspaceXMinMm + centerU / (imageSize.width() - 1.0) *
|
||||
(workspaceXMaxMm - workspaceXMinMm);
|
||||
const double centerY = workspaceYMaxMm - centerV / (imageSize.height() - 1.0) *
|
||||
(workspaceYMaxMm - workspaceYMinMm);
|
||||
return {{centerX - obstacleSizeXmm * 0.5,
|
||||
centerY - obstacleSizeYmm * 0.5,
|
||||
tableZmm},
|
||||
{centerX + obstacleSizeXmm * 0.5,
|
||||
centerY + obstacleSizeYmm * 0.5,
|
||||
tableZmm + obstacleSizeZmm}};
|
||||
}
|
||||
|
||||
bool ObstaclePlanner::segmentIntersectsAabb(const SafetyPoint3D& start,
|
||||
const SafetyPoint3D& goal,
|
||||
const SafetyAabb& box)
|
||||
{
|
||||
if (!box.isValid()) return false;
|
||||
const auto origin = pointArray(start);
|
||||
const auto finish = pointArray(goal);
|
||||
const auto lower = pointArray(box.minimum);
|
||||
const auto upper = pointArray(box.maximum);
|
||||
double tMinimum = 0.0;
|
||||
double tMaximum = 1.0;
|
||||
for (int axis = 0; axis < 3; ++axis) {
|
||||
const double direction = finish[axis] - origin[axis];
|
||||
if (std::abs(direction) < 1e-12) {
|
||||
if (origin[axis] < lower[axis] || origin[axis] > upper[axis]) return false;
|
||||
continue;
|
||||
}
|
||||
double enter = (lower[axis] - origin[axis]) / direction;
|
||||
double leave = (upper[axis] - origin[axis]) / direction;
|
||||
if (enter > leave) std::swap(enter, leave);
|
||||
tMinimum = std::max(tMinimum, enter);
|
||||
tMaximum = std::min(tMaximum, leave);
|
||||
if (tMinimum > tMaximum) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
SafetyPathResult ObstaclePlanner::planLiftOverPath(
|
||||
const SafetyPoint3D& start,
|
||||
const SafetyPoint3D& goal,
|
||||
const SafetyAabb* obstacle,
|
||||
double clearanceMm,
|
||||
double overheadMm,
|
||||
double maxWorkspaceZmm)
|
||||
{
|
||||
if (!obstacle) {
|
||||
SafetyPathResult result = makeResult(true, false, false, false,
|
||||
"NO_OBSTACLE", QString::fromUtf8("未检测到障碍物,输出直达路径"));
|
||||
result.waypoints = {start, goal};
|
||||
return result;
|
||||
}
|
||||
if (!obstacle->isValid() || clearanceMm < 0.0 || overheadMm < 0.0) {
|
||||
return makeResult(false, true, false, false,
|
||||
"INVALID_INPUT", QString::fromUtf8("障碍物或安全参数无效"));
|
||||
}
|
||||
|
||||
const SafetyAabb safeBox = obstacle->inflated(clearanceMm);
|
||||
if (safeBox.contains(start)) {
|
||||
SafetyPathResult result = makeResult(false, true, true, false,
|
||||
"START_INSIDE_SAFE_BOX", QString::fromUtf8("起点位于障碍物安全区内"));
|
||||
result.obstacle = *obstacle;
|
||||
result.safeBox = safeBox;
|
||||
return result;
|
||||
}
|
||||
if (safeBox.contains(goal)) {
|
||||
SafetyPathResult result = makeResult(false, true, true, false,
|
||||
"GOAL_INSIDE_SAFE_BOX", QString::fromUtf8("目标点位于障碍物安全区内"));
|
||||
result.obstacle = *obstacle;
|
||||
result.safeBox = safeBox;
|
||||
return result;
|
||||
}
|
||||
|
||||
if (!segmentIntersectsAabb(start, goal, safeBox)) {
|
||||
SafetyPathResult result = makeResult(true, true, false, false,
|
||||
"DIRECT_PATH", QString::fromUtf8("障碍物未阻挡直线路径"));
|
||||
result.obstacle = *obstacle;
|
||||
result.safeBox = safeBox;
|
||||
result.waypoints = {start, goal};
|
||||
return result;
|
||||
}
|
||||
|
||||
const double safeZ = obstacle->maximum.z + overheadMm;
|
||||
if (safeZ > maxWorkspaceZmm) {
|
||||
SafetyPathResult result = makeResult(false, true, true, true,
|
||||
"SAFE_HEIGHT_OUT_OF_RANGE", QString::fromUtf8("绕行安全高度超过工作区上限"));
|
||||
result.safeZMm = safeZ;
|
||||
result.obstacle = *obstacle;
|
||||
result.safeBox = safeBox;
|
||||
return result;
|
||||
}
|
||||
|
||||
const SafetyPoint3D first{start.x, start.y, safeZ};
|
||||
const SafetyPoint3D second{goal.x, goal.y, safeZ};
|
||||
const QVector<SafetyPoint3D> path{start, first, second, goal};
|
||||
for (int index = 0; index < path.size() - 1; ++index) {
|
||||
if (segmentIntersectsAabb(path[index], path[index + 1], safeBox)) {
|
||||
SafetyPathResult result = makeResult(false, true, true, true,
|
||||
"LIFT_PATH_STILL_BLOCKED", QString::fromUtf8("抬高路径仍与安全包围盒相交"));
|
||||
result.safeZMm = safeZ;
|
||||
result.obstacle = *obstacle;
|
||||
result.safeBox = safeBox;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
SafetyPathResult result = makeResult(true, true, true, true,
|
||||
"LIFT_OVER_PATH", QString::fromUtf8("直线路径受阻,已生成抬高绕行路径"));
|
||||
result.safeZMm = safeZ;
|
||||
result.obstacle = *obstacle;
|
||||
result.safeBox = safeBox;
|
||||
result.waypoints = path;
|
||||
return result;
|
||||
}
|
||||
|
||||
QImage ObstaclePlanner::createDemoScene(
|
||||
bool obstaclePresent,
|
||||
double obstacleCenterXmm,
|
||||
double obstacleCenterYmm,
|
||||
double workspaceXMinMm,
|
||||
double workspaceXMaxMm,
|
||||
double workspaceYMinMm,
|
||||
double workspaceYMaxMm,
|
||||
double obstacleSizeXmm,
|
||||
double obstacleSizeYmm,
|
||||
const QSize& imageSize)
|
||||
{
|
||||
QImage image(imageSize, QImage::Format_RGB32);
|
||||
image.fill(QColor(238, 238, 238));
|
||||
QPainter painter(&image);
|
||||
painter.setPen(QPen(QColor(220, 220, 220), 1));
|
||||
for (int x = 0; x < image.width(); x += 80) painter.drawLine(x, 0, x, image.height());
|
||||
for (int y = 0; y < image.height(); y += 80) painter.drawLine(0, y, image.width(), y);
|
||||
if (!obstaclePresent) return image;
|
||||
const double u = (obstacleCenterXmm - workspaceXMinMm) /
|
||||
(workspaceXMaxMm - workspaceXMinMm) * (image.width() - 1.0);
|
||||
const double v = (workspaceYMaxMm - obstacleCenterYmm) /
|
||||
(workspaceYMaxMm - workspaceYMinMm) * (image.height() - 1.0);
|
||||
const double width = obstacleSizeXmm / (workspaceXMaxMm - workspaceXMinMm) *
|
||||
(image.width() - 1.0);
|
||||
const double height = obstacleSizeYmm / (workspaceYMaxMm - workspaceYMinMm) *
|
||||
(image.height() - 1.0);
|
||||
const QRectF rectangle(u - width * 0.5, v - height * 0.5, width, height);
|
||||
painter.fillRect(rectangle, QColor(230, 0, 0));
|
||||
painter.setPen(QPen(QColor(120, 0, 0), 3));
|
||||
painter.drawRect(rectangle);
|
||||
return image;
|
||||
}
|
||||
|
||||
bool ObstaclePlanner::selfTest(QString* report)
|
||||
{
|
||||
QStringList failures;
|
||||
const SafetyPoint3D start{100.0, 0.0, 50.0};
|
||||
const SafetyPoint3D goal{700.0, 0.0, 50.0};
|
||||
const SafetyAabb blocking{{325.0, -50.0, 0.0}, {475.0, 50.0, 120.0}};
|
||||
const SafetyAabb offPath{{325.0, 160.0, 0.0}, {475.0, 260.0, 120.0}};
|
||||
|
||||
auto expect = [&failures](const QString& name,
|
||||
const SafetyPathResult& result,
|
||||
const QString& status,
|
||||
bool valid) {
|
||||
if (result.statusCode != status || result.valid != valid)
|
||||
failures << name + ": " + result.statusCode;
|
||||
};
|
||||
expect("no_obstacle", planLiftOverPath(start, goal, nullptr), "NO_OBSTACLE", true);
|
||||
expect("off_path", planLiftOverPath(start, goal, &offPath), "DIRECT_PATH", true);
|
||||
expect("blocking", planLiftOverPath(start, goal, &blocking), "LIFT_OVER_PATH", true);
|
||||
const SafetyPoint3D goalInside{400.0, 0.0, 60.0};
|
||||
expect("goal_inside", planLiftOverPath(start, goalInside, &blocking),
|
||||
"GOAL_INSIDE_SAFE_BOX", false);
|
||||
expect("height_limit", planLiftOverPath(start, goal, &blocking, 50.0, 80.0, 150.0),
|
||||
"SAFE_HEIGHT_OUT_OF_RANGE", false);
|
||||
|
||||
const QImage scene = createDemoScene(true, 400.0, 0.0,
|
||||
0.0, 800.0, -300.0, 300.0, 150.0, 100.0);
|
||||
QRect detected;
|
||||
if (!detectRedObstacle(scene, detected)) {
|
||||
failures << "red_detection";
|
||||
} else {
|
||||
const SafetyAabb detectedBox = pixelBoxToKnownCuboid(detected, scene.size(),
|
||||
0.0, 800.0, -300.0, 300.0, 150.0, 100.0, 120.0);
|
||||
const double centerX = (detectedBox.minimum.x + detectedBox.maximum.x) * 0.5;
|
||||
const double centerY = (detectedBox.minimum.y + detectedBox.maximum.y) * 0.5;
|
||||
if (std::hypot(centerX - 400.0, centerY) > 2.0) failures << "red_mapping";
|
||||
}
|
||||
|
||||
std::mt19937 generator(20260720u);
|
||||
std::uniform_real_distribution<double> centerXDistribution(260.0, 540.0);
|
||||
std::uniform_real_distribution<double> centerYDistribution(-180.0, 180.0);
|
||||
std::uniform_real_distribution<double> endpointYDistribution(-240.0, 240.0);
|
||||
int randomPassed = 0;
|
||||
constexpr int randomCases = 500;
|
||||
for (int caseIndex = 0; caseIndex < randomCases; ++caseIndex) {
|
||||
const double centerX = centerXDistribution(generator);
|
||||
const double centerY = centerYDistribution(generator);
|
||||
const SafetyAabb obstacle{{centerX - 75.0, centerY - 50.0, 0.0},
|
||||
{centerX + 75.0, centerY + 50.0, 120.0}};
|
||||
const SafetyPoint3D randomStart{60.0, endpointYDistribution(generator), 50.0};
|
||||
const SafetyPoint3D randomGoal{740.0, endpointYDistribution(generator), 50.0};
|
||||
const SafetyPathResult result = planLiftOverPath(randomStart, randomGoal, &obstacle);
|
||||
if (!result.valid) continue;
|
||||
bool collisionFree = true;
|
||||
for (int index = 0; index < result.waypoints.size() - 1; ++index) {
|
||||
if (segmentIntersectsAabb(result.waypoints[index],
|
||||
result.waypoints[index + 1],
|
||||
result.safeBox)) {
|
||||
collisionFree = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (collisionFree) ++randomPassed;
|
||||
}
|
||||
if (randomPassed != randomCases)
|
||||
failures << QString("random=%1/%2").arg(randomPassed).arg(randomCases);
|
||||
|
||||
if (report) {
|
||||
*report = failures.isEmpty()
|
||||
? QString::fromUtf8("5个确定性用例和500组随机路径全部通过")
|
||||
: QString::fromUtf8("自检失败: ") + failures.join(", ");
|
||||
}
|
||||
return failures.isEmpty();
|
||||
}
|
||||
Reference in New Issue
Block a user