[nav] Document code (#1120)

* Start documenting navigation better. removing some commented out code and dead code. added clang format

* Document environment, minor commenting

* Document search, remove useless comments

* Add gate search documentation

* Add diagram, minor changes

* Add utilities section

* Format, make consistent

jetson/nav/.clang-format

@@ -0,0 +1,66 @@
+# Generated from CLion C/C++ Code Style settings
+BasedOnStyle: LLVM
+AccessModifierOffset: -4
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: None
+AlignOperands: Align
+AllowAllArgumentsOnNextLine: false
+AllowAllConstructorInitializersOnNextLine: false
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: Always
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: Always
+AllowShortLambdasOnASingleLine: All
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterReturnType: None
+AlwaysBreakTemplateDeclarations: Yes
+BreakBeforeBraces: Custom
+BraceWrapping:
+  AfterCaseLabel: false
+  AfterClass: false
+  AfterControlStatement: Never
+  AfterEnum: false
+  AfterFunction: false
+  AfterNamespace: false
+  AfterUnion: false
+  BeforeCatch: false
+  BeforeElse: false
+  IndentBraces: false
+  SplitEmptyFunction: false
+  SplitEmptyRecord: true
+BreakBeforeBinaryOperators: None
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializers: BeforeColon
+BreakInheritanceList: BeforeColon
+ColumnLimit: 0
+CompactNamespaces: false
+ContinuationIndentWidth: 8
+IndentCaseLabels: true
+IndentPPDirectives: None
+IndentWidth: 4
+KeepEmptyLinesAtTheStartOfBlocks: true
+MaxEmptyLinesToKeep: 2
+NamespaceIndentation: All
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PointerAlignment: Left
+ReflowComments: false
+SpaceAfterCStyleCast: true
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: false
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 0
+SpacesInAngles: false
+SpacesInCStyleCastParentheses: false
+SpacesInContainerLiterals: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+TabWidth: 4
+UseTab: Never

jetson/nav/cache.hpp

@@ -1,7 +1,5 @@
 #pragma once
 
-#include <iostream>
-
 /***
  * Readings from the sensor may occasionally have invalid readings.
  * A cache attempts to resolve this by saving old values for a bit
@@ -12,10 +10,10 @@
 template<typename T>
 class Cache {
 public:
-    Cache(int neededHits, int neededMisses, T invalidDefault) :
-            mNeededHits(neededHits),
-            mNeededMisses(neededMisses),
-            mInvalidDefault(invalidDefault) {}
+    Cache(int neededHits, int neededMisses, T invalidDefault)
+        : mNeededHits(neededHits),
+          mNeededMisses(neededMisses),
+          mInvalidDefault(invalidDefault) {}
 
     T get() const {
         return mValid ? mCurrentReading : mInvalidDefault;
@@ -25,7 +23,7 @@ class Cache {
         return mValid;
     }
 
-    void reset(){
+    void reset() {
         mCurrentReading = mInvalidDefault;
         mValid = false;
         mHits = 0;

jetson/nav/courseProgress.cpp

@@ -6,34 +6,34 @@ void CourseProgress::setCourse(Course const& course) {
         mCourse = course;
         clearProgress();
     }
-}
+}// setCourse()
 
 std::deque<Waypoint> const& CourseProgress::getRemainingWaypoints() const {
     return mRemainingWaypoints;
-}
+}// getRemainingWaypoints()
 
 Waypoint const& CourseProgress::getCurrentWaypoint() const {
     return mRemainingWaypoints.front();
-}
+}// getCurrentWaypoint()
 
 Waypoint CourseProgress::completeCurrentWaypoint() {
     Waypoint waypoint = mRemainingWaypoints.front();
     mRemainingWaypoints.pop_front();
     return waypoint;
-}
+}// completeCurrentWaypoint()
 
 void CourseProgress::clearProgress() {
     mRemainingWaypoints.assign(mCourse.waypoints.begin(), mCourse.waypoints.end());
-}
+}// clearProgress()
 
 Course const& CourseProgress::getCourse() const {
     return mCourse;
-}
+}// getCourse()
 
 int32_t CourseProgress::getCompletedWaypointCount() const {
     return static_cast<int32_t>(mCourse.num_waypoints - mRemainingWaypoints.size());
-}
+}// getCompletedWaypointCount()
 
 Waypoint const& CourseProgress::getLastCompletedWaypoint() const {
     return mCourse.waypoints.at(getCompletedWaypointCount() - 1);
-}
+}// getLastCompletedWaypoint()

jetson/nav/courseProgress.hpp

@@ -1,7 +1,7 @@
 #pragma once
 
-#include <deque>
 #include "rover_msgs/Course.hpp"
+#include <deque>
 
 
 using namespace rover_msgs;

jetson/nav/environment.cpp

@@ -1,20 +1,19 @@
-#include <cmath>
+#include "environment.hpp"
 
 #include "utilities.hpp"
-#include "environment.hpp"
 
-Environment::Environment(const rapidjson::Document& config) :
-        mConfig(config),
-        mTargetLeft(mConfig["navThresholds"]["cacheHitMax"].GetInt(), mConfig["navThresholds"]["cacheMissMax"].GetInt(), {-1, -1, -1}),
-        mTargetRight(mConfig["navThresholds"]["cacheHitMax"].GetInt(), mConfig["navThresholds"]["cacheMissMax"].GetInt(), {-1, -1, -1}),
-        mPostOneLat(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
-        mPostOneLong(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
-        mPostTwoLat(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
-        mPostTwoLong(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
-        mLeftTargetBearing(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
-        mLeftTargetDistance(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
-        mRightTargetBearing(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
-        mRightTargetDistance(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()) {}
+Environment::Environment(const rapidjson::Document& config)
+    : mConfig(config),
+      mTargetLeft(mConfig["navThresholds"]["cacheHitMax"].GetInt(), mConfig["navThresholds"]["cacheMissMax"].GetInt(), {-1, -1, -1}),
+      mTargetRight(mConfig["navThresholds"]["cacheHitMax"].GetInt(), mConfig["navThresholds"]["cacheMissMax"].GetInt(), {-1, -1, -1}),
+      mPostOneLat(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
+      mPostOneLong(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
+      mPostTwoLat(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
+      mPostTwoLong(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
+      mLeftTargetBearing(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
+      mLeftTargetDistance(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
+      mRightTargetBearing(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()),
+      mRightTargetDistance(mConfig["gate"]["filterSize"].GetInt(), mConfig["gate"]["filterProportion"].GetDouble()) {}
 
 /***
  * @param targets New target data from perception to update our filters.
@@ -25,24 +24,21 @@ void Environment::setTargets(TargetList const& targets) {
     mTargetLeft.put((leftTargetRaw.distance != -1 && leftTargetRaw.id != -1), leftTargetRaw);
     mTargetRight.put((rightTargetRaw.distance != -1 && rightTargetRaw.id != -1), rightTargetRaw);
 
-    if (mTargetLeft.isValid()){
+    if (mTargetLeft.isValid()) {
         mLeftTargetBearing.push(mTargetLeft.get().bearing);
         mLeftTargetDistance.push(mTargetLeft.get().distance);
-    }
-    else{
+    } else {
         mLeftTargetDistance.reset();
         mLeftTargetBearing.reset();
     }
-    if (mTargetRight.isValid()){
+    if (mTargetRight.isValid()) {
         mRightTargetBearing.push(mTargetRight.get().bearing);
         mRightTargetDistance.push(mTargetRight.get().distance);
-    }
-    else{
+    } else {
         mRightTargetDistance.reset();
         mRightTargetBearing.reset();
     }
-
-}
+}// setTargets()
 
 /***
  * Update our estimate for where the post is with our current filtered values.
@@ -95,64 +91,64 @@ void Environment::updatePost(std::shared_ptr<Rover> const& rover, std::shared_pt
         double cosine = cos(degreeToRadian(rover->odometry().latitude_deg, rover->odometry().latitude_min));
         rover->setLongMeterInMinutes(60 / (EARTH_CIRCUM * cosine / 360));
     }
-}
+}// updatePost()
 
 void Environment::setObstacle(Obstacle const& obstacle) {
     mObstacle = obstacle;
-}
+}// setObstacle()
 
 Obstacle Environment::getObstacle() {
     return mObstacle;
-}
+}// getObstacle()
 
 Target Environment::getLeftTarget() const {
     return {mLeftTargetDistance.get(), mLeftTargetBearing.get(), mTargetLeft.get().id};//mTargetLeft.get();
-}
+}// getLeftTarget()
 
 Target Environment::getRightTarget() const {
     return {mRightTargetDistance.get(), mRightTargetBearing.get(), mTargetRight.get().id};//mTargetRight.get();
-}
+}// getRightTarget()
 
 void Environment::setBaseGateID(int baseGateId) {
     mBaseGateId = baseGateId;
-}
+}// setBaseGateID()
 
 bool Environment::hasNewPostUpdate() const {
     return mHasNewPostUpdate;
-}
+}// hasNewPostUpdate()
 
 bool Environment::hasGateLocation() const {
     return hasPostOneLocation() && hasPostTwoLocation();
-}
+}// hasGateLocation()
 
 bool Environment::hasPostOneLocation() const {
     return mPostOneLat.ready();
-}
+}// hasPostOneLocation()
 
 bool Environment::hasPostTwoLocation() const {
     return mPostTwoLat.ready();
-}
+}// hasPostTwoLocation()
 
-void Environment::setShouldLookForGate(bool gate){
+void Environment::setShouldLookForGate(bool gate) {
     mLookForGate = gate;
-}
+}// setShouldLookForGate()
 
 Odometry Environment::getPostOneLocation() const {
     return createOdom(mPostOneLat.get(), mPostOneLong.get());
-}
+}// getPostOneLocation()
 
 Odometry Environment::getPostTwoLocation() const {
     return createOdom(mPostTwoLat.get(), mPostTwoLong.get());
-}
+}// getPostTwoLocation()
 
 // Offset of the post in our linearized cartesian space.
 Vector2d Environment::getPostOneOffsetInCartesian(Odometry cur) const {
     return getOffsetInCartesian(cur, getPostOneLocation());
-}
+}// getPostOneOffsetInCartesian()
 
 Vector2d Environment::getPostTwoOffsetInCartesian(Odometry cur) const {
     return getOffsetInCartesian(cur, getPostTwoLocation());
-}
+}// getPostTwoOffsetInCartesian()
 
 std::optional<Target> Environment::tryGetTargetWithId(int32_t id) const {
     if (mTargetLeft.get().id == id && mTargetLeft.get().distance > 0.0) {
@@ -161,4 +157,4 @@ std::optional<Target> Environment::tryGetTargetWithId(int32_t id) const {
         return {mTargetRight.get()};
     }
     return std::nullopt;
-}
+}// tryGetTargetWithId()

jetson/nav/environment.hpp

@@ -1,13 +1,13 @@
 #pragma once
 
-#include <vector>
+#include <memory>
 #include <optional>
 
 #include <eigen3/Eigen/Core>
 
-#include "rover.hpp"
-#include "filter.hpp"
 #include "cache.hpp"
+#include "filter.hpp"
+#include "rover.hpp"
 #include "rover_msgs/Obstacle.hpp"
 #include "rover_msgs/TargetList.hpp"
 

jetson/nav/filter.hpp

@@ -1,8 +1,9 @@
 #pragma once
 
+#include <algorithm>
 #include <array>
 #include <numeric>
-#include <algorithm>
+#include <vector>
 
 /***
  * A filter that combines multiple readings into one.
@@ -37,46 +38,46 @@ class Filter {
         mFilterCount = std::min(mFilterCount + 1, size());
         mSortedValues.assign(mValues.begin(), mValues.end());
         std::sort(mSortedValues.begin(), mSortedValues.end());
-    }
+    }// push()
 
     void reset() {
         mFilterCount = 0;
-    }
+    }// reset()
 
     //if we have values decrease count by 1
     void decrementCount() {
         mFilterCount = std::max(mFilterCount - 1, size_t{});
-    }
+    }// decrementCount()
 
     [[nodiscard]] size_t size() const {
         return mValues.size();
-    }
+    }// size()
 
     [[nodiscard]] size_t filterCount() const {
         return mFilterCount;
-    }
+    }// filterCount()
 
     /***
      * @return If we have enough readings to use the filter
      */
     [[nodiscard]] bool ready() const {
         return mFilterCount > 0;
-    }
+    }// ready()
 
     [[nodiscard]] bool full() const {
         return mFilterCount == size();
-    }
+    }// full()
 
     /***
      * @return Filtered reading if full, or else the most recent reading if we don't have enough readings yet.
      */
     [[nodiscard]] T get() const {
-//        return mValues[mHead];
+        //        return mValues[mHead];
         if (!full()) {
             return mValues[mHead];
         }
         auto begin = mSortedValues.begin() + (mProportion * size() / 4);
         auto end = mSortedValues.end() - (mProportion * size() / 4);
         return std::accumulate(begin, end, T{}) / (end - begin);
-    }
+    }// get()
 };