diff --git a/configs/dev.json b/configs/dev.json index 9128d4e0..8635cdb4 100644 --- a/configs/dev.json +++ b/configs/dev.json @@ -18,6 +18,7 @@ }, "pathing": { "laps": 2, + "upload_distance_buffer_m": 50.0, "dubins": { "turning_radius": 30.0, "point_separation": 20.0 diff --git a/configs/jetson.json b/configs/jetson.json index 17bd1675..fa0a819a 100644 --- a/configs/jetson.json +++ b/configs/jetson.json @@ -18,6 +18,7 @@ }, "pathing": { "laps": 10, + "upload_distance_buffer_m": 50.0, "dubins": { "turning_radius": 5.0, "point_separation": 20.0 diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index 10d92eb1..50960f5b 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -48,6 +48,9 @@ class MissionState { void setInitPath(const MissionPath& init_path); MissionPath getInitPath(); + void setNextWaypointPath(const MissionPath& next_waypoint_path); + MissionPath getNextWaypointPath(); + void setCoveragePath(const MissionPath& coverage_path); MissionPath getCoveragePath(); @@ -139,6 +142,8 @@ class MissionState { std::mutex init_path_mut; // for reading/writing the initial path MissionPath init_path; + std::mutex next_waypoint_path_mut; // for reading/writing the next waypoint path + MissionPath next_waypoint_path; std::mutex coverage_path_mut; // for reading/writing the coverage path MissionPath coverage_path; std::mutex airdrop_path_mut; diff --git a/include/network/mavlink.hpp b/include/network/mavlink.hpp index 814ef863..6f072db5 100644 --- a/include/network/mavlink.hpp +++ b/include/network/mavlink.hpp @@ -83,6 +83,7 @@ class MavlinkClient { mavsdk::Telemetry::RcStatus get_conn_status(); bool armAndHover(std::shared_ptr state); bool startMission(); + bool clearMission(); /* * Triggers a relay on the ArduPilot @@ -92,10 +93,9 @@ class MavlinkClient { */ bool triggerRelay(int relay_number, bool state); + // Safety Functions void KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY(); - - // rtl - void rtl(); + void returnToLaunch(); private: mavsdk::Mavsdk mavsdk; diff --git a/include/pathing/static.hpp b/include/pathing/static.hpp index da9bb561..b8a615b9 100644 --- a/include/pathing/static.hpp +++ b/include/pathing/static.hpp @@ -302,11 +302,27 @@ class AirdropApproachPathing { */ RRTPoint getCurrentLoc(std::shared_ptr state); -MissionPath generateInitialPath(std::shared_ptr state); +/** + * Calculates the approximate angle of exit out of a waypoint path + * + * @param path ==> the waypoint path + * @param state ==> mission state + * @return angle of exit (+x-axis is 0 deg, ccw is positive) + */ +double calculateFinalAngle( + const MissionPath& path, + const std::optional>& cartesianConverter); + +std::vector generateInitialPath(std::shared_ptr state); + +std::vector +generateNextWaypointPath(std::shared_ptr state, double start_angle); -MissionPath generateSearchPath(std::shared_ptr state); +std::vector +generateSearchPath(std::shared_ptr state, double start_angle); -MissionPath generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal); +std::vector +generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal); std::pair estimateAreaCoveredAndPathLength(const std::vector &goals, const Environment &airspace); diff --git a/include/ticks/path_gen.hpp b/include/ticks/path_gen.hpp index 11a3489c..76e0537d 100644 --- a/include/ticks/path_gen.hpp +++ b/include/ticks/path_gen.hpp @@ -27,8 +27,7 @@ class PathGenTick : public Tick { void init() override; Tick* tick() override; private: - std::future init_path; - std::future coverage_path; + std::future paths_future; void startPathGeneration(); }; diff --git a/include/utilities/obc_config.hpp b/include/utilities/obc_config.hpp index c58169ed..f986cd8f 100644 --- a/include/utilities/obc_config.hpp +++ b/include/utilities/obc_config.hpp @@ -106,6 +106,7 @@ struct AirdropApproachConfig { struct PathingConfig { int laps; + double upload_distance_buffer_m; DubinsConfig dubins; RRTConfig rrt; AirdropCoverageConfig coverage; diff --git a/src/core/mission_state.cpp b/src/core/mission_state.cpp index 38e14021..ca5f38fa 100644 --- a/src/core/mission_state.cpp +++ b/src/core/mission_state.cpp @@ -79,6 +79,16 @@ MissionPath MissionState::getInitPath() { return this->init_path; } +void MissionState::setNextWaypointPath(const MissionPath& next_waypoint_path) { + Lock lock(this->next_waypoint_path_mut); + this->next_waypoint_path = next_waypoint_path; +} + +MissionPath MissionState::getNextWaypointPath() { + Lock lock(this->next_waypoint_path_mut); + return this->next_waypoint_path; +} + void MissionState::setCoveragePath(const MissionPath& coverage_path) { Lock lock(this->coverage_path_mut); this->coverage_path = coverage_path; diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index 32f4054a..3d260d19 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -571,7 +571,7 @@ DEF_GCS_HANDLE(Post, camera, runpipeline) { DEF_GCS_HANDLE(Post, rtl) { LOG_REQUEST("POST", "/rtl"); - state->getMav()->rtl(); + state->getMav()->returnToLaunch(); LOG_RESPONSE(INFO, "RTL activated", OK); } diff --git a/src/network/mavlink.cpp b/src/network/mavlink.cpp index 19346af5..3dba2099 100644 --- a/src/network/mavlink.cpp +++ b/src/network/mavlink.cpp @@ -186,7 +186,7 @@ MavlinkClient::MavlinkClient(OBCConfig config) }); this->telemetry->subscribe_heading([this](mavsdk::Telemetry::Heading heading) { - VLOG_F(DEBUG, "Heading: %d", heading.heading_deg); + VLOG_F(DEBUG, "Heading: %f deg", heading.heading_deg); Lock lock(this->data_mut); this->data.heading_deg = heading.heading_deg; }); @@ -511,12 +511,27 @@ bool MavlinkClient::startMission() { return true; } +/** + * Clears the existng mission + */ +bool MavlinkClient::clearMission() { + LOG_F(INFO, "Sending clear mission command"); + auto start_result = this->mission->clear_mission(); + if (start_result != mavsdk::MissionRaw::Result::Success) { + LOG_S(ERROR) << "FAIL: Mission could not be cleared " << start_result; + return false; + } + + LOG_F(INFO, "Mission Cleared!"); + return true; +} + void MavlinkClient::KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY() { LOG_F(ERROR, "KILLING THE PLANE: SETTING AFS_TERMINATE TO 1"); auto result = this->param->set_param_int("AFS_TERMINATE", 1); LOG_S(ERROR) << "KILL RESULT: " << result; } -void MavlinkClient::rtl() { +void MavlinkClient::returnToLaunch() { this->action->return_to_launch(); } diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp index 83cd6949..2039251d 100644 --- a/src/pathing/static.cpp +++ b/src/pathing/static.cpp @@ -21,7 +21,7 @@ #include "utilities/rng.hpp" RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Polygon bounds, - const OBCConfig &config, std::vector obstacles, std::vector angles) + const OBCConfig& config, std::vector obstacles, std::vector angles) : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), search_radius(search_radius), rewire_radius(config.pathing.rrt.rewire_radius), @@ -34,7 +34,7 @@ RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Poly } RRT::RRT(RRTPoint start, std::vector goals, double search_radius, Environment airspace, - const OBCConfig &config, std::vector angles) + const OBCConfig& config, std::vector angles) : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint), search_radius(search_radius), rewire_radius(config.pathing.rrt.rewire_radius), @@ -96,7 +96,7 @@ bool RRT::RRTIteration(int tries, int current_goal_index) { const RRTPoint sample = generateSamplePoint(); // returns all dubins options from the tree to the sample - const std::vector, RRTOption>> &options = + const std::vector, RRTOption>>& options = tree.pathingOptions(sample, config.point_fetch_method); // returns true if the node is successfully added to the tree @@ -126,22 +126,19 @@ bool RRT::RRTIteration(int tries, int current_goal_index) { return true; } -bool RRT::epochEvaluation(std::shared_ptr goal_node, - std::shared_ptr goal_parent, +bool RRT::epochEvaluation(std::shared_ptr goal_node, std::shared_ptr goal_parent, int current_goal_index) { // If a single epoch has not been passed, mark this goal as the first // benchmark. if (goal_node == nullptr) { - goal_node = sampleToGoal(current_goal_index, - TOTAL_OPTIONS_FOR_GOAL_CONNECTION, - goal_parent); + goal_node = + sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION, goal_parent); return false; } std::shared_ptr new_parent = nullptr; - std::shared_ptr new_node = sampleToGoal(current_goal_index, - TOTAL_OPTIONS_FOR_GOAL_CONNECTION, - new_parent); + std::shared_ptr new_node = + sampleToGoal(current_goal_index, TOTAL_OPTIONS_FOR_GOAL_CONNECTION, new_parent); if (new_node == nullptr) { return false; @@ -174,7 +171,7 @@ RRT::getOptionsToGoal(int current_goal_index, int total_options) const { // Generates goal specific points based on current Waypoints and list og // Angles for (const double angle : angles) { - const XYZCoord &goal = tree.getAirspace().getGoal(current_goal_index); + const XYZCoord& goal = tree.getAirspace().getGoal(current_goal_index); goal_points.push_back(RRTPoint(goal, angle)); } @@ -188,36 +185,35 @@ RRT::getOptionsToGoal(int current_goal_index, int total_options) const { // gets all options for each of the goals, and puts them into a unified list // TODO ? maybe better for a max heap? - for (const RRTPoint &goal : goal_points) { - const std::vector, RRTOption>> &options = + for (const RRTPoint& goal : goal_points) { + const std::vector, RRTOption>>& options = // For now, we use optimal pathing tree.pathingOptions(goal, PointFetchMethod::Enum::NONE, NUMBER_OPTIONS_EACH); - for (const auto &[node, option] : options) { + for (const auto& [node, option] : options) { all_options.push_back({goal, {node, option}}); } } - std::sort(all_options.begin(), all_options.end(), [](const auto &a, const auto &b) { - auto &[a_goal, a_paths] = a; - auto &[a_node, a_option] = a_paths; - auto &[b_goal, b_paths] = b; - auto &[b_node, b_option] = b_paths; + std::sort(all_options.begin(), all_options.end(), [](const auto& a, const auto& b) { + auto& [a_goal, a_paths] = a; + auto& [a_node, a_option] = a_paths; + auto& [b_goal, b_paths] = b; + auto& [b_node, b_option] = b_paths; return a_option.length + a_node->getCost() < b_option.length + b_node->getCost(); }); return all_options; } -std::shared_ptr RRT::sampleToGoal(int current_goal_index, - int total_options, +std::shared_ptr RRT::sampleToGoal(int current_goal_index, int total_options, std::shared_ptr& parent) const { // gets all options for each of the goals - const auto &all_options = getOptionsToGoal(current_goal_index, total_options); + const auto& all_options = getOptionsToGoal(current_goal_index, total_options); // - for (const auto &[goal, pair] : all_options) { - auto &[anchor_node, option] = pair; + for (const auto& [goal, pair] : all_options) { + auto& [anchor_node, option] = pair; std::shared_ptr new_node = tree.generateNode(anchor_node, goal, option); @@ -242,8 +238,7 @@ bool RRT::connectToGoal(int current_goal_index, int total_options) { return true; } -void RRT::addNodeToTree(std::shared_ptr goal_node, - std::shared_ptr parent, +void RRT::addNodeToTree(std::shared_ptr goal_node, std::shared_ptr parent, int current_goal_index) { // add the node to the tree tree.addNode(parent, goal_node); @@ -261,7 +256,7 @@ void RRT::addNodeToTree(std::shared_ptr goal_node, double height_difference = tree.getAirspace().getGoal(current_goal_index).z - start_height; double height_increment = height_difference / local_path.size(); - for (XYZCoord &point : local_path) { + for (XYZCoord& point : local_path) { point.z = start_height; start_height += height_increment; } @@ -272,9 +267,9 @@ void RRT::addNodeToTree(std::shared_ptr goal_node, } std::shared_ptr RRT::parseOptions( - const std::vector, RRTOption>> &options, - const RRTPoint &sample) { - for (auto &[node, option] : options) { + const std::vector, RRTOption>>& options, + const RRTPoint& sample) { + for (auto& [node, option] : options) { /* * stop if * 1. the node is null @@ -301,9 +296,9 @@ std::shared_ptr RRT::parseOptions( void RRT::optimizeTree(std::shared_ptr sample) { tree.RRTStar(sample, rewire_radius); } -ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint &start, double scan_radius, +ForwardCoveragePathing::ForwardCoveragePathing(const RRTPoint& start, double scan_radius, Polygon bounds, Polygon airdrop_zone, - const OBCConfig &config, + const OBCConfig& config, std::vector obstacles) : start(start), scan_radius(scan_radius), @@ -348,7 +343,7 @@ std::vector ForwardCoveragePathing::coverageOptimal() const { // generates the endpoints for the lines (including headings) for (int i = 0; i < configs.size(); i++) { - const auto &config = configs[i]; + const auto& config = configs[i]; std::vector waypoints = airspace.getAirdropWaypoints(scan_radius, config.first, config.second); @@ -385,7 +380,7 @@ std::vector ForwardCoveragePathing::coverageOptimal() const { } std::vector ForwardCoveragePathing::generatePath( - const std::vector &dubins_options, const std::vector &waypoints) const { + const std::vector& dubins_options, const std::vector& waypoints) const { std::vector path; // height adjustement @@ -397,7 +392,7 @@ std::vector ForwardCoveragePathing::generatePath( double height_increment = height_difference / path_coordinates.size(); - for (XYZCoord &coord : path_coordinates) { + for (XYZCoord& coord : path_coordinates) { coord.z = height; height += height_increment; } @@ -410,7 +405,7 @@ std::vector ForwardCoveragePathing::generatePath( dubins.generatePoints(waypoints[i], waypoints[i + 1], dubins_options[i].dubins_path, dubins_options[i].has_straight); - for (XYZCoord &coord : path_coordinates) { + for (XYZCoord& coord : path_coordinates) { coord.z = config.altitude_m; } @@ -426,9 +421,9 @@ HoverCoveragePathing::HoverCoveragePathing(std::shared_ptr state) state{state} {} // Function to calculate the center of the polygon -XYZCoord calculateCenter(const std::vector &polygon) { +XYZCoord calculateCenter(const std::vector& polygon) { XYZCoord center(0.0, 0.0, 0.0); - for (const auto &point : polygon) { + for (const auto& point : polygon) { center.x += point.x; center.y += point.y; } @@ -438,24 +433,24 @@ XYZCoord calculateCenter(const std::vector &polygon) { } // Function to scale the polygon -void scalePolygon(std::vector &polygon, double scaleFactor) { +void scalePolygon(std::vector& polygon, double scaleFactor) { // Step 1: Calculate the center of the polygon XYZCoord center = calculateCenter(polygon); // Step 2: Translate the polygon to the origin - for (auto &point : polygon) { + for (auto& point : polygon) { point.x -= center.x; point.y -= center.y; } // Step 3: Scale the polygon - for (auto &point : polygon) { + for (auto& point : polygon) { point.x *= scaleFactor; point.y *= scaleFactor; } // Step 4: Translate the polygon back to its original center - for (auto &point : polygon) { + for (auto& point : polygon) { point.x += center.x; point.y += center.y; } @@ -508,9 +503,9 @@ std::vector HoverCoveragePathing::run() { return hover_points; } -AirdropApproachPathing::AirdropApproachPathing(const RRTPoint &start, const XYZCoord &goal, +AirdropApproachPathing::AirdropApproachPathing(const RRTPoint& start, const XYZCoord& goal, XYZCoord wind, Polygon bounds, - const OBCConfig &config, + const OBCConfig& config, std::vector obstacles) : start(start), goal(goal), @@ -552,7 +547,7 @@ RRTPoint AirdropApproachPathing::getDropLocation() const { return RRTPoint(drop_location, angle); } -std::vector> generateGoalListDeviations(const std::vector &goals, +std::vector> generateGoalListDeviations(const std::vector& goals, XYZCoord deviation_point) { std::vector> goal_list_deviations; for (int i = 0; i < goals.size() + 1; i++) { @@ -564,8 +559,8 @@ std::vector> generateGoalListDeviations(const std::vector< return goal_list_deviations; } -std::vector> generateRankedNewGoalsList(const std::vector &goals, - const Environment &mapping_bounds) { +std::vector> generateRankedNewGoalsList(const std::vector& goals, + const Environment& mapping_bounds) { // generate deviation points randomly in the mapping region std::vector deviation_points; for (int i = 0; i < 200; i++) { @@ -574,7 +569,7 @@ std::vector> generateRankedNewGoalsList(const std::vector< // each deviation point can be inserted between any two goals std::vector> new_goals_list; - for (const XYZCoord &deviation_point : deviation_points) { + for (const XYZCoord& deviation_point : deviation_points) { std::vector> goal_list_deviations = generateGoalListDeviations(goals, deviation_point); new_goals_list.insert(new_goals_list.end(), goal_list_deviations.begin(), @@ -583,7 +578,7 @@ std::vector> generateRankedNewGoalsList(const std::vector< // run each goal list and get the area covered and the length of the path std::vector> area_length_pairs; - for (const std::vector &new_goals : new_goals_list) { + for (const std::vector& new_goals : new_goals_list) { area_length_pairs.push_back(mapping_bounds.estimateAreaCoveredAndPathLength(new_goals)); } @@ -595,11 +590,11 @@ std::vector> generateRankedNewGoalsList(const std::vector< } std::sort(ranked_new_goals_list.begin(), ranked_new_goals_list.end(), - [](const auto &a, const auto &b) { return a.first > b.first; }); + [](const auto& a, const auto& b) { return a.first > b.first; }); // return the ranked list of new goals lists std::vector> ranked_goals; - for (const auto &pair : ranked_new_goals_list) { + for (const auto& pair : ranked_new_goals_list) { ranked_goals.push_back(pair.second); } @@ -619,7 +614,22 @@ RRTPoint getCurrentLoc(std::shared_ptr state) { return RRTPoint(start_xyz, start_angle); } -MissionPath generateInitialPath(std::shared_ptr state) { +double calculateFinalAngle( + const MissionPath& path, + const std::optional>& cartesianConverter) { + const auto& coords = path.get(); + if (coords.size() < 2) { + return 0.0; // should probably either have angle between now and point + // or use assert for force size >=2 + } + + XYZCoord pt1 = cartesianConverter->toXYZ(coords[coords.size() - 2]); + XYZCoord pt2 = cartesianConverter->toXYZ(coords[coords.size() - 1]); + + return std::atan2(pt2.y - pt1.y, pt2.x - pt1.x); +} + +std::vector generateInitialPath(std::shared_ptr state) { // first waypoint is start // the other waypoitns is the goals @@ -649,40 +659,87 @@ MissionPath generateInitialPath(std::shared_ptr state) { std::vector path = rrt.getPointsToGoal(); std::vector output_coords; - for (const XYZCoord &waypoint : path) { + for (const XYZCoord& waypoint : path) { output_coords.push_back(state->getCartesianConverter()->toLatLng(waypoint)); } - return MissionPath(MissionPath::Type::FORWARD, output_coords); + return output_coords; } -MissionPath generateSearchPath(std::shared_ptr state) { +std::vector generateNextWaypointPath(std::shared_ptr state, + double start_angle) { + if (state->mission_params.getWaypoints().size() < 1) { + loguru::set_thread_name("Static Pathing"); + LOG_F(ERROR, "Not enough waypoints to generate a path, requires >=1, num waypoints: %s", + std::to_string(state->mission_params.getWaypoints().size()).c_str()); + return {}; + } + + std::vector goals = state->mission_params.getWaypoints(); + + if (state->config.pathing.rrt.generate_deviations) { + Environment mapping_bounds(state->mission_params.getAirdropBoundary(), {}, {}, goals, {}); + goals = generateRankedNewGoalsList(goals, mapping_bounds)[0]; + } + + RRTPoint start(goals.back(), start_angle); + + // add buffer to the start point so that we dont loopty loop + double buffer_m = state->config.pathing.upload_distance_buffer_m; + if (buffer_m > 0.0) { + start.coord.x += buffer_m * std::cos(start_angle); + start.coord.y += buffer_m * std::sin(start_angle); + } + + RRT rrt(start, goals, SEARCH_RADIUS, state->mission_params.getFlightBoundary(), state->config, + {}, {}); + + rrt.run(); + + std::vector path = rrt.getPointsToGoal(); + + std::vector output_coords; + for (const XYZCoord& waypoint : path) { + output_coords.push_back(state->getCartesianConverter()->toLatLng(waypoint)); + } + + return output_coords; +} + +std::vector generateSearchPath(std::shared_ptr state, double start_angle) { std::vector gps_coords; if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { - RRTPoint start(state->mission_params.getWaypoints().back(), 0); + if (state->mission_params.getWaypoints().size() < 1) { + loguru::set_thread_name("Static Pathing"); + LOG_F(ERROR, + "Waypoint path is empty. Failed to generate search path"); + return {}; + } + RRTPoint start(state->mission_params.getWaypoints().back(), start_angle); + double scan_radius = state->config.pathing.coverage.camera_vision_m; ForwardCoveragePathing pathing(start, scan_radius, state->mission_params.getFlightBoundary(), state->mission_params.getAirdropBoundary(), state->config); - for (const auto &coord : pathing.run()) { + for (const auto& coord : pathing.run()) { gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord)); } - return MissionPath(MissionPath::Type::FORWARD, gps_coords); + return gps_coords; } else { // hover HoverCoveragePathing pathing(state); - for (const auto &coord : pathing.run()) { + for (const auto& coord : pathing.run()) { gps_coords.push_back(state->getCartesianConverter()->toLatLng(coord)); } - return MissionPath(MissionPath::Type::HOVER, gps_coords, - state->config.pathing.coverage.hover.hover_time_s); + return gps_coords; } } -MissionPath generateAirdropApproach(std::shared_ptr state, const GPSCoord &goal) { +std::vector generateAirdropApproach(std::shared_ptr state, + const GPSCoord& goal) { std::shared_ptr mav = state->getMav(); /* Note: this function was neutered right before we attempted to fly at the 2024 competition @@ -709,7 +766,7 @@ MissionPath generateAirdropApproach(std::shared_ptr state, const G std::vector gps_path; // XYZCoord pt = state->getCartesianConverter().value().toXYZ(goal); - for (const XYZCoord &wpt : xyz_path) { + for (const XYZCoord& wpt : xyz_path) { gps_path.push_back(state->getCartesianConverter().value().toLatLng(wpt)); } @@ -725,5 +782,5 @@ MissionPath generateAirdropApproach(std::shared_ptr state, const G // gps_path.push_back(goal); // gps_path.push_back(goal); - return MissionPath(MissionPath::Type::FORWARD, gps_path); + return gps_path; } diff --git a/src/ticks/airdrop_prep.cpp b/src/ticks/airdrop_prep.cpp index fb58dc26..6c223f68 100644 --- a/src/ticks/airdrop_prep.cpp +++ b/src/ticks/airdrop_prep.cpp @@ -67,7 +67,8 @@ Tick* AirdropPrepTick::tick() { target.coordinate().latitude(), target.coordinate().longitude(), target.coordinate().altitude()); - state->setAirdropPath(generateAirdropApproach(state, target.coordinate())); + state->setAirdropPath(MissionPath(MissionPath::Type::FORWARD, + generateAirdropApproach(state, target.coordinate()))); LOG_F(INFO, "Generated approach path"); diff --git a/src/ticks/fly_search.cpp b/src/ticks/fly_search.cpp index 1d733d3c..6dbe7c4f 100644 --- a/src/ticks/fly_search.cpp +++ b/src/ticks/fly_search.cpp @@ -2,6 +2,7 @@ #include #include +#include #include "pathing/environment.hpp" #include "ticks/airdrop_prep.hpp" @@ -28,17 +29,9 @@ void FlySearchTick::init() { this->airdrop_boundary = this->state->mission_params.getAirdropBoundary(); this->last_photo_time = getUnixTime_ms(); - // note: I didn't get around to testing if 1 would be a better value than 0 - // to see if the mission start can be forced. - if (!this->state->getMav()->setMissionItem(1)) { - LOG_F(ERROR, "Failed to reset Mission"); - } - - this->mission_started = this->state->getMav()->startMission(); - - // I have another one here because idk how startmIssion behaves exactly - if (!this->state->getMav()->setMissionItem(1)) { - LOG_F(ERROR, "Failed to reset Mission"); + while (!this->mission_started) { + this->mission_started = this->state->getMav()->startMission(); + std::this_thread::sleep_for(100ms); } LOG_F(INFO, "Total Waypoint #: %zu", this->state->getMav()->totalWaypoints()); @@ -51,11 +44,6 @@ Tick* FlySearchTick::tick() { return new AirdropPrepTick(this->state); } - if (!this->mission_started) { - this->mission_started = this->state->getMav()->startMission(); - return nullptr; - } - bool isMissionFinished = state->getMav()->isMissionFinished(); if (isMissionFinished) { diff --git a/src/ticks/fly_waypoints.cpp b/src/ticks/fly_waypoints.cpp index ed8250b2..3384ff41 100644 --- a/src/ticks/fly_waypoints.cpp +++ b/src/ticks/fly_waypoints.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include "pathing/static.hpp" #include "ticks/fly_search.hpp" @@ -18,22 +19,12 @@ FlyWaypointsTick::FlyWaypointsTick(std::shared_ptr state, Tick* ne : Tick(state, TickID::FlyWaypoints), next_tick(next_tick) {} void FlyWaypointsTick::init() { - // note: I didn't get around to testing if 1 would be a better value than 0 - // to see if the mission start can be forced. - if (!this->state->getMav()->setMissionItem(1)) { - LOG_F(ERROR, "Failed to reset Mission"); + while (!this->mission_started) { + this->mission_started = this->state->getMav()->startMission(); + std::this_thread::sleep_for(100ms); } - - this->mission_started = this->state->getMav()->startMission(); - state->getCamera()->startStreaming(); - this->last_photo_time = getUnixTime_ms(); - - // I have another one here because idk how startmIssion behaves exactly - if (!this->state->getMav()->setMissionItem(1)) { - LOG_F(ERROR, "Failed to reset Mission"); - } - state->decrementLapsRemaining(); + LOG_F(INFO, "Started FlyWaypointsTick, Laps Remaining: %d", state->getLapsRemaining()); } @@ -80,32 +71,10 @@ Tick* FlyWaypointsTick::tick() { return nullptr; } - if (state->getLapsRemaining() > 0) { - // regenerate path - std::future init_path; - init_path = std::async(std::launch::async, generateInitialPath, this->state); - auto init_status = init_path.wait_for(std::chrono::milliseconds(0)); - int count_ms = 2500; - const int wait_time_ms = 100; - - while (init_status != std::future_status::ready && count_ms > 0) { - LOG_F(WARNING, "Waiting for path to be generated..."); - std::this_thread::sleep_for(std::chrono::milliseconds(wait_time_ms)); - init_status = init_path.wait_for(std::chrono::milliseconds(0)); - count_ms -= wait_time_ms; - } - - if (count_ms <= 0) { - LOG_F(ERROR, "Path generation took too long. Trying Again..."); - return nullptr; - } - - state->setInitPath(init_path.get()); - return new MavUploadTick( this->state, new FlyWaypointsTick(this->state, new FlySearchTick(this->state)), - state->getInitPath(), false); + state->getNextWaypointPath(), false); } return new MavUploadTick( diff --git a/src/ticks/mav_upload.cpp b/src/ticks/mav_upload.cpp index 24e18518..225c7270 100644 --- a/src/ticks/mav_upload.cpp +++ b/src/ticks/mav_upload.cpp @@ -24,6 +24,7 @@ std::chrono::milliseconds MavUploadTick::getWait() const { } void MavUploadTick::init() { + this->state->getMav()->clearMission(); this->mav_uploaded = std::async(std::launch::async, &MavlinkClient::uploadMissionUntilSuccess, this->state->getMav(), diff --git a/src/ticks/path_gen.cpp b/src/ticks/path_gen.cpp index fe690df0..eb792507 100644 --- a/src/ticks/path_gen.cpp +++ b/src/ticks/path_gen.cpp @@ -18,17 +18,14 @@ PathGenTick::PathGenTick(std::shared_ptr state) : Tick(state, Tick std::chrono::milliseconds PathGenTick::getWait() const { return PATH_GEN_TICK_WAIT; } void PathGenTick::init() { + assert(this->state->getCartesianConverter().has_value()); startPathGeneration(); } Tick* PathGenTick::tick() { - auto init_status = this->init_path.wait_for(0ms); - auto search_status = this->coverage_path.wait_for(0ms); - if (init_status == std::future_status::ready && - search_status == std::future_status::ready) { + auto status = this->paths_future.wait_for(0ms); + if (status == std::future_status::ready) { LOG_F(INFO, "Initial and Coverage paths generated"); - state->setInitPath(this->init_path.get()); - state->setCoveragePath(this->coverage_path.get()); return new PathValidateTick(this->state); } @@ -36,6 +33,36 @@ Tick* PathGenTick::tick() { } void PathGenTick::startPathGeneration() { - this->init_path = std::async(std::launch::async, generateInitialPath, this->state); - this->coverage_path = std::async(std::launch::async, generateSearchPath, this->state); + this->paths_future = std::async(std::launch::async, [this]() { + const int NUM_WAYPOINTS_TO_REMOVE = + std::floor(this->state->config.pathing.upload_distance_buffer_m / + this->state->config.pathing.dubins.point_separation); + + std::vector init_gps = generateInitialPath(this->state); + MissionPath init = MissionPath(MissionPath::Type::FORWARD, init_gps); + double angle1 = calculateFinalAngle(init, this->state->getCartesianConverter()); + + std::vector next_gps = generateNextWaypointPath(this->state, angle1); + assert(next_gps.size() >= NUM_WAYPOINTS_TO_REMOVE); + next_gps.erase(next_gps.begin(), next_gps.begin() + NUM_WAYPOINTS_TO_REMOVE);; + MissionPath next = MissionPath(MissionPath::Type::FORWARD, next_gps); + double angle2 = calculateFinalAngle(next, this->state->getCartesianConverter()); + + + std::vector coverage_gps = generateSearchPath(this->state, angle2); + assert(coverage_gps.size() >= NUM_WAYPOINTS_TO_REMOVE); + coverage_gps.erase(coverage_gps.begin(), coverage_gps.begin() + NUM_WAYPOINTS_TO_REMOVE);; + + MissionPath coverage; + if (this->state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) { + coverage = MissionPath(MissionPath::Type::FORWARD, coverage_gps); + } else { + coverage = MissionPath(MissionPath::Type::HOVER, coverage_gps, + this->state->config.pathing.coverage.hover.hover_time_s); + } + + this->state->setInitPath(init); + this->state->setNextWaypointPath(next); + this->state->setCoveragePath(coverage); + }); } diff --git a/src/utilities/obc_config.cpp b/src/utilities/obc_config.cpp index df604da2..aeec5f6b 100644 --- a/src/utilities/obc_config.cpp +++ b/src/utilities/obc_config.cpp @@ -60,6 +60,7 @@ OBCConfig::OBCConfig(int argc, char* argv[]) { SET_CONFIG_OPT(pathing, laps); SET_CONFIG_OPT(pathing, rrt, iterations_per_waypoint); + SET_CONFIG_OPT(pathing, upload_distance_buffer_m); SET_CONFIG_OPT(pathing, rrt, rewire_radius); SET_CONFIG_OPT(pathing, rrt, optimize); SET_CONFIG_OPT(pathing, rrt, generate_deviations); diff --git a/tests/unit/pathing_test.cpp b/tests/unit/pathing_test.cpp index 1b8ce2ea..9cc999e9 100644 --- a/tests/unit/pathing_test.cpp +++ b/tests/unit/pathing_test.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include #include @@ -21,61 +22,182 @@ #include "utilities/http.hpp" #include "utilities/obc_config.hpp" -// TODO: currnetly failing because of the same issue as gcs_server_test.cpp -// Once that file is fixed this one should be trivial - -/* -#define DECLARE_HANDLER_PARAMS(STATE, REQ, RESP) \ - int argc = 2; \ - char path1[] = "bin/obcpp"; \ - char path2[] = "../../../configs/dev-config.json"; \ - char *paths[] = {path1, path2}; \ - char **paths_ptr = paths; \ - std::shared_ptr STATE = std::make_shared(OBCConfig(argc, paths_ptr)); \ - httplib::Request REQ; \ - httplib::Response RESP +#include "handler_params.hpp" +// TODO: fails fom the tick ever switching // copies over code verbatim from the gcs test, and then generates a path -TEST(StaticPathingTest, RRTTest) { - // First upload a mission so that we generate a path +// TEST(StaticPathingTest, RRTTest) { +// // First upload a mission so that we generate a path +// DECLARE_HANDLER_PARAMS(state, req, resp); +// req.body = resources::mission_json_2020; +// state->setTick(new MissionPrepTick(state)); + +// GCS_HANDLE(Post, mission)(state, req, resp); + +// state->doTick(); + +// EXPECT_EQ(state->getTickID(), TickID::PathGen); +// do { // wait for path to generate +// auto wait = state->doTick(); +// std::this_thread::sleep_for(wait); +// } while (state->getInitPath().get().empty()); +// // have an initial path, but waiting for validation +// EXPECT_FALSE(state->getInitPath().get().empty()); +// EXPECT_EQ(state->getTickID(), TickID::PathValidate); + +// // actually new test +// // validate the path +// Environment env(state->mission_params.getFlightBoundary(), {}, {}, {}, {}); + +// std::vector path = state->getInitPath().get(); + +// for (GPSCoord &point : path) { +// const XYZCoord xyz_point = state->getCartesianConverter().value().toXYZ(point); + +// EXPECT_TRUE(env.isPointInBounds(xyz_point)); +// } + +// // PathingPlot plotter("pathing_output", state->config.getFlightBoundary(), +// // state->config.getAirdropBoundary(), state->config.getWaypoints()); + +// // std::vector path_coords; + +// // for (auto wpt : path) { +// // path_coords.push_back(state->getCartesianConverter().value().toXYZ(wpt)); +// // } + +// // plotter.addFinalPolyline(path_coords); +// // plotter.output("unit_test_path", PathOutputType::BOTH); +// } + +/** + * Checks final angles in each axis and quadrant (8 checks) + */ +TEST(StaticPathingTest, FinalAngleQuadrant) { DECLARE_HANDLER_PARAMS(state, req, resp); - req.body = resources::mission_json_2020; - state->setTick(new MissionPrepTick(state)); - GCS_HANDLE(Post, mission)(state, req, resp); + GPSProtoVec bounds; + *bounds.Add() = makeGPSCoord(0.0, 0.0, 0.0); + *bounds.Add() = makeGPSCoord(1.0, 1.0, 0.0); + CartesianConverter converter(bounds); + state->setCartesianConverter(converter); - state->doTick(); - EXPECT_EQ(state->getTickID(), TickID::PathGen); - do { // wait for path to generate - auto wait = state->doTick(); - std::this_thread::sleep_for(wait); - } while (state->getInitPath().get().empty()); - // have an initial path, but waiting for validation - EXPECT_FALSE(state->getInitPath().get().empty()); - EXPECT_EQ(state->getTickID(), TickID::PathValidate); + struct TestCheck { + double dx; + double dy; + double expected_angle; + }; + + std::vector checks = { + {1.0, 0.0, 0.0}, + {0.0, 1.0, M_PI / 2.0}, + {-1.0, 0.0, M_PI}, + {0.0, -1.0, -M_PI / 2.0}, + {1.0, 1.0, M_PI / 4.0}, + {-1.0, 1.0, 3.0 * M_PI / 4.0}, + {-1.0, -1.0, -3.0 * M_PI / 4.0}, + {1.0, -1.0, -M_PI / 4.0} + }; - // actually new test - // validate the path - Environment env(state->mission_params.getFlightBoundary(), {}, {}, {}); + XYZCoord start_xyz(0.0, 0.0, 0.0); + GPSCoord start_gps = converter.toLatLng(start_xyz); - std::vector path = state->getInitPath().get(); + for (const auto& check : checks) { + XYZCoord end_xyz(check.dx, check.dy, 0.0); + GPSCoord end_gps = converter.toLatLng(end_xyz); - for (GPSCoord &point : path) { - const XYZCoord xyz_point = state->getCartesianConverter().value().toXYZ(point); + MissionPath path(MissionPath::Type::FORWARD, {start_gps, end_gps}); - EXPECT_TRUE(env.isPointInBounds(xyz_point)); + double calculated_angle = calculateFinalAngle(path, state->getCartesianConverter()); + + if (check.expected_angle == M_PI && calculated_angle < 0) { + EXPECT_NEAR(-M_PI, calculated_angle, 0.001); + } else { + EXPECT_NEAR(check.expected_angle, calculated_angle, 0.001); + } } +} + +/** + * Verify our implementation of degenrate paths + */ +TEST(StaticPathingTest, FinalAngleLessThanTwoPoints) { + DECLARE_HANDLER_PARAMS(state, req, resp); - // PathingPlot plotter("pathing_output", state->config.getFlightBoundary(), - // state->config.getAirdropBoundary(), state->config.getWaypoints()); + GPSProtoVec bounds; + *bounds.Add() = makeGPSCoord(0.0, 0.0, 0.0); + *bounds.Add() = makeGPSCoord(1.0, 1.0, 0.0); + CartesianConverter converter(bounds); + state->setCartesianConverter(converter); - // std::vector path_coords; + XYZCoord start_xyz(0.0, 0.0, 0.0); + GPSCoord start_gps = converter.toLatLng(start_xyz); - // for (auto wpt : path) { - // path_coords.push_back(state->getCartesianConverter().value().toXYZ(wpt)); - // } + MissionPath empty_path; + EXPECT_EQ(0.0, calculateFinalAngle(empty_path, state->getCartesianConverter())); - // plotter.addFinalPolyline(path_coords); - // plotter.output("unit_test_path", PathOutputType::BOTH); + MissionPath one_point_path(MissionPath::Type::FORWARD, {start_gps}); + EXPECT_EQ(0.0, calculateFinalAngle(one_point_path, state->getCartesianConverter())); } -*/ \ No newline at end of file + + +/** + * Makes sure last two points are taken + */ +TEST(StaticPathingTest, FinalAngleLastTwoPoints) { + DECLARE_HANDLER_PARAMS(state, req, resp); + + GPSProtoVec bounds; + *bounds.Add() = makeGPSCoord(0.0, 0.0, 0.0); + *bounds.Add() = makeGPSCoord(1.0, 1.0, 0.0); + CartesianConverter converter(bounds); + state->setCartesianConverter(converter); + + XYZCoord pt1_xyz(0.0, 0.0, 0.0); + XYZCoord pt2_xyz(1.0, -1.0, 0.0); + XYZCoord pt3_xyz(1.0, 1.0, 0.0); + + GPSCoord pt1 = converter.toLatLng(pt1_xyz); + GPSCoord pt2 = converter.toLatLng(pt2_xyz); + GPSCoord pt3 = converter.toLatLng(pt3_xyz); + + MissionPath path(MissionPath::Type::FORWARD, {pt1, pt2, pt3}); + + // final direction is from (1, -1) to (1, 1) -> dx=0, dy=2 -> M_PI / 2.0 + double expected_angle = M_PI / 2.0; + EXPECT_NEAR(expected_angle, calculateFinalAngle(path, state->getCartesianConverter()), 0.001); +} + +/** + * Accuracy test on Dubins + * + * @param Separation + */ +TEST(StaticPathingTest, FinalAngleDubinsPath) { + DECLARE_HANDLER_PARAMS(state, req, resp); + + GPSProtoVec bounds; + *bounds.Add() = makeGPSCoord(0.0, 0.0, 0.0); + *bounds.Add() = makeGPSCoord(1.0, 1.0, 0.0); + CartesianConverter converter(bounds); + state->setCartesianConverter(converter); + + const double EXPECTED_END_ANGLE = M_PI / 2.0; + + Dubins dubins(30.0, 30.0); + RRTPoint start_pt(XYZCoord(0.0, 0.0, 0.0), 0.0); + RRTPoint end_pt(XYZCoord(300.0, 300.0, 0.0), EXPECTED_END_ANGLE); + + // Dubins Path + std::vector dubins_xyz = dubins.dubinsPath(start_pt, end_pt); + std::vector dubins_gps; + for (const auto& xyz : dubins_xyz) { + dubins_gps.push_back(converter.toLatLng(xyz)); + } + MissionPath path(MissionPath::Type::FORWARD, dubins_gps); + + double calculated_angle = calculateFinalAngle(path, state->getCartesianConverter()); + + // Allow some tolerance due to point separation (10 degrees --> 0.174 rad) + EXPECT_NEAR(EXPECTED_END_ANGLE, calculated_angle, 0.174); +} \ No newline at end of file