Chore/spaceout waypoints (#199)

* refactor config

* beautiful, beautiful macros

* fix lint & add camera vision config option

* dubins is now in config

* removed all instances of constants

---------

Co-authored-by: Tyler Lentz <tlentz@ucsd.edu>

configs/config.json

@@ -14,23 +14,33 @@
  "altitude_m": 30.0
  },
  "pathing": {
- "_comment": "Point Fetch Method 2: NEAREST",
- "_comment2": "Drop Method 1: UNGUIDED",
+ "dubins": {
+ "turning_radius": 30.0,
+ "point_separation": 20.0
+ },
  "rrt": {
- "iterations_per_waypoint": 200,
- "rewire_radius": 200.0,
+ "iterations_per_waypoint": 512,
+ "rewire_radius": 256.0,
  "optimize": true,
- "point_fetch_methods": 2, 
+ "point_fetch_method": "nearest", 
  "allowed_to_skip_waypoints": false
  },
  "coverage": {
- "coverage_altitude_m": 30.0,
- "optimize": true,
- "vertical": false,
- "one_way": false
+ "altitude_m": 30.0,
+ "camera_vision_m": 20,
+ "method": "hover",
+ "hover": {
+ "pictures_per_stop": 1,
+ "hover_time_s": 5
+ },
+ "forward": {
+ "optimize": true,
+ "vertical": false,
+ "one_way": false
+ }
  },
  "approach": {
- "drop_method": 1,
+ "drop_method": "unguided",
  "drop_angle_rad": 2.52134317,
  "drop_altitude_m": 26.0,
  "guided_drop_distance_m": 50.0,
@@ -50,10 +60,10 @@
  "images_dir": "/obcpp/tests/integration/images/saliency/"
  },
  "lucid": {
- "sensor_shuttle_mode": "Rolling",
+ "sensor_shutter_mode": "Rolling",
 
  "acquisition_frame_rate_enable": true,
- "target_brightness": 70,
+ "target_brightness": 20,
  "exposure_auto": "Continuous",
  "exposure_time": 3000,
  "exposure_auto_damping": 1,

configs/dev-config.json

@@ -14,23 +14,33 @@
  "altitude_m": 30.0
  },
  "pathing": {
- "_comment": "Point Fetch Method 2: NEAREST",
- "_comment2": "Drop Method 1: UNGUIDED",
+ "dubins": {
+ "turning_radius": 30.0,
+ "point_separation": 20.0
+ },
  "rrt": {
- "iterations_per_waypoint": 200,
- "rewire_radius": 200.0,
+ "iterations_per_waypoint": 512,
+ "rewire_radius": 256.0,
  "optimize": true,
- "point_fetch_methods": 2, 
+ "point_fetch_method": "nearest",
  "allowed_to_skip_waypoints": false
  },
  "coverage": {
- "coverage_altitude_m": 30.0,
- "optimize": true,
- "vertical": false,
- "one_way": false
+ "altitude_m": 30.0,
+ "camera_vision_m": 20,
+ "method": "hover",
+ "hover": {
+ "pictures_per_stop": 1,
+ "hover_time_s": 5
+ },
+ "forward": {
+ "optimize": true,
+ "vertical": false,
+ "one_way": false
+ }
  },
  "approach": {
- "drop_method": 1,
+ "drop_method": "unguided",
  "drop_angle_rad": 2.52134317,
  "drop_altitude_m": 26.0,
  "guided_drop_distance_m": 50.0,
@@ -50,10 +60,10 @@
  "images_dir": "/workspaces/obcpp/tests/integration/images/saliency/"
  },
  "lucid": {
- "sensor_shuttle_mode": "Rolling",
+ "sensor_shutter_mode": "Rolling",
 
  "acquisition_frame_rate_enable": true,
- "target_brightness": 70,
+ "target_brightness": 20,
  "exposure_auto": "Continuous",
  "exposure_time": 3000,
  "exposure_auto_damping": 1,

include/camera/interface.hpp

@@ -15,6 +15,7 @@
 
 #include "network/mavlink.hpp"
 #include "utilities/datatypes.hpp"
+#include "utilities/obc_config.hpp"
 
 using json = nlohmann::json;
 using Mat = cv::Mat;

include/pathing/static.hpp

@@ -23,19 +23,10 @@
 class RRT {
  public:
  RRT(RRTPoint start, std::vector<XYZCoord> goals, double search_radius, Polygon bounds,
- std::vector<Polygon> obstacles = {}, std::vector<double> angles = {},
- RRTConfig config = {.iterations_per_waypoint = ITERATIONS_PER_WAYPOINT,
- .rewire_radius = REWIRE_RADIUS,
- .optimize = false,
- .point_fetch_method = POINT_FETCH_METHODS::NEAREST,
- .allowed_to_skip_waypoints = false});
+ const OBCConfig &config, std::vector<Polygon> obstacles = {},
+ std::vector<double> angles = {});
  RRT(RRTPoint start, std::vector<XYZCoord> goals, double search_radius, Environment airspace,
- std::vector<double> angles = {},
- RRTConfig config = {.iterations_per_waypoint = ITERATIONS_PER_WAYPOINT,
- .rewire_radius = REWIRE_RADIUS,
- .optimize = false,
- .point_fetch_method = POINT_FETCH_METHODS::NONE,
- .allowed_to_skip_waypoints = false});
+ const OBCConfig &config, std::vector<double> angles = {});
 
  /**
  * RRT(-star) algorithm
@@ -190,15 +181,16 @@ class RRT {
  * Limitations
  * - Cannot path through non-convex shapes
  * - Does not check if path is inbounds or not
+ *
+ * Notes:
+ * - this implementation is for fixed wing planes, which is not currently being used. However,
+ * it is kept here because it is very possible we will eventually switch back to it.
  */
-class CoveragePathing {
+class ForwardCoveragePathing {
  public:
- CoveragePathing(const RRTPoint &start, double scan_radius, Polygon bounds, Polygon airdrop_zone,
- std::vector<Polygon> obstacles = {},
- AirdropSearchConfig config = {.coverage_altitude_m = 30.0,
- .optimize = false,
- .vertical = false,
- .one_way = false});
+ ForwardCoveragePathing(const RRTPoint &start, double scan_radius, Polygon bounds,
+ Polygon airdrop_zone, const OBCConfig &config,
+ std::vector<Polygon> obstacles = {});
 
  /**
  * Generates a path of parallel lines to cover a given area
@@ -236,21 +228,32 @@ class CoveragePathing {
  const RRTPoint start; // start location (doesn't have to be near polygon)
  const Environment airspace; // information aobut the airspace
  const Dubins dubins; // dubins object to generate paths
- const AirdropSearchConfig config;
+ const AirdropCoverageConfig config;
+};
+
+/**
+ * Class that performs coverage pathing over a given search area, given that the plane has
+ * hovering capabilities and that we want to be taking pictures while hovering over the zone.
+ *
+ * This outputs a series of XYZ Coordinates which represent a points at which the plane
+ * should hover and take a picture.
+ */
+class HoverCoveragePathing {
+ public:
+ HoverCoveragePathing(Polygon flight_bounds, Polygon drop_zone);
+
+ std::vector<XYZCoord> run();
+
+ private:
+ Polygon flight_bounds;
+ Polygon drop_zone;
 };
 
 class AirdropApproachPathing {
  public:
  AirdropApproachPathing(const RRTPoint &start, const XYZCoord &goal, RRTPoint wind,
- Polygon bounds, std::vector<Polygon> obstacles = {},
- AirdropApproachConfig config = {
- .drop_method = UNGUIDED,
- .bottle_ids = {1, 2, 3, 4, 5},
- .drop_angle_rad = DROP_ANGLE_RAD,
- // .drop_angle_rad = M_PI * 3 / 4,
- .drop_altitude_m = DROP_ALTITUDE_M,
- .guided_drop_distance_m = GUIDED_DROP_DISTANCE_M,
- .unguided_drop_distance_m = UNGUIDED_DROP_DISTANCE_M});
+ Polygon bounds, const OBCConfig &config,
+ std::vector<Polygon> obstacles = {});
  /**
  * Generates a path to the drop location
  *
@@ -268,11 +271,13 @@ class AirdropApproachPathing {
  const RRTPoint start;
  const Environment airspace;
  const Dubins dubins;
- const AirdropApproachConfig config;
+ const OBCConfig config;
 
  RRTPoint wind;
 };
 
 std::vector<GPSCoord> generateInitialPath(std::shared_ptr<MissionState> state);
 
+std::vector<GPSCoord> generateSearchPath(std::shared_ptr<MissionState> state);
+
 #endif // INCLUDE_PATHING_STATIC_HPP_

include/pathing/tree.hpp

@@ -10,6 +10,7 @@
 #include "pathing/dubins.hpp"
 #include "pathing/environment.hpp"
 #include "utilities/datatypes.hpp"
+#include "utilities/obc_config.hpp"
 #include "utilities/rng.hpp"
 
 class RRTNode;
@@ -196,7 +197,7 @@ class RRTTree {
  * @return ==> mininum sorted list of pairs of <node, path>
  */
  std::vector<std::pair<RRTNode*, RRTOption>> pathingOptions(
- const RRTPoint& end, POINT_FETCH_METHODS path_option = POINT_FETCH_METHODS::NONE,
+ const RRTPoint& end, PointFetchMethod::Enum path_option = PointFetchMethod::Enum::NONE,
  int quantity_options = MAX_DUBINS_OPTIONS_TO_PARSE) const;
 
  /** DOES RRT* for the program

include/utilities/constants.hpp

@@ -14,10 +14,6 @@ const size_t MAX_CV_PIPELINES = 5;
 const double TWO_PI = 2 * M_PI;
 const double HALF_PI = M_PI / 2;
 
-// FROM OBC PYTHON
-const double TURNING_RADIUS = 30.0;
-const double POINT_SEPARATION = 10.0;
-
 // RRT CONSTANTS
 const int ITERATIONS_PER_WAYPOINT = 256; // number of times RRT is ran per waypoint
 const double SEARCH_RADIUS =

include/utilities/datatypes.hpp

@@ -92,79 +92,6 @@ using Polyline = std::vector<XYZCoord>;
 
 using GPSProtoVec = google::protobuf::RepeatedPtrField<GPSCoord>;
 
-enum POINT_FETCH_METHODS {
- NONE, // check RRT against every node (path optimal, but incredibly slow)
- RANDOM, // check ~k randomly sampled nodes from the tree.
- NEAREST // check ~$p$ nodes closest to the sampled node (best performance/time ratio from
- // rudimentary testing)
-};
-
-struct RRTConfig {
- int iterations_per_waypoint; // number of iterations run between two waypoints
- double rewire_radius; // maximum distance from sampled point to optimize during RRT*
- bool optimize; // run RRT* if true
- POINT_FETCH_METHODS point_fetch_method;
- bool allowed_to_skip_waypoints; // if true, will skip waypoints if it can not connect after 1
- // RRT iteration
-};
-
-struct AirdropSearchConfig {
- double coverage_altitude_m;
- bool optimize; // whether to ignore the config below and run all ways.
- bool vertical; // if true, will search in vertical lines
- bool one_way; // if true, path returned will only be in 1 direction
-};
-
-struct AirdropApproachConfig {
- drop_mode drop_method;
- std::unordered_set<int> bottle_ids;
- double drop_angle_rad;
- double drop_altitude_m;
- double guided_drop_distance_m;
- double unguided_drop_distance_m;
-};
 
-struct CameraConfig {
- // either "mock" or "lucid"
- std::string type;
- // directory to save images to
- std::string save_dir;
- struct {
- // directory to randomly pick images from
- // for the mock camera
- std::string images_dir;
- } mock;
- // All comments will reference the nodes for the Triton 200s
- // https://support.thinklucid.com/triton-tri200s/
- struct {
- // Image Format Control (https://support.thinklucid.com/triton-tri200s/#2976)
- std::string sensor_shutter_mode; // Either "Rolling" or "GlobalReset"
-
- // Acquisition Control (https://support.thinklucid.com/triton-tri200s/#2934)
- bool acquisition_frame_rate_enable;
- int64_t target_brightness;
- std::string exposure_auto; // either "Continuous" or "Off"
- double exposure_time; // manual exposure time. only applies when exposure_auto is "Off"
- std::string exposure_auto_algorithm; // either "Median" or "Mean"
- double exposure_auto_damping;
- double exposure_auto_upper_limit;
- double exposure_auto_lower_limit;
-
- // Stream settings
- bool stream_auto_negotiate_packet_size;
- bool stream_packet_resend_enable;
-
- // Device Control (https://support.thinklucid.com/triton-tri200s/#2959)
- std::string device_link_throughput_limit_mode; // Either "On" or "Off"
- int64_t device_link_throughput_limit; // for Triton 200S: max 125,000,000 min 31,250,000
-
- // Analog Control (https://support.thinklucid.com/triton-tri200s/#2953)
- bool gamma_enable;
- double gamma;
- std::string gain_auto; // either "Continuous" or "Off"
- double gain_auto_upper_limit;
- double gain_auto_lower_limit;
- } lucid;
-};
 
 #endif // INCLUDE_UTILITIES_DATATYPES_HPP_