From 01c7347b2fb804558afc636e4964263bde3eea9d Mon Sep 17 00:00:00 2001 From: Emerson-Ramey Date: Sun, 31 Dec 2023 06:12:03 +0000 Subject: [PATCH] remove smach from failure_identification.py --- .../failure_identification.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/navigation/failure_identification/failure_identification.py b/src/navigation/failure_identification/failure_identification.py index 27f9833a1..844aca39a 100755 --- a/src/navigation/failure_identification/failure_identification.py +++ b/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)