Update failure identification to use new state machine messaging (umr…

…over#628)

src/navigation/failure_identification/failure_identification.py

@@ -8,9 +8,8 @@
 import pandas as pd
 import rospy
 from geometry_msgs.msg import Twist
-from mrover.msg import MotorsStatus
+from mrover.msg import MotorsStatus, StateMachineStateUpdate
 from nav_msgs.msg import Odometry
-from smach_msgs.msg import SmachContainerStatus
 from std_msgs.msg import Bool
 
 from util.ros_utils import get_rosparam
@@ -39,7 +38,7 @@ class FailureIdentifier:
     last_recorded_recovery_time: Optional[rospy.Time] = None
 
     def __init__(self):
-        nav_status_sub = message_filters.Subscriber("smach/container_status", SmachContainerStatus)
+        nav_status_sub = message_filters.Subscriber("nav_state", StateMachineStateUpdate)
         cmd_vel_sub = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_update)
         drive_status_sub = message_filters.Subscriber("drive_status", MotorsStatus)
         odometry_sub = message_filters.Subscriber("global_ekf/odometry", Odometry)
@@ -96,13 +95,13 @@ def write_to_csv(self):
             else:
                 self._df.to_csv(self.path_name, mode="a", header=False)
 
-    def stuck_button_update(self, stuck_button: Bool):
+    def stuck_button_update(self, stuck_button: Bool) -> None:
         self.cur_stuck = stuck_button.data
 
-    def cmd_vel_update(self, cmd_vel: Twist):
+    def cmd_vel_update(self, cmd_vel: Twist) -> None:
         self.cur_cmd = cmd_vel
 
-    def update(self, nav_status: SmachContainerStatus, drive_status: MotorsStatus, odometry: Odometry):
+    def update(self, nav_status: StateMachineStateUpdate, drive_status: MotorsStatus, odometry: Odometry) -> None:
         """
         Updates the current row of the data frame with the latest data from the rover
         then appends the row to the data frame
@@ -113,10 +112,10 @@ def update(self, nav_status: SmachContainerStatus, drive_status: MotorsStatus, o
         publishes a message to the /nav_stuck topic indicating if the rover is stuck
         """
 
+        # test recovery state using the stuck button on the GUI rather than analyzing data
         TEST_RECOVERY_STATE = get_rosparam("failure_identification/test_recovery_state", False)
-
         # if the state is 'done' or 'off', write the data frame to a csv file if we were collecting
-        if nav_status.active_states[0] == "DoneState" or nav_status.active_states[0] == "OffState":
+        if nav_status.state == "DoneState" or nav_status.state == "OffState":
             self.write_to_csv()
             if self.actively_collecting and self.data_collecting_mode:
                 self.actively_collecting = False
@@ -174,7 +173,7 @@ def update(self, nav_status: SmachContainerStatus, drive_status: MotorsStatus, o
         # publish the watchdog status if the nav state is not recovery
         if TEST_RECOVERY_STATE:
             self.stuck_publisher.publish(Bool(self.cur_stuck))
-        elif nav_status.active_states[0] != "RecoveryState":
+        elif nav_status.state != "RecoveryState":
             if (
                 self.last_recorded_recovery_time is None
                 or rospy.Time.now() - self.last_recorded_recovery_time > rospy.Duration(POST_RECOVERY_GRACE_PERIOD)