.

guidance/dp_guidance/dp_guidance/dp_guidance_node.py

@@ -37,6 +37,7 @@ def __init__(self):
 
         self.init_pos = False
         self.eta_received = False
+        self.eta_logger = True
 
         self.eta = np.array([0, 0, 0])
         self.eta_ref = np.array([0, 0, 0])
@@ -68,20 +69,22 @@ def guidance_callback(self):
                 self.get_logger().info(f"Reference initialized at {self.xd[0:3]}")
                 self.init_pos = True
             last_waypoint = self.waypoints[-1]
-            self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, 0])
+            self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, self.eta[2]])
             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])
+            # odom_msg = state_to_odometrymsg(x_next[:3])
+            odom_msg = state_to_odometrymsg(self.eta_ref)
             self.guidance_publisher.publish(odom_msg)
 
         else:
-            if not self.eta_received:
+            if not self.eta_received and self.eta_logger:
                 self.get_logger().info("Waiting for eta")
+                self.eta_logger = False
 
             if not self.waiting_message_printed:
                 self.get_logger().info('Waiting for waypoints to be received')

guidance/hybridpath_guidance/CMakeLists.txt

@@ -22,6 +22,7 @@ install(DIRECTORY
 install(PROGRAMS
   hybridpath_guidance/hybridpath_guidance_node.py
   hybridpath_guidance/waypoint_node.py
+  hybridpath_guidance/noisy_odom_publisher.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml

@@ -4,4 +4,4 @@
       lambda_val: 0.15 # Curvature constant
       path_generator_order: 1 # Differentiability order
       dt: 0.1 # Time step
-      mu: 0.15 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind
+      mu: 0.25 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind

guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py

@@ -66,6 +66,40 @@ class HybridPathGenerator:
         path (Path): The path object.
     """
     def __init__(self, WP: list[Point], r: int, lambda_val: float):
+        self.WP = WP
+        self.r = r
+        self.lambda_val = lambda_val
+        self.order = 2*r + 1
+
+    def create_path(self, WP: list[Point]) -> None:
+        """
+        Create a path object.
+
+        Args:
+            WP (list[Point]): A list of waypoints.
+
+        Returns:
+            None
+        """
+        self.update_waypoints(WP)
+        self._initialize_path()
+        self._calculate_subpaths()
+
+    def _initialize_path(self):
+        self.path = Path()
+
+        self.path.coeff.a = []
+        self.path.coeff.b = []
+        self.path.coeff.a_der = []
+        self.path.coeff.b_der = []
+        self.path.LinSys.A = None
+        self.path.LinSys.bx = []
+        self.path.LinSys.by = []
+
+        self.path.NumSubpaths = len(self.WP) - 1
+        self.path.Order = self.order
+
+    def update_waypoints(self, WP: list[Point]) -> None:
         # Convert the waypoints to a numpy array
         WP_arr = np.array([[wp.x, wp.y] for wp in WP])
 
@@ -77,13 +111,6 @@ def __init__(self, WP: list[Point], r: int, lambda_val: float):
 
         else:
             self.WP = WP_arr
-        self.r = r
-        self.lambda_val = lambda_val
-        self.order = 2*r + 1
-        self.path = Path()
-        self.path.NumSubpaths = len(self.WP) - 1
-        self.path.Order = self.order
-        self._calculate_subpaths()
 
     def _construct_A_matrix(self) -> np.ndarray:
         """
@@ -280,6 +307,12 @@ def _calculate_subpaths(self) -> None:
             for k, (a, b) in enumerate(zip(a_derivatives, b_derivatives)):
                 self._append_derivatives(k, a, b)
 
+    def get_path(self) -> Path:
+        """
+        Get the hybrid path.
+        """
+        return self.path
+
     @staticmethod
     def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> float:
         """
@@ -295,7 +328,9 @@ def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> flo
             float: The updated position along the hybrid path.
 
         """
-        signals = HybridPathSignals(path, s)
+        signals = HybridPathSignals()
+        signals.update_path(path)
+        signals.update_s(s)
         v_s = signals.get_vs(u_desired)
         s_new = s + (v_s + w) * dt
         return s_new
@@ -314,13 +349,14 @@ class HybridPathSignals:
         Path (Path): The path object.
         s (float): The path parameter.
     """
-    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)
+    def __init__(self):
+        # if not isinstance(path, Path):
+        #     raise TypeError("path must be an instance of Path")
+        self.path = None
+        self.s = None
 
-    def _clamp_s(self, s: float, num_subpaths: int) -> float:
+    @staticmethod
+    def _clamp_s(s: float, num_subpaths: int) -> float:
         """
         Ensures s is within the valid range of [0, num_subpaths].
 
@@ -506,4 +542,29 @@ def get_w(self, mu: float, eta: np.ndarray) -> float:
         w = (mu / np.linalg.norm(eta_d_s)) * np.dot(eta_d_s, error)
 
         return w
+
+    def update_path(self, path: Path) -> None:
+        """
+        Update the path object.
+
+        Args:
+            path (Path): The new path object.
+
+        Returns:
+            None
+        """
+        self.path = path
+        self.update_s(0.)
+
+    def update_s(self, s: float) -> None:
+        """
+        Update the path parameter.
+
+        Args:
+            s (float): The new path parameter.
+
+        Returns:
+            None
+        """
+        self.s = self._clamp_s(s, self.path.NumSubpaths)
 

guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py

@@ -2,14 +2,15 @@
 import numpy as np
 import rclpy
 from rclpy.node import Node
-from geometry_msgs.msg import Pose2D
-from std_msgs.msg import Float32
+from geometry_msgs.msg import Pose2D, Point
+from std_msgs.msg import Float32, Float64MultiArray
 from vortex_msgs.msg import HybridpathReference
-from vortex_msgs.srv import Waypoint
+from vortex_msgs.srv import Waypoint, DesiredVelocity
 from nav_msgs.msg import Odometry
 from transforms3d.euler import quat2euler
 from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals
 from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy
+import threading
 
 
 qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, 
@@ -28,8 +29,12 @@ def __init__(self):
             ])
 
         self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback)
+        self.u_desired_server = self.create_service(DesiredVelocity, 'u_desired', self.u_desired_callback)
         self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile)
         self.yaw_publisher = self.create_publisher(Float32, 'yaw', 1)
+        self.s_publisher = self.create_publisher(Float32, 's', 1)
+        self.w_publisher = self.create_publisher(Float32, 'w', 1)
+        self.wp_arr_publisher = self.create_publisher(Float64MultiArray, 'waypoints', 1)
         self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1)
 
         # Get parameters
@@ -39,74 +44,128 @@ def __init__(self):
         self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value
         self.eta = np.zeros(3)
 
-        self.u_desired = 0.2 # Desired velocity
+        self.u_desired = 0.3 # Desired velocity
+
+        self.waypoints = []
+        self.path = None
+        self.s = 0.
+        self.w = 0.
+
+        # Initialize path generator
+        self.generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val)
+
+        # Initialize signals
+        self.signals = HybridPathSignals()
 
         # Flags for logging
         self.waypoints_received = False
         self.waiting_message_printed = False
+        self.first_pos_flag = False
+        self.eta_received = False
 
         # Timer for guidance
         timer_period = 0.1
         self.timer = self.create_timer(timer_period, self.guidance_callback)
 
+        self.lock = threading.Lock()
+
+    def u_desired_callback(self, request, response):
+        self.u_desired = request.u_desired
+        self.get_logger().info(f"Received desired velocity: {self.u_desired}")
+        response.success = True
+        return response
+
+
     def waypoint_callback(self, request, response):
-        self.get_logger().info('Received waypoints, generating path...')
-        self.waypoints = request.waypoint
 
-        generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val)
-        self.path = generator.path
+        with self.lock:
+            if self.eta_received:
+                self.waypoints = [Point(x=self.eta[0], y=self.eta[1])]
 
-        self.waypoints_received = True
-        self.waiting_message_printed = False  # Reset this flag to handle multiple waypoint sets
+                self.get_logger().info('Received waypoints, generating path...')
 
-        self.s = 0
-        signals = HybridPathSignals(self.path, self.s)
-        self.w = signals.get_w(self.mu, self.eta)
-
+                new_waypoints = request.waypoint
+
+                for point in new_waypoints:
+                    self.waypoints.append(point)
+
+                self.generator.create_path(self.waypoints)
+                self.path = self.generator.get_path()
+
+                self.waypoints_received = True
+                self.waiting_message_printed = False  # Reset this flag to handle multiple waypoint sets
+                self.first_pos_flag = False
+
+                self.s = 0.
+                self.signals.update_path(self.path)
+                self.w = self.signals.get_w(self.mu, self.eta)
+
+                wp_arr = Float64MultiArray()
+                wp_list = self.generator.WP.tolist()
+                wp_arr.data = [coordinate for wp in wp_list for coordinate in wp]
+                self.wp_arr_publisher.publish(wp_arr)
+
         response.success = True
+
         return response
 
     def eta_callback(self, msg: Odometry):
         yaw_msg = Float32()
         self.eta = self.odom_to_eta(msg)
-        yaw = float(self.eta[2])
-        yaw_msg.data = yaw
+        self.yaw = float(self.eta[2])
+        yaw_msg.data = self.yaw
         self.yaw_publisher.publish(yaw_msg)
+        self.eta_received = True
 
     def guidance_callback(self):
-        if self.waypoints_received:
-            self.s = HybridPathGenerator.update_s(self.path, self.dt, self.u_desired, self.s, self.w)
-            signals = HybridPathSignals(self.path, self.s)
-            self.w = signals.get_w(self.mu, self.eta)
+        with self.lock:
+            if self.waypoints_received:
+                self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w)
+                self.signals.update_s(self.s)
+                self.w = self.signals.get_w(self.mu, self.eta)
+
+                pos = self.signals.get_position()
 
-            pos = signals.get_position()
-            pos_der = signals.get_derivatives()[0]
-            pos_dder = signals.get_derivatives()[1]
+                if not self.first_pos_flag:
+                    self.get_logger().info(f"First position: {pos}")
+                    self.first_pos_flag = True
 
-            psi = 0.#signals.get_heading()
-            psi_der = 0.#signals.get_heading_derivative()
-            psi_dder = 0.#signals.get_heading_second_derivative()
+                # pos[0] = self.eta[0]
+                pos_der = self.signals.get_derivatives()[0]
+                pos_dder = self.signals.get_derivatives()[1]
 
-            hp_msg = HybridpathReference()
-            hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi)
-            hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der)
-            hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder)
+                psi = 0. #signals.get_heading()
+                psi_der = 0.#signals.get_heading_derivative()
+                psi_dder = 0.#signals.get_heading_second_derivative()
 
-            hp_msg.w = signals.get_w(self.mu, self.eta)
-            hp_msg.v_s = signals.get_vs(self.u_desired)
-            hp_msg.v_ss = signals.get_vs_derivative(self.u_desired)
+                hp_msg = HybridpathReference()
+                hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi)
+                hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der)
+                hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder)
 
-            self.guidance_publisher.publish(hp_msg)
+                hp_msg.w = self.signals.get_w(self.mu, self.eta)
+                hp_msg.v_s = self.signals.get_vs(self.u_desired)
+                hp_msg.v_ss = self.signals.get_vs_derivative(self.u_desired)
 
-            if self.s >= self.path.NumSubpaths:
-                self.waypoints_received = False
-                self.waiting_message_printed = False
-                self.get_logger().info('Last waypoint reached')
+                self.guidance_publisher.publish(hp_msg)
 
-        else:
-            if not self.waiting_message_printed:
-                self.get_logger().info('Waiting for waypoints to be received')
-                self.waiting_message_printed = True
+                if self.s >= self.path.NumSubpaths:
+                    self.waypoints_received = False
+                    self.waiting_message_printed = False
+                    self.get_logger().info('Last waypoint reached')
+
+            else:
+                if not self.waiting_message_printed:
+                    self.get_logger().info('Waiting for waypoints to be received')
+                    self.waiting_message_printed = True
+
+        s_msg = Float32()
+        s_msg.data = self.s
+        self.s_publisher.publish(s_msg)
+
+        w_msg = Float32()
+        w_msg.data = self.w
+        self.w_publisher.publish(w_msg)
 
     @staticmethod
     def odom_to_eta(msg: Odometry) -> np.ndarray:
@@ -129,7 +188,10 @@ def odom_to_eta(msg: Odometry) -> np.ndarray:
         # Convert quaternion to Euler angles
         yaw = quat2euler(orientation_list)[2]
 
+        # yaw = np.deg2rad(yaw)
+
         state = np.array([x, y, yaw])
+
         return state
 
 def main(args=None):