almost done with compute control

rb_ws/src/buggy/buggy/controller/controller_node.py

@@ -10,7 +10,8 @@
 from nav_msgs.msg import Odometry
 
 sys.path.append("/rb_ws/src/buggy/buggy")
-from rb_ws.src.buggy.buggy.util.trajectory_old import Trajectory
+from util.trajectory_old import Trajectory
+from controller.stanley_controller import StanleyController
 
 class Controller(Node):
 
@@ -38,20 +39,20 @@ def __init__(self):
         self.declare_parameter("controller_name", "stanley")
         match self.get_parameter("controller_name"):
             case "stanley":
-                self.controller = StanleyController(start_index = start_index) #IMPORT STANLEY
+                self.controller = StanleyController(start_index = start_index, buggy_name = self.get_namespace(), node=self) #IMPORT STANLEY
             case _:
                 self.get_logger().error("Invalid Controller Name!")
                 raise Exception("Invalid Controller Argument")
 
         # Publishers
-        self.init_check_publisher = self.create_subscription(Bool,
-            "debug/init_safety_check", queue_size=1
+        self.init_check_publisher = self.create_publisher(Bool,
+            "debug/init_safety_check", 1
         )
-        self.steer_publisher = self.create_subscription.Publisher(
-            Float64, "/buggy/steering", queue_size=1
+        self.steer_publisher = self.create_publisher(
+            Float64, "/buggy/steering", 1
         )
-        self.heading_publisher = self.create_subscription.Publisher(
-            Float32, "/auton/debug/heading", queue_size=1
+        self.heading_publisher = self.create_publisher(
+            Float32, "/auton/debug/heading", 1
         )
 
         # Subscribers

rb_ws/src/buggy/buggy/controller/controller_superclass.py

@@ -0,0 +1,50 @@
+from abc import ABC, abstractmethod
+import sys
+
+from sensor_msgs.msg import NavSatFix
+from nav_msgs.msg import Odometry
+
+
+sys.path.append("/rb_ws/src/buggy/buggy")
+from rb_ws.src.buggy.buggy.util.trajectory_old import Trajectory
+
+class Controller(ABC):
+    """
+    Base class for all controllers.
+
+    The controller takes in the current state of the buggy and a reference
+    trajectory. It must then compute the desired control output.
+
+    The method that it does this by is up to the implementation, of course.
+    Example schemes include Pure Pursuit, Stanley, and LQR.
+    """
+
+    # TODO: move this to a constants class
+    NAND_WHEELBASE = 1.3
+    SC_WHEELBASE = 1.104
+
+    def __init__(self, start_index: int, namespace : str, node) -> None:
+        self.namespace = namespace
+        if namespace.upper() == '/NAND':
+            Controller.WHEELBASE = self.NAND_WHEELBASE
+        else:
+            Controller.WHEELBASE = self.SC_WHEELBASE
+
+        self.current_traj_index = start_index
+        self.node = node
+
+    @abstractmethod
+    def compute_control(
+        self, state_msg: Odometry, trajectory: Trajectory,
+    ) -> float:
+        """
+        Computes the desired control output given the current state and reference trajectory
+
+        Args:
+            state: (Odometry): state of the buggy, including position, attitude and associated twists
+            trajectory (Trajectory): reference trajectory
+
+        Returns:
+            float (desired steering angle)
+        """
+        raise NotImplementedError