Skip to content

Fixing repathing in static cpp #355

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
ClaytonMa wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fixing repathing in static cpp #355

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion include/pathing/static.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ class RRT {
* @param tries ==> number of points it attempts to sample
* @return ==> whether or not the goal was reached
*/
bool RRTIteration(int tries, int current_goal_index);
bool RRTIteration(int tries, int current_goal_index, int retry_count = 0);

/**
* Evaluates a certain interval to determine if the algorithm is making
Expand Down
94 changes: 66 additions & 28 deletions src/pathing/static.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,11 +61,12 @@ void RRT::run() {

for (int current_goal_index = 0; current_goal_index < total_goals; current_goal_index++) {
// tries to connect directly to the goal
tree.setCurrentHead(tree.getRoot());

if (connectToGoal(current_goal_index)) {
continue;
}

// run the RRT algorithm if it can not connect
RRTIteration(iterations_per_waypoint, current_goal_index);
}
}
Expand All @@ -75,13 +76,15 @@ std::vector<XYZCoord> RRT::getPointsToGoal() const {
return flight_path;
}

bool RRT::RRTIteration(int tries, int current_goal_index) {
// FIX 1: removed `= 0` default argument from the definition (must only be in the .hpp declaration)
bool RRT::RRTIteration(int tries, int current_goal_index, int retry_count) {
const int epoch_interval = tries / NUM_EPOCHS;
int current_epoch = epoch_interval;

std::shared_ptr<RRTNode> goal_node = nullptr;
std::shared_ptr<RRTNode> goal_parent = nullptr;

int nullptr_count = 0;
const int max_minor_errors = 3;
for (int i = 0; i < tries; i++) {
if (i == current_epoch) {
// generates a new node (not connect), and adds and breaks if it is
Expand All @@ -102,6 +105,15 @@ bool RRT::RRTIteration(int tries, int current_goal_index) {
// returns true if the node is successfully added to the tree
std::shared_ptr<RRTNode> new_node = parseOptions(options, sample);

if (new_node == nullptr) {
nullptr_count++;
}

if(nullptr_count == max_minor_errors){
tree.setCurrentHead(tree.getRoot());
nullptr_count=0;
}

if (new_node != nullptr && config.optimize) {
optimizeTree(new_node);
}
Expand All @@ -114,10 +126,19 @@ bool RRT::RRTIteration(int tries, int current_goal_index) {
LOG_F(WARNING, "Failed to connect to goal on iteration: [%s]. Trying again...",
std::to_string(current_goal_index).c_str());

tree.setCurrentHead(tree.getRoot());

const int MAX_RETRIES = 5;
if (retry_count >= MAX_RETRIES) {
LOG_F(ERROR, "Exceeded max retries for goal [%s], skipping.",
std::to_string(current_goal_index).c_str());
return false;
}

if (!config.allowed_to_skip_waypoints &&
!connectToGoal(current_goal_index, std::numeric_limits<int>::max())) {
// will always return true (unless it turns into a pseudo-infinite loop)
return RRTIteration(tries, current_goal_index);
// FIX 2: actually recurse with incremented retry_count instead of falling through
return RRTIteration(tries, current_goal_index, retry_count + 1);
} else {
return false;
}
Expand All @@ -129,6 +150,8 @@ bool RRT::RRTIteration(int tries, int current_goal_index) {
bool RRT::epochEvaluation(std::shared_ptr<RRTNode> goal_node,
std::shared_ptr<RRTNode> goal_parent,
int current_goal_index) {
tree.setCurrentHead(tree.getRoot());

// If a single epoch has not been passed, mark this goal as the first
// benchmark.
if (goal_node == nullptr) {
Expand Down Expand Up @@ -606,39 +629,41 @@ std::vector<std::vector<XYZCoord>> generateRankedNewGoalsList(const std::vector<
return ranked_goals;
}

RRTPoint getCurrentLoc(std::shared_ptr<MissionState> state) {
std::shared_ptr<MavlinkClient> mav = state->getMav();
std::pair<double, double> start_lat_long = mav->latlng_deg();

GPSCoord start_gps =
makeGPSCoord(start_lat_long.first, start_lat_long.second, mav->altitude_agl_m());

double angle_correction = (90 - mav->heading_deg()) * M_PI / 180.0;
double start_angle = (angle_correction < 0) ? (angle_correction + 2 * M_PI) : angle_correction;
XYZCoord start_xyz = state->getCartesianConverter().value().toXYZ(start_gps);
return RRTPoint(start_xyz, start_angle);
}

/* TODO - doesn't match compeition spec

1. First waypoint is not home
2. we omit the first waypoint (FATAL) - this means we never tell the computer to hit it
3. We don't know where home location is - we rely on geofencing to not fly out of bounds
*/
MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
// first waypoint is start

// the other waypoitns is the goals
if (state->mission_params.getWaypoints().size() < 1) {
if (state->mission_params.getWaypoints().size() < 2) {
loguru::set_thread_name("Static Pathing");
LOG_F(ERROR, "Not enough waypoints to generate a path, requires >=1, num waypoints: %s",
LOG_F(ERROR, "Not enough waypoints to generate a path, required 2+, existing waypoints: %s",
std::to_string(state->mission_params.getWaypoints().size()).c_str());
return {};
}

std::vector<XYZCoord> goals = state->mission_params.getWaypoints();
std::vector<XYZCoord> goals;

// Copy elements from the second element to the last element of source into
// destination all other methods of copying over crash???
for (int i = 1; i < state->mission_params.getWaypoints().size(); i++) {
goals.emplace_back(state->mission_params.getWaypoints()[i]);
}

// update goals here
if (state->config.pathing.rrt.generate_deviations) {
Environment mapping_bounds(state->mission_params.getAirdropBoundary(), {}, {}, goals, {});
goals = generateRankedNewGoalsList(goals, mapping_bounds)[0];
}

RRTPoint start = getCurrentLoc(state);
double init_angle =
std::atan2(goals.front().y - state->mission_params.getWaypoints().front().y,
goals.front().x - state->mission_params.getWaypoints().front().x);
RRTPoint start(state->mission_params.getWaypoints().front(), init_angle);
start.coord.z = state->config.takeoff.altitude_m;

RRT rrt(start, goals, SEARCH_RADIUS, state->mission_params.getFlightBoundary(), state->config,
Expand All @@ -649,8 +674,10 @@ MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
std::vector<XYZCoord> path = rrt.getPointsToGoal();

std::vector<GPSCoord> output_coords;
for (const XYZCoord &waypoint : path) {
output_coords.push_back(state->getCartesianConverter()->toLatLng(waypoint));
output_coords.push_back(
state->getCartesianConverter()->toLatLng(state->mission_params.getWaypoints().front()));
for (const XYZCoord &wpt : path) {
output_coords.push_back(state->getCartesianConverter()->toLatLng(wpt));
}

return MissionPath(MissionPath::Type::FORWARD, output_coords);
Expand All @@ -659,9 +686,11 @@ MissionPath generateInitialPath(std::shared_ptr<MissionState> state) {
MissionPath generateSearchPath(std::shared_ptr<MissionState> state) {
std::vector<GPSCoord> gps_coords;
if (state->config.pathing.coverage.method == AirdropCoverageMethod::Enum::FORWARD) {
RRTPoint start(state->mission_params.getWaypoints().back(), 0);
double scan_radius = state->config.pathing.coverage.camera_vision_m;
RRTPoint start(state->mission_params.getWaypoints().front(), 0);

// TODO , change the starting point to be something closer to loiter
// region
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);
Expand All @@ -683,7 +712,13 @@ MissionPath generateSearchPath(std::shared_ptr<MissionState> state) {
}

MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const GPSCoord &goal) {
// finds starting location
std::shared_ptr<MavlinkClient> mav = state->getMav();
std::pair<double, double> start_lat_long = {38.315339, -76.548108};

GPSCoord start_gps =
makeGPSCoord(start_lat_long.first, start_lat_long.second, mav->altitude_agl_m());

/*
Note: this function was neutered right before we attempted to fly at the 2024 competition
because we suddenly began running into an infinite loop during the execution of this
Expand All @@ -692,7 +727,10 @@ MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const G
instead of trying to formulate our own path.
*/

RRTPoint start_rrt = getCurrentLoc(state);
double start_angle = 90 - mav->heading_deg();
XYZCoord start_xyz = state->getCartesianConverter().value().toXYZ(start_gps);
RRTPoint start_rrt(start_xyz, start_angle);

// pathing
XYZCoord goal_xyz = state->getCartesianConverter().value().toXYZ(goal);
AirdropApproachPathing airdrop_planner(start_rrt, goal_xyz, mav->wind(),
Expand Down Expand Up @@ -726,4 +764,4 @@ MissionPath generateAirdropApproach(std::shared_ptr<MissionState> state, const G
// gps_path.push_back(goal);

return MissionPath(MissionPath::Type::FORWARD, gps_path);
}
}
Loading