Gate removal from navigation (#592)

* Deletion changes

* Last gate deletes

* Fix formatting

---------

Co-authored-by: Prabhleen Pawar <[email protected]>

config/navigation.yaml

@@ -15,12 +15,6 @@ drive:
     driving_p: 10.0
   lookahead_distance: 1.0
 
-gate:
-  stop_thresh: 0.2
-  drive_fwd_thresh: 0.34
-  approach_distance: 2.0
-  post_radius: 0.7
-
 search:
   stop_thresh: 0.5
   drive_fwd_thresh: 0.34

msg/WaypointType.msg

@@ -1,4 +1,3 @@
 int32 NO_SEARCH = 0
 int32 POST = 1
-int32 GATE = 2
 int32 val

src/navigation/context.py

@@ -26,26 +26,6 @@
 
 tf_broadcaster: tf2_ros.StaticTransformBroadcaster = tf2_ros.StaticTransformBroadcaster()
 
-POST_RADIUS = get_rosparam("gate/post_radius", 0.7)
-
-
-@dataclass
-class Gate:
-    post1: np.ndarray
-    post2: np.ndarray
-
-    def get_post_shapes(self) -> tuple[Point, Point]:
-        """
-        Creates a circular path of RADIUS around each post for checking intersection with our path
-        :return: tuple of the two shapely Point objects representing the posts
-        """
-
-        # Find circle of both posts
-        post1_shape = Point(self.post1[:2]).buffer(POST_RADIUS)
-        post2_shape = Point(self.post2[:2]).buffer(POST_RADIUS)
-
-        return post1_shape, post2_shape
-
 
 @dataclass
 class Rover:
@@ -123,36 +103,6 @@ def current_fid_pos(self, odom_override: bool = True) -> Optional[np.ndarray]:
 
         return self.get_fid_pos(current_waypoint.fiducial_id, in_odom)
 
-    def other_gate_fid_pos(self) -> Optional[np.ndarray]:
-        """
-        retrieves the position of the other gate post (which is 1 + current id) if we are looking for a gate
-        """
-        assert self.ctx.course
-        current_waypoint = self.ctx.course.current_waypoint()
-        if self.ctx.course.look_for_gate() and current_waypoint is not None:
-            return self.get_fid_pos(current_waypoint.fiducial_id + 1, self.ctx.use_odom)
-        else:
-            return None
-
-    def current_gate(self, odom_override: bool = True) -> Optional[Gate]:
-        """
-        retrieves the position of the gate (if we know where it is, and we are looking for one)
-        :param: odom_override if false will force it to be in the map frame, true will mean use odom if we are using it (set by rosparam)
-        """
-
-        if self.ctx.course:
-            current_waypoint = self.ctx.course.current_waypoint()
-            if current_waypoint is None or not self.ctx.course.look_for_gate():
-                return None
-
-            post1 = self.get_fid_pos(current_waypoint.fiducial_id, self.ctx.use_odom and odom_override)
-            post2 = self.get_fid_pos(current_waypoint.fiducial_id + 1, self.ctx.use_odom and odom_override)
-            if post1 is None or post2 is None:
-                return None
-            return Gate(post1[:2], post2[:2])
-        else:
-            return None
-
 
 @dataclass
 class Course:
@@ -188,17 +138,6 @@ def current_waypoint(self) -> Optional[mrover.msg.Waypoint]:
             return None
         return self.course_data.waypoints[self.waypoint_index]
 
-    def look_for_gate(self) -> bool:
-        """
-        Returns whether the currently active waypoint (if it exists) indicates
-        that we should go to a gate
-        """
-        waypoint = self.current_waypoint()
-        if waypoint is not None:
-            return waypoint.type.val == mrover.msg.WaypointType.GATE
-        else:
-            return False
-
     def look_for_post(self) -> bool:
         """
         Returns whether the currently active waypoint (if it exists) indicates
@@ -261,8 +200,6 @@ class Context:
     tf_listener: tf2_ros.TransformListener
     vel_cmd_publisher: rospy.Publisher
     search_point_publisher: rospy.Publisher
-    gate_point_publisher: rospy.Publisher
-    gate_path_publisher: rospy.Publisher
     vis_publisher: rospy.Publisher
     course_listener: rospy.Subscriber
     stuck_listener: rospy.Subscriber
@@ -285,8 +222,6 @@ def __init__(self):
         self.vel_cmd_publisher = rospy.Publisher("cmd_vel", Twist, queue_size=1)
         self.vis_publisher = rospy.Publisher("nav_vis", Marker, queue_size=1)
         self.search_point_publisher = rospy.Publisher("search_path", GPSPointList, queue_size=1)
-        self.gate_path_publisher = rospy.Publisher("gate_path", GPSPointList, queue_size=1)
-        self.gate_point_publisher = rospy.Publisher("estimated_gate_location", GPSPointList, queue_size=1)
         self.enable_auton_service = rospy.Service("enable_auton", mrover.srv.PublishEnableAuton, self.recv_enable_auton)
         self.stuck_listener = rospy.Subscriber("nav_stuck", Bool, self.stuck_callback)
         self.course = None