From 94fecd2b9a1292c2d2b270c808be96bb2bd8a567 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Thu, 20 Nov 2014 14:48:47 -0800 Subject: [PATCH 01/16] Geotools: implement polygon offset --- .../core/helpers/geoTools/LineTools.java | 18 +++++++++-- .../core/helpers/geoTools/PolygonTools.java | 31 +++++++++++++++++++ 2 files changed, 47 insertions(+), 2 deletions(-) create mode 100644 Core/src/org/droidplanner/core/helpers/geoTools/PolygonTools.java diff --git a/Core/src/org/droidplanner/core/helpers/geoTools/LineTools.java b/Core/src/org/droidplanner/core/helpers/geoTools/LineTools.java index e310e79d2f..8130ae56da 100644 --- a/Core/src/org/droidplanner/core/helpers/geoTools/LineTools.java +++ b/Core/src/org/droidplanner/core/helpers/geoTools/LineTools.java @@ -23,6 +23,11 @@ public static LineCoord2D findExternalPoints(ArrayList crosses) { */ public static Coord2D FindLineIntersection(LineCoord2D first, LineCoord2D second) throws Exception { + return FindLineIntersection(first, second, false); + } + + public static Coord2D FindLineIntersection(LineCoord2D first, LineCoord2D second, + boolean projectIntersetion) throws Exception { double denom = ((first.getEnd().getX() - first.getStart().getX()) * (second.getEnd().getY() - second .getStart().getY())) - ((first.getEnd().getY() - first.getStart().getY()) * (second.getEnd().getX() - second @@ -39,8 +44,11 @@ public static Coord2D FindLineIntersection(LineCoord2D first, LineCoord2D second - ((first.getStart().getX() - second.getStart().getX()) * (first.getEnd().getY() - first .getStart().getY())); double s = numer2 / denom; - if ((r < 0 || r > 1) || (s < 0 || s > 1)) - throw new Exception("No Intersection"); + if (!projectIntersetion) { + if ((r < 0 || r > 1) || (s < 0 || s > 1)){ + throw new Exception("No Intersection"); + } + } // Find intersection point double x = first.getStart().getX() + (r * (first.getEnd().getX() - first.getStart().getX())); @@ -75,4 +83,10 @@ public static LineCoord2D findClosestLineToPoint(Coord2D point, List offsetLines = new ArrayList(); + for (LineCoord2D line : polygon.getLines()) { + offsetLines.add(LineTools.getParallelLineToTheLeft(line,10)); + } + + ArrayList path = new ArrayList(); + offsetLines.add(offsetLines.get(0)); + offsetLines.add(offsetLines.get(1)); + for (int i = 1; i < offsetLines.size(); i++) { + try { + path.add(LineTools.FindLineIntersection(offsetLines.get(i-1), offsetLines.get(i),true)); + } catch (Exception e) { + e.printStackTrace(); + } + } + Polygon polygonwithOffset = new Polygon(); + polygonwithOffset.addPoints(path); + return polygonwithOffset; + } + +} From 1dd65938cb838ec6cf2655a91c3362e0d4114dac Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Mon, 24 Nov 2014 17:50:59 -0800 Subject: [PATCH 02/16] Core: Implement a basic 3D survey object --- .../core/mission/MissionItemType.java | 5 +- .../core/mission/survey/Survey3D.java | 79 +++++++++++++++++++ 2 files changed, 83 insertions(+), 1 deletion(-) create mode 100644 Core/src/org/droidplanner/core/mission/survey/Survey3D.java diff --git a/Core/src/org/droidplanner/core/mission/MissionItemType.java b/Core/src/org/droidplanner/core/mission/MissionItemType.java index 0de58b124e..41d309d303 100644 --- a/Core/src/org/droidplanner/core/mission/MissionItemType.java +++ b/Core/src/org/droidplanner/core/mission/MissionItemType.java @@ -11,6 +11,7 @@ import org.droidplanner.core.mission.commands.SetServo; import org.droidplanner.core.mission.commands.Takeoff; import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey3D; import org.droidplanner.core.mission.waypoints.Circle; import org.droidplanner.core.mission.waypoints.Land; import org.droidplanner.core.mission.waypoints.RegionOfInterest; @@ -21,7 +22,7 @@ public enum MissionItemType { WAYPOINT("Waypoint"), SPLINE_WAYPOINT("Spline Waypoint"), TAKEOFF("Takeoff"), RTL( "Return to Launch"), LAND("Land"), CIRCLE("Circle"), ROI("Region of Interest"), SURVEY( - "Survey"), CYLINDRICAL_SURVEY("Structure Scan"), CHANGE_SPEED("Change Speed"), CAMERA_TRIGGER("Camera Trigger"), EPM_GRIPPER("EPM"), SET_SERVO("Set Servo"), CONDITION_YAW("Set Yaw"); + "Survey"), CYLINDRICAL_SURVEY("Structure Scan"), CHANGE_SPEED("Change Speed"), CAMERA_TRIGGER("Camera Trigger"), EPM_GRIPPER("EPM"), SET_SERVO("Set Servo"), CONDITION_YAW("Set Yaw"), SURVEY3D("Survey3D"); private final String name; @@ -57,6 +58,8 @@ public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentE return new RegionOfInterest(referenceItem); case SURVEY: return new Survey(referenceItem.getMission(), Collections. emptyList()); + case SURVEY3D: + return new Survey3D(referenceItem.getMission(), Collections. emptyList()); case CYLINDRICAL_SURVEY: return new StructureScanner(referenceItem); case SET_SERVO: diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java new file mode 100644 index 0000000000..99b0a39804 --- /dev/null +++ b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java @@ -0,0 +1,79 @@ +package org.droidplanner.core.mission.survey; + +import java.util.ArrayList; +import java.util.List; + +import org.droidplanner.core.helpers.coordinates.Coord2D; +import org.droidplanner.core.helpers.geoTools.PolygonTools; +import org.droidplanner.core.helpers.units.Altitude; +import org.droidplanner.core.helpers.units.Length; +import org.droidplanner.core.mission.Mission; +import org.droidplanner.core.mission.MissionItem; +import org.droidplanner.core.mission.MissionItemType; +import org.droidplanner.core.polygon.Polygon; +import org.droidplanner.core.survey.CameraInfo; + +import com.MAVLink.common.msg_mission_item; +import com.MAVLink.enums.MAV_CMD; +import com.MAVLink.enums.MAV_FRAME; + +public class Survey3D extends MissionItem { + + public Polygon polygon = new Polygon(); + private CameraInfo camera = new CameraInfo(); + + public Survey3D(Mission mission, List points) { + super(mission); + polygon.addPoints(points); + } + + public void setCameraInfo(CameraInfo camera) { + this.camera = camera; + mission.notifyMissionUpdate(); + } + + public List getPath() { + return PolygonTools.offsetPolygon(polygon).getPoints(); + } + + @Override + public List packMissionItem() { + List list = new ArrayList(); + for (Coord2D point: getPath()) { + list.add(packSurveyPoint(point, new Altitude(0))); + } + return list; + } + + public static msg_mission_item packSurveyPoint(Coord2D point, Length altitude) { + msg_mission_item mavMsg = new msg_mission_item(); + mavMsg.autocontinue = 1; + mavMsg.target_component = 1; + mavMsg.target_system = 1; + mavMsg.frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT; + mavMsg.command = MAV_CMD.MAV_CMD_NAV_WAYPOINT; + mavMsg.x = (float) point.getX(); + mavMsg.y = (float) point.getY(); + mavMsg.z = (float) altitude.valueInMeters(); + mavMsg.param1 = 0f; + mavMsg.param2 = 0f; + mavMsg.param3 = 0f; + mavMsg.param4 = 0f; + return mavMsg; + } + + @Override + public void unpackMAVMessage(msg_mission_item mavMsg) { + + } + + @Override + public MissionItemType getType() { + return MissionItemType.SURVEY3D; + } + + public CameraInfo getCamera() { + return camera; + } + +} From 8cc3b18d42103fbebbbf17b2f0fe85e30617b3ac Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Mon, 24 Nov 2014 17:51:18 -0800 Subject: [PATCH 03/16] Android: Added base implementation for the Survey3D object --- .../fragment_editor_detail_survey3d.xml | 169 ++++++++++++++++++ .../android/proxy/mission/MissionProxy.java | 8 +- .../proxy/mission/item/MissionItemProxy.java | 6 + .../item/fragments/MissionDetailFragment.java | 3 + .../fragments/MissionSurvey3DFragment.java | 110 ++++++++++++ 5 files changed, 294 insertions(+), 2 deletions(-) create mode 100644 Android/res/layout/fragment_editor_detail_survey3d.xml create mode 100644 Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java diff --git a/Android/res/layout/fragment_editor_detail_survey3d.xml b/Android/res/layout/fragment_editor_detail_survey3d.xml new file mode 100644 index 0000000000..7dfd783431 --- /dev/null +++ b/Android/res/layout/fragment_editor_detail_survey3d.xml @@ -0,0 +1,169 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java b/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java index 43cde512a8..441bb18a17 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java +++ b/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java @@ -19,6 +19,7 @@ import org.droidplanner.core.mission.commands.ReturnToHome; import org.droidplanner.core.mission.commands.Takeoff; import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey3D; import org.droidplanner.core.mission.waypoints.SpatialCoordItem; import org.droidplanner.core.mission.waypoints.SplineWaypoint; import org.droidplanner.core.mission.waypoints.Waypoint; @@ -139,11 +140,11 @@ public void removeItemList(List items) { * 2D points making up the survey */ public void addSurveyPolygon(List points) { - Survey survey = new Survey(mMission, points); + Survey3D survey = new Survey3D(mMission, points); mMissionItems.add(new MissionItemProxy(this, survey)); mMission.addMissionItem(survey); try { - survey.build(); + //survey.build(); } catch (Exception e) { e.printStackTrace(); } @@ -615,7 +616,10 @@ public List> getPolygonsPath() { for (MissionItem item : mMission.getItems()) { if (item instanceof Survey) { polygonPaths.add(((Survey)item).polygon.getPoints()); + }else if (item instanceof Survey3D) { + polygonPaths.add(((Survey3D)item).polygon.getPoints()); } + } return polygonPaths; } diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java b/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java index 7c3a7c28bf..d32292fd53 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java @@ -14,12 +14,15 @@ import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.commands.Takeoff; import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey3D; import org.droidplanner.core.mission.waypoints.Circle; import org.droidplanner.core.mission.waypoints.SpatialCoordItem; import org.droidplanner.core.mission.waypoints.SplineWaypoint; import org.droidplanner.core.mission.waypoints.StructureScanner; import org.droidplanner.core.survey.grid.Grid; +import com.MAVLink.common.msg_mission_item; + import android.content.Context; import android.graphics.Color; import android.view.LayoutInflater; @@ -115,6 +118,9 @@ public List getPath(Coord2D previousPoint) { pathPoints.addAll(grid.gridPoints); } break; + case SURVEY3D: + pathPoints.addAll(((Survey3D) mMissionItem).getPath()); + break; case CYLINDRICAL_SURVEY: StructureScanner survey = (StructureScanner)mMissionItem; pathPoints.addAll(survey.getPath()); diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java index c98ddbeab7..73105841af 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java @@ -89,6 +89,9 @@ public static MissionDetailFragment newInstance(MissionItemType itemType) { case SURVEY: fragment = new MissionSurveyFragment(); break; + case SURVEY3D: + fragment = new MissionSurvey3DFragment(); + break; case TAKEOFF: fragment = new MissionTakeoffFragment(); break; diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java new file mode 100644 index 0000000000..744c60dc71 --- /dev/null +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java @@ -0,0 +1,110 @@ +package org.droidplanner.android.proxy.mission.item.fragments; + +import java.util.List; + +import org.droidplanner.R; +import org.droidplanner.R.id; +import org.droidplanner.android.proxy.mission.item.adapters.CamerasAdapter; +import org.droidplanner.android.widgets.spinnerWheel.CardWheelHorizontalView; +import org.droidplanner.android.widgets.spinnerWheel.adapters.NumericWheelAdapter; +import org.droidplanner.android.widgets.spinners.SpinnerSelfSelect; +import org.droidplanner.core.mission.MissionItemType; +import org.droidplanner.core.mission.survey.Survey3D; +import org.droidplanner.core.survey.CameraInfo; + +import android.content.Context; +import android.os.Bundle; +import android.util.Log; +import android.view.View; +import android.widget.CheckBox; +import android.widget.CompoundButton; +import android.widget.Spinner; + +public class MissionSurvey3DFragment extends MissionDetailFragment implements + CardWheelHorizontalView.OnCardWheelChangedListener, CompoundButton.OnCheckedChangeListener { + + private CheckBox checkBoxAdvanced; + private CardWheelHorizontalView startAltitudeStepPicker, stepHeightStepPicker; + private SpinnerSelfSelect cameraSpinner; + private CamerasAdapter cameraAdapter; + private List missionItems; + + @Override + protected int getResource() { + return R.layout.fragment_editor_detail_survey3d; + } + + @Override + public void onViewCreated(View view, Bundle savedInstanceState) { + super.onViewCreated(view, savedInstanceState); + final Context context = getActivity().getApplicationContext(); + + Log.d("DEBUG", "onViewCreated"); + typeSpinner.setSelection(commandAdapter.getPosition(MissionItemType.SURVEY3D)); + + missionItems = (List) getMissionItems(); + // Use the first one as reference. + final Survey3D firstItem = missionItems.get(0); + + cameraAdapter = new CamerasAdapter(getActivity(), + android.R.layout.simple_spinner_dropdown_item); + cameraSpinner = (SpinnerSelfSelect) view.findViewById(id.cameraFileSpinner); + cameraSpinner.setAdapter(cameraAdapter); + cameraSpinner.setOnSpinnerItemSelectedListener(this); + cameraAdapter.setTitle(firstItem.getCamera().name); + + startAltitudeStepPicker = (CardWheelHorizontalView) view + .findViewById(R.id.startAltitudePicker); + startAltitudeStepPicker.setViewAdapter(new NumericWheelAdapter(context, + R.layout.wheel_text_centered, MIN_ALTITUDE, MAX_ALTITUDE, "%d m")); + startAltitudeStepPicker.addChangingListener(this); + //startAltitudeStepPicker.setCurrentValue((int) firstItem.getCoordinate().getAltitude().valueInMeters()); + + stepHeightStepPicker = (CardWheelHorizontalView) view.findViewById(R.id.heightStepPicker); + stepHeightStepPicker.setViewAdapter(new NumericWheelAdapter(context, + R.layout.wheel_text_centered, 1, MAX_ALTITUDE, "%d m")); + stepHeightStepPicker.addChangingListener(this); + //stepHeightStepPicker.setCurrentValue((int) firstItem.getEndAltitude().valueInMeters()); + + checkBoxAdvanced = (CheckBox) view.findViewById(R.id.checkBoxSurveyCrossHatch); + checkBoxAdvanced.setOnCheckedChangeListener(this); + //checkBoxAdvanced.setChecked(firstItem.isCrossHatchEnabled()); + + } + + @Override + public void onCheckedChanged(CompoundButton buttonView, boolean isChecked) { + //getItem().enableCrossHatch(isChecked); + getMissionProxy().getMission().notifyMissionUpdate(); + } + + @Override + public void onChanged(CardWheelHorizontalView cardWheel, int oldValue, int newValue) { + switch (cardWheel.getId()) { + case R.id.startAltitudePicker: + //getItem().setAltitude(new Altitude(newValue)); + break; + case R.id.heightStepPicker: + //getItem().setAltitudeStep(newValue); + break; + } + getMissionProxy().getMission().notifyMissionUpdate(); + } + + @Override + public void onSpinnerItemSelected(Spinner spinner, int position) { + if (spinner.getId() == id.cameraFileSpinner) { + CameraInfo cameraInfo = cameraAdapter.getCamera(position); + cameraAdapter.setTitle(cameraInfo.name); + for (Survey3D scan : missionItems) { + scan.setCameraInfo(cameraInfo); + } + getMissionProxy().getMission().notifyMissionUpdate(); + } + } + + private Survey3D getItem() { + Survey3D cylindricalSurvey = (Survey3D) getMissionItems().get(0); + return cylindricalSurvey; + } +} From c2383777f927e651b4f03a8284b67d5cee561ddc Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Mon, 24 Nov 2014 17:48:47 -0800 Subject: [PATCH 04/16] GeoTools: Add a polygon complex/simple test in PolygonTools --- .../core/helpers/geoTools/PolygonTools.java | 40 ++++++++++++++++--- .../helpers/geoTools/PolygonToolsTest.java | 31 ++++++++++++++ 2 files changed, 65 insertions(+), 6 deletions(-) create mode 100644 Core/test/org/droidplanner/core/helpers/geoTools/PolygonToolsTest.java diff --git a/Core/src/org/droidplanner/core/helpers/geoTools/PolygonTools.java b/Core/src/org/droidplanner/core/helpers/geoTools/PolygonTools.java index 6a4ee5bafb..b86dab84ba 100644 --- a/Core/src/org/droidplanner/core/helpers/geoTools/PolygonTools.java +++ b/Core/src/org/droidplanner/core/helpers/geoTools/PolygonTools.java @@ -1,31 +1,59 @@ package org.droidplanner.core.helpers.geoTools; import java.util.ArrayList; +import java.util.List; import org.droidplanner.core.helpers.coordinates.Coord2D; import org.droidplanner.core.polygon.Polygon; public class PolygonTools { - public static Polygon offsetPolygon(Polygon polygon) { - ArrayList offsetLines = new ArrayList(); + public static Polygon offsetPolygon(Polygon polygon) throws Exception { + if (!PolygonTools.isSimplePolygon(polygon)) { + throw new Exception("Complex Polygon"); + } + + ArrayList offsetLines = new ArrayList(); for (LineCoord2D line : polygon.getLines()) { - offsetLines.add(LineTools.getParallelLineToTheLeft(line,10)); + offsetLines.add(LineTools.getParallelLineToTheLeft(line, 10)); } - + ArrayList path = new ArrayList(); offsetLines.add(offsetLines.get(0)); offsetLines.add(offsetLines.get(1)); for (int i = 1; i < offsetLines.size(); i++) { try { - path.add(LineTools.FindLineIntersection(offsetLines.get(i-1), offsetLines.get(i),true)); + path.add(LineTools.FindLineIntersection(offsetLines.get(i - 1), offsetLines.get(i), + true)); } catch (Exception e) { e.printStackTrace(); - } + } } Polygon polygonwithOffset = new Polygon(); polygonwithOffset.addPoints(path); return polygonwithOffset; } + public static boolean isSimplePolygon(Polygon polygon) { + List lines = polygon.getLines(); + for (LineCoord2D line1 : lines) { + for (LineCoord2D line2 : lines) { + if (line1.equals(line2)) { + continue; + } + try{ + Coord2D intersection = LineTools.FindLineIntersection(line1, line2); + if (intersection.equals(line1.getStart()) + || intersection.equals(line1.getEnd()) + || intersection.equals(line2.getStart()) + || intersection.equals(line2.getEnd())) { + continue; + } + return false; + } catch (Exception e) { + } + } + } + return true; + } } diff --git a/Core/test/org/droidplanner/core/helpers/geoTools/PolygonToolsTest.java b/Core/test/org/droidplanner/core/helpers/geoTools/PolygonToolsTest.java new file mode 100644 index 0000000000..c124b90919 --- /dev/null +++ b/Core/test/org/droidplanner/core/helpers/geoTools/PolygonToolsTest.java @@ -0,0 +1,31 @@ +package org.droidplanner.core.helpers.geoTools; + +import org.droidplanner.core.helpers.coordinates.Coord2D; +import org.droidplanner.core.polygon.Polygon; + +import junit.framework.TestCase; + +public class PolygonToolsTest extends TestCase { + + public void testIsSimplePolygon() { + Polygon polygon = new Polygon(); + + polygon.addPoint(new Coord2D(0, 0)); + polygon.addPoint(new Coord2D(0, 1)); + polygon.addPoint(new Coord2D(1, 1)); + polygon.addPoint(new Coord2D(1, 0)); + + assertTrue(PolygonTools.isSimplePolygon(polygon)); + } + + public void testIsComplexPolygon() { + Polygon polygon = new Polygon(); + + polygon.addPoint(new Coord2D(0, 0)); + polygon.addPoint(new Coord2D(1, 1)); + polygon.addPoint(new Coord2D(0, 1)); + polygon.addPoint(new Coord2D(1, 0)); + + assertFalse(PolygonTools.isSimplePolygon(polygon)); + } +} \ No newline at end of file From 3aafcd890beab21f9b4172264a4c4088712d275e Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Mon, 24 Nov 2014 17:49:49 -0800 Subject: [PATCH 05/16] Survey3D: Test the polygon before drawing it on the screen. --- .../src/org/droidplanner/core/mission/survey/Survey3D.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java index 99b0a39804..298daa3f36 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java @@ -33,7 +33,12 @@ public void setCameraInfo(CameraInfo camera) { } public List getPath() { - return PolygonTools.offsetPolygon(polygon).getPoints(); + try { + List path = PolygonTools.offsetPolygon(polygon).getPoints(); + return path; + } catch (Exception e) { + return new ArrayList(); + } } @Override From 7d2634e517f9bd060211c9deb644d18f8f73a01e Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 25 Nov 2014 10:43:58 -0800 Subject: [PATCH 06/16] PolygonTools: method for testing the orientation of a polygon --- .../core/helpers/geoTools/PolygonTools.java | 9 +++++++++ .../helpers/geoTools/PolygonToolsTest.java | 18 ++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/Core/src/org/droidplanner/core/helpers/geoTools/PolygonTools.java b/Core/src/org/droidplanner/core/helpers/geoTools/PolygonTools.java index b86dab84ba..4f9c902dac 100644 --- a/Core/src/org/droidplanner/core/helpers/geoTools/PolygonTools.java +++ b/Core/src/org/droidplanner/core/helpers/geoTools/PolygonTools.java @@ -56,4 +56,13 @@ public static boolean isSimplePolygon(Polygon polygon) { } return true; } + + public static boolean isClockWisePolygon(Polygon polygon) { + double sum = 0; + for (LineCoord2D line : polygon.getLines()) { + sum += (line.getEnd().getX() - line.getStart().getX()) + / (line.getEnd().getY() + line.getStart().getY()); + } + return sum > 0 ? false : true; + } } diff --git a/Core/test/org/droidplanner/core/helpers/geoTools/PolygonToolsTest.java b/Core/test/org/droidplanner/core/helpers/geoTools/PolygonToolsTest.java index c124b90919..18421321eb 100644 --- a/Core/test/org/droidplanner/core/helpers/geoTools/PolygonToolsTest.java +++ b/Core/test/org/droidplanner/core/helpers/geoTools/PolygonToolsTest.java @@ -28,4 +28,22 @@ public void testIsComplexPolygon() { assertFalse(PolygonTools.isSimplePolygon(polygon)); } + + public void testIsCWPolygon() { + Polygon polygonCCW = new Polygon(); + polygonCCW.addPoint(new Coord2D(0, 0)); + polygonCCW.addPoint(new Coord2D(0, 1)); + polygonCCW.addPoint(new Coord2D(1, 1)); + polygonCCW.addPoint(new Coord2D(1, 0)); + + assertFalse(PolygonTools.isClockWisePolygon(polygonCCW)); + + Polygon polygonCW = new Polygon(); + polygonCW.addPoint(new Coord2D(1, 0)); + polygonCW.addPoint(new Coord2D(1, 1)); + polygonCW.addPoint(new Coord2D(0, 1)); + polygonCW.addPoint(new Coord2D(0, 0)); + + assertTrue(PolygonTools.isClockWisePolygon(polygonCW)); + } } \ No newline at end of file From 89d1ebf9cd2c6048d1b90d205c7d99e798771096 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 25 Nov 2014 10:44:18 -0800 Subject: [PATCH 07/16] Polygon: Use the orientation to draw the polygon offset --- .../org/droidplanner/core/helpers/geoTools/LineTools.java | 8 +++++--- .../droidplanner/core/helpers/geoTools/PolygonTools.java | 4 ++-- .../org/droidplanner/core/mission/survey/Survey3D.java | 2 +- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/Core/src/org/droidplanner/core/helpers/geoTools/LineTools.java b/Core/src/org/droidplanner/core/helpers/geoTools/LineTools.java index 8130ae56da..86de6fdc82 100644 --- a/Core/src/org/droidplanner/core/helpers/geoTools/LineTools.java +++ b/Core/src/org/droidplanner/core/helpers/geoTools/LineTools.java @@ -83,9 +83,11 @@ public static LineCoord2D findClosestLineToPoint(Coord2D point, List offsetLines = new ArrayList(); for (LineCoord2D line : polygon.getLines()) { - offsetLines.add(LineTools.getParallelLineToTheLeft(line, 10)); + offsetLines.add(LineTools.getParallelLine(line, offset,PolygonTools.isClockWisePolygon(polygon))); } ArrayList path = new ArrayList(); diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java index 298daa3f36..6d129c346e 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java @@ -34,7 +34,7 @@ public void setCameraInfo(CameraInfo camera) { public List getPath() { try { - List path = PolygonTools.offsetPolygon(polygon).getPoints(); + List path = PolygonTools.offsetPolygon(polygon, 10).getPoints(); return path; } catch (Exception e) { return new ArrayList(); From ee07807524d4b0e18dfd0c4bd054516ad97762f7 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 25 Nov 2014 11:47:07 -0800 Subject: [PATCH 08/16] Survey: Renamed Survey to Survey2D --- .../core/mission/survey/{Survey.java => Survey2D.java} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename Core/src/org/droidplanner/core/mission/survey/{Survey.java => Survey2D.java} (100%) diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey.java b/Core/src/org/droidplanner/core/mission/survey/Survey2D.java similarity index 100% rename from Core/src/org/droidplanner/core/mission/survey/Survey.java rename to Core/src/org/droidplanner/core/mission/survey/Survey2D.java From 97419b610a4f9e7c58cf8c4e5810738387e3ed9f Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 25 Nov 2014 11:47:39 -0800 Subject: [PATCH 09/16] Survey: Fixed imports broken by last commit --- .../android/proxy/mission/MissionProxy.java | 8 ++++---- .../android/proxy/mission/item/MissionItemProxy.java | 8 ++++---- .../mission/item/fragments/MissionDetailFragment.java | 4 ++-- .../mission/item/fragments/MissionSurveyFragment.java | 10 +++++----- .../proxy/mission/item/markers/PolygonMarkerInfo.java | 8 ++++---- .../mission/item/markers/SurveyMarkerInfoProvider.java | 6 +++--- .../org/droidplanner/core/mission/MissionItemType.java | 4 ++-- .../org/droidplanner/core/mission/survey/Survey2D.java | 4 ++-- .../core/mission/waypoints/StructureScanner.java | 6 +++--- 9 files changed, 29 insertions(+), 29 deletions(-) diff --git a/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java b/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java index 441bb18a17..66d432b9a9 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java +++ b/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java @@ -18,7 +18,7 @@ import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.commands.ReturnToHome; import org.droidplanner.core.mission.commands.Takeoff; -import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey2D; import org.droidplanner.core.mission.survey.Survey3D; import org.droidplanner.core.mission.waypoints.SpatialCoordItem; import org.droidplanner.core.mission.waypoints.SplineWaypoint; @@ -511,7 +511,7 @@ public void move(MissionItemProxy item, Coord2D position) { mMission.notifyMissionUpdate(); } - public void movePolygonPoint(Survey survey, int index, Coord2D position) { + public void movePolygonPoint(Survey2D survey, int index, Coord2D position) { survey.polygon.movePoint(position, index); try { survey.build(); @@ -614,8 +614,8 @@ public float makeAndUploadDronie() { public List> getPolygonsPath() { ArrayList> polygonPaths = new ArrayList>(); for (MissionItem item : mMission.getItems()) { - if (item instanceof Survey) { - polygonPaths.add(((Survey)item).polygon.getPoints()); + if (item instanceof Survey2D) { + polygonPaths.add(((Survey2D)item).polygon.getPoints()); }else if (item instanceof Survey3D) { polygonPaths.add(((Survey3D)item).polygon.getPoints()); } diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java b/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java index d32292fd53..bc0ab5ec5e 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java @@ -13,7 +13,7 @@ import org.droidplanner.core.helpers.units.Length; import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.commands.Takeoff; -import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey2D; import org.droidplanner.core.mission.survey.Survey3D; import org.droidplanner.core.mission.waypoints.Circle; import org.droidplanner.core.mission.waypoints.SpatialCoordItem; @@ -113,7 +113,7 @@ public List getPath(Coord2D previousPoint) { break; case SURVEY: - Grid grid = ((Survey) mMissionItem).grid; + Grid grid = ((Survey2D) mMissionItem).grid; if (grid != null) { pathPoints.addAll(grid.gridPoints); } @@ -163,8 +163,8 @@ public View getListViewItemView(Context context, ViewGroup parent) { } catch (Exception e) { // Do nothing when last item doesn't have an altitude } - } else if (mMissionItem instanceof Survey) { - altitudeView.setText(((Survey) mMissionItem).surveyData.getAltitude().toString()); + } else if (mMissionItem instanceof Survey2D) { + altitudeView.setText(((Survey2D) mMissionItem).surveyData.getAltitude().toString()); } else if (mMissionItem instanceof Takeoff) { altitudeView.setText(((Takeoff) mMissionItem).getFinishedAlt().toString()); diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java index 73105841af..6621e0b99b 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java @@ -14,7 +14,7 @@ import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.MissionItemType; import org.droidplanner.core.mission.commands.MissionCMD; -import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey2D; import org.droidplanner.core.mission.waypoints.StructureScanner; import org.droidplanner.core.util.Pair; @@ -163,7 +163,7 @@ public void onViewCreated(View view, Bundle savedInstanceState) { final MissionItemProxy itemProxy = mSelectedProxies.get(0); final MissionItem currentItem = itemProxy.getMissionItem(); - if ((currentItem instanceof Survey)) { + if ((currentItem instanceof Survey2D)) { list.clear(); list.add(MissionItemType.SURVEY); } else { diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java index 94dc9040f4..4cb9e39502 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java @@ -12,7 +12,7 @@ import org.droidplanner.core.drone.DroneInterfaces; import org.droidplanner.core.helpers.units.Altitude; import org.droidplanner.core.mission.MissionItemType; -import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey2D; import org.droidplanner.core.model.Drone; import org.droidplanner.core.survey.CameraInfo; @@ -46,7 +46,7 @@ public class MissionSurveyFragment extends MissionDetailFragment implements public TextView lengthView; private CamerasAdapter cameraAdapter; - private List surveyList; + private List surveyList; @Override protected int getResource() { @@ -71,7 +71,7 @@ public void onViewCreated(View view, Bundle savedInstanceState) { final Context context = getActivity().getApplicationContext(); - this.surveyList = ((List) getMissionItems()); + this.surveyList = ((List) getMissionItems()); cameraAdapter = new CamerasAdapter(getActivity(), android.R.layout.simple_spinner_dropdown_item); @@ -122,7 +122,7 @@ public void onSpinnerItemSelected(Spinner spinner, int position) { if (spinner.getId() == id.cameraFileSpinner) { CameraInfo cameraInfo = cameraAdapter.getCamera(position); cameraAdapter.setTitle(cameraInfo.name); - for (Survey survey : surveyList) { + for (Survey2D survey : surveyList) { survey.setCameraInfo(cameraInfo); } @@ -139,7 +139,7 @@ public void onChanged(CardWheelHorizontalView cardWheel, int oldValue, int newVa case R.id.overlapPicker: case R.id.sidelapPicker: try { - for (Survey survey : surveyList) { + for (Survey2D survey : surveyList) { survey.update(mAnglePicker.getCurrentValue(), new Altitude(mAltitudePicker.getCurrentValue()), mOverlapPicker.getCurrentValue(), mSidelapPicker.getCurrentValue()); diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java b/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java index ebfc3a0fc4..b56ee60ac0 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java @@ -2,23 +2,23 @@ import org.droidplanner.android.maps.MarkerInfo; import org.droidplanner.core.helpers.coordinates.Coord2D; -import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey2D; /** */ public class PolygonMarkerInfo extends MarkerInfo.SimpleMarkerInfo { private Coord2D mPoint; - private Survey survey; + private Survey2D survey; private int polygonIndex; - public PolygonMarkerInfo(Coord2D point, Survey mSurvey, int index) { + public PolygonMarkerInfo(Coord2D point, Survey2D mSurvey, int index) { mPoint = point; survey = mSurvey; polygonIndex = index; } - public Survey getSurvey(){ + public Survey2D getSurvey(){ return survey; } diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/markers/SurveyMarkerInfoProvider.java b/Android/src/org/droidplanner/android/proxy/mission/item/markers/SurveyMarkerInfoProvider.java index 8e9a482056..050476c91a 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/markers/SurveyMarkerInfoProvider.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/markers/SurveyMarkerInfoProvider.java @@ -6,18 +6,18 @@ import org.droidplanner.android.maps.MarkerInfo; import org.droidplanner.android.proxy.mission.item.MissionItemProxy; import org.droidplanner.core.helpers.coordinates.Coord2D; -import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey2D; /** * */ public class SurveyMarkerInfoProvider { - private final Survey mSurvey; + private final Survey2D mSurvey; private final List mPolygonMarkers = new ArrayList(); protected SurveyMarkerInfoProvider(MissionItemProxy origin) { - mSurvey = (Survey) origin.getMissionItem(); + mSurvey = (Survey2D) origin.getMissionItem(); updateMarkerInfoList(); } diff --git a/Core/src/org/droidplanner/core/mission/MissionItemType.java b/Core/src/org/droidplanner/core/mission/MissionItemType.java index 41d309d303..d0d1a427b6 100644 --- a/Core/src/org/droidplanner/core/mission/MissionItemType.java +++ b/Core/src/org/droidplanner/core/mission/MissionItemType.java @@ -10,7 +10,7 @@ import org.droidplanner.core.mission.commands.ReturnToHome; import org.droidplanner.core.mission.commands.SetServo; import org.droidplanner.core.mission.commands.Takeoff; -import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey2D; import org.droidplanner.core.mission.survey.Survey3D; import org.droidplanner.core.mission.waypoints.Circle; import org.droidplanner.core.mission.waypoints.Land; @@ -57,7 +57,7 @@ public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentE case ROI: return new RegionOfInterest(referenceItem); case SURVEY: - return new Survey(referenceItem.getMission(), Collections. emptyList()); + return new Survey2D(referenceItem.getMission(), Collections. emptyList()); case SURVEY3D: return new Survey3D(referenceItem.getMission(), Collections. emptyList()); case CYLINDRICAL_SURVEY: diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey2D.java b/Core/src/org/droidplanner/core/mission/survey/Survey2D.java index 3d9ae6bbcd..fb03638405 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey2D.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey2D.java @@ -20,13 +20,13 @@ import com.MAVLink.enums.MAV_CMD; import com.MAVLink.enums.MAV_FRAME; -public class Survey extends MissionItem { +public class Survey2D extends MissionItem { public Polygon polygon = new Polygon(); public SurveyData surveyData = new SurveyData(); public Grid grid; - public Survey(Mission mission, List points) { + public Survey2D(Mission mission, List points) { super(mission); polygon.addPoints(points); } diff --git a/Core/src/org/droidplanner/core/mission/waypoints/StructureScanner.java b/Core/src/org/droidplanner/core/mission/waypoints/StructureScanner.java index edfcd72930..96df8f3f6d 100644 --- a/Core/src/org/droidplanner/core/mission/waypoints/StructureScanner.java +++ b/Core/src/org/droidplanner/core/mission/waypoints/StructureScanner.java @@ -11,7 +11,7 @@ import org.droidplanner.core.mission.Mission; import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.MissionItemType; -import org.droidplanner.core.mission.survey.Survey; +import org.droidplanner.core.mission.survey.Survey2D; import org.droidplanner.core.polygon.Polygon; import org.droidplanner.core.survey.CameraInfo; import org.droidplanner.core.survey.SurveyData; @@ -77,13 +77,13 @@ private void packHatch(List list) { survey.update(0.0, survey.getAltitude(), survey.getOverlap(), survey.getSidelap()); GridBuilder grid = new GridBuilder(polygon, survey, corner); for (Coord2D point : grid.generate(false).gridPoints) { - list.add(Survey.packSurveyPoint(point, getTopHeight())); + list.add(Survey2D.packSurveyPoint(point, getTopHeight())); } survey.update(90.0, survey.getAltitude(), survey.getOverlap(), survey.getSidelap()); GridBuilder grid2 = new GridBuilder(polygon, survey, corner); for (Coord2D point : grid2.generate(false).gridPoints) { - list.add(Survey.packSurveyPoint(point, getTopHeight())); + list.add(Survey2D.packSurveyPoint(point, getTopHeight())); } } catch (Exception e) { // Should never fail, since it has good polygons } From ce0335cfb294dc5ef221cbeed56d294d03b32568 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 25 Nov 2014 11:51:07 -0800 Subject: [PATCH 10/16] Survey: Make survey3D similar to Survey2D --- .../item/fragments/MissionSurvey3DFragment.java | 2 +- .../droidplanner/core/mission/survey/Survey3D.java | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java index 744c60dc71..406f24b845 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java @@ -51,7 +51,7 @@ public void onViewCreated(View view, Bundle savedInstanceState) { cameraSpinner = (SpinnerSelfSelect) view.findViewById(id.cameraFileSpinner); cameraSpinner.setAdapter(cameraAdapter); cameraSpinner.setOnSpinnerItemSelectedListener(this); - cameraAdapter.setTitle(firstItem.getCamera().name); + cameraAdapter.setTitle(firstItem.getCamera()); startAltitudeStepPicker = (CardWheelHorizontalView) view .findViewById(R.id.startAltitudePicker); diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java index 6d129c346e..4fbe75ba95 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java @@ -5,13 +5,13 @@ import org.droidplanner.core.helpers.coordinates.Coord2D; import org.droidplanner.core.helpers.geoTools.PolygonTools; -import org.droidplanner.core.helpers.units.Altitude; import org.droidplanner.core.helpers.units.Length; import org.droidplanner.core.mission.Mission; import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.MissionItemType; import org.droidplanner.core.polygon.Polygon; import org.droidplanner.core.survey.CameraInfo; +import org.droidplanner.core.survey.SurveyData; import com.MAVLink.common.msg_mission_item; import com.MAVLink.enums.MAV_CMD; @@ -20,7 +20,7 @@ public class Survey3D extends MissionItem { public Polygon polygon = new Polygon(); - private CameraInfo camera = new CameraInfo(); + public SurveyData surveyData = new SurveyData(); public Survey3D(Mission mission, List points) { super(mission); @@ -28,7 +28,7 @@ public Survey3D(Mission mission, List points) { } public void setCameraInfo(CameraInfo camera) { - this.camera = camera; + surveyData.setCameraInfo(camera); mission.notifyMissionUpdate(); } @@ -45,7 +45,7 @@ public List getPath() { public List packMissionItem() { List list = new ArrayList(); for (Coord2D point: getPath()) { - list.add(packSurveyPoint(point, new Altitude(0))); + list.add(packSurveyPoint(point, surveyData.getAltitude())); } return list; } @@ -77,8 +77,8 @@ public MissionItemType getType() { return MissionItemType.SURVEY3D; } - public CameraInfo getCamera() { - return camera; + public String getCamera() { + return surveyData.getCameraName(); } } From f7d1323d653e4071dad50ce12a1c04ab15b9904e Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 25 Nov 2014 14:03:47 -0800 Subject: [PATCH 11/16] Survey: Extract super Survey class --- .../android/proxy/mission/MissionProxy.java | 9 ++- .../proxy/mission/item/MissionItemProxy.java | 3 +- .../item/fragments/MissionSurveyFragment.java | 3 +- .../item/markers/PolygonMarkerInfo.java | 8 +-- .../markers/SurveyMarkerInfoProvider.java | 6 +- .../core/mission/survey/Survey.java | 65 +++++++++++++++++++ .../core/mission/survey/Survey2D.java | 39 +---------- .../core/mission/survey/Survey3D.java | 48 ++------------ 8 files changed, 88 insertions(+), 93 deletions(-) create mode 100644 Core/src/org/droidplanner/core/mission/survey/Survey.java diff --git a/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java b/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java index 66d432b9a9..275f7a6e00 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java +++ b/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java @@ -18,6 +18,7 @@ import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.commands.ReturnToHome; import org.droidplanner.core.mission.commands.Takeoff; +import org.droidplanner.core.mission.survey.Survey; import org.droidplanner.core.mission.survey.Survey2D; import org.droidplanner.core.mission.survey.Survey3D; import org.droidplanner.core.mission.waypoints.SpatialCoordItem; @@ -511,7 +512,7 @@ public void move(MissionItemProxy item, Coord2D position) { mMission.notifyMissionUpdate(); } - public void movePolygonPoint(Survey2D survey, int index, Coord2D position) { + public void movePolygonPoint(Survey survey, int index, Coord2D position) { survey.polygon.movePoint(position, index); try { survey.build(); @@ -614,10 +615,8 @@ public float makeAndUploadDronie() { public List> getPolygonsPath() { ArrayList> polygonPaths = new ArrayList>(); for (MissionItem item : mMission.getItems()) { - if (item instanceof Survey2D) { - polygonPaths.add(((Survey2D)item).polygon.getPoints()); - }else if (item instanceof Survey3D) { - polygonPaths.add(((Survey3D)item).polygon.getPoints()); + if (item instanceof Survey) { + polygonPaths.add(((Survey)item).polygon.getPoints()); } } diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java b/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java index bc0ab5ec5e..e01fc1da58 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java @@ -13,6 +13,7 @@ import org.droidplanner.core.helpers.units.Length; import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.commands.Takeoff; +import org.droidplanner.core.mission.survey.Survey; import org.droidplanner.core.mission.survey.Survey2D; import org.droidplanner.core.mission.survey.Survey3D; import org.droidplanner.core.mission.waypoints.Circle; @@ -164,7 +165,7 @@ public View getListViewItemView(Context context, ViewGroup parent) { // Do nothing when last item doesn't have an altitude } } else if (mMissionItem instanceof Survey2D) { - altitudeView.setText(((Survey2D) mMissionItem).surveyData.getAltitude().toString()); + altitudeView.setText(((Survey) mMissionItem).surveyData.getAltitude().toString()); } else if (mMissionItem instanceof Takeoff) { altitudeView.setText(((Takeoff) mMissionItem).getFinishedAlt().toString()); diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java index 4cb9e39502..539b915c3d 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java @@ -12,6 +12,7 @@ import org.droidplanner.core.drone.DroneInterfaces; import org.droidplanner.core.helpers.units.Altitude; import org.droidplanner.core.mission.MissionItemType; +import org.droidplanner.core.mission.survey.Survey; import org.droidplanner.core.mission.survey.Survey2D; import org.droidplanner.core.model.Drone; import org.droidplanner.core.survey.CameraInfo; @@ -122,7 +123,7 @@ public void onSpinnerItemSelected(Spinner spinner, int position) { if (spinner.getId() == id.cameraFileSpinner) { CameraInfo cameraInfo = cameraAdapter.getCamera(position); cameraAdapter.setTitle(cameraInfo.name); - for (Survey2D survey : surveyList) { + for (Survey survey : surveyList) { survey.setCameraInfo(cameraInfo); } diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java b/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java index b56ee60ac0..ebfc3a0fc4 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java @@ -2,23 +2,23 @@ import org.droidplanner.android.maps.MarkerInfo; import org.droidplanner.core.helpers.coordinates.Coord2D; -import org.droidplanner.core.mission.survey.Survey2D; +import org.droidplanner.core.mission.survey.Survey; /** */ public class PolygonMarkerInfo extends MarkerInfo.SimpleMarkerInfo { private Coord2D mPoint; - private Survey2D survey; + private Survey survey; private int polygonIndex; - public PolygonMarkerInfo(Coord2D point, Survey2D mSurvey, int index) { + public PolygonMarkerInfo(Coord2D point, Survey mSurvey, int index) { mPoint = point; survey = mSurvey; polygonIndex = index; } - public Survey2D getSurvey(){ + public Survey getSurvey(){ return survey; } diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/markers/SurveyMarkerInfoProvider.java b/Android/src/org/droidplanner/android/proxy/mission/item/markers/SurveyMarkerInfoProvider.java index 050476c91a..8e9a482056 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/markers/SurveyMarkerInfoProvider.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/markers/SurveyMarkerInfoProvider.java @@ -6,18 +6,18 @@ import org.droidplanner.android.maps.MarkerInfo; import org.droidplanner.android.proxy.mission.item.MissionItemProxy; import org.droidplanner.core.helpers.coordinates.Coord2D; -import org.droidplanner.core.mission.survey.Survey2D; +import org.droidplanner.core.mission.survey.Survey; /** * */ public class SurveyMarkerInfoProvider { - private final Survey2D mSurvey; + private final Survey mSurvey; private final List mPolygonMarkers = new ArrayList(); protected SurveyMarkerInfoProvider(MissionItemProxy origin) { - mSurvey = (Survey2D) origin.getMissionItem(); + mSurvey = (Survey) origin.getMissionItem(); updateMarkerInfoList(); } diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey.java b/Core/src/org/droidplanner/core/mission/survey/Survey.java new file mode 100644 index 0000000000..276c77bba2 --- /dev/null +++ b/Core/src/org/droidplanner/core/mission/survey/Survey.java @@ -0,0 +1,65 @@ +package org.droidplanner.core.mission.survey; + +import java.util.List; + +import org.droidplanner.core.helpers.coordinates.Coord2D; +import org.droidplanner.core.helpers.units.Length; +import org.droidplanner.core.mission.Mission; +import org.droidplanner.core.mission.MissionItem; +import org.droidplanner.core.polygon.Polygon; +import org.droidplanner.core.survey.CameraInfo; +import org.droidplanner.core.survey.SurveyData; + +import com.MAVLink.common.msg_mission_item; +import com.MAVLink.enums.MAV_CMD; +import com.MAVLink.enums.MAV_FRAME; + +public abstract class Survey extends MissionItem { + + public Polygon polygon = new Polygon(); + public SurveyData surveyData = new SurveyData(); + + protected Survey(Mission mission) { + super(mission); + } + + protected Survey(MissionItem item) { + super(item); + } + + protected Survey(Mission mission, List points) { + super(mission); + polygon.addPoints(points); + } + + public void setCameraInfo(CameraInfo camera) { + surveyData.setCameraInfo(camera); + mission.notifyMissionUpdate(); + } + + public String getCamera() { + return surveyData.getCameraName(); + } + + public abstract void build() throws Exception; + + @Override + public final void unpackMAVMessage(msg_mission_item mavMsg) { + } + + public static msg_mission_item packSurveyPoint(Coord2D point, Length altitude) { + msg_mission_item mavMsg = new msg_mission_item(); + mavMsg.autocontinue = 1; + mavMsg.frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT; + mavMsg.command = MAV_CMD.MAV_CMD_NAV_WAYPOINT; + mavMsg.x = (float) point.getX(); + mavMsg.y = (float) point.getY(); + mavMsg.z = (float) altitude.valueInMeters(); + mavMsg.param1 = 0f; + mavMsg.param2 = 0f; + mavMsg.param3 = 0f; + mavMsg.param4 = 0f; + return mavMsg; + } + +} \ No newline at end of file diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey2D.java b/Core/src/org/droidplanner/core/mission/survey/Survey2D.java index fb03638405..149568ddea 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey2D.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey2D.java @@ -7,28 +7,19 @@ import org.droidplanner.core.helpers.units.Altitude; import org.droidplanner.core.helpers.units.Length; import org.droidplanner.core.mission.Mission; -import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.MissionItemType; import org.droidplanner.core.mission.commands.CameraTrigger; -import org.droidplanner.core.polygon.Polygon; -import org.droidplanner.core.survey.CameraInfo; -import org.droidplanner.core.survey.SurveyData; import org.droidplanner.core.survey.grid.Grid; import org.droidplanner.core.survey.grid.GridBuilder; import com.MAVLink.common.msg_mission_item; -import com.MAVLink.enums.MAV_CMD; -import com.MAVLink.enums.MAV_FRAME; -public class Survey2D extends MissionItem { +public class Survey2D extends Survey { - public Polygon polygon = new Polygon(); - public SurveyData surveyData = new SurveyData(); public Grid grid; public Survey2D(Mission mission, List points) { - super(mission); - polygon.addPoints(points); + super(mission, points); } public void update(double angle, Altitude altitude, double overlap, double sidelap) { @@ -36,11 +27,6 @@ public void update(double angle, Altitude altitude, double overlap, double sidel mission.notifyMissionUpdate(); } - public void setCameraInfo(CameraInfo camera) { - surveyData.setCameraInfo(camera); - mission.notifyMissionUpdate(); - } - public void build() throws Exception { // TODO find better point than (0,0) to reference the grid grid = null; @@ -72,27 +58,6 @@ private void packGridPoints(List list) { } } - public static msg_mission_item packSurveyPoint(Coord2D point, Length altitude) { - msg_mission_item mavMsg = new msg_mission_item(); - mavMsg.autocontinue = 1; - mavMsg.frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT; - mavMsg.command = MAV_CMD.MAV_CMD_NAV_WAYPOINT; - mavMsg.x = (float) point.getX(); - mavMsg.y = (float) point.getY(); - mavMsg.z = (float) altitude.valueInMeters(); - mavMsg.param1 = 0f; - mavMsg.param2 = 0f; - mavMsg.param3 = 0f; - mavMsg.param4 = 0f; - return mavMsg; - } - - @Override - public void unpackMAVMessage(msg_mission_item mavMsg) { - // TODO Auto-generated method stub - - } - @Override public MissionItemType getType() { return MissionItemType.SURVEY; diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java index 4fbe75ba95..77b16394ba 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java @@ -5,31 +5,15 @@ import org.droidplanner.core.helpers.coordinates.Coord2D; import org.droidplanner.core.helpers.geoTools.PolygonTools; -import org.droidplanner.core.helpers.units.Length; import org.droidplanner.core.mission.Mission; -import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.MissionItemType; -import org.droidplanner.core.polygon.Polygon; -import org.droidplanner.core.survey.CameraInfo; -import org.droidplanner.core.survey.SurveyData; import com.MAVLink.common.msg_mission_item; -import com.MAVLink.enums.MAV_CMD; -import com.MAVLink.enums.MAV_FRAME; -public class Survey3D extends MissionItem { - - public Polygon polygon = new Polygon(); - public SurveyData surveyData = new SurveyData(); +public class Survey3D extends Survey { public Survey3D(Mission mission, List points) { - super(mission); - polygon.addPoints(points); - } - - public void setCameraInfo(CameraInfo camera) { - surveyData.setCameraInfo(camera); - mission.notifyMissionUpdate(); + super(mission, points); } public List getPath() { @@ -50,35 +34,15 @@ public List packMissionItem() { return list; } - public static msg_mission_item packSurveyPoint(Coord2D point, Length altitude) { - msg_mission_item mavMsg = new msg_mission_item(); - mavMsg.autocontinue = 1; - mavMsg.target_component = 1; - mavMsg.target_system = 1; - mavMsg.frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT; - mavMsg.command = MAV_CMD.MAV_CMD_NAV_WAYPOINT; - mavMsg.x = (float) point.getX(); - mavMsg.y = (float) point.getY(); - mavMsg.z = (float) altitude.valueInMeters(); - mavMsg.param1 = 0f; - mavMsg.param2 = 0f; - mavMsg.param3 = 0f; - mavMsg.param4 = 0f; - return mavMsg; - } - - @Override - public void unpackMAVMessage(msg_mission_item mavMsg) { - - } - @Override public MissionItemType getType() { return MissionItemType.SURVEY3D; } - public String getCamera() { - return surveyData.getCameraName(); + @Override + public void build() throws Exception { + // TODO Auto-generated method stub + } } From cbb5a3ebc351015c46126b445a928c6192f70d76 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 25 Nov 2014 14:09:26 -0800 Subject: [PATCH 12/16] Survey: Removed the Structure Scanner class It was replaced by the Survey3D --- .../proxy/mission/item/MissionItemProxy.java | 9 +- .../item/fragments/MissionDetailFragment.java | 25 +-- .../MissionStructureScannerFragment.java | 143 --------------- .../item/fragments/MissionSurveyFragment.java | 2 +- .../item/markers/MissionItemMarkerInfo.java | 7 +- .../core/mission/MissionItemType.java | 9 +- .../core/mission/survey/Survey2D.java | 2 +- .../mission/waypoints/StructureScanner.java | 169 ------------------ 8 files changed, 16 insertions(+), 350 deletions(-) delete mode 100644 Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionStructureScannerFragment.java delete mode 100644 Core/src/org/droidplanner/core/mission/waypoints/StructureScanner.java diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java b/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java index e01fc1da58..2f70cc2f9c 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java @@ -19,11 +19,8 @@ import org.droidplanner.core.mission.waypoints.Circle; import org.droidplanner.core.mission.waypoints.SpatialCoordItem; import org.droidplanner.core.mission.waypoints.SplineWaypoint; -import org.droidplanner.core.mission.waypoints.StructureScanner; import org.droidplanner.core.survey.grid.Grid; -import com.MAVLink.common.msg_mission_item; - import android.content.Context; import android.graphics.Color; import android.view.LayoutInflater; @@ -113,7 +110,7 @@ public List getPath(Coord2D previousPoint) { } break; - case SURVEY: + case SURVEY2D: Grid grid = ((Survey2D) mMissionItem).grid; if (grid != null) { pathPoints.addAll(grid.gridPoints); @@ -122,10 +119,6 @@ public List getPath(Coord2D previousPoint) { case SURVEY3D: pathPoints.addAll(((Survey3D) mMissionItem).getPath()); break; - case CYLINDRICAL_SURVEY: - StructureScanner survey = (StructureScanner)mMissionItem; - pathPoints.addAll(survey.getPath()); - break; case TAKEOFF: break; default: diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java index 6621e0b99b..9331886e92 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java @@ -14,8 +14,7 @@ import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.MissionItemType; import org.droidplanner.core.mission.commands.MissionCMD; -import org.droidplanner.core.mission.survey.Survey2D; -import org.droidplanner.core.mission.waypoints.StructureScanner; +import org.droidplanner.core.mission.survey.Survey; import org.droidplanner.core.util.Pair; import android.app.Activity; @@ -86,7 +85,7 @@ public static MissionDetailFragment newInstance(MissionItemType itemType) { case RTL: fragment = new MissionRTLFragment(); break; - case SURVEY: + case SURVEY2D: fragment = new MissionSurveyFragment(); break; case SURVEY3D: @@ -101,9 +100,6 @@ public static MissionDetailFragment newInstance(MissionItemType itemType) { case SPLINE_WAYPOINT: fragment = new MissionSplineWaypointFragment(); break; - case CYLINDRICAL_SURVEY: - fragment = new MissionStructureScannerFragment(); - break; case CAMERA_TRIGGER: fragment = new MissionCameraTriggerFragment(); break; @@ -163,18 +159,15 @@ public void onViewCreated(View view, Bundle savedInstanceState) { final MissionItemProxy itemProxy = mSelectedProxies.get(0); final MissionItem currentItem = itemProxy.getMissionItem(); - if ((currentItem instanceof Survey2D)) { + if ((currentItem instanceof Survey)) { list.clear(); - list.add(MissionItemType.SURVEY); + list.add(MissionItemType.SURVEY2D); + list.add(MissionItemType.SURVEY3D); } else { - list.remove(MissionItemType.SURVEY); + list.remove(MissionItemType.SURVEY2D); + list.add(MissionItemType.SURVEY3D); } - if ((currentItem instanceof StructureScanner)) { - list.clear(); - list.add(MissionItemType.CYLINDRICAL_SURVEY); - } - if (mMissionProxy.getItems().indexOf(itemProxy) != 0) { list.remove(MissionItemType.TAKEOFF); } @@ -190,7 +183,6 @@ public void onViewCreated(View view, Bundle savedInstanceState) { list.remove(MissionItemType.CIRCLE); list.remove(MissionItemType.ROI); list.remove(MissionItemType.WAYPOINT); - list.remove(MissionItemType.CYLINDRICAL_SURVEY); } final TextView waypointIndex = (TextView) view.findViewById(R.id.WaypointIndex); @@ -219,8 +211,7 @@ else if(mSelectedProxies.size() > 1){ list.remove(MissionItemType.TAKEOFF); list.remove(MissionItemType.LAND); list.remove(MissionItemType.RTL); - list.remove(MissionItemType.SURVEY); - list.remove(MissionItemType.CYLINDRICAL_SURVEY); + list.remove(MissionItemType.SURVEY2D); } else{ //Invalid state. We should not have been able to get here. diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionStructureScannerFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionStructureScannerFragment.java deleted file mode 100644 index a20251bbd7..0000000000 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionStructureScannerFragment.java +++ /dev/null @@ -1,143 +0,0 @@ -package org.droidplanner.android.proxy.mission.item.fragments; - -import java.util.List; - -import org.droidplanner.R; -import org.droidplanner.R.id; -import org.droidplanner.android.proxy.mission.item.adapters.CamerasAdapter; -import org.droidplanner.android.widgets.spinnerWheel.CardWheelHorizontalView; -import org.droidplanner.android.widgets.spinnerWheel.adapters.NumericWheelAdapter; -import org.droidplanner.android.widgets.spinners.SpinnerSelfSelect; -import org.droidplanner.core.helpers.units.Altitude; -import org.droidplanner.core.mission.MissionItemType; -import org.droidplanner.core.mission.waypoints.StructureScanner; -import org.droidplanner.core.survey.CameraInfo; - -import android.content.Context; -import android.os.Bundle; -import android.util.Log; -import android.view.View; -import android.widget.CheckBox; -import android.widget.CompoundButton; -import android.widget.Spinner; - -public class MissionStructureScannerFragment extends MissionDetailFragment - implements CardWheelHorizontalView.OnCardWheelChangedListener, - CompoundButton.OnCheckedChangeListener { - - private CheckBox checkBoxAdvanced; - private CardWheelHorizontalView radiusPicker, startAltitudeStepPicker, - stepHeightStepPicker, mNumberStepsPicker; - private SpinnerSelfSelect cameraSpinner; - private CamerasAdapter cameraAdapter; - private List missionItems; - - @Override - protected int getResource() { - return R.layout.fragment_editor_detail_structure_scanner; - } - - @Override - public void onViewCreated(View view, Bundle savedInstanceState) { - super.onViewCreated(view, savedInstanceState); - final Context context = getActivity().getApplicationContext(); - - Log.d("DEBUG", "onViewCreated"); - typeSpinner.setSelection(commandAdapter - .getPosition(MissionItemType.CYLINDRICAL_SURVEY)); - - missionItems = (List) getMissionItems(); - // Use the first one as reference. - final StructureScanner firstItem = missionItems.get(0); - - cameraAdapter = new CamerasAdapter(getActivity(), - android.R.layout.simple_spinner_dropdown_item); - cameraSpinner = (SpinnerSelfSelect) view.findViewById(id.cameraFileSpinner); - cameraSpinner.setAdapter(cameraAdapter); - cameraSpinner.setOnSpinnerItemSelectedListener(this); - cameraAdapter.setTitle(firstItem.getCamera()); - - radiusPicker = (CardWheelHorizontalView) view - .findViewById(R.id.radiusPicker); - radiusPicker.setViewAdapter(new NumericWheelAdapter(context, - R.layout.wheel_text_centered, 2, 100, "%d m")); - radiusPicker.addChangingListener(this); - radiusPicker.setCurrentValue((int) firstItem.getRadius() - .valueInMeters()); - - startAltitudeStepPicker = (CardWheelHorizontalView) view - .findViewById(R.id.startAltitudePicker); - startAltitudeStepPicker.setViewAdapter(new NumericWheelAdapter(context, - R.layout.wheel_text_centered, MIN_ALTITUDE, MAX_ALTITUDE, - "%d m")); - startAltitudeStepPicker.addChangingListener(this); - startAltitudeStepPicker.setCurrentValue((int) firstItem - .getCoordinate().getAltitude().valueInMeters()); - - stepHeightStepPicker = (CardWheelHorizontalView) view - .findViewById(R.id.heightStepPicker); - stepHeightStepPicker.setViewAdapter(new NumericWheelAdapter(context, - R.layout.wheel_text_centered, 1, MAX_ALTITUDE, - "%d m")); - stepHeightStepPicker.addChangingListener(this); - stepHeightStepPicker.setCurrentValue((int) firstItem.getEndAltitude() - .valueInMeters()); - - mNumberStepsPicker = (CardWheelHorizontalView) view - .findViewById(R.id.stepsPicker); - mNumberStepsPicker.setViewAdapter(new NumericWheelAdapter(context, - R.layout.wheel_text_centered, 1, 10, "%d")); - mNumberStepsPicker.addChangingListener(this); - mNumberStepsPicker.setCurrentValue(firstItem.getNumberOfSteps()); - - checkBoxAdvanced = (CheckBox) view - .findViewById(R.id.checkBoxSurveyCrossHatch); - checkBoxAdvanced.setOnCheckedChangeListener(this); - checkBoxAdvanced.setChecked(firstItem.isCrossHatchEnabled()); - - } - - @Override - public void onCheckedChanged(CompoundButton buttonView, boolean isChecked) { - getItem().enableCrossHatch(isChecked); - getMissionProxy().getMission().notifyMissionUpdate(); - } - - @Override - public void onChanged(CardWheelHorizontalView cardWheel, int oldValue, - int newValue) { - switch (cardWheel.getId()) { - case R.id.radiusPicker: - getItem().setRadius(newValue); - break; - case R.id.startAltitudePicker: - getItem().setAltitude( new Altitude(newValue)); - break; - case R.id.heightStepPicker: - getItem().setAltitudeStep(newValue); - break; - case R.id.stepsPicker: - getItem().setNumberOfSteps(newValue); - break; - } - getMissionProxy().getMission().notifyMissionUpdate(); - } - - @Override - public void onSpinnerItemSelected(Spinner spinner, int position) { - if (spinner.getId() == id.cameraFileSpinner) { - CameraInfo cameraInfo = cameraAdapter.getCamera(position); - cameraAdapter.setTitle(cameraInfo.name); - for (StructureScanner scan : missionItems) { - scan.setCamera(cameraInfo); - } - getMissionProxy().getMission().notifyMissionUpdate(); - } - } - - private StructureScanner getItem() { - StructureScanner cylindricalSurvey = (StructureScanner) getMissionItems() - .get(0); - return cylindricalSurvey; - } -} diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java index 539b915c3d..69a3c824f1 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java @@ -115,7 +115,7 @@ public void onViewCreated(View view, Bundle savedInstanceState) { mAltitudePicker.addChangingListener(this); - typeSpinner.setSelection(commandAdapter.getPosition(MissionItemType.SURVEY)); + typeSpinner.setSelection(commandAdapter.getPosition(MissionItemType.SURVEY2D)); } @Override diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/markers/MissionItemMarkerInfo.java b/Android/src/org/droidplanner/android/proxy/mission/item/markers/MissionItemMarkerInfo.java index 34918bbb13..c8d992e3ee 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/markers/MissionItemMarkerInfo.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/markers/MissionItemMarkerInfo.java @@ -41,12 +41,9 @@ public static List newInstance(MissionItemProxy origin) { case SPLINE_WAYPOINT: markerInfos.add(new SplineWaypointMarkerInfo(origin)); break; - - case CYLINDRICAL_SURVEY: - markerInfos.add(new StructureScannerMarkerInfoProvider(origin)); - break; - case SURVEY: + case SURVEY2D: + case SURVEY3D: markerInfos.addAll(new SurveyMarkerInfoProvider(origin).getMarkersInfos()); break; diff --git a/Core/src/org/droidplanner/core/mission/MissionItemType.java b/Core/src/org/droidplanner/core/mission/MissionItemType.java index d0d1a427b6..b991b972d6 100644 --- a/Core/src/org/droidplanner/core/mission/MissionItemType.java +++ b/Core/src/org/droidplanner/core/mission/MissionItemType.java @@ -16,13 +16,12 @@ import org.droidplanner.core.mission.waypoints.Land; import org.droidplanner.core.mission.waypoints.RegionOfInterest; import org.droidplanner.core.mission.waypoints.SplineWaypoint; -import org.droidplanner.core.mission.waypoints.StructureScanner; import org.droidplanner.core.mission.waypoints.Waypoint; public enum MissionItemType { WAYPOINT("Waypoint"), SPLINE_WAYPOINT("Spline Waypoint"), TAKEOFF("Takeoff"), RTL( - "Return to Launch"), LAND("Land"), CIRCLE("Circle"), ROI("Region of Interest"), SURVEY( - "Survey"), CYLINDRICAL_SURVEY("Structure Scan"), CHANGE_SPEED("Change Speed"), CAMERA_TRIGGER("Camera Trigger"), EPM_GRIPPER("EPM"), SET_SERVO("Set Servo"), CONDITION_YAW("Set Yaw"), SURVEY3D("Survey3D"); + "Return to Launch"), LAND("Land"), CIRCLE("Circle"), ROI("Region of Interest"), SURVEY2D( + "Survey"), CHANGE_SPEED("Change Speed"), CAMERA_TRIGGER("Camera Trigger"), EPM_GRIPPER("EPM"), SET_SERVO("Set Servo"), CONDITION_YAW("Set Yaw"), SURVEY3D("Survey3D"); private final String name; @@ -56,12 +55,10 @@ public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentE return new Circle(referenceItem); case ROI: return new RegionOfInterest(referenceItem); - case SURVEY: + case SURVEY2D: return new Survey2D(referenceItem.getMission(), Collections. emptyList()); case SURVEY3D: return new Survey3D(referenceItem.getMission(), Collections. emptyList()); - case CYLINDRICAL_SURVEY: - return new StructureScanner(referenceItem); case SET_SERVO: return new SetServo(referenceItem); case CONDITION_YAW: diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey2D.java b/Core/src/org/droidplanner/core/mission/survey/Survey2D.java index 149568ddea..fc159a94f2 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey2D.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey2D.java @@ -60,7 +60,7 @@ private void packGridPoints(List list) { @Override public MissionItemType getType() { - return MissionItemType.SURVEY; + return MissionItemType.SURVEY2D; } } diff --git a/Core/src/org/droidplanner/core/mission/waypoints/StructureScanner.java b/Core/src/org/droidplanner/core/mission/waypoints/StructureScanner.java deleted file mode 100644 index 96df8f3f6d..0000000000 --- a/Core/src/org/droidplanner/core/mission/waypoints/StructureScanner.java +++ /dev/null @@ -1,169 +0,0 @@ -package org.droidplanner.core.mission.waypoints; - -import java.util.ArrayList; -import java.util.List; - -import org.droidplanner.core.helpers.coordinates.Coord2D; -import org.droidplanner.core.helpers.coordinates.Coord3D; -import org.droidplanner.core.helpers.geoTools.GeoTools; -import org.droidplanner.core.helpers.units.Altitude; -import org.droidplanner.core.helpers.units.Length; -import org.droidplanner.core.mission.Mission; -import org.droidplanner.core.mission.MissionItem; -import org.droidplanner.core.mission.MissionItemType; -import org.droidplanner.core.mission.survey.Survey2D; -import org.droidplanner.core.polygon.Polygon; -import org.droidplanner.core.survey.CameraInfo; -import org.droidplanner.core.survey.SurveyData; -import org.droidplanner.core.survey.grid.GridBuilder; - -import com.MAVLink.common.msg_mission_item; -import com.MAVLink.enums.MAV_CMD; - -public class StructureScanner extends SpatialCoordItem { - private Length radius = new Length(10.0); - private Altitude heightStep = new Altitude(5); - private int numberOfSteps = 2; - private boolean crossHatch = false; - SurveyData survey = new SurveyData(); - - public StructureScanner(Mission mission, Coord3D coord) { - super(mission,coord); - } - - public StructureScanner(MissionItem item) { - super(item); - } - - @Override - public List packMissionItem() { - List list = new ArrayList(); - packROI(list); - packCircles(list); - if (crossHatch) { - packHatch(list); - } - return list; - } - - private void packROI(List list) { - RegionOfInterest roi = new RegionOfInterest(mission, new Coord3D( - coordinate, new Altitude(0.0))); - list.addAll(roi.packMissionItem()); - } - - private void packCircles(List list) { - for (double altitude = coordinate.getAltitude().valueInMeters(); altitude <= getTopHeight().valueInMeters(); altitude += heightStep.valueInMeters()) { - Circle circle = new Circle(mission, new Coord3D(coordinate, new Altitude(altitude))); - circle.setRadius(radius.valueInMeters()); - list.addAll(circle.packMissionItem()); - } - } - - private void packHatch(List list) { - Polygon polygon = new Polygon(); - for (double angle = 0; angle <= 360; angle += 10) { - polygon.addPoint(GeoTools.newCoordFromBearingAndDistance(coordinate, - angle, radius.valueInMeters())); - } - - Coord2D corner = GeoTools.newCoordFromBearingAndDistance(coordinate, - -45, radius.valueInMeters()*2); - - - survey.setAltitude(getTopHeight()); - - try { - survey.update(0.0, survey.getAltitude(), survey.getOverlap(), survey.getSidelap()); - GridBuilder grid = new GridBuilder(polygon, survey, corner); - for (Coord2D point : grid.generate(false).gridPoints) { - list.add(Survey2D.packSurveyPoint(point, getTopHeight())); - } - - survey.update(90.0, survey.getAltitude(), survey.getOverlap(), survey.getSidelap()); - GridBuilder grid2 = new GridBuilder(polygon, survey, corner); - for (Coord2D point : grid2.generate(false).gridPoints) { - list.add(Survey2D.packSurveyPoint(point, getTopHeight())); - } - } catch (Exception e) { // Should never fail, since it has good polygons - } - - } - - public List getPath() { - List path = new ArrayList(); - for (msg_mission_item msg_mission_item : packMissionItem()) { - if (msg_mission_item.command == MAV_CMD.MAV_CMD_NAV_WAYPOINT) { - path.add(new Coord2D(msg_mission_item.x, msg_mission_item.y)); - } - if (msg_mission_item.command == MAV_CMD.MAV_CMD_NAV_LOITER_TURNS) { - for (double angle = 0; angle <= 360; angle += 12) { - path.add(GeoTools.newCoordFromBearingAndDistance(coordinate,angle, radius.valueInMeters())); - } - } - - } - return path; - - } - - @Override - public void unpackMAVMessage(msg_mission_item mavMsg) { - } - - @Override - public MissionItemType getType() { - return MissionItemType.CYLINDRICAL_SURVEY; - } - - - - private Altitude getTopHeight() { - return new Altitude(coordinate.getAltitude().valueInMeters()+ (numberOfSteps-1)*heightStep.valueInMeters()); - } - - public Altitude getEndAltitude() { - return heightStep; - } - - public int getNumberOfSteps() { - return numberOfSteps; - } - - public Length getRadius() { - return radius; - } - - public Coord2D getCenter() { - return coordinate; - } - - public void setRadius(int newValue) { - radius = new Length(newValue); - } - - public void enableCrossHatch(boolean isEnabled) { - crossHatch = isEnabled; - } - - public boolean isCrossHatchEnabled() { - return crossHatch; - } - - public void setAltitudeStep(int newValue) { - heightStep = new Altitude(newValue); - } - - public void setNumberOfSteps(int newValue) { - numberOfSteps = newValue; - } - - public void setCamera(CameraInfo cameraInfo) { - survey.setCameraInfo(cameraInfo); - } - - public String getCamera() { - return survey.getCameraName(); - } - -} From 2be279cb8702005d5f64559ec20384348fee9021 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 25 Nov 2014 14:24:22 -0800 Subject: [PATCH 13/16] Map: Fix polygon markers anchor point --- .../android/proxy/mission/item/markers/PolygonMarkerInfo.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java b/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java index ebfc3a0fc4..898204683b 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/markers/PolygonMarkerInfo.java @@ -34,7 +34,7 @@ public float getAnchorU() { @Override public float getAnchorV() { - return 0.5f; + return 1.0f; } @Override From 22970731e9808022eeadb2a1e60350b37fb64484 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 25 Nov 2014 17:16:51 -0800 Subject: [PATCH 14/16] Survey: Make objects of survey type replaceble by each other --- .../org/droidplanner/core/mission/MissionItemType.java | 9 +++------ .../src/org/droidplanner/core/mission/survey/Survey.java | 4 ++++ .../org/droidplanner/core/mission/survey/Survey2D.java | 9 +++++++-- .../org/droidplanner/core/mission/survey/Survey3D.java | 9 +++++++-- 4 files changed, 21 insertions(+), 10 deletions(-) diff --git a/Core/src/org/droidplanner/core/mission/MissionItemType.java b/Core/src/org/droidplanner/core/mission/MissionItemType.java index b991b972d6..f54e6aa26b 100644 --- a/Core/src/org/droidplanner/core/mission/MissionItemType.java +++ b/Core/src/org/droidplanner/core/mission/MissionItemType.java @@ -1,8 +1,5 @@ package org.droidplanner.core.mission; -import java.util.Collections; - -import org.droidplanner.core.helpers.coordinates.Coord2D; import org.droidplanner.core.mission.commands.CameraTrigger; import org.droidplanner.core.mission.commands.ChangeSpeed; import org.droidplanner.core.mission.commands.ConditionYaw; @@ -21,7 +18,7 @@ public enum MissionItemType { WAYPOINT("Waypoint"), SPLINE_WAYPOINT("Spline Waypoint"), TAKEOFF("Takeoff"), RTL( "Return to Launch"), LAND("Land"), CIRCLE("Circle"), ROI("Region of Interest"), SURVEY2D( - "Survey"), CHANGE_SPEED("Change Speed"), CAMERA_TRIGGER("Camera Trigger"), EPM_GRIPPER("EPM"), SET_SERVO("Set Servo"), CONDITION_YAW("Set Yaw"), SURVEY3D("Survey3D"); + "Survey"), CHANGE_SPEED("Change Speed"), CAMERA_TRIGGER("Camera Trigger"), EPM_GRIPPER("EPM"), SET_SERVO("Set Servo"), CONDITION_YAW("Set Yaw"), SURVEY3D("Structure Scanner"); private final String name; @@ -56,9 +53,9 @@ public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentE case ROI: return new RegionOfInterest(referenceItem); case SURVEY2D: - return new Survey2D(referenceItem.getMission(), Collections. emptyList()); + return new Survey2D(referenceItem); case SURVEY3D: - return new Survey3D(referenceItem.getMission(), Collections. emptyList()); + return new Survey3D(referenceItem); case SET_SERVO: return new SetServo(referenceItem); case CONDITION_YAW: diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey.java b/Core/src/org/droidplanner/core/mission/survey/Survey.java index 276c77bba2..c6f0cc39b4 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey.java @@ -25,6 +25,10 @@ protected Survey(Mission mission) { protected Survey(MissionItem item) { super(item); + if (item instanceof Survey) { + polygon = ((Survey) item).polygon; + surveyData = ((Survey) item).surveyData; + } } protected Survey(Mission mission, List points) { diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey2D.java b/Core/src/org/droidplanner/core/mission/survey/Survey2D.java index fc159a94f2..d4ba6d7717 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey2D.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey2D.java @@ -7,6 +7,7 @@ import org.droidplanner.core.helpers.units.Altitude; import org.droidplanner.core.helpers.units.Length; import org.droidplanner.core.mission.Mission; +import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.MissionItemType; import org.droidplanner.core.mission.commands.CameraTrigger; import org.droidplanner.core.survey.grid.Grid; @@ -18,8 +19,12 @@ public class Survey2D extends Survey { public Grid grid; - public Survey2D(Mission mission, List points) { - super(mission, points); + public Survey2D(MissionItem item) { + super(item); + } + + public Survey2D(Mission mMission, List points) { + super(mMission, points); } public void update(double angle, Altitude altitude, double overlap, double sidelap) { diff --git a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java index 77b16394ba..be60e26f76 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey3D.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java @@ -6,14 +6,19 @@ import org.droidplanner.core.helpers.coordinates.Coord2D; import org.droidplanner.core.helpers.geoTools.PolygonTools; import org.droidplanner.core.mission.Mission; +import org.droidplanner.core.mission.MissionItem; import org.droidplanner.core.mission.MissionItemType; import com.MAVLink.common.msg_mission_item; public class Survey3D extends Survey { - public Survey3D(Mission mission, List points) { - super(mission, points); + public Survey3D(MissionItem item) { + super(item); + } + + public Survey3D(Mission mMission, List points) { + super(mMission, points); } public List getPath() { From ff0c85ff04cd1921236ca6d85ebb7f82323871b4 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 25 Nov 2014 17:17:32 -0800 Subject: [PATCH 15/16] SurveyUI: Rework the ui to fit the new survey types --- ...agment_editor_detail_structure_scanner.xml | 169 ------------------ .../fragment_editor_detail_survey3d.xml | 2 +- Android/res/values-zh-rCN/strings.xml | 2 +- Android/res/values/strings.xml | 2 +- .../item/fragments/MissionDetailFragment.java | 2 +- .../fragments/MissionSurvey3DFragment.java | 2 + .../item/fragments/MissionSurveyFragment.java | 2 + 7 files changed, 8 insertions(+), 173 deletions(-) delete mode 100644 Android/res/layout/fragment_editor_detail_structure_scanner.xml diff --git a/Android/res/layout/fragment_editor_detail_structure_scanner.xml b/Android/res/layout/fragment_editor_detail_structure_scanner.xml deleted file mode 100644 index 7dfd783431..0000000000 --- a/Android/res/layout/fragment_editor_detail_structure_scanner.xml +++ /dev/null @@ -1,169 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Android/res/layout/fragment_editor_detail_survey3d.xml b/Android/res/layout/fragment_editor_detail_survey3d.xml index 7dfd783431..2881522597 100644 --- a/Android/res/layout/fragment_editor_detail_survey3d.xml +++ b/Android/res/layout/fragment_editor_detail_survey3d.xml @@ -49,7 +49,7 @@ android:layout_width="wrap_content" android:layout_height="wrap_content" android:layout_marginLeft="12dp" - android:text="@string/waypointType_BuildingMapper" + android:text="@string/waypointType_structureScanner" android:textAllCaps="true" /> 悬停 兴趣区域 勘测多边形 - 结构扫描 + 结构扫描 在当前位置降落模型。你必须手动退出自动模式来锁定电机。 diff --git a/Android/res/values/strings.xml b/Android/res/values/strings.xml index ad39e4ff21..a97fef14f3 100644 --- a/Android/res/values/strings.xml +++ b/Android/res/values/strings.xml @@ -189,7 +189,7 @@ Loiter Region of Interest Survey Polygon - Structure Scan + Structure Scan Land vehicle at the current location. You must manually exit Auto mode to disarm motors. diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java index 9331886e92..70c9c2b015 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionDetailFragment.java @@ -165,7 +165,7 @@ public void onViewCreated(View view, Bundle savedInstanceState) { list.add(MissionItemType.SURVEY3D); } else { list.remove(MissionItemType.SURVEY2D); - list.add(MissionItemType.SURVEY3D); + list.remove(MissionItemType.SURVEY3D); } if (mMissionProxy.getItems().indexOf(itemProxy) != 0) { diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java index 406f24b845..8436deba4b 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java @@ -100,6 +100,8 @@ public void onSpinnerItemSelected(Spinner spinner, int position) { scan.setCameraInfo(cameraInfo); } getMissionProxy().getMission().notifyMissionUpdate(); + }else{ + super.onSpinnerItemSelected(spinner,position); } } diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java index 69a3c824f1..aa4bee577c 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java @@ -129,6 +129,8 @@ public void onSpinnerItemSelected(Spinner spinner, int position) { onChanged(mAnglePicker, 0, 0); getMissionProxy().getMission().notifyMissionUpdate(); + }else{ + super.onSpinnerItemSelected(spinner,position); } } From 07cb6234bf61a5aa270329b58bb96ab779de7929 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Mon, 1 Dec 2014 11:51:52 -0800 Subject: [PATCH 16/16] Android: Update the mission UI when a new waypoint type is selected. --- .../proxy/mission/item/fragments/MissionSurvey3DFragment.java | 1 + .../proxy/mission/item/fragments/MissionSurveyFragment.java | 1 + 2 files changed, 2 insertions(+) diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java index 8436deba4b..49d3b01b1a 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java @@ -102,6 +102,7 @@ public void onSpinnerItemSelected(Spinner spinner, int position) { getMissionProxy().getMission().notifyMissionUpdate(); }else{ super.onSpinnerItemSelected(spinner,position); + getMissionProxy().getMission().notifyMissionUpdate(); } } diff --git a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java index aa4bee577c..605b552b1c 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurveyFragment.java @@ -131,6 +131,7 @@ public void onSpinnerItemSelected(Spinner spinner, int position) { getMissionProxy().getMission().notifyMissionUpdate(); }else{ super.onSpinnerItemSelected(spinner,position); + getMissionProxy().getMission().notifyMissionUpdate(); } }