funky filter reference

guidance/dp_guidance/dp_guidance/dp_guidance_node.py

@@ -7,6 +7,7 @@
 from vortex_msgs.srv import Waypoint
 from nav_msgs.msg import Odometry
 from transforms3d.euler import quat2euler
+from std_msgs.msg import String
 
 from dp_guidance.conversions import odometrymsg_to_state, state_to_odometrymsg
 from dp_guidance.reference_filter import ReferenceFilter
@@ -21,8 +22,10 @@ def __init__(self):
             ])
 
         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.eta_subscriber_ = self.create_subscription(Odometry, '/sensor/seapath/odom/ned', self.eta_callback, 1)
         self.guidance_publisher = self.create_publisher(Odometry, 'guidance/dp/reference', 1)
+        self.active_controller_subscriber = self.create_subscription(String, 'mission/controller', self.active_controller_callback, 10)
+        self.active_controller = False
 
         # Get parameters
         self.dt = self.get_parameter('dp_guidance.dt').get_parameter_value().double_value
@@ -52,29 +55,36 @@ def waypoint_callback(self, request, response):
         response.success = True
         return response
 
+    def active_controller_callback(self, msg: String):
+        if msg.data == 'DP':
+            self.active_controller = True
+        else:
+            self.active_controller = False
+
     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
+        if self.waypoints_received and self.active_controller:
+            #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]}')
+            self.get_logger().info(f'eta_ref : {self.eta_ref}')
 
             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.get_logger().info('Waiting for controller switching')
                 self.waiting_message_printed = True
 
 def main(args=None):

guidance/dp_guidance/dp_guidance/reference_filter.py

@@ -7,7 +7,7 @@
 class ReferenceFilter:
     def __init__(self):
         zeta = 0.8
-        omega_b = 0.05
+        omega_b = 0.5
         omega_n = omega_b/np.sqrt(1-2*zeta**2 + np.sqrt(4*zeta**4 - 4*zeta**2 + 2))
 
         I = np.eye(3)

guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py

@@ -67,7 +67,7 @@ class HybridPathGenerator:
     """
     def __init__(self, WP: list[Point], r: int, lambda_val: float):
         # Convert the waypoints to a numpy array
-        WP_arr = np.array([[int(wp.x), int(wp.y)] for wp in WP])
+        WP_arr = np.array([[wp.x, wp.y] for wp in WP])
 
         if len(WP_arr) == 2: # The generator must have at least 3 waypoints to work
             self.WP = np.array([WP_arr[0], [(WP_arr[0][0] + WP_arr[1][0])/2, (WP_arr[0][1] + WP_arr[1][1])/2], WP_arr[1]])
@@ -296,7 +296,7 @@ def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> flo
 
         """
         signals = HybridPathSignals(path, s)
-        v_s = signals.get_vs()
+        v_s = signals.get_vs(u_desired)
         s_new = s + (v_s + w) * dt
         return s_new
 
@@ -314,12 +314,11 @@ class HybridPathSignals:
         Path (Path): The path object.
         s (float): The path parameter.
     """
-    def __init__(self, path: Path, s: float, u_desired: float = 0.5):
+    def __init__(self, path: Path, s: float):
         if not isinstance(path, Path):
             raise TypeError("path must be an instance of Path")
         self.path = path
         self.s = self._clamp_s(s, self.path.NumSubpaths)
-        self.u_desired = u_desired
 
     def _clamp_s(self, s: float, num_subpaths: int) -> float:
         """
@@ -445,7 +444,7 @@ def get_heading_second_derivative(self) -> float:
         psi_dder = (p_der[0] * p_ddder[1] - p_der[1] * p_ddder[0]) / (p_der[0]**2 + p_der[1]**2) - 2 * (p_der[0] * p_dder[1] - p_dder[0] * p_der[1]) * (p_der[0] * p_dder[0] - p_dder[1] * p_der[0]) / ((p_der[0]**2 + p_der[1]**2)**2)
         return psi_dder
 
-    def get_vs(self) -> float:
+    def get_vs(self, u_desired) -> float:
         """
         Calculate the reference velocity.
 
@@ -459,10 +458,10 @@ def get_vs(self) -> float:
         p_der = self.get_derivatives()[0]
 
         # Calculate the reference velocity
-        v_s = self.u_desired / np.linalg.norm(p_der)
+        v_s = u_desired / np.linalg.norm(p_der)
         return v_s
 
-    def get_vs_derivative(self) -> float:
+    def get_vs_derivative(self, u_desired) -> float:
         """
         Calculate the derivative of the reference velocity.
 
@@ -477,7 +476,7 @@ def get_vs_derivative(self) -> float:
         p_dder = self.get_derivatives()[1]
 
         # Calculate the derivative of the reference velocity
-        v_s_s = -self.u_desired * (np.dot(p_der, p_dder)) / (np.sqrt(p_der[0]**2 + p_der[1]**2)**3)
+        v_s_s = -u_desired * (np.dot(p_der, p_dder)) / (np.sqrt(p_der[0]**2 + p_der[1]**2)**3)
         return v_s_s
 
     def get_w(self, mu: float, eta: np.ndarray) -> float:

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, Int32
+from std_msgs.msg import Bool, Float32
 
 from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals
 
@@ -27,7 +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)
+        self.switching_criteria_subscriber = self.create_subscription(Float32, 'guidance/hybridpath/switching', self.switching_callback, 10)
 
         # Get parameters
         self.lambda_val = self.get_parameter('hybridpath_guidance.lambda_val').get_parameter_value().double_value
@@ -36,7 +36,9 @@ 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
+        self.switching_waypoint = 1.0
+
+        self.u_desired = 0.25 # Desired velocity
 
         # Flags for logging
         self.waypoints_received = False
@@ -58,13 +60,12 @@ def waypoint_callback(self, request, response):
 
         self.s = 0
         signals = HybridPathSignals(self.path, self.s)
-        self.u_desired = signals.u_desired
         self.w = signals.get_w(self.mu, self.eta)
 
         response.success = True
         return response
 
-    def switching_callback(self, msg: Int32):
+    def switching_callback(self, msg: Float32):
         print(msg.data)
         self.switching_waypoint = msg.data
 
@@ -91,8 +92,8 @@ def guidance_callback(self):
             hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder)
 
             hp_msg.w = signals.get_w(self.mu, self.eta)
-            hp_msg.v_s = signals.get_vs()
-            hp_msg.v_ss = signals.get_vs_derivative()
+            hp_msg.v_s = signals.get_vs(self.u_desired)
+            hp_msg.v_ss = signals.get_vs_derivative(self.u_desired)
 
             self.guidance_publisher.publish(hp_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, Int32
+from std_msgs.msg import String, Bool, Float32
 
 class MissionPlannerClient(Node):
     """
@@ -30,7 +30,7 @@ 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)
+        self.switching_waypoint_publisher = self.create_publisher(Float32, 'guidance/hybridpath/switching', 10)
 
     def timer_callback(self):
         msg = String()
@@ -39,8 +39,8 @@ def timer_callback(self):
         self.get_logger().info(f'Publishing: {self.active_controller}')
 
         # Switching criteria
-        msg = Int32()
-        msg.data = 1
+        msg = Float32()
+        msg.data = 1.0
         self.switching_waypoint_publisher.publish(msg)
 
     def guidance_callback(self, msg):

motion/lqr_controller/config/lqr_config.yaml

@@ -1,5 +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]
+      Q: [20.0, 20.0, 1.0, 1.0, 1.0, 1.0]  # State costs for [x, y, heading, x_dot, y_dot, heading_dot]
       R: [1.0, 1.0, 1.0]  # Control cost weight

motion/lqr_controller/lqr_controller/lqr_controller_node.py

@@ -47,7 +47,7 @@ def __init__(self):
 
         self.LQR = LQRController(M, D, Q, R, heading_ref)
 
-        self.x_ref = [5, 10, 0]
+        self.x_ref = [0, 0, 0]
         self.state = [0, 0, 0, 0, 0, 0]
 
         self.enabled = False

motion/pid_controller/CMakeLists.txt

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

motion/pid_controller/config/pid_config.yaml

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    pid_controller:
+      Kp: [23.04, 34.56, 4.032] # Proportional costs for [x, y, heading]
+      Ki: [2.76, 4.1, 0.48]  # Integral costs for [x, y, heading]
+      Kd: [28.7648, 43.1472, 5.048384]  # Derivative costs for [x, y, heading]

motion/pid_controller/launch/pid_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():
+    pid_controller_node = Node(
+            package='pid_controller',
+            executable='pid_controller_node.py',
+            name='pid_controller_node',
+            parameters=[
+                os.path.join(get_package_share_directory('pid_controller'),'config','pid_config.yaml'),
+                os.path.join(get_package_share_directory('asv_setup'), 'config', 'robots', 'freya.yaml')
+            ],
+            output='screen',
+        )
+    return LaunchDescription([
+        pid_controller_node
+    ])

motion/pid_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>pid_controller</name>
+  <version>0.0.0</version>
+  <description>This is an implementation of the PID 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>

motion/pid_controller/pid_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