reference filter WIP

guidance/dp_guidance/config/dp_guidance_config.yaml

@@ -1,9 +1,4 @@
 /**:
   ros__parameters:
-    hybridpath_guidance:
-      lambda_val: 0.15 # Curvature constant
-      path_generator_order: 1 # Differentiability order
-      time_to_max_speed: 10.0 # Time to reach maximum speed
-      dt: 0.1 # Time step
-      u_desired: 0.5 # Desired speed
-      mu: 0.03 # Tuning parameter 
+    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

@@ -8,117 +8,59 @@
 from nav_msgs.msg import Odometry
 from transforms3d.euler import quat2euler
 
+from dp_guidance.conversions import odometrymsg_to_state
 from dp_guidance.hybridpath import HybridPathGenerator, HybridPathSignals
 
 class Guidance(Node):
     def __init__(self):
-        super().__init__("hp_guidance")
+        super().__init__("dp_guidance")
         self.declare_parameters(
             namespace='',
             parameters=[
-                ('dp_guidance.lambda_val', 0.15),
-                ('dp_guidance.path_generator_order', 1),
-                ('dp_guidance.time_to_max_speed', 10.0),
-                ('dp_guidance.dt', 0.1),
-                ('dp_guidance.u_desired', 0.5),
-                ('dp_guidance.mu', 0.03)
+                ('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.lambda_val = self.get_parameter('dp_guidance.lambda_val').get_parameter_value().double_value
-        self.path_generator_order = self.get_parameter('dp_guidance.path_generator_order').get_parameter_value().integer_value
         self.dt = self.get_parameter('dp_guidance.dt').get_parameter_value().double_value
-        self.u_desired = self.get_parameter('dp_guidance.u_desired').get_parameter_value().double_value
-        self.mu = self.get_parameter('dp_guidance.mu').get_parameter_value().double_value
-        self.eta = np.zeros(3)
 
         # Flags for logging
         self.waypoints_received = False
         self.waiting_message_printed = False
 
+        self.eta_d = np.array([0, 0, 0])
+
         # Timer for guidance
         timer_period = 0.1
         self.timer = self.create_timer(timer_period, self.guidance_callback)
 
     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
+        self.eta_d = self.waypoints[0] # Choosing first waypoint as desired eta
         self.waypoints_received = True
         self.waiting_message_printed = False  # Reset this flag to handle multiple waypoint sets
-        self.s = 0
-        signals = HybridPathSignals(self.path, self.s)
-        self.w = signals.get_w(self.mu, self.eta)
         response.success = True
         return response
 
     def eta_callback(self, msg: Odometry):
-        self.eta = self.odom_to_eta(msg)
+        self.eta = odometrymsg_to_state(msg)[:3]
 
     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)
-
-            pos = signals.get_position()
-            pos_der = signals.get_derivatives()[0]
-            pos_dder = signals.get_derivatives()[1]
-
-            psi = signals.get_heading()
-            psi_der = signals.get_heading_derivative()
-            psi_dder = signals.get_heading_second_derivative()
-
-            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)
-
-            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)
-
-            self.guidance_publisher.publish(hp_msg)
+
+            odom_msg = Odometry()
 
-            if self.s >= self.path.NumSubpaths:
-                self.waypoints_received = False
-                self.waiting_message_printed = False
-                self.get_logger().info('Last waypoint reached, s = %f' % self.s)
+            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
 
-    @staticmethod
-    def odom_to_eta(msg: Odometry) -> np.ndarray:
-        """
-        Converts an Odometry message to 3DOF eta vector.
-
-        Args:
-            msg (Odometry): The Odometry message to convert.
-
-        Returns:
-            np.ndarray: The eta vector.
-        """
-        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
-        yaw = quat2euler(orientation_list)[2]
-
-        state = np.array([x, y, yaw])
-        return state
-
 def main(args=None):
     rclpy.init(args=args)
     guidance = Guidance()

guidance/dp_guidance/dp_guidance/reference_filter.py

@@ -6,10 +6,6 @@
 
 class ReferenceFilter:
     def __init__(self):
-        self.init_system()
-
-    def init_system(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))