diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index 91fef61..051f4d0 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/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 diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/buggy/controller/controller_superclass.py index e69de29..6944ccd 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_superclass.py +++ b/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 \ No newline at end of file