#ifndef NOMINMAX #define NOMINMAX #endif #include "ObstaclePlanner.h" #include #include #include #include #include #include #include #include namespace { std::array 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(rgb.constBits()), static_cast(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> 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& lhs, const std::vector& 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 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 centerXDistribution(260.0, 540.0); std::uniform_real_distribution centerYDistribution(-180.0, 180.0); std::uniform_real_distribution 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(); }