tuning

guidance/dp_guidance/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.8)
+project(dp_guidance)
+
+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
+  dp_guidance/dp_guidance_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

guidance/dp_guidance/README.md

@@ -0,0 +1,15 @@
+# Dynamic Positioning (DP) Guidance
+
+This package provides the implementation of DP guidance for the Vortex ASV.
+
+## Usage
+
+To use the DP guidance launch it using: `ros2 launch dp_guidance dp_guidance.launch`
+
+To run with custom waypoints (replace example waypoints with actual waypoints, and add as many prefered):
+
+`ros2 service call waypoint_list vortex_msgs/srv/Waypoint "waypoint: [{x: 0.0, y: 0.0, z: 0.0}, {x: 5.0, y: 5.0, z: 0.0}]"`
+
+## Configuration
+
+You can configure the behavior of the hybrid path guidance by modifying the parameters in the `config` directory. 

guidance/dp_guidance/config/dp_guidance_config.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    dp_guidance:
+      dt: 0.1 # Time step

guidance/dp_guidance/dp_guidance/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

guidance/dp_guidance/dp_guidance/dp_guidance_node.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Pose2D
+from vortex_msgs.srv import Waypoint
+from nav_msgs.msg import Odometry
+from transforms3d.euler import quat2euler
+
+from dp_guidance.conversions import odometrymsg_to_state, state_to_odometrymsg
+from dp_guidance.reference_filter import ReferenceFilter
+
+class Guidance(Node):
+    def __init__(self):
+        super().__init__("dp_guidance")
+        self.declare_parameters(
+            namespace='',
+            parameters=[
+                ('dp_guidance.dt', 0.1)
+            ])
+
+        self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback)
+        self.eta_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1)
+        self.guidance_publisher = self.create_publisher(Odometry, 'guidance/dp/reference', 1)
+
+        # Get parameters
+        self.dt = self.get_parameter('dp_guidance.dt').get_parameter_value().double_value
+
+        # Flags for logging
+        self.waypoints_received = False
+        self.waiting_message_printed = False
+
+        self.init_pos = False
+
+        self.eta = np.array([0, 0, 0])
+        self.eta_ref = np.array([0, 0, 0])
+
+        self.xd = np.zeros(9)
+
+        self.reference_filter = ReferenceFilter()
+
+        # Timer for guidance
+        timer_period = 0.1
+        self.timer = self.create_timer(timer_period, self.guidance_callback)
+
+    def waypoint_callback(self, request, response):
+        self.waypoints = request.waypoint
+        self.get_logger().info(f'Received waypoints: {self.waypoints}')
+        self.waypoints_received = True
+        self.waiting_message_printed = False  # Reset this flag to handle multiple waypoint sets
+        response.success = True
+        return response
+
+    def eta_callback(self, msg: Odometry):
+        self.eta = odometrymsg_to_state(msg)[:3]
+
+    def guidance_callback(self):
+        if self.waypoints_received:
+            if not self.init_pos:
+                self.xd[0:3] = self.eta
+                self.init_pos = True
+            last_waypoint = self.waypoints[-1]
+            self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, 0])
+            x_next = self.reference_filter.step(self.eta_ref, self.xd)
+            self.xd = x_next
+            self.get_logger().info(f'x_next[0]: {x_next[0]}')
+            self.get_logger().info(f'x_next[0]: {x_next[1]}')
+            self.get_logger().info(f'x_next[0]: {x_next[2]}')
+
+            odom_msg = Odometry()
+            odom_msg = state_to_odometrymsg(x_next[:3])
+            self.guidance_publisher.publish(odom_msg)
+
+        else:
+            if not self.waiting_message_printed:
+                self.get_logger().info('Waiting for waypoints to be received')
+                self.waiting_message_printed = True
+
+def main(args=None):
+    rclpy.init(args=args)
+    guidance = Guidance()
+    rclpy.spin(guidance)
+    guidance.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

guidance/dp_guidance/dp_guidance/reference_filter.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import numpy as np
+import control
+import matplotlib.pyplot as plt
+
+class ReferenceFilter:
+    def __init__(self):
+        zeta = 0.8
+        omega_b = 0.05
+        omega_n = omega_b/np.sqrt(1-2*zeta**2 + np.sqrt(4*zeta**4 - 4*zeta**2 + 2))
+
+        I = np.eye(3)
+        Omega = 2*zeta*omega_n*I
+        # Gamma = omega_n**2*I
+        Delta = zeta*I
+        Ad = np.zeros((9,9))
+        Ad[0:3,3:6] = I
+        Ad[3:6,6:9] = I
+        Ad[6:9,0:3] = -Omega**3
+        Ad[6:9,3:6] = -(2*Delta+I)@Omega**2
+        Ad[6:9,6:9] = -(2*Delta+I)@Omega
+
+        Bd = np.zeros((9,3))
+        Bd[6:9,:] = Omega**3
+
+        sys = control.ss(Ad, Bd, np.zeros((9,9)), np.zeros((9,3)))
+        sysd = control.c2d(sys, 0.1)
+
+        self.Ad = sysd.A
+        self.Bd = sysd.B
+
+    def step(self, r, xd):
+        x_next = self.Ad@xd + self.Bd@r
+        return x_next
+
+    def get_eta(self, xd):
+        return xd[:,0:3]
+
+    def get_nu(self, xd):
+        nu = np.zeros((len(xd),3))
+        for i in range(len(xd)):
+            psi = xd[i,2]
+            nu[i,:] = (self.rotationMatrix(psi).transpose())@xd[i,3:6]
+        return nu
+
+    @staticmethod
+    def rotationMatrix(psi):
+        R = np.array([[np.cos(psi), -np.sin(psi), 0],
+                [np.sin(psi), np.cos(psi), 0],
+                [0, 0, 1]])
+        return R
+
+
+if __name__ == "__main__":
+    refFilter = ReferenceFilter()
+    ref_pos = np.array([5, 10, 0])
+    t = np.arange(0, 100, 0.1)
+    N = len(t)
+    x_d = np.zeros((N, 9))
+
+    for k in range(1,N):
+        x_d[k,:] = refFilter.step(ref_pos, x_d[k-1, :])
+
+    eta_d = refFilter.get_eta(x_d)
+    nu_d = refFilter.get_nu(x_d)
+    plt.figure()
+    plt.plot(t, eta_d[:,0])
+
+    plt.figure()
+    plt.plot(t, nu_d[:,0])
+    plt.show()

guidance/dp_guidance/launch/dp_guidance.launch.py

@@ -0,0 +1,16 @@
+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():
+    dp_guidance_node = Node(
+            package='dp_guidance',
+            executable='dp_guidance_node.py',
+            name='dp_guidance_node',
+            parameters=[os.path.join(get_package_share_directory('dp_guidance'),'config','dp_guidance_config.yaml')],
+            output='screen',
+        )
+    return LaunchDescription([
+        dp_guidance_node
+    ])

guidance/dp_guidance/package.xml

@@ -0,0 +1,21 @@
+<?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>dp_guidance</name>
+  <version>0.0.0</version>
+  <description>This package provides the implementation of hybrid path guidance for the Vortex ASV.</description>
+  <maintainer email="[email protected]">vortex</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>nav_msgs</depend>
+  <depend>python-transforms3d-pip</depend>
+  <depend>geometry_msgs</depend>
+  <depend>vortex_msgs</depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py

@@ -65,6 +65,7 @@ def waypoint_callback(self, request, response):
         return response
 
     def switching_callback(self, msg: Int32):
+        print(msg.data)
         self.switching_waypoint = msg.data
 
     def eta_callback(self, msg: Odometry):
@@ -100,7 +101,7 @@ def guidance_callback(self):
                 self.waiting_message_printed = False
                 self.get_logger().info('Last waypoint reached')
                 #self.switching_publisher.publish(True)
-            if self.s >= self.path.NumSubpaths-1:
+            if self.s >= self.path.NumSubpaths-self.switching_waypoint:
                 self.switching_publisher.publish(Bool(data=True))
 
         else:

motion/hybridpath_controller/config/hybridpath_controller_config.yaml

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

simulation/asv_simulator/asv_simulator/asv_simulator_node.py

@@ -189,13 +189,16 @@ def plot_matplotlib(self):
         state_y = [pose.pose.position.y for pose in self.state_path.poses]
         ref_x = [pose.pose.position.x for pose in self.xref_path.poses]
         ref_y = [pose.pose.position.y for pose in self.xref_path.poses]
+        waypoints_x = [point.x for point in self.waypoints]
+        waypoints_y = [point.y for point in self.waypoints]
 
         time = np.linspace(0, self.T, len(state_x))
         tau_time = np.linspace(0, self.T, len(self.tau_history))
 
         plt.figure()
         plt.plot(state_x, state_y, label='State Path')
         plt.plot(ref_x, ref_y, label='Reference Path')
+        plt.scatter(waypoints_x, waypoints_y, color='red', label='Waypoints')  # Plotting waypoints in red
         plt.title('ASV Path')
         plt.xlabel('X')
         plt.ylabel('Y')