diff --git a/control/velocity_controller_lqr/README.md b/control/velocity_controller_lqr/README.md new file mode 100644 index 000000000..8e8788928 --- /dev/null +++ b/control/velocity_controller_lqr/README.md @@ -0,0 +1,47 @@ +## Overview: +--- +Contains the LQR controller package for the AUV Orca. The controller utilizes an LQR optimal controller (imported from the python control library), and controls pitch, yaw and surge. The controller is meant to traverse larger distances. + +#### Tuning of Parameters: + To tune parameters look at the [config file](https://github.com/vortexntnu/vortex-auv/blob/velocity-controller-to-lifecycle/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml) file: + + +#### Launching the package: +1. Navigate to your ros2ws directory then (change your own path after cd or skip first line entirely), +```bash +cd ~/ros2ws/ && +colcon build --packages-select velocity_controller_lqr && +source install/setup.bash +``` + +2. Then simply run the node via the launch file +```bash + ros2 launch velocity_controller_lqr velocity_controller_lqr.launch.py +``` + +3. You will notice that not much is currently happening, however that is because this is a lifecycle node and you can now transition between the node states through the terminal. + +#### Transitioning between states manually: +The ROS2 node is implemented using lifecycle nodes, which are managed externally by a lifecycle manager i.e a finite state machine. If you want to manually test the node do the following: + +**From Unconfigured ---> Inactive** +```bash +ros2 lifecycle set /velocity_controller_lqr_node configure +``` + +**From Configured ---> Active** +```bash +ros2 lifecycle set /velocity_controller_lqr_node activate +``` + +**From Active ---> Inactive** +```bash +ros2 lifecycle set /velocity_controller_lqr_node deactivate +``` + +For the full state diagram you can refer to the figure below, sourced from the official ROS2 Documentation: +![image info](./figures/ros2_transition_diagram.png) + + +### Theory +--- diff --git a/control/velocity_controller_lqr/README.txt b/control/velocity_controller_lqr/README.txt deleted file mode 100644 index c49e405ea..000000000 --- a/control/velocity_controller_lqr/README.txt +++ /dev/null @@ -1 +0,0 @@ -This package contains the velocity controller for the AUV Orca. The controller utilizes an LQR optimal controller (imported from the python control library), and controls pitch, yaw and surge. The controller is meant to traverse larger distances. diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml index 727c1c71e..6b1f6503d 100644 --- a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -1,6 +1,14 @@ /**: ros__parameters: - dt: 0.1 + + # topics: + # odom_topic: /orca/odom + # twist_topic: /dvl/twist + # pose_topic: /dvl/pose + # guidance_topic: /guidance/los + # thrust_topic: /thrust/wrench_input + # softwareoperation_topic: /softwareOperationMode + # killswitch_topic: /softwareKillSwitch LQR_params: q_surge: 75 @@ -17,7 +25,6 @@ i_weight: 0.5 - inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + dt: 0.1 - #Clamp parameter - max_force: 99.5 + inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] diff --git a/control/velocity_controller_lqr/figures/ros2_transition_diagram.png b/control/velocity_controller_lqr/figures/ros2_transition_diagram.png new file mode 100644 index 000000000..bfd9ee9c2 Binary files /dev/null and b/control/velocity_controller_lqr/figures/ros2_transition_diagram.png differ diff --git a/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py index 6669744d5..cda6e32b3 100644 --- a/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py +++ b/control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py @@ -2,7 +2,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch_ros.actions import Node +from launch_ros.actions import LifecycleNode def generate_launch_description() -> LaunchDescription: @@ -29,7 +29,7 @@ def generate_launch_description() -> LaunchDescription: "orca.yaml", ) - velocity_controller_node = Node( + velocity_controller_node = LifecycleNode( package="velocity_controller_lqr", executable="velocity_controller_lqr_node.py", name="velocity_controller_lqr_node", diff --git a/control/velocity_controller_lqr/package.xml b/control/velocity_controller_lqr/package.xml index a27ed4771..b60804d44 100644 --- a/control/velocity_controller_lqr/package.xml +++ b/control/velocity_controller_lqr/package.xml @@ -2,7 +2,7 @@ velocity_controller_lqr - 1.0.0 + 0.6.9 Velocity controller package for the AUV Orca cyprian MIT diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py old mode 100755 new mode 100644 index aae5aeb4a..fdbeac82a --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 - import numpy as np import rclpy from geometry_msgs.msg import ( @@ -8,7 +7,8 @@ WrenchStamped, ) from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node +from rclpy.lifecycle import LifecycleNode +from rclpy.lifecycle.node import LifecycleState, TransitionCallbackReturn from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy from std_msgs.msg import Bool, String from velocity_controller_lqr.velocity_controller_lqr_lib import ( @@ -20,124 +20,191 @@ from vortex_msgs.msg import LOSGuidance -class LinearQuadraticRegulator(Node): +class LinearQuadraticRegulator(LifecycleNode): def __init__(self): + # ----------------------- DEFINE RELIABILITY ------------------------ super().__init__("velocity_controller_lqr_node") - - self.get_topics() - - best_effort_qos = QoSProfile( + self.best_effort_qos = QoSProfile( reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, depth=1, ) - # ---------------------------- SUBSCRIBERS --------------------------- + self.reliable_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=2, + ) + + # ---------------- CALLBACK VARIABLES INITIALIZATION ---------------- + self.coriolis_matrix = np.zeros((3, 3)) + self.inertia_matrix = np.zeros((3, 3)) + self.states = State() + self.guidance_values = GuidanceValues() + self.lqr_params = LQRParameters() + self.dt = None + # --------------------------- SUBSCRIBERS -------------------------- + self.pose_subscriber = None + self.twist_subscriber = None + self.operation_mode_subscriber = None + self.killswitch_subscriber = None + self.guidance_subscriber = None + # ------------------------ CONTROLLER MODES ------------------------ + self.killswitch = None + self.operation_mode = None + # --------------------------- PUBLISHERS --------------------------- + self.publisherLQR = None + # ----------------------------- TIMERS ----------------------------- + self.control_timer = None + # ------------------ ROS2 PARAMETERS AND CONTROLLER ------------------ + self.get_and_reshape_inertia_matrix() + self.controller = LQRController(self.lqr_params, self.inertia_matrix) + + def on_configure(self, previous_state: LifecycleState) -> TransitionCallbackReturn: + self.declare_params() + self.get_logger().info("1") + self.get_params() + self.get_logger().info("2") + # -------------------------- GET ALL TOPICS ------------------------- + ( + pose_topic, + twist_topic, + guidance_topic, + thrust_topic, + software_operation_topic, + killswitch_topic, + ) = self.get_topics() self.pose_subscriber = self.create_subscription( PoseWithCovarianceStamped, - self.pose_topic, + pose_topic, self.pose_callback, - qos_profile=best_effort_qos, + qos_profile=self.best_effort_qos, ) self.twist_subscriber = self.create_subscription( TwistWithCovarianceStamped, - self.twist_topic, + twist_topic, self.twist_callback, - qos_profile=best_effort_qos, + qos_profile=self.best_effort_qos, ) - self.operationmode_subscriber = self.create_subscription( + self.operation_mode_subscriber = self.create_subscription( String, - self.operation_mode_topic, + software_operation_topic, self.operation_callback, - qos_profile=2, + qos_profile=self.reliable_qos, ) self.killswitch_subscriber = self.create_subscription( Bool, - self.killswitch_topic, + killswitch_topic, self.killswitch_callback, - qos_profile=2, + qos_profile=self.reliable_qos, ) self.guidance_subscriber = self.create_subscription( LOSGuidance, - self.los_topic, + guidance_topic, self.guidance_callback, - qos_profile=best_effort_qos, + qos_profile=self.best_effort_qos, ) # ---------------------------- PUBLISHERS ---------------------------- - self.publisherLQR = self.create_publisher( - WrenchStamped, self.wrench_input_topic, best_effort_qos + self.publisherLQR = self.create_lifecycle_publisher( + WrenchStamped, thrust_topic, self.reliable_qos ) # ------------------------------ TIMERS ------------------------------ - dt = self.declare_parameter("dt", 0.1).get_parameter_value().double_value + dt = self.dt self.control_timer = self.create_timer(dt, self.control_loop) + self.control_timer.cancel() - # ------------------ ROS2 PARAMETERS AND CONTROLLER ------------------ - self.lqr_params = LQRParameters() - inertia_matrix = self.get_parameters() - self.controller = LQRController(self.lqr_params, inertia_matrix) + return TransitionCallbackReturn.SUCCESS - # ---------------- CALLBACK VARIABLES INITIALIZATION ----------------- - self.coriolis_matrix = np.zeros((3, 3)) - self.states = State() - self.guidance_values = GuidanceValues() + def on_activate(self, previous_state: LifecycleState) -> TransitionCallbackReturn: + self.control_timer.reset() + self.controller.reset_controller() + return super().on_activate(previous_state) + + def on_deactivate(self, previous_state: LifecycleState) -> TransitionCallbackReturn: + self.control_timer.cancel() + self.controller.reset_controller() + return super().on_deactivate(previous_state) + + def on_cleanup(self, previous_state: LifecycleState) -> TransitionCallbackReturn: + self.destroy_publisher(self.publisherLQR) + self.destroy_timer(self.control_timer) + return TransitionCallbackReturn.SUCCESS - def get_topics(self): - """Get the topics from the parameter file.""" - topics = [ - "pose", - "twist", - "los", - "wrench_input", - "operation_mode", - "killswitch", - ] - for topic in topics: - if topic == "los": - self.declare_parameter("topics.guidance." + topic, "_") - setattr( - self, - topic + "_topic", - self.get_parameter("topics.guidance." + topic).value, - ) - continue - self.declare_parameter("topics." + topic, "_") - setattr( - self, - topic + "_topic", - self.get_parameter("topics." + topic).value, - ) - - def get_parameters(self): - """Updates the LQR_params in the LQR_parameters Dataclass, and gets the inertia matrix from config. + def on_shutdown(self, previous_state: LifecycleState) -> TransitionCallbackReturn: + self.destroy_publisher(self.publisherLQR) + self.destroy_timer(self.control_timer) + return TransitionCallbackReturn.SUCCESS + + def get_topics(self) -> None: + """Get the topics from the parameter server. Returns: - inertia_matrix: np.array: The inertia matrix of the AUV + pose_topic: str: The topic for accessing the pose data from the parameter file + twist_topic: str: The topic for accessing the twist data from the parameter file + guidance_topic: str: The topic for accessing the guidance data the parameter file + thrust_topic: str: The topic for accessing the thrust data from the parameter file + software_operation_mode_topic: str: The topic for accessing the operation mode from the parameter file + killswitch_topic: str: The topic for accessing the killswitch bool from the parameter file + """ - self.declare_parameter("LQR_params.q_surge", 75) - self.declare_parameter("LQR_params.q_pitch", 175) - self.declare_parameter("LQR_params.q_yaw", 175) + self.declare_parameter("topics.pose", "_") + self.declare_parameter("topics.twist", "_") + self.declare_parameter("topics.guidance.los", "_") + self.declare_parameter("topics.wrench_input", "_") + self.declare_parameter("topics.operation_mode", "_") + self.declare_parameter("topics.killswitch", "_") + + pose_topic = self.get_parameter("topics.pose").value + twist_topic = self.get_parameter("topics.twist").value + guidance_topic = self.get_parameter("topics.guidance.los").value + thrust_topic = self.get_parameter("topics.wrench_input").value + operation_topic = self.get_parameter("topics.operation_mode").value + killswitch_topic = self.get_parameter("topics.killswitch").value + + return ( + pose_topic, + twist_topic, + guidance_topic, + thrust_topic, + operation_topic, + killswitch_topic, + ) - self.declare_parameter("LQR_params.r_surge", 0.3) - self.declare_parameter("LQR_params.r_pitch", 0.4) - self.declare_parameter("LQR_params.r_yaw", 0.4) + def declare_params(self) -> None: + """Declares parameters that are to be used from the configuration file.""" + self.get_logger().info("cool") - self.declare_parameter("LQR_params.i_surge", 0.3) - self.declare_parameter("LQR_params.i_pitch", 0.4) - self.declare_parameter("LQR_params.i_yaw", 0.3) + self.declare_parameter("LQR_params.q_surge", 0.0) + self.declare_parameter("LQR_params.q_pitch", 0.0) + self.declare_parameter("LQR_params.q_yaw", 0.0) - self.declare_parameter("LQR_params.i_weight", 0.5) + self.get_logger().info("yaw") - self.declare_parameter("max_force", 99.5) - self.declare_parameter( - "inertia_matrix", [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] - ) + self.declare_parameter("LQR_params.r_surge", 0.0) + self.declare_parameter("LQR_params.r_pitch", 0.0) + self.declare_parameter("LQR_params.r_yaw", 0.0) + + self.get_logger().info("shaw") + + self.declare_parameter("LQR_params.i_surge", 0.0) + self.declare_parameter("LQR_params.i_pitch", 0.0) + self.declare_parameter("LQR_params.i_yaw", 0.0) + + self.get_logger().info("garhamat") + + self.declare_parameter("LQR_params.i_weight", 0.0) + + self.declare_parameter("dt", 0.0) + self.declare_parameter("propulsion.thrusters.max", 0.0) + def get_params(self) -> None: + """Gets the declared parameters from the configuration file.""" self.lqr_params.q_surge = self.get_parameter("LQR_params.q_surge").value self.lqr_params.q_pitch = self.get_parameter("LQR_params.q_pitch").value self.lqr_params.q_yaw = self.get_parameter("LQR_params.q_yaw").value @@ -151,17 +218,22 @@ def get_parameters(self): self.lqr_params.i_yaw = self.get_parameter("LQR_params.i_yaw").value self.lqr_params.i_weight = self.get_parameter("LQR_params.i_weight").value - self.lqr_params.max_force = self.get_parameter("max_force").value + self.lqr_params.max_force = self.get_parameter("propulsion.thrusters.max").value - inertia_matrix_flat = self.get_parameter("inertia_matrix").value - inertia_matrix = np.array(inertia_matrix_flat).reshape((3, 3)) + self.dt = self.get_parameter("dt").value - return inertia_matrix + def get_and_reshape_inertia_matrix(self) -> None: + """Gets the inertia matrix from config and reshapes it to proper np array.""" + self.declare_parameter("inertia_matrix") + self.inertia_matrix = self.get_parameter("inertia_matrix").value + inertia_matrix_reshaped = np.array(self.inertia_matrix).reshape((3, 3)) - # ---------------------------------------------------------------CALLBACK FUNCTIONS--------------------------------------------------------------- + self.inertia_matrix = inertia_matrix_reshaped - def pose_callback(self, msg: PoseWithCovarianceStamped): - """Callback function for the pose data from DVL. + # ------------------------- CALLBACK FUNCTIONS --------------------------- + + def pose_callback(self, msg: PoseWithCovarianceStamped) -> None: + """Callback function for the pose data from sensors. Parameters: msg: PoseWithCovarianceStamped The pose data from the DVL. @@ -173,15 +245,15 @@ def pose_callback(self, msg: PoseWithCovarianceStamped): msg.pose.pose.orientation.z, ) - def operation_callback(self, msg: String): + def operation_callback(self, msg: String) -> None: """Callback function for the operation mode data. Parameters: String: msg: The operation mode data from the AUV. """ - self.controller.operation_mode = msg.data + self.operation_mode = msg.data - def twist_callback(self, msg: TwistWithCovarianceStamped): + def twist_callback(self, msg: TwistWithCovarianceStamped) -> None: """Callback function for the Twist data from DVL. Parameters: msg: TwistWithCovarianceStamped The twist data from the DVL. @@ -196,7 +268,7 @@ def twist_callback(self, msg: TwistWithCovarianceStamped): msg.twist.twist.linear.z, ) - def guidance_callback(self, msg: LOSGuidance): + def guidance_callback(self, msg: LOSGuidance) -> None: """Callback function for the guidance data. Parameters: LOSGuidance: msg: The guidance data from the LOS guidance system. @@ -206,39 +278,37 @@ def guidance_callback(self, msg: LOSGuidance): self.guidance_values.pitch = msg.pitch self.guidance_values.yaw = msg.yaw - def killswitch_callback(self, msg: Bool): + def killswitch_callback(self, msg: Bool) -> None: """Callback function for the killswitch data. Parameters: String: msg: The killswitch data from the AUV. """ - self.controller.killswitch = msg.data - if self.controller.killswitch: + if msg.data == True: self.controller.reset_controller() + self.killswitch = True + else: + self.killswitch = False # ---------------------------------------------------------------PUBLISHER FUNCTIONS------------------------------------------------------------- - def control_loop(self): + def control_loop(self) -> None: """The control loop that calculates the input for the LQR controller.""" + if self.killswitch == True or self.operation_mode != "autonomous mode": + self.controller.reset_controller() + return + msg = WrenchStamped() u = self.controller.calculate_lqr_u( self.coriolis_matrix, self.states, self.guidance_values ) - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = "base_link" + msg.wrench.force.x = float(u[0]) msg.wrench.torque.y = float(u[1]) msg.wrench.torque.z = float(u[2]) - if ( - self.controller.killswitch == False - and self.controller.operation_mode == "autonomous mode" - ): - self.publisherLQR.publish(msg) - - else: - self.controller.reset_controller() + self.publisherLQR.publish(msg) # ----------------------------------------------------------------------MAIN FUNCTION---------------------------------------------------------------- diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py index af0528ee0..2f845bc50 100644 --- a/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py +++ b/control/velocity_controller_lqr/velocity_controller_lqr/velocity_controller_lqr_lib.py @@ -76,8 +76,6 @@ class LQRParameters: i_yaw: float = 0.0 i_weight: float = 0.0 max_force: float = 0.0 - operation_mode: str = "xbox mode" - killswitch: bool = True class LQRController: @@ -223,10 +221,8 @@ def set_params(self, parameters: LQRParameters) -> None: self.i_pitch = parameters.i_pitch self.i_yaw = parameters.i_yaw self.i_weight = parameters.i_weight - self.max_force = parameters.max_force - self.operation_mode = parameters.operation_mode - self.killswitch = parameters.killswitch + self.max_force = parameters.max_force def set_matrices(self, inertia_matrix: np.array) -> None: """Adjusts the matrices for the LQR controller.