Added a Deviation and Segment Cost Objective to Incentivize Tacking

[ethanhu912]

Description

• Resolves PATH: add a tacking objective #930
• Added two objectives, DeviationObjective and SegmentCountObjective:
  • DeviationObjective penalizes a path the further it gets from the line connecting the start position and goal position. This avoids paths that deviate extremely far from the start-end line with only one or two tacks.
  • SegmentCountObjective penalizes a path for each segment that exists in the path. That means that in situations where the boat would need to tack, the boat does not tack excessively as well.
  • Ideally, a balance can be achieved where the boat is not tacking excessively but also not deviating extremely far from the direct path towards the goal.
• Added unit tests for DeviationObjective and a cost_diagnostic script to determine the costs for various sample paths (2 tacks vs. 6 tacks, etc.)

Verification

• Unit tests passed.
• Cost_diagnostic script returns the lowest cost for our ideal path (6 tacks)