add docstrings and fix get_approach_target_state

src/navigation/context.py

@@ -106,7 +106,7 @@ def get_target_pos(self, id: str, in_odom_frame: bool = True) -> Optional[np.nda
     def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray]:
         """
         Retrieves the position of the current fiducial or object (and we are looking for it)
-        :param: odom_override if false will force it to be in the map frame
+        :param odom_override: if false will force it to be in the map frame
         """
         assert self.ctx.course
         in_odom = self.ctx.use_odom and odom_override
@@ -126,6 +126,10 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray]
 
 
 class LongRangeTagStore:
+    """
+    Context class to represent the tags seen in the long range camera
+    """
+
     @dataclass
     class TagData:
         hit_count: int
@@ -144,6 +148,13 @@ def __init__(self, ctx: Context, min_hits: int = MIN_HITS, max_hits: int = MAX_H
         self.max_hits = max_hits
 
     def push_frame(self, tags: List[LongRangeTag]) -> None:
+        """
+        Loops through our current list of our stored tags and checks if the new message includes each tag or doesn't. 
+        If it does include it, we will increment our hit count for that tag id and reset the time we saw it.
+        If it does not include it, we will decrement our hit count for that tag id, and if the hit count becomes zero, then we remove it from our stored list.
+        If there are tag ids in the new message that we don't have stored, we will add it to our stored list.
+        :param tags: a list of LongRangeTags sent by perception, which includes an id and bearing for each tag in the list
+        """
         for _, cur_tag in list(self.__data.items()):
             tags_ids = [tag.id for tag in tags]
             if cur_tag.tag.id not in tags_ids:
@@ -161,6 +172,11 @@ def push_frame(self, tags: List[LongRangeTag]) -> None:
                 self.__data[tag.id] = self.TagData(hit_count=INCREMENT_WEIGHT, tag=tag, time=rospy.get_time())
 
     def get(self, tag_id: int) -> Optional[LongRangeTag]:
+        """
+        Returns the corresponding tag if the tag has been seen by the long range camera enough times recently
+        :param tag_id: id corresponding to the tag we want to return
+        :return: LongRangeTag if we have seen the tag enough times recently in the long range camera, otherwise return None
+        """
         if len(self.__data) == 0:
             return None
         if tag_id not in self.__data:
@@ -234,7 +250,7 @@ def look_for_object(self) -> bool:
     def is_complete(self) -> bool:
         return self.waypoint_index == len(self.course_data.waypoints)
 
-    def check_approach(self) -> Optional[State]:
+    def get_approach_target_state(self) -> Optional[State]:
         """
         Returns one of the approach states (ApproachPostState, LongRangeState, or ApproachObjectState)
         if we are looking for a post or object and we see it in one of the cameras (ZED or long range)

src/navigation/long_range.py

@@ -50,9 +50,8 @@ def get_target_pos(self, context) -> Optional[np.ndarray]:
 
         direction_to_tag = bearing_rotation_mat @ rover_direction[:2]
 
-        direction_to_tag = normalized(direction_to_tag)
         distance = DIST_AHEAD
-        direction_to_tag = np.array([direction_to_tag[0], direction_to_tag[1], 0.0])
+        direction_to_tag = np.hstack((direction_to_tag, [0.0]))
         tag_position = rover_position + direction_to_tag * distance
         return tag_position
 

src/navigation/search.py

@@ -149,7 +149,8 @@ def on_loop(self, context) -> State:
         context.rover.send_drive_command(cmd_vel)
 
         # returns either ApproachPostState, LongRangeState, ApproachObjectState, or None
-        if context.course.check_approach() is not None:
-            return context.course.check_approach()
+        approach_state = context.course.get_approach_target_state()
+        if approach_state is not None:
+            return approach_state
 
         return self

src/navigation/waypoint.py

@@ -35,8 +35,9 @@ def on_loop(self, context) -> State:
             return post_backup.PostBackupState()
 
         # returns either ApproachPostState, LongRangeState, ApproachObjectState, or None
-        if context.course.check_approach() is not None:
-            return context.course.check_approach()
+        approach_state = context.course.get_approach_target_state()
+        if approach_state is not None:
+            return approach_state
 
         # Attempt to find the waypoint in the TF tree and drive to it
         try:
@@ -51,7 +52,7 @@ def on_loop(self, context) -> State:
                 if not context.course.look_for_post() and not context.course.look_for_object():
                     # We finished a regular waypoint, go onto the next one
                     context.course.increment_waypoint()
-                elif context.course.look_for_post() or context.course.look_for_object():
+                else:
                     # We finished a waypoint associated with a post or mallet, but we have not seen it yet.
                     return search.SearchState()