State library (#559)

* state library and tests * working simple test * fixed external data test, added comment * added messagse for and code for state ros message publishing server * basically fully integrated with current nav system, tested on a few scenarios in sim * address comments: fix and integrate publisher + logging levels * style * style w correct black version * fix mypy issues and add stop funcitonality * Cleanup imports, improve some type hinting for member vars, fix wrong call to context * address comments * fix missing config * fix style * fix import issue --------- Co-authored-by: qhdwight <[email protected]>
umrover · Oct 31, 2023 · 263466d · 263466d
1 parent c19fae3
commit 263466d
Show file tree

Hide file tree

Showing 24 changed files with 738 additions and 420 deletions.
diff --git a/config/navigation.yaml b/config/navigation.yaml
@@ -26,6 +26,7 @@ single_fiducial:
   stop_thresh: 1.0
   fiducial_stop_threshold: 1.75
   post_avoidance_multiplier: 1.42
+  post_radius: 0.7
 
 waypoint:
   stop_thresh: 0.5

diff --git a/msg/StateMachineStateUpdate.msg b/msg/StateMachineStateUpdate.msg
@@ -0,0 +1,2 @@
+string stateMachineName
+string state
diff --git a/msg/StateMachineStructure.msg b/msg/StateMachineStructure.msg
@@ -0,0 +1,2 @@
+string machineName
+StateMachineTransition[] transitions
diff --git a/msg/StateMachineTransition.msg b/msg/StateMachineTransition.msg
@@ -0,0 +1,2 @@
+string origin
+string[] destinations
diff --git a/setup.py b/setup.py
@@ -8,6 +8,7 @@
     packages=[
         "localization",
         "util",
+        "util.state_lib",
         "navigation",
         "navigation.failure_identification",
         "esw",

diff --git a/src/navigation/approach_post.py b/src/navigation/approach_post.py
@@ -1,60 +1,47 @@
 import tf2_ros
-from aenum import Enum, NoAlias
-from geometry_msgs.msg import Twist
-from util.ros_utils import get_rosparam
-
-from context import Context
-from state import BaseState
-
 
-class ApproachPostStateTransitions(Enum):
-    _settings_ = NoAlias
+from util.ros_utils import get_rosparam
+from util.state_lib.state import State
 
-    finished_fiducial = "WaypointState"
-    continue_fiducial_id = "ApproachPostState"
-    no_fiducial = "SearchState"
-    recovery_state = "RecoveryState"
+from navigation import search, waypoint
 
 
-class ApproachPostState(BaseState):
+class ApproachPostState(State):
     STOP_THRESH = get_rosparam("single_fiducial/stop_thresh", 0.7)
     FIDUCIAL_STOP_THRESHOLD = get_rosparam("single_fiducial/fiducial_stop_threshold", 1.75)
     DRIVE_FWD_THRESH = get_rosparam("waypoint/drive_fwd_thresh", 0.34)  # 20 degrees
 
-    def __init__(self, context: Context):
-        own_transitions = [ApproachPostStateTransitions.continue_fiducial_id.name]  # type: ignore
-        super().__init__(context, own_transitions, add_outcomes=[transition.name for transition in ApproachPostStateTransitions])  # type: ignore
+    def on_enter(self, context) -> None:
+        pass
+
+    def on_exit(self, context) -> None:
+        pass
 
-    def evaluate(self, ud) -> str:
+    def on_loop(self, context) -> State:
         """
         Drive towards a single fiducial if we see it and stop within a certain threshold if we see it.
         Else conduct a search to find it.
         :param ud:  State machine user data
         :return:    Next state
         """
-        fid_pos = self.context.env.current_fid_pos()
+        fid_pos = context.env.current_fid_pos()
         if fid_pos is None:
-            # We have arrived at the waypoint where the fiducial should be but we have not seen it yet
-            cmd_vel = Twist()
-            cmd_vel.linear.x = 0.0
-            self.context.rover.send_drive_command(cmd_vel)
-            return ApproachPostStateTransitions.no_fiducial.name  # type: ignore
+            return search.SearchState()
 
         try:
-            cmd_vel, arrived = self.context.rover.driver.get_drive_command(
+            cmd_vel, arrived = context.rover.driver.get_drive_command(
                 fid_pos,
-                self.context.rover.get_pose(in_odom_frame=True),
+                context.rover.get_pose(in_odom_frame=True),
                 self.STOP_THRESH,
                 self.DRIVE_FWD_THRESH,
-                in_odom=self.context.use_odom,
+                in_odom=context.use_odom,
             )
             if arrived:
-                self.context.env.arrived_at_post = True
-                self.context.env.last_post_location = self.context.env.current_fid_pos(odom_override=False)
-                print(f"set last post location to {self.context.env.last_post_location}.")
-                self.context.course.increment_waypoint()
-                return ApproachPostStateTransitions.finished_fiducial.name  # type: ignore
-            self.context.rover.send_drive_command(cmd_vel)
+                context.env.arrived_at_post = True
+                context.env.last_post_location = context.env.current_fid_pos(odom_override=False)
+                context.course.increment_waypoint()
+                return waypoint.WaypointState()
+            context.rover.send_drive_command(cmd_vel)
         except (
             tf2_ros.LookupException,
             tf2_ros.ConnectivityException,
@@ -63,4 +50,4 @@ def evaluate(self, ud) -> str:
             # TODO: probably go into some waiting state
             pass
 
-        return ApproachPostStateTransitions.continue_fiducial_id.name  # type: ignore
+        return self
diff --git a/src/navigation/context.py b/src/navigation/context.py
@@ -11,13 +11,12 @@
 import tf2_ros
 from geometry_msgs.msg import Twist
 from mrover.msg import Waypoint, GPSWaypoint, EnableAuton, WaypointType, GPSPointList
-from shapely.geometry import Point
 from std_msgs.msg import Time, Bool
-from util.SE3 import SE3
-from util.ros_utils import get_rosparam
 from visualization_msgs.msg import Marker
 
-from drive import DriveController
+from util.SE3 import SE3
+
+from navigation.drive import DriveController
 
 TAG_EXPIRATION_TIME_SECONDS = 60
 
@@ -162,7 +161,7 @@ def setup_course(ctx: Context, waypoints: List[Tuple[Waypoint, SE3]]) -> Course:
     return Course(ctx=ctx, course_data=mrover.msg.Course([waypoint[0] for waypoint in waypoints]))
 
 
-def convert_gps_to_cartesian(waypoint: GPSWaypoint) -> Waypoint:
+def convert_gps_to_cartesian(waypoint: GPSWaypoint) -> Tuple[Waypoint, SE3]:
     """
     Converts a GPSWaypoint into a "Waypoint" used for publishing to the CourseService.
     """

diff --git a/src/navigation/drive.py b/src/navigation/drive.py
@@ -5,6 +5,7 @@
 
 import numpy as np
 from geometry_msgs.msg import Twist
+
 from util.SE3 import SE3
 from util.np_utils import angle_to_rotate, normalized
 from util.ros_utils import get_rosparam
@@ -71,33 +72,33 @@ def _get_state_machine_output(
         # if we are at the target position, reset the controller and return a zero command
         if abs(linear_error) < completion_thresh:
             self.reset()
-            return (Twist(), True)
+            return Twist(), True
 
         if self._driver_state == self.DriveMode.STOPPED:
             # if the drive mode is STOP (we know we aren't at the target) so we must start moving towards it
             # just switch to the TURN_IN_PLACE state for now under the assumption that we need to turn to face the target
             # return a zero command and False to indicate we aren't at the target (and are also not in the correct state to figure out how to get there)
             self._driver_state = self.DriveMode.TURN_IN_PLACE
-            return (Twist(), False)
+            return Twist(), False
 
         elif self._driver_state == self.DriveMode.TURN_IN_PLACE:
             # if we are in the TURN_IN_PLACE state, we need to turn to face the target
             # if we are within the turn threshold to face the target, we can start driving straight towards it
             if abs(angular_error) < turn_in_place_thresh:
                 self._driver_state = self.DriveMode.DRIVE_FORWARD
-                return (Twist(), False)
+                return Twist(), False
 
             # IVT (Intermediate Value Theorem) check. If the sign of the angular error has changed, this means we've crossed through 0 error
             # in order to prevent osciallation, we 'give up' and just switch to the drive forward state
             elif self._last_angular_error is not None and np.sign(self._last_angular_error) != np.sign(angular_error):
                 self._driver_state = self.DriveMode.DRIVE_FORWARD
-                return (Twist(), False)
+                return Twist(), False
 
             # if neither of those things are true, we need to turn in place towards our target heading, so set the z component of the output Twist message
             else:
                 cmd_vel = Twist()
                 cmd_vel.angular.z = np.clip(angular_error * TURNING_P, MIN_TURNING_EFFORT, MAX_TURNING_EFFORT)
-                return (cmd_vel, False)
+                return cmd_vel, False
 
         elif self._driver_state == self.DriveMode.DRIVE_FORWARD:
             # if we are driving straight towards the target and our last angular error was inside the threshold
@@ -112,13 +113,13 @@ def _get_state_machine_output(
             cur_angular_is_outside = abs(angular_error) >= turn_in_place_thresh
             if cur_angular_is_outside and last_angular_was_inside:
                 self._driver_state = self.DriveMode.TURN_IN_PLACE
-                return (Twist(), False)
+                return Twist(), False
             # otherwise we compute a drive command with both a linear and angular component in the Twist message
             else:
                 cmd_vel = Twist()
                 cmd_vel.linear.x = np.clip(linear_error * DRIVING_P, MIN_DRIVING_EFFORT, MAX_DRIVING_EFFORT)
                 cmd_vel.angular.z = np.clip(angular_error * TURNING_P, MIN_TURNING_EFFORT, MAX_TURNING_EFFORT)
-                return (cmd_vel, False)
+                return cmd_vel, False
         else:
             raise ValueError(f"Invalid drive state {self._driver_state}")
 
@@ -189,7 +190,7 @@ def get_drive_command(
 
         if np.linalg.norm(target_pos - rover_pos) < completion_thresh:
             self.reset()
-            return (Twist(), True)
+            return Twist(), True
 
         if path_start is not None:
             target_pos = self.get_lookahead_pt(path_start, target_pos, rover_pos, LOOKAHEAD_DISTANCE)

diff --git a/src/navigation/failure_identification/__init__.py b/src/navigation/failure_identification/__init__.py
diff --git a/src/navigation/failure_identification/failure_identification.py b/src/navigation/failure_identification/failure_identification.py
@@ -10,12 +10,12 @@
 from geometry_msgs.msg import Twist
 from mrover.msg import MotorsStatus
 from nav_msgs.msg import Odometry
-from pandas import DataFrame
 from smach_msgs.msg import SmachContainerStatus
 from std_msgs.msg import Bool
+
 from util.ros_utils import get_rosparam
 
-from watchdog import WatchDog
+from navigation.failure_identification.watchdog import WatchDog
 
 DATAFRAME_MAX_SIZE = get_rosparam("failure_identification/dataframe_max_size", 200)
 POST_RECOVERY_GRACE_PERIOD = get_rosparam("failure_identification/post_recovery_grace_period", 5.0)
@@ -28,7 +28,7 @@ class FailureIdentifier:
     """
 
     stuck_publisher: rospy.Publisher
-    _df: DataFrame
+    _df: pd.DataFrame
     watchdog: WatchDog
     actively_collecting: bool
     cur_cmd: Twist
@@ -164,7 +164,7 @@ def update(self, nav_status: SmachContainerStatus, drive_status: MotorsStatus, o
             cur_row[f"wheel_{wheel_num}_velocity"] = drive_status.joint_states.velocity[wheel_num]
 
         # update the data frame with the cur row
-        self._df = pd.concat([self._df, DataFrame([cur_row])])
+        self._df = pd.concat([self._df, pd.DataFrame([cur_row])])
 
         if len(self._df) == DATAFRAME_MAX_SIZE:
             self.write_to_csv()

diff --git a/src/navigation/failure_identification/watchdog.py b/src/navigation/failure_identification/watchdog.py
@@ -1,7 +1,8 @@
 from typing import Tuple
 
 import numpy as np
-from pandas import DataFrame
+import pandas as pd
+
 from util.SO3 import SO3
 from util.ros_utils import get_rosparam
 
@@ -11,14 +12,14 @@
 
 
 class WatchDog:
-    def get_start_end_positions(self, dataframe: DataFrame) -> Tuple[np.ndarray, np.ndarray]:
+    def get_start_end_positions(self, dataframe: pd.DataFrame) -> Tuple[np.ndarray, np.ndarray]:
         start_x, start_y, start_z = dataframe["x"].iloc[0], dataframe["y"].iloc[0], dataframe["z"].iloc[0]
         start_pos = np.array([start_x, start_y, start_z])
         end_x, end_y, end_z = dataframe["x"].iloc[-1], dataframe["y"].iloc[-1], dataframe["z"].iloc[-1]
         end_pos = np.array([end_x, end_y, end_z])
         return start_pos, end_pos
 
-    def get_start_end_rotations(self, dataframe: DataFrame) -> Tuple[SO3, SO3]:
+    def get_start_end_rotations(self, dataframe: pd.DataFrame) -> Tuple[SO3, SO3]:
         start_rot = np.array(
             [
                 dataframe["rot_x"].iloc[0],
@@ -37,7 +38,7 @@ def get_start_end_rotations(self, dataframe: DataFrame) -> Tuple[SO3, SO3]:
         )
         return SO3(start_rot), SO3(end_rot)
 
-    def get_start_end_time(self, dataframe: DataFrame) -> Tuple[float, float]:
+    def get_start_end_time(self, dataframe: pd.DataFrame) -> Tuple[float, float]:
         start_time = dataframe["time"].iloc[0]
         end_time = dataframe["time"].iloc[-1]
         return start_time, end_time
@@ -79,7 +80,7 @@ def check_linear_stuck(self, delta_time, delta_pos, dataframe) -> bool:
         print(linear_velocity, LINEAR_THRESHOLD)
         return linear_velocity < LINEAR_THRESHOLD
 
-    def is_stuck(self, dataframe: DataFrame) -> bool:
+    def is_stuck(self, dataframe: pd.DataFrame) -> bool:
         if len(dataframe) > WINDOW_SIZE:
             dataframe_sliced = dataframe.tail(WINDOW_SIZE)
             # get the start and end position and rotation
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,2 @@
		string machineName
		StateMachineTransition[] transitions