From 1b27f221b980b42ee9b2f5ac68211aabeca332f5 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 14 Nov 2018 12:01:26 -0800 Subject: [PATCH 1/2] Add camera start to transect entry even when taking images throughout survey area. --- src/MissionManager/CorridorScanComplexItem.cc | 17 +- src/MissionManager/SurveyComplexItem.cc | 368 ++----------------------- src/MissionManager/SurveyComplexItem.h | 2 +- src/MissionManager/TransectStyleComplexItem.cc | 6 +- 4 files changed, 33 insertions(+), 360 deletions(-) diff --git a/src/MissionManager/CorridorScanComplexItem.cc b/src/MissionManager/CorridorScanComplexItem.cc index a0efe38..907332e 100644 --- a/src/MissionManager/CorridorScanComplexItem.cc +++ b/src/MissionManager/CorridorScanComplexItem.cc @@ -172,7 +172,7 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& i //qDebug() << "_buildAndAppendMissionItems"; for (const QList& transect: _transects) { - bool entryPoint = true; + bool transectEntry = true; //qDebug() << "start transect"; for (const CoordInfo_t& transectCoordInfo: transect) { @@ -193,7 +193,7 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& i missionItemParent); items.append(item); - if (firstOverallPoint && addTriggerAtBeginning) { + if (triggerCamera() && firstOverallPoint && addTriggerAtBeginning) { // Start triggering addTriggerAtBeginning = false; item = new MissionItem(seqNum++, @@ -210,9 +210,12 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& i } firstOverallPoint = false; - if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && !imagesEverywhere) { - if (entryPoint) { - // Start of transect, start triggering + // Possibly add trigger start/stop to survey area entrance/exit + if (triggerCamera()) { + if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && transectEntry) { + // Start of transect, always start triggering. We do this even if we are taking images everywhere. + // This allows a restart of the mission in mid-air without losing images from the entire mission. + // At most you may lose part of a transect. item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, @@ -224,7 +227,8 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& i false, // isCurrentItem missionItemParent); items.append(item); - } else { + transectEntry = false; + } else if (!imagesEverywhere && !transectEntry){ // End of transect, stop triggering item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, @@ -238,7 +242,6 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& i missionItemParent); items.append(item); } - entryPoint = !entryPoint; } } } diff --git a/src/MissionManager/SurveyComplexItem.cc b/src/MissionManager/SurveyComplexItem.cc index 97b5b40..8a0fac3 100644 --- a/src/MissionManager/SurveyComplexItem.cc +++ b/src/MissionManager/SurveyComplexItem.cc @@ -604,212 +604,6 @@ double SurveyComplexItem::_clampGridAngle90(double gridAngle) return gridAngle; } -#if 0 -int SurveyComplexItem::_gridGenerator(const QList& polygonPoints, QList>& transectSegments, bool refly) -{ - int cameraShots = 0; - - double gridAngle = _gridAngleFact.rawValue().toDouble(); - double gridSpacing = _gridSpacingFact.rawValue().toDouble(); - - gridAngle = _clampGridAngle90(gridAngle); - gridAngle += refly ? 90 : 0; - qCDebug(SurveyComplexItemLog) << "Clamped grid angle" << gridAngle; - - qCDebug(SurveyComplexItemLog) << "SurveyComplexItem::_gridGenerator gridSpacing:gridAngle:refly" << gridSpacing << gridAngle << refly; - - transectSegments.clear(); - - // Convert polygon to bounding rect - - qCDebug(SurveyComplexItemLog) << "Polygon"; - QPolygonF polygon; - for (int i=0; i lineList; - - // Transects are generated to be as long as the largest width/height of the bounding rect plus some fudge factor. - // This way they will always be guaranteed to intersect with a polygon edge no matter what angle they are rotated to. - // They are initially generated with the transects flowing from west to east and then points within the transect north to south. - double maxWidth = qMax(boundingRect.width(), boundingRect.height()) + 2000.0; - double halfWidth = maxWidth / 2.0; - double transectX = boundingCenter.x() - halfWidth; - double transectXMax = transectX + maxWidth; - while (transectX < transectXMax) { - double transectYTop = boundingCenter.y() - halfWidth; - double transectYBottom = boundingCenter.y() + halfWidth; - - lineList += QLineF(_rotatePoint(QPointF(transectX, transectYTop), boundingCenter, gridAngle), _rotatePoint(QPointF(transectX, transectYBottom), boundingCenter, gridAngle)); - transectX += gridSpacing; - } - - // Now intersect the lines with the polygon - QList intersectLines; -#if 1 - _intersectLinesWithPolygon(lineList, polygon, intersectLines); -#else - // This is handy for debugging grid problems, not for release - intersectLines = lineList; -#endif - - // Less than two transects intersected with the polygon: - // Create a single transect which goes through the center of the polygon - // Intersect it with the polygon - if (intersectLines.count() < 2) { - _mapPolygon.center(); - QLineF firstLine = lineList.first(); - QPointF lineCenter = firstLine.pointAt(0.5); - QPointF centerOffset = boundingCenter - lineCenter; - firstLine.translate(centerOffset); - lineList.clear(); - lineList.append(firstLine); - intersectLines = lineList; - _intersectLinesWithPolygon(lineList, polygon, intersectLines); - } - - // Make sure all lines are going to same direction. Polygon intersection leads to line which - // can be in varied directions depending on the order of the intesecting sides. - QList resultLines; - _adjustLineDirection(intersectLines, resultLines); - - // Calc camera shots here if there are no images in turnaround - if (_triggerCamera() && !_imagesEverywhere()) { - for (int i=0; i transectPoints; - const QLineF& line = resultLines[i]; - - float turnaroundPosition = _turnaroundDistance() / line.length(); - - if (i & 1) { - transectLine = QLineF(line.p2(), line.p1()); - } else { - transectLine = QLineF(line.p1(), line.p2()); - } - - // Build the points along the transect - - if (_hasTurnaround()) { - transectPoints.append(transectLine.pointAt(-turnaroundPosition)); - } - - // Polygon entry point - transectPoints.append(transectLine.p1()); - - // For hover and capture we need points for each camera location - if (_triggerCamera() && _hoverAndCaptureEnabled()) { - if (_triggerDistance() < transectLine.length()) { - int innerPoints = floor(transectLine.length() / _triggerDistance()); - qCDebug(SurveyComplexItemLog) << "innerPoints" << innerPoints; - float transectPositionIncrement = _triggerDistance() / transectLine.length(); - for (int i=0; i& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent) -{ - double altitude = _gridAltitudeFact.rawValue().toDouble(); - bool altitudeRelative = _gridAltitudeRelativeFact.rawValue().toBool(); - - qCDebug(SurveyComplexItemLog) << "_appendWaypointToMission seq:trigger" << seqNum << (cameraTrigger != CameraTriggerNone); - - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - cameraTrigger == CameraTriggerHoverAndCapture ? _hoverAndCaptureDelaySeconds : 0, // Hold time (delay for hover and capture to settle vehicle before image is taken) - 0.0, 0.0, - std::numeric_limits::quiet_NaN(), // Yaw unchanged - coord.latitude(), - coord.longitude(), - altitude, - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - - switch (cameraTrigger) { - case CameraTriggerOff: - case CameraTriggerOn: - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - cameraTrigger == CameraTriggerOn ? _triggerDistance() : 0, - 0, // shutter integration (ignore) - cameraTrigger == CameraTriggerOn ? 1 : 0, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - break; - case CameraTriggerHoverAndCapture: - item = new MissionItem(seqNum++, - MAV_CMD_IMAGE_START_CAPTURE, - MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - 0, // Interval (none) - 1, // Take 1 photo - NAN, NAN, NAN, NAN, // param 4-7 reserved - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); -#if 0 - // This generates too many commands. Pulling out for now, to see if image quality is still high enough. - item = new MissionItem(seqNum++, - MAV_CMD_NAV_DELAY, - MAV_FRAME_MISSION, - 0.5, // Delay in seconds, give some time for image to be taken - -1, -1, -1, // No time - 0, 0, 0, // Param 5-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); -#endif - default: - break; - } - - return seqNum; -} -#endif - bool SurveyComplexItem::_nextTransectCoord(const QList& transectPoints, int pointIndex, QGeoCoordinate& coord) { if (pointIndex > transectPoints.count()) { @@ -836,7 +630,7 @@ void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; for (const QList& transect: _transects) { - bool entryPoint = true; + bool transectEntry = true; for (const CoordInfo_t& transectCoordInfo: transect) { item = new MissionItem(seqNum++, @@ -858,12 +652,12 @@ void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, item = new MissionItem(seqNum++, MAV_CMD_IMAGE_START_CAPTURE, MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - 0, // Interval (none) - 1, // Take 1 photo - NAN, NAN, NAN, NAN, // param 4-7 reserved - true, // autoContinue - false, // isCurrentItem + 0, // Reserved (Set to 0) + 0, // Interval (none) + 1, // Take 1 photo + qQNaN(), qQNaN(), qQNaN(), qQNaN(), // param 4-7 reserved + true, // autoContinue + false, // isCurrentItem missionItemParent); items.append(item); } @@ -885,9 +679,12 @@ void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, } firstOverallPoint = false; - if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && triggerCamera() && !hoverAndCaptureEnabled() && !imagesEverywhere) { - if (entryPoint) { - // Start of transect, start triggering + // Possibly add trigger start/stop to survey area entrance/exit + if (triggerCamera() && !hoverAndCaptureEnabled() && transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge) { + if (transectEntry) { + // Start of transect, always start triggering. We do this even if we are taking images everywhere. + // This allows a restart of the mission in mid-air without losing images from the entire mission. + // At most you may lose part of a transect. item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, @@ -899,7 +696,8 @@ void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, false, // isCurrentItem missionItemParent); items.append(item); - } else { + transectEntry = false; + } else if (!imagesEverywhere && !transectEntry){ // End of transect, stop triggering item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, @@ -913,12 +711,11 @@ void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, missionItemParent); items.append(item); } - entryPoint = !entryPoint; } } } - if (triggerCamera() && !hoverAndCaptureEnabled() && imagesEverywhere) { + if (triggerCamera() && !hoverAndCaptureEnabled() && imagesEverywhere) { // Stop triggering MissionItem* item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, @@ -934,118 +731,6 @@ void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, } } -#if 0 -bool SurveyComplexItem::_buildAndAppendMissionItems(QList& items, QObject* missionItemParent) -{ - int seqNum = _sequenceNumber; - bool firstWaypointTrigger = false; - -#if 1 - // FIXME: Hack - bool hasRefly = false; - bool buildRefly = false; -#endif - - qCDebug(SurveyComplexItemLog) << QStringLiteral("hasTurnaround(%1) triggerCamera(%2) hoverAndCapture(%3) imagesEverywhere(%4) hasRefly(%5) buildRefly(%6) ").arg(_hasTurnaround()).arg(_triggerCamera()).arg(_hoverAndCaptureEnabled()).arg(_imagesEverywhere()).arg(hasRefly).arg(buildRefly); - -#if 0 - QList>& transectSegments = buildRefly ? _reflyTransectSegments : _transectSegments; -#endif - - if (!buildRefly && _imagesEverywhere()) { - firstWaypointTrigger = true; - } - - for (const QList& transect: _transects) { - int pointIndex = 0; - QGeoCoordinate coord; - CameraTriggerCode cameraTrigger; - - qCDebug(SurveyComplexItemLog) << "transect.count" << transect.count(); - - if (_hasTurnaround()) { - // Add entry turnaround point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - seqNum = _appendWaypointToMission(items, seqNum, coord, firstWaypointTrigger ? CameraTriggerOn : CameraTriggerNone, missionItemParent); - firstWaypointTrigger = false; - } - - // Add polygon entry point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - if (firstWaypointTrigger) { - cameraTrigger = CameraTriggerOn; - } else { - cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn); - } - seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); - firstWaypointTrigger = false; - - // Add internal hover and capture points - if (_hoverAndCaptureEnabled()) { - int lastHoverAndCaptureIndex = segment.count() - 1 - (_hasTurnaround() ? 1 : 0); - qCDebug(SurveyComplexItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex; - for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) { - if (!_nextTransectCoord(segment, pointIndex, coord)) { - return false; - } - seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent); - } - } - - // Add polygon exit point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff); - seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); - - if (_hasTurnaround()) { - // Add exit turnaround point - if (!_nextTransectCoord(segment, pointIndex++, coord)) { - return false; - } - seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent); - } - - qCDebug(SurveyComplexItemLog) << "last PointIndex" << pointIndex; - } - - if (((hasRefly && buildRefly) || !hasRefly) && _imagesEverywhere()) { - // Turn off camera at end of survey - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - 0.0, // trigger distance (off) - 0, 0, 0, 0, 0, 0, // param 2-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - - return true; -} -#endif - -#if 0 -void SurveyComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) -{ - int seqNum = _sequenceNumber; - bool refly = refly90Degrees()->rawValue().toBool(); - - if (!_appendMissionItemsWorker(items, missionItemParent, seqNum, refly, false /* buildRefly */)) { - return; - } - - if (refly) { - _appendMissionItemsWorker(items, missionItemParent, seqNum, refly, true /* buildRefly */); - } -} -#endif bool SurveyComplexItem::_hasTurnaround(void) const { @@ -1057,23 +742,6 @@ double SurveyComplexItem::_turnaroundDistance(void) const return _turnAroundDistanceFact.rawValue().toDouble(); } -#if 0 -bool SurveyComplexItem::_triggerCamera(void) const -{ - return _triggerDistance() > 0; -} - -bool SurveyComplexItem::_imagesEverywhere(void) const -{ - return _triggerCamera() && _cameraTriggerInTurnaroundFact.rawValue().toBool(); -} - -bool SurveyComplexItem::_hoverAndCaptureEnabled(void) const -{ - return hoverAndCaptureAllowed() && !_imagesEverywhere() && _triggerCamera() && _hoverAndCaptureFact.rawValue().toBool(); -} -#endif - void SurveyComplexItem::_rebuildTransectsPhase1(void) { bool split = splitConcavePolygons()->rawValue().toBool(); @@ -1384,7 +1052,7 @@ void SurveyComplexItem::_rebuildTransectsPhase1WorkerSplitPolygons(bool refly) // TODO figure out tangent origin // TODO improve selection of entry points // qCDebug(SurveyComplexItemLog) << "Transects from polynom p " << p; - _rebuildTranscetsFromPolygon(refly, *p, tangentOrigin, vMatch); + _rebuildTransectsFromPolygon(refly, *p, tangentOrigin, vMatch); } } @@ -1535,7 +1203,7 @@ bool SurveyComplexItem::_VertexIsReflex(const QPolygonF& polygon, const QPointF* } -void SurveyComplexItem::_rebuildTranscetsFromPolygon(bool refly, const QPolygonF& polygon, const QGeoCoordinate& tangentOrigin, const QPointF* const transitionPoint) +void SurveyComplexItem::_rebuildTransectsFromPolygon(bool refly, const QPolygonF& polygon, const QGeoCoordinate& tangentOrigin, const QPointF* const transitionPoint) { // Generate transects diff --git a/src/MissionManager/SurveyComplexItem.h b/src/MissionManager/SurveyComplexItem.h index 3383963..0124844 100644 --- a/src/MissionManager/SurveyComplexItem.h +++ b/src/MissionManager/SurveyComplexItem.h @@ -117,7 +117,7 @@ private: void _rebuildTransectsPhase1WorkerSinglePolygon(bool refly); void _rebuildTransectsPhase1WorkerSplitPolygons(bool refly); /// Adds to the _transects array from one polygon - void _rebuildTranscetsFromPolygon(bool refly, const QPolygonF& polygon, const QGeoCoordinate& tangentOrigin, const QPointF* const transitionPoint); + void _rebuildTransectsFromPolygon(bool refly, const QPolygonF& polygon, const QGeoCoordinate& tangentOrigin, const QPointF* const transitionPoint); // Decompose polygon into list of convex sub polygons void _PolygonDecomposeConvex(const QPolygonF& polygon, QList& decomposedPolygons); // return true if vertex a can see vertex b diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index c8c4f3d..d616d75 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -719,10 +719,12 @@ int TransectStyleComplexItem::lastSequenceNumber(void) const } - if (!hoverAndCaptureEnabled()) { + if (!hoverAndCaptureEnabled() && triggerCamera()) { if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) { - // Only one camera start and on camera stop + // One camera start/stop for beginning/end of entire survey itemCount += 2; + // One camera start for each transect + itemCount += _transects.count(); } else { // Each transect will have a camera start and stop in it itemCount += _transects.count() * 2; From 373fbed3f98bf799ef986c54c6dd92ce9037d818 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 15 Nov 2018 14:06:47 -0800 Subject: [PATCH 2/2] Fix mission item generation --- src/MissionManager/CorridorScanComplexItem.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/MissionManager/CorridorScanComplexItem.cc b/src/MissionManager/CorridorScanComplexItem.cc index 907332e..7dcfec1 100644 --- a/src/MissionManager/CorridorScanComplexItem.cc +++ b/src/MissionManager/CorridorScanComplexItem.cc @@ -211,8 +211,8 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& i firstOverallPoint = false; // Possibly add trigger start/stop to survey area entrance/exit - if (triggerCamera()) { - if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && transectEntry) { + if (triggerCamera() && transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge) { + if (transectEntry) { // Start of transect, always start triggering. We do this even if we are taking images everywhere. // This allows a restart of the mission in mid-air without losing images from the entire mission. // At most you may lose part of a transect.