Skip to content

Commit

Permalink
Chore/spaceout waypoints (#199)
Browse files Browse the repository at this point in the history 
* 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 <[email protected]>
  • Loading branch information
@AskewParity @Tyler-Lentz
AskewParity and Tyler-Lentz authored Jun 11, 2024
1 parent c93fc64 commit d294f9f
Show file tree
Hide file tree
Showing 22 changed files with 583 additions and 363 deletions.
34 changes: 22 additions & 12 deletions configs/config.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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,
Expand Down
34 changes: 22 additions & 12 deletions configs/dev-config.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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,
Expand Down
1 change: 1 addition & 0 deletions include/camera/interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
65 changes: 35 additions & 30 deletions include/pathing/static.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
*
Expand All @@ -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_
3 changes: 2 additions & 1 deletion include/pathing/tree.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down
4 changes: 0 additions & 4 deletions include/utilities/constants.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand Down
73 changes: 0 additions & 73 deletions include/utilities/datatypes.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_
Loading

0 comments on commit d294f9f

Please sign in to comment.