Merge pull request #42 from ethz-asl/fix/frustum_altitude_catch

Catch if altitude is set too small.

Author: rikba

polygon_coverage_ros/include/polygon_coverage_ros/coverage_planner.h

@@ -215,7 +215,13 @@ class CoveragePlanner : public PolygonPlannerBase {
           break;
         }
         if (!altitude_.has_value()) {
-          ROS_WARN("No altitude specified. Creating default altitude 1m.");
+          ROS_WARN("No altitude specified. Creating default altitude of 1m.");
+          altitude_ = std::make_optional(1.0);
+        } else if (altitude_.value() < std::numeric_limits<double>::epsilon()) {
+          ROS_WARN(
+              "Altitude to small %.3fm. Please set altitude through first "
+              "z-value of polygon. Setting default altitude of 1m.",
+              altitude_.value());
           altitude_ = std::make_optional(1.0);
         }
         sensor_model_ = std::make_shared<Frustum>(