implemented switching

guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py

@@ -7,7 +7,7 @@
 from vortex_msgs.srv import Waypoint
 from nav_msgs.msg import Odometry
 from transforms3d.euler import quat2euler
-from std_msgs.msg import Bool
+from std_msgs.msg import Bool, Int32
 
 from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals
 
@@ -27,6 +27,7 @@ def __init__(self):
         self.eta_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1)
         self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1)
         self.switching_publisher = self.create_publisher(Bool, 'guidance/hybridpath/finished', 10)
+        self.switching_criteria_subscriber = self.create_subscription(Int32, 'guidance/hybridpath/switching', self.switching_callback, 10)
 
         # Get parameters
         self.lambda_val = self.get_parameter('hybridpath_guidance.lambda_val').get_parameter_value().double_value
@@ -35,6 +36,8 @@ def __init__(self):
         self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value
         self.eta = np.zeros(3)
 
+        self.switching_waypoint = 1
+
         # Flags for logging
         self.waypoints_received = False
         self.waiting_message_printed = False
@@ -61,6 +64,9 @@ def waypoint_callback(self, request, response):
         response.success = True
         return response
 
+    def switching_callback(self, msg: Int32):
+        self.switching_waypoint = msg.data
+
     def eta_callback(self, msg: Odometry):
         self.eta = self.odom_to_eta(msg)
 

mission/mission_planner/mission_planner/mission_planner.py

@@ -4,7 +4,7 @@
 from rclpy.node import Node
 from vortex_msgs.srv import MissionParameters#, s
 from geometry_msgs.msg import Point
-from std_msgs.msg import String, Bool
+from std_msgs.msg import String, Bool, Int32
 
 class MissionPlannerClient(Node):
     """
@@ -30,13 +30,19 @@ def __init__(self):
         self.active_controller_publisher = self.create_publisher(String, 'mission/controller', 10)
         self.hybridpath_guidance_subscriber = self.create_subscription(Bool, 'guidance/hybridpath/finished', self.guidance_callback, 10)
         self.active_controller_timer = self.create_timer(1.0, self.timer_callback)
+        self.switching_waypoint_publisher = self.create_publisher(Int32, 'guidance/hybridpath/switching', 10)
 
     def timer_callback(self):
         msg = String()
         msg.data = self.active_controller
         self.active_controller_publisher.publish(msg)
         self.get_logger().info(f'Publishing: {self.active_controller}')
 
+        # Switching criteria
+        msg = Int32()
+        msg.data = 1
+        self.switching_waypoint_publisher.publish(msg)
+
     def guidance_callback(self, msg):
         if msg.data == True:
             self.active_controller = 'DP'

motion/hybridpath_controller/config/hybridpath_controller_config.yaml

@@ -1,5 +1,5 @@
 /**:
   ros__parameters:
     hybridpath_controller:
-      K1_diag: [10.0, 10.0, 10.0] # First gain matrix
-      K2_diag: [60.0, 60.0, 60.0] # Second gain matrix
+      K1_diag: [1.0, 1.0, 1.0] # First gain matrix
+      K2_diag: [60.0, 59.0, 47.0] # Second gain matrix

motion/lqr_controller/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.8)
+project(lqr_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+lqr_controller/lqr_controller_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

motion/lqr_controller/config/lqr_config.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    lqr_controller:
+      Q: [20.0, 20.0, 1.0, 1.0, 1.0, 1.0]  # State costs for [x, y, heading, surge, sway, yaw]
+      R: [1.0, 1.0, 1.0]  # Control cost weight

motion/lqr_controller/launch/lqr_controller.launch.py

@@ -0,0 +1,19 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    lqr_controller_node = Node(
+            package='lqr_controller',
+            executable='lqr_controller_node.py',
+            name='lqr_controller_node',
+            parameters=[
+                os.path.join(get_package_share_directory('lqr_controller'),'config','lqr_config.yaml'),
+                os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml')
+            ],
+            output='screen',
+        )
+    return LaunchDescription([
+        lqr_controller_node
+    ])

motion/lqr_controller/lqr_controller/conversions.py

@@ -0,0 +1,59 @@
+import numpy as np
+
+from transforms3d.euler import euler2quat, quat2euler
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import PoseStamped
+
+
+def odometrymsg_to_state(msg):
+    x = msg.pose.pose.position.x
+    y = msg.pose.pose.position.y
+    orientation_q = msg.pose.pose.orientation
+    orientation_list = [
+        orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z
+    ]
+
+    # Convert quaternion to Euler angles, extract yaw
+    yaw = quat2euler(orientation_list)[2]
+
+    vx = msg.twist.twist.linear.x
+    vy = msg.twist.twist.linear.y
+    vyaw = msg.twist.twist.angular.z
+
+    state = np.array([x, y, yaw, vx, vy, vyaw])
+    return state
+
+def state_to_odometrymsg(state):
+    orientation_list_next = euler2quat(0, 0, state[2])
+
+    odometry_msg = Odometry()
+    odometry_msg.pose.pose.position.x = state[0]
+    odometry_msg.pose.pose.position.y = state[1]
+    odometry_msg.pose.pose.position.z = 0.0
+    odometry_msg.pose.pose.orientation.w = orientation_list_next[0]
+    odometry_msg.pose.pose.orientation.x = orientation_list_next[1]
+    odometry_msg.pose.pose.orientation.y = orientation_list_next[2]
+    odometry_msg.pose.pose.orientation.z = orientation_list_next[3]
+
+    return odometry_msg
+
+def state_to_posestamped(state, frame_id, stamp):
+    orientation_list_next = euler2quat(0, 0, state[2])
+
+    posestamped_msg = PoseStamped()
+
+    posestamped_msg.header.frame_id = frame_id
+    posestamped_msg.header.stamp = stamp
+
+    posestamped_msg.pose.position.x = state[0]
+    posestamped_msg.pose.position.y = state[1]
+    posestamped_msg.pose.position.z = 0.0
+    posestamped_msg.pose.orientation.w = orientation_list_next[0]
+    posestamped_msg.pose.orientation.x = orientation_list_next[1]
+    posestamped_msg.pose.orientation.y = orientation_list_next[2]
+    posestamped_msg.pose.orientation.z = orientation_list_next[3]
+
+    return posestamped_msg
+
+def ssa(angle):
+    return (angle + np.pi) % (2 * np.pi) - np.pi

motion/lqr_controller/lqr_controller/lqr_controller.py

@@ -0,0 +1,39 @@
+import numpy as np
+from scipy.linalg import solve_continuous_are
+
+class LQRController:
+    def __init__(self, M: float, D: list[float], Q: list[float], R: list[float], heading_ref: float = 0.0):
+        self.M = M
+        self.M_inv = np.linalg.inv(self.M)
+        self.D = D  
+
+        self.A = np.zeros((6,6))
+        self.B  = np.zeros((6,3))
+        self.C = np.zeros((3,6))
+        self.C[:3, :3] = np.eye(3)
+
+        self.Q = np.diag(Q)
+        print(f"Q: {self.Q}")
+        self.R = np.diag(R)
+
+        self.linearize_model(heading_ref)
+
+    def calculate_control_input(self, x, x_ref):
+        tau = -self.K_LQR @ x + self.K_r @ x_ref
+        return tau
+
+    def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]:
+        R = np.array(
+            [[np.cos(heading), -np.sin(heading), 0],
+            [np.sin(heading), np.cos(heading), 0],
+            [0, 0, 1]]
+        )
+
+        self.A[:3,3:] = R
+        self.A[3:, 3:] = - self.M_inv @ self.D
+
+        self.B[3:,:] = self.M_inv
+
+        self.P = solve_continuous_are(self.A, self.B, self.Q, self.R)
+        self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P)
+        self.K_r = np.linalg.inv(self.C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B)

motion/lqr_controller/lqr_controller/lqr_controller_node.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+
+import rclpy
+import numpy as np
+from rclpy.node import Node
+from geometry_msgs.msg import Wrench
+from nav_msgs.msg import Odometry
+from lqr_controller.lqr_controller import LQRController
+from lqr_controller.conversions import odometrymsg_to_state
+from time import sleep
+from std_msgs.msg import String
+
+from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy
+
+qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, 
+                         reliability=QoSReliabilityPolicy.BEST_EFFORT)
+
+class LQRControllerNode(Node):
+    def __init__(self):
+        super().__init__("lqr_controller_node")
+
+        self.declare_parameters(
+            namespace='',
+            parameters=[
+                ('lqr_controller.Q', [10.0, 10.0, 5.0, 0.1, 1.0, 5.0]),
+                ('lqr_controller.R', [1.0, 1.0, 1.0]),
+                ('physical.inertia_matrix', [90.5, 0.0, 0.0, 0.0, 167.5, 12.25, 0.0, 12.25, 42.65]),
+                ('physical.damping_matrix_diag', [77.55, 162.5, 42.65])
+            ])
+
+        self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile)
+        self.guidance_subscriber_ = self.create_subscription(Odometry, "guidance/dp/reference", self.guidance_cb, 1)
+        self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1)
+
+        self.active_controller_subscriber = self.create_subscription(String, 'mission/controller', self.active_controller_callback, 10)
+        self.active_controller = False
+
+        Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value
+        R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value
+        D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value
+        M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value
+
+        D = np.diag(D_diag)
+        M = np.reshape(M, (3, 3))
+
+        heading_ref = 0.0
+
+        self.LQR = LQRController(M, D, Q, R, heading_ref)
+
+        self.x_ref = [5, 10, 0]
+        self.state = [0, 0, 0, 0, 0, 0]
+
+        self.enabled = False
+
+        controller_period = 0.1
+        self.controller_timer_ = self.create_timer(controller_period, self.controller_callback)
+
+        self.get_logger().info("lqr_controller_node started")
+
+    def state_cb(self, msg):
+        self.state = odometrymsg_to_state(msg)
+
+    def guidance_cb(self, msg):
+        self.x_ref = odometrymsg_to_state(msg)[:3]
+
+    def controller_callback(self):
+        if hasattr(self, 'state') and hasattr(self, 'x_ref') and self.active_controller:
+            self.LQR.linearize_model(self.state[2])
+            control_input = self.LQR.calculate_control_input(self.state, self.x_ref)
+            wrench_msg = Wrench()
+            wrench_msg.force.x  = control_input[0]
+            wrench_msg.force.y  = control_input[1]
+            wrench_msg.torque.z = control_input[2]
+            self.wrench_publisher_.publish(wrench_msg)
+
+    def active_controller_callback(self, msg):
+        if msg.data == 'DP':
+            self.active_controller = True
+        else:
+            self.active_controller = False
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LQRControllerNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

motion/lqr_controller/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lqr_controller</name>
+  <version>0.0.0</version>
+  <description>This is an implementation of the LQR DP controller for the ASV</description>
+  <maintainer email="[email protected]">vortex</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>python-transforms3d-pip</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>numpy</depend>
+  <depend>matplotlib</depend>
+  <depend>scipy</depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

simulation/asv_simulator/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.8)
+project(asv_simulator)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+asv_simulator/asv_simulator_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

simulation/asv_simulator/README.md

@@ -0,0 +1,16 @@
+# ASV simulator
+This package provides the implementation of the asv_simulator for the Vortex ASV.
+
+## Usage
+
+Launch controller and guidance `ros2 launch asv_setup hybridpath.launch.py`
+Launch the asv_simulator `ros2 launch asv_simulator asv_simulator.launch.py`
+
+## Configuration
+
+You can configure the settings of the simulator and waypoints to be used by modifying the parameters in the `config` directory. 
+
+## Foxglove 
+
+To visualize the simulation in Foxglove, launch foxglove bridge `ros2 run foxglove_bridge foxglove_bridge`
+