From b68fa60fb3b6c7178a4243435cd4f16e5e42260b Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Thu, 4 Apr 2024 02:12:16 -0400 Subject: [PATCH] fix overshooting in long range state --- src/navigation/context.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/navigation/context.py b/src/navigation/context.py index 8a05e3219..110451a1e 100644 --- a/src/navigation/context.py +++ b/src/navigation/context.py @@ -120,7 +120,7 @@ def current_target_pos(self, odom_override: bool = True) -> Optional[np.ndarray] return self.get_target_pos(f"fiducial{current_waypoint.tag_id}", in_odom) elif current_waypoint.type.val == WaypointType.MALLET: return self.get_target_pos("hammer", in_odom) - elif current_waypoint.type == WaypointType.WATER_BOTTLE: + elif current_waypoint.type.val == WaypointType.WATER_BOTTLE: return self.get_target_pos("bottle", in_odom) else: return None @@ -151,13 +151,13 @@ def __init__(self, ctx: Context, min_hits: int = MIN_HITS, max_hits: int = MAX_H 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 include it, we will increment our hit count for that tag id, store the new tag information, 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 """ + tags_ids = [tag.id for tag in tags] 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: cur_tag.hit_count -= DECREMENT_WEIGHT if cur_tag.hit_count <= 0: @@ -171,6 +171,9 @@ def push_frame(self, tags: List[LongRangeTag]) -> None: for tag in tags: if tag.id not in self.__data: self.__data[tag.id] = self.TagData(hit_count=INCREMENT_WEIGHT, tag=tag, time=rospy.get_time()) + else: + self.__data[tag.id].tag = tag + self.__data[tag.id].time = rospy.get_time() def get(self, tag_id: int) -> Optional[LongRangeTag]: """