diff --git a/Android/res/layout/fragment_editor_detail_structure_scanner.xml b/Android/res/layout/fragment_editor_detail_survey3d.xml similarity index 99% rename from Android/res/layout/fragment_editor_detail_structure_scanner.xml rename to Android/res/layout/fragment_editor_detail_survey3d.xml index 7dfd783431..2881522597 100644 --- a/Android/res/layout/fragment_editor_detail_structure_scanner.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/MissionProxy.java b/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java index 43cde512a8..275f7a6e00 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java +++ b/Android/src/org/droidplanner/android/proxy/mission/MissionProxy.java @@ -19,6 +19,8 @@ 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; import org.droidplanner.core.mission.waypoints.Waypoint; @@ -139,11 +141,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(); } @@ -616,6 +618,7 @@ public List> getPolygonsPath() { if (item instanceof Survey) { polygonPaths.add(((Survey)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..2f70cc2f9c 100644 --- a/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java +++ b/Android/src/org/droidplanner/android/proxy/mission/item/MissionItemProxy.java @@ -14,10 +14,11 @@ 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; import org.droidplanner.core.mission.waypoints.SplineWaypoint; -import org.droidplanner.core.mission.waypoints.StructureScanner; import org.droidplanner.core.survey.grid.Grid; import android.content.Context; @@ -109,15 +110,14 @@ public List getPath(Coord2D previousPoint) { } break; - case SURVEY: - Grid grid = ((Survey) mMissionItem).grid; + case SURVEY2D: + Grid grid = ((Survey2D) mMissionItem).grid; if (grid != null) { pathPoints.addAll(grid.gridPoints); } break; - case CYLINDRICAL_SURVEY: - StructureScanner survey = (StructureScanner)mMissionItem; - pathPoints.addAll(survey.getPath()); + case SURVEY3D: + pathPoints.addAll(((Survey3D) mMissionItem).getPath()); break; case TAKEOFF: break; @@ -157,7 +157,7 @@ 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) { + } else if (mMissionItem instanceof Survey2D) { altitudeView.setText(((Survey) mMissionItem).surveyData.getAltitude().toString()); } else if (mMissionItem instanceof Takeoff) { 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..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 @@ -15,7 +15,6 @@ 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.waypoints.StructureScanner; import org.droidplanner.core.util.Pair; import android.app.Activity; @@ -86,9 +85,12 @@ public static MissionDetailFragment newInstance(MissionItemType itemType) { case RTL: fragment = new MissionRTLFragment(); break; - case SURVEY: + case SURVEY2D: fragment = new MissionSurveyFragment(); break; + case SURVEY3D: + fragment = new MissionSurvey3DFragment(); + break; case TAKEOFF: fragment = new MissionTakeoffFragment(); break; @@ -98,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; @@ -162,16 +161,13 @@ public void onViewCreated(View view, Bundle savedInstanceState) { 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.remove(MissionItemType.SURVEY3D); } - if ((currentItem instanceof StructureScanner)) { - list.clear(); - list.add(MissionItemType.CYLINDRICAL_SURVEY); - } - if (mMissionProxy.getItems().indexOf(itemProxy) != 0) { list.remove(MissionItemType.TAKEOFF); } @@ -187,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); @@ -216,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/MissionSurvey3DFragment.java b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java new file mode 100644 index 0000000000..49d3b01b1a --- /dev/null +++ b/Android/src/org/droidplanner/android/proxy/mission/item/fragments/MissionSurvey3DFragment.java @@ -0,0 +1,113 @@ +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()); + + 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(); + }else{ + super.onSpinnerItemSelected(spinner,position); + getMissionProxy().getMission().notifyMissionUpdate(); + } + } + + private Survey3D getItem() { + Survey3D cylindricalSurvey = (Survey3D) 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 94dc9040f4..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 @@ -13,6 +13,7 @@ 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 +47,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 +72,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); @@ -114,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 @@ -128,6 +129,9 @@ public void onSpinnerItemSelected(Spinner spinner, int position) { onChanged(mAnglePicker, 0, 0); getMissionProxy().getMission().notifyMissionUpdate(); + }else{ + super.onSpinnerItemSelected(spinner,position); + getMissionProxy().getMission().notifyMissionUpdate(); } } @@ -139,7 +143,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/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/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 diff --git a/Core/src/org/droidplanner/core/helpers/geoTools/LineTools.java b/Core/src/org/droidplanner/core/helpers/geoTools/LineTools.java index e310e79d2f..86de6fdc82 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,12 @@ public static LineCoord2D findClosestLineToPoint(Coord2D point, List offsetLines = new ArrayList(); + for (LineCoord2D line : polygon.getLines()) { + offsetLines.add(LineTools.getParallelLine(line, offset,PolygonTools.isClockWisePolygon(polygon))); + } + + 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; + } + + 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; + } + + 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/src/org/droidplanner/core/mission/MissionItemType.java b/Core/src/org/droidplanner/core/mission/MissionItemType.java index 0de58b124e..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; @@ -10,18 +7,18 @@ 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; 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"); + "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("Structure Scanner"); private final String name; @@ -55,10 +52,10 @@ public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentE return new Circle(referenceItem); case ROI: return new RegionOfInterest(referenceItem); - case SURVEY: - return new Survey(referenceItem.getMission(), Collections. emptyList()); - case CYLINDRICAL_SURVEY: - return new StructureScanner(referenceItem); + case SURVEY2D: + return new Survey2D(referenceItem); + case SURVEY3D: + 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 3d9ae6bbcd..c6f0cc39b4 100644 --- a/Core/src/org/droidplanner/core/mission/survey/Survey.java +++ b/Core/src/org/droidplanner/core/mission/survey/Survey.java @@ -1,39 +1,39 @@ 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.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 Survey extends MissionItem { +public abstract class Survey extends MissionItem { public Polygon polygon = new Polygon(); public SurveyData surveyData = new SurveyData(); - public Grid grid; - public Survey(Mission mission, List points) { + protected Survey(Mission mission) { super(mission); - polygon.addPoints(points); } - - public void update(double angle, Altitude altitude, double overlap, double sidelap) { - surveyData.update(angle, altitude, overlap, sidelap); - mission.notifyMissionUpdate(); + + protected Survey(MissionItem item) { + super(item); + if (item instanceof Survey) { + polygon = ((Survey) item).polygon; + surveyData = ((Survey) item).surveyData; + } + } + + protected Survey(Mission mission, List points) { + super(mission); + polygon.addPoints(points); } public void setCameraInfo(CameraInfo camera) { @@ -41,37 +41,16 @@ public void setCameraInfo(CameraInfo camera) { mission.notifyMissionUpdate(); } - public void build() throws Exception { - // TODO find better point than (0,0) to reference the grid - grid = null; - GridBuilder gridBuilder = new GridBuilder(polygon, surveyData, new Coord2D(0, 0)); - polygon.checkIfValid(); - grid = gridBuilder.generate(true); + public String getCamera() { + return surveyData.getCameraName(); } - + + public abstract void build() throws Exception; + @Override - public List packMissionItem() { - try { - List list = new ArrayList(); - build(); - - list.addAll((new CameraTrigger(mission, surveyData.getLongitudinalPictureDistance())).packMissionItem()); - packGridPoints(list); - list.addAll((new CameraTrigger(mission, new Length(0.0)).packMissionItem())); - - return list; - } catch (Exception e) { - return new ArrayList(); - } - } - - private void packGridPoints(List list) { - for (Coord2D point : grid.gridPoints) { - msg_mission_item mavMsg = packSurveyPoint(point,surveyData.getAltitude()); - list.add(mavMsg); - } - } - + 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; @@ -87,15 +66,4 @@ public static msg_mission_item packSurveyPoint(Coord2D point, Length altitude) { return mavMsg; } - @Override - public void unpackMAVMessage(msg_mission_item mavMsg) { - // TODO Auto-generated method stub - - } - - @Override - public MissionItemType getType() { - return MissionItemType.SURVEY; - } - -} +} \ 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 new file mode 100644 index 0000000000..d4ba6d7717 --- /dev/null +++ b/Core/src/org/droidplanner/core/mission/survey/Survey2D.java @@ -0,0 +1,71 @@ +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.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; +import org.droidplanner.core.survey.grid.GridBuilder; + +import com.MAVLink.common.msg_mission_item; + +public class Survey2D extends Survey { + + public Grid grid; + + 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) { + surveyData.update(angle, altitude, overlap, sidelap); + mission.notifyMissionUpdate(); + } + + public void build() throws Exception { + // TODO find better point than (0,0) to reference the grid + grid = null; + GridBuilder gridBuilder = new GridBuilder(polygon, surveyData, new Coord2D(0, 0)); + polygon.checkIfValid(); + grid = gridBuilder.generate(true); + } + + @Override + public List packMissionItem() { + try { + List list = new ArrayList(); + build(); + + list.addAll((new CameraTrigger(mission, surveyData.getLongitudinalPictureDistance())).packMissionItem()); + packGridPoints(list); + list.addAll((new CameraTrigger(mission, new Length(0.0)).packMissionItem())); + + return list; + } catch (Exception e) { + return new ArrayList(); + } + } + + private void packGridPoints(List list) { + for (Coord2D point : grid.gridPoints) { + msg_mission_item mavMsg = packSurveyPoint(point,surveyData.getAltitude()); + list.add(mavMsg); + } + } + + @Override + public MissionItemType getType() { + return MissionItemType.SURVEY2D; + } + +} 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..be60e26f76 --- /dev/null +++ b/Core/src/org/droidplanner/core/mission/survey/Survey3D.java @@ -0,0 +1,53 @@ +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.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(MissionItem item) { + super(item); + } + + public Survey3D(Mission mMission, List points) { + super(mMission, points); + } + + public List getPath() { + try { + List path = PolygonTools.offsetPolygon(polygon, 10).getPoints(); + return path; + } catch (Exception e) { + return new ArrayList(); + } + } + + @Override + public List packMissionItem() { + List list = new ArrayList(); + for (Coord2D point: getPath()) { + list.add(packSurveyPoint(point, surveyData.getAltitude())); + } + return list; + } + + @Override + public MissionItemType getType() { + return MissionItemType.SURVEY3D; + } + + @Override + public void build() throws Exception { + // TODO Auto-generated method stub + + } + +} 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 edfcd72930..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.Survey; -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(Survey.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())); - } - } 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(); - } - -} 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..18421321eb --- /dev/null +++ b/Core/test/org/droidplanner/core/helpers/geoTools/PolygonToolsTest.java @@ -0,0 +1,49 @@ +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)); + } + + 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