publish gate position in map frame (#506)

umrover · May 23, 2023 · 405851c · 405851c
1 parent f3314c8
commit 405851c
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 9 deletions.
diff --git a/src/navigation/context.py b/src/navigation/context.py
@@ -136,18 +136,19 @@ def other_gate_fid_pos(self) -> Optional[np.ndarray]:
         else:
             return None
 
-    def current_gate(self) -> Optional[Gate]:
+    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)
-            post2 = self.get_fid_pos(current_waypoint.fiducial_id + 1, self.ctx.use_odom)
+            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])

diff --git a/src/navigation/gate.py b/src/navigation/gate.py
@@ -271,11 +271,10 @@ def evaluate(self, ud):
             self.context.rover.previous_state = GateTraverseStateTransitions.continue_gate_traverse.name  # type: ignore
             return GateTraverseStateTransitions.recovery_state.name  # type: ignore
 
-        # self.context.gate_path_publisher.publish(
-        #     GPSPointList([convert_cartesian_to_gps(pt) for pt in self.traj.coordinates])
-        # )
-        self.context.gate_point_publisher.publish(
-            GPSPointList([convert_cartesian_to_gps(p) for p in [gate.post1, gate.post2]])
-        )
+        map_gate = self.context.env.current_gate(odom_override=False)
+        if map_gate is not None:
+            self.context.gate_point_publisher.publish(
+                GPSPointList([convert_cartesian_to_gps(p) for p in [map_gate.post1, map_gate.post2]])
+            )
         self.context.rover.send_drive_command(cmd_vel)
         return GateTraverseStateTransitions.continue_gate_traverse.name  # type: ignore