Fix Navigation shutdown behavior (#627)

* Edit waypoint msg files, fix nav shutdown behavior, use threading.Event, rename some things to tag

* Formatting

* Update course publisher

* Fix integration test

* Format

* Fix ordering

msg/GPSWaypoint.msg

@@ -1,4 +1,4 @@
+uint8 tag_id
 float64 latitude_degrees
 float64 longitude_degrees
-WaypointType type
-int32 id
+WaypointType type

msg/Waypoint.msg

@@ -1,3 +1,3 @@
-int32 fiducial_id
+uint8 tag_id
 string tf_id
 WaypointType type

scripts/debug_course_publisher.py

@@ -11,24 +11,26 @@
 """
 The purpose of this file is for testing courses in the sim 
 without needing the auton GUI. You can add waypoints with
-and without fiducials and these will get published to nav
+and without tags and these will get published to nav
 """
 
 
 if __name__ == "__main__":
     rospy.init_node("debug_course_publisher")
 
     waypoints = [
-        # ("course1", -2, -6, 0),
-        # ("course1", -3, -3, -1),
-        # ("course2", -5, -5, 0)
         (
-            Waypoint(fiducial_id=0, tf_id="course0", type=WaypointType(val=WaypointType.NO_SEARCH)),
-            # SE3(position=np.array([-844.37, 10351.56, 0])),
-            SE3(position=np.array([-8, -8, 0])),
+            Waypoint(tag_id=2, tf_id="course0", type=WaypointType(val=WaypointType.NO_SEARCH)),
+            SE3(position=np.array([4, 4, 0])),
+        ),
+        (
+            Waypoint(tag_id=0, tf_id="course1", type=WaypointType(val=WaypointType.POST)),
+            SE3(position=np.array([-2, -2, 0])),
+        ),
+        (
+            Waypoint(tag_id=1, tf_id="course2", type=WaypointType(val=WaypointType.POST)),
+            SE3(position=np.array([11, -10, 0])),
         ),
-        # (Waypoint(fiducial_id=0, tf_id="course1"), SE3(position=np.array([-3, -3, -1]))),
-        # (Waypoint(fiducial_id=0, tf_id="course2"), SE3(position=np.array([-5, -5, 0]))),
     ]
 
     publish_waypoints([convert_waypoint_to_gps(waypoint) for waypoint in waypoints])

src/navigation/context.py

@@ -100,7 +100,7 @@ def current_fid_pos(self, odom_override: bool = True) -> Optional[np.ndarray]:
             print("CURRENT WAYPOINT IS NONE")
             return None
 
-        return self.get_fid_pos(current_waypoint.fiducial_id, in_odom)
+        return self.get_fid_pos(current_waypoint.tag_id, in_odom)
 
 
 @dataclass
@@ -173,10 +173,10 @@ def convert_gps_to_cartesian(waypoint: GPSWaypoint) -> Tuple[Waypoint, SE3]:
     )
     # zero the z-coordinate of the odom because even though the altitudes are set to zero,
     # two points on a sphere are not going to have the same z coordinate
-    # navigation algorithmns currently require all coordinates to have zero as the z coordinate
+    # navigation algorithms currently require all coordinates to have zero as the z coordinate
     odom[2] = 0
 
-    return Waypoint(fiducial_id=waypoint.id, tf_id=f"course{waypoint.id}", type=waypoint.type), SE3(position=odom)
+    return Waypoint(tag_id=waypoint.tag_id, tf_id=f"course{waypoint.tag_id}", type=waypoint.type), SE3(position=odom)
 
 
 def convert_cartesian_to_gps(coordinate: np.ndarray) -> GPSWaypoint:

src/navigation/navigation.py

@@ -1,7 +1,5 @@
 #!/usr/bin/env python3
 
-import signal
-import sys
 import threading
 
 import rospy
@@ -50,6 +48,7 @@ def run(self):
     def stop(self):
         # Requests current state to go into 'terminated' to cleanly exit state machine
         self.state_machine.stop()
+        self.state_machine_server.stop()
         self.join()
         self.state_machine.context.rover.send_drive_stop()
 
@@ -59,18 +58,7 @@ def main():
     rospy.init_node("navigation")
     context = Context()
     navigation = Navigation(context)
-
-    # Define custom handler for Ctrl-C that shuts down smach properly
-    def sigint_handler(_sig, _frame):
-        navigation.stop()
-        rospy.signal_shutdown("keyboard interrupt")
-        try:
-            sys.exit(0)
-        except SystemExit:
-            # TODO: not really sure why needed but it is bugging me! >:(
-            pass
-
-    signal.signal(signal.SIGINT, sigint_handler)
+    rospy.on_shutdown(navigation.stop)
     navigation.start()
 
 

src/navigation/search.py

@@ -61,7 +61,7 @@ def spiral_traj(
         :param coverage_radius:     radius of the spiral search pattern (float)
         :param distance_between_spirals:    distance between each spiral (float)
         :param segments_per_rotation:     number of segments per spiral (int), for example, 4 segments per rotation would be a square spiral, 8 segments per rotation would be an octagonal spiral
-        :param fid_id:      fiducial id to associate with this trajectory (int)
+        :param fid_id:      tag id to associate with this trajectory (int)
         :return:    SearchTrajectory object
         """
         zero_centered_spiral_r2 = cls.gen_spiral_coordinates(
@@ -99,7 +99,7 @@ def on_enter(self, context) -> None:
                 self.SPIRAL_COVERAGE_RADIUS,
                 self.DISTANCE_BETWEEN_SPIRALS,
                 self.SEGMENTS_PER_ROTATION,
-                search_center.fiducial_id,
+                search_center.tag_id,
             )
             self.prev_target = None
 

src/navigation/waypoint.py

@@ -20,7 +20,7 @@ def on_exit(self, context) -> None:
     def on_loop(self, context) -> State:
         """
         Handle driving to a waypoint defined by a linearized cartesian position.
-        If the waypoint is associated with a fiducial id, go into that state early if we see it,
+        If the waypoint is associated with a tag id, go into that state early if we see it,
         otherwise wait until we get there to conduct a more thorough search.
         :param ud:  State machine user data
         :return:    Next state
@@ -52,7 +52,7 @@ def on_loop(self, context) -> State:
                     # We finished a regular waypoint, go onto the next one
                     context.course.increment_waypoint()
                 else:
-                    # We finished a waypoint associated with a fiducial id, but we have not seen it yet.
+                    # We finished a waypoint associated with a tag id, but we have not seen it yet.
                     return search.SearchState()
 
             if context.rover.stuck:

src/util/course_publish_helpers.py

@@ -1,16 +1,16 @@
+from typing import List, Tuple
+
+import pymap3d
 import rospy
-from mrover.srv import PublishEnableAuton
 from mrover.msg import EnableAuton, GPSWaypoint, Waypoint
-from typing import Tuple
+from mrover.srv import PublishEnableAuton
 from util.SE3 import SE3
-import pymap3d
-
 
 REF_LAT = rospy.get_param("gps_linearization/reference_point_latitude")
 REF_LON = rospy.get_param("gps_linearization/reference_point_longitude")
 
 
-def publish_waypoints(waypoints):
+def publish_waypoints(waypoints: List[GPSWaypoint]):
     rospy.wait_for_service("enable_auton")
     try:
         publish_enable = rospy.ServiceProxy("enable_auton", PublishEnableAuton)
@@ -23,4 +23,4 @@ def publish_waypoints(waypoints):
 def convert_waypoint_to_gps(waypoint_pose_pair: Tuple[Waypoint, SE3]) -> GPSWaypoint:
     waypoint, pose = waypoint_pose_pair
     lat, lon, _ = pymap3d.enu2geodetic(pose.position[0], pose.position[1], pose.position[2], REF_LAT, REF_LON, 0.0)
-    return GPSWaypoint(lat, lon, waypoint.type, waypoint.fiducial_id)
+    return GPSWaypoint(waypoint.tag_id, lat, lon, waypoint.type)

src/util/state_lib/state_machine.py

@@ -2,7 +2,7 @@
 from collections import defaultdict
 from dataclasses import dataclass
 from enum import Enum
-from threading import Lock
+from threading import Lock, Event
 from typing import DefaultDict, Set, List, Callable, Any, Optional
 
 from util.state_lib.state import State, ExitState
@@ -32,8 +32,7 @@ class StateMachine:
     off_state: Optional[State]
     log_level: LogLevel
     logger: Callable[[str], None]
-    on: bool
-    onLock: Lock
+    stop_event: Event
 
     def __init__(
         self,
@@ -53,8 +52,7 @@ def __init__(
         self.off_state = None
         self.log_level = log_level
         self.logger = logger
-        self.on = True
-        self.onLock = Lock()
+        self.stop_event = Event()
 
     def __update(self):
         with self.state_lock:
@@ -77,8 +75,7 @@ def __update(self):
                 self.current_state.on_enter(self.context)
 
     def stop(self):
-        with self.onLock:
-            self.on = False
+        self.stop_event.set()
 
     def run(self, update_rate: float = float("inf"), warning_handle: Callable = print):
         """
@@ -88,15 +85,12 @@ def run(self, update_rate: float = float("inf"), warning_handle: Callable = prin
         """
         target_loop_time = None if update_rate == float("inf") else (1.0 / update_rate)
         self.current_state.on_enter(self.context)
-        is_on = True
 
-        while is_on:
+        while not self.stop_event.is_set():
             start = time.time()
             self.__update()
             if isinstance(self.current_state, ExitState):
                 break
-            with self.onLock:
-                is_on = self.on
             elapsed_time = time.time() - start
             if target_loop_time is not None and elapsed_time < target_loop_time:
                 time.sleep(target_loop_time - elapsed_time)

src/util/state_lib/state_publisher_server.py

@@ -16,8 +16,7 @@ class StatePublisher:
     state_machine: StateMachine
     __structure_thread: threading.Thread
     __state_thread: threading.Thread
-    __stop_lock: threading.Lock
-    __stop: bool
+    __stop_event: threading.Event
 
     def __init__(
         self,
@@ -30,7 +29,7 @@ def __init__(
         self.state_machine = state_machine
         self.structure_publisher = rospy.Publisher(structure_pub_topic, StateMachineStructure, queue_size=1)
         self.state_publisher = rospy.Publisher(state_pub_topic, StateMachineStateUpdate, queue_size=1)
-        self.__stop_lock = threading.Lock()
+        self.__stop_event = threading.Event()
         self.__structure_thread = threading.Thread(
             target=self.run_at_interval, args=(self.publish_structure, structure_update_rate_hz)
         )
@@ -42,8 +41,7 @@ def __init__(
         self.__state_thread.start()
 
     def stop(self) -> None:
-        with self.__stop_lock:
-            self.__stop = True
+        self.__stop_event.set()
 
     def publish_structure(self) -> None:
         structure = StateMachineStructure()
@@ -67,9 +65,8 @@ def run_at_interval(self, func: Callable[[], None], update_hz: float):
         desired_loop_time = 1.0 / update_hz
         while True:
             start_time = time.time()
-            with self.__stop_lock:
-                if self.__stop:
-                    break
+            if self.__stop_event.is_set():
+                break
             func()
             elapsed_time = time.time() - start_time
             if desired_loop_time - elapsed_time > 0:

test/integration/integration.py

@@ -26,7 +26,7 @@ def test_integration(self):
         waypoint_in_world = SE3(position=np.array([-5.5, -5.5, 0.0]))
         waypoints = [
             (
-                Waypoint(fiducial_id=0, tf_id="course0", type=WaypointType(val=WaypointType.NO_SEARCH)),
+                Waypoint(tag_id=0, tf_id="course0", type=WaypointType(val=WaypointType.NO_SEARCH)),
                 waypoint_in_world,
             ),
         ]