Clean up math by precomputing the conditionals

[declan34]

Add pre-computing and naming the results of the code segments like the one below. For example, instead of tags[0].angle < 0 and tags[1].angle < 0 pre-compute the value and use something like this.heading == "south_west".

from: core/states/approaching_gate.py

if len(tags) == 2 and self.gate_detection_attempts >= 5 and leg_type == "GATE":
  self.logger.info("Gate detected, beginning navigation")
  
  # compute the angle across from the gate
  # depending where the rover is facing, this is computed differently
  if tags[0].angle < 0 and tags[1].angle < 0:  # both tags on the right
      larger = min(tags[0].angle, tags[1].angle)
      smaller = max(tags[0].angle, tags[1].angle)
      combinedAngle = abs(larger) - abs(smaller)
  elif tags[0].angle >= 0 and tags[1].angle >= 0:  # both tags on the left
      larger = max(tags[0].angle, tags[1].angle)
      smaller = min(tags[0].angle, tags[1].angle)
      combinedAngle = larger - smaller
  else:  # one tag on left, one on right
      combinedAngle = abs(tags[0].angle) + abs(tags[1].angle)

Originally posted by @thomas-gleiforst in MissouriMRDT/Autonomy_Software#102 (comment)

[RexBerry]

Would reducing the number of conditionals be desirable? I believe we could remove a significant amount of branching in core/states/approaching_gate.py.

combinedAngle could be computed as:

higher = max(tags[0].angle, tags[1].angle)
lower = min(tags[0].angle, tags[1].angle)
combinedAngle = higher - lower

and angleToMidpoint as:

angleToMidpoint = (interfaces.nav_board.heading() + (tags[0].angle + tags[1].angle) / 2) % 360

Though additional conditionals might be necessary to ensure that angleToMidpoint isn't in the opposite direction of what is desired. I have not yet looked at other areas that could be refactored.

[Byrdman32]

@RexBerry That is one of the goals in this project. We want to make it so that the code is more readable and more functional. If you think this method that you mentioned above is the way we should go make a branch off of dev and you can start working on the project. Just make sure that in all your commit messages you reference this issue number so we can easily track progress.

For the branch name I would recommend feature/conditionals_cleanup

Example Commit Message:
git commit -m "Cleaning up the branching in approaching_gate.py #105"

[Byrdman32]

@RexBerry I was reminded that a project we were working on at the end of last year ended up getting rid of a lot of the conditionals that you started cleaning up yesterday. We haven't completely finished that project which is why it hasn't been merged to dev yet.

Before doing much more on this project I would advice merging feature/gate_search into your branch. Since the changes it will make will get rid of a fair bit of your work.

If you are in your branch run git merge feature/gate_search to merge the code from gate search into your branch.

[Byrdman32]

@RexBerry Make sure when you are making commits that you reference this issue number. I have fixed the commit messages that you have already made.

[autc7]

Hey guys, I'm having a bit of trouble understanding all the eedits being made here, where can I go to edit the code?

[RexBerry]

@autc7 My changes are on the feature/conditionals_cleanup branch.

[copper-head]

To clarify this issue, there is a new gate_approach.py file located in the feature/gate_search branch. Before any work is continued that file needs to effectively replace the gate_approach file currently used in all other branches as the new algorithm in that file was much more reliable when tested. That being said it still needs to be tested some to be prudent.

Next Steps to resolve this issue:

1. Test the algorithm in branch feature/gate_search to make sure that it is working. Try using a number of approach routes to get a solid test distribution.
2. Compare that code with the current approaching_gate.py code in dev branches and look for differences that could potentially cause issues.

Before merging to a main branch we will also need to modify the code to move the two algorithms that are computing the gps points to the algorithms folder, instead of having the a functions defined within approaching_gate.py itself. The functions that need moved are at the bottom of the approaching_gate.py file (find_gate_path and camera_to_gps_coordinate). Once the above to things have been completed I'll outline the next steps.

If you want to ask any questions, or have me go through with you and explain the code for better understanding, feel free to reach out to me on discord. My name is Duncan Truitt in RoveSoDiscord.

[Byrdman32]

@copper-head the feature/gate_search branch was merged into the branch for this issue.