Added reference filter for x_ref guidance

guidance/dp_guidance/CMakeLists.txt

@@ -24,12 +24,4 @@ install(PROGRAMS
   DESTINATION lib/${PROJECT_NAME}
 )
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  find_package(ament_cmake_pytest REQUIRED)
-  ament_add_pytest_test(python_tests tests)
-  set(ament_cmake_copyright_FOUND TRUE)
-  set(ament_cmake_cpplint_FOUND TRUE)
-endif()
-
 ament_package()

guidance/dp_guidance/dp_guidance/dp_guidance_node.py

@@ -1,15 +1,15 @@
 #!/usr/bin/env python3
+
 import numpy as np
 import rclpy
 from rclpy.node import Node
 from geometry_msgs.msg import Pose2D
-from vortex_msgs.msg import HybridpathReference
 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
-from dp_guidance.hybridpath import HybridPathGenerator, HybridPathSignals
+from dp_guidance.conversions import odometrymsg_to_state, state_to_odometrymsg
+from dp_guidance.reference_filter import ReferenceFilter
 
 class Guidance(Node):
     def __init__(self):
@@ -31,16 +31,22 @@ def __init__(self):
         self.waypoints_received = False
         self.waiting_message_printed = False
 
-        self.eta_d = np.array([0, 0, 0])
+        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.get_logger().info('Received waypoints, generating path...')
         self.waypoints = request.waypoint
-        self.eta_d = self.waypoints[0] # Choosing first waypoint as desired eta
+        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
@@ -51,9 +57,19 @@ def eta_callback(self, msg: Odometry):
 
     def guidance_callback(self):
         if self.waypoints_received:
-
-            odom_msg = Odometry()
+            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:

motion/pid_controller/pid_controller/pid_controller_node.py

@@ -39,8 +39,6 @@ def __init__(self):
         self.x_ref = np.array([0, 0, 0])
         self.state = np.array([0, 0, 0, 0, 0, 0])
 
-        self.x_ref = np.array([10, 10, 0])
-
         self.enabled = False
 
         self.controller_period = 0.1

simulation/asv_simulator/asv_simulator/asv_simulator_node.py

@@ -51,7 +51,8 @@ def __init__(self):
 
         # subscribe to thrust input and guidance
         self.wrench_subscriber_ = self.create_subscription(Wrench, "thrust/wrench_input", self.wrench_input_cb, 1)
-        self.guidance_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.guidance_cb, 1)
+        self.guidance_hp_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.guidance_hp_cb, 1)
+        self.guidance_dp_subscriber_ = self.create_subscription(Odometry, 'guidance/dp/reference', self.guidance_dp_cb, 1)
 
         # publish state (seapath)
         self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odom/ned", 1)
@@ -129,12 +130,19 @@ def show_progressbar(self):
         bar = '[' + '=' * filled_length + ' ' * (10 - filled_length) + ']'
         self.get_logger().info(f"Progress: {bar} {progress:.1f}%")
 
-    def guidance_cb(self, msg):
+    def guidance_hp_cb(self, msg):
         if isinstance(msg, HybridpathReference):
             self.x_ref[:3] = np.array([msg.eta_d.x, msg.eta_d.y, msg.eta_d.theta])
             self.yaw_ref_publisher_.publish(Float32(data=self.x_ref[2]))
         else:
             self.get_logger().error(f"Received message of type {type(msg).__name__}, expected HybridpathReference")
+
+    def guidance_dp_cb(self, msg):
+        if isinstance(msg, Odometry):
+            self.x_ref[:3] = odometrymsg_to_state(msg)[:3]
+            self.yaw_ref_publisher_.publish(Float32(data=self.x_ref[2]))
+        else:
+            self.get_logger().error(f"Received message of type {type(msg).__name__}, expected Odometry")
 
     def wrench_input_cb(self, msg):
         self.tau = np.array([msg.force.x, msg.force.y, msg.torque.z])
@@ -194,8 +202,8 @@ def plot_matplotlib(self):
         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.plot(state_x, state_y, label='State Path') 
+        plt.plot(ref_x, ref_y, label='Reference Path', linestyle='--')
         plt.title('ASV Path')
         plt.xlabel('X')
         plt.ylabel('Y')

simulation/asv_simulator/config/asv_sim_config.yaml

@@ -4,5 +4,5 @@
       plot_matplotlib_enabled: true
       progress_bar_enabled: true
       hybridpath_path_following: false
-      T: 200
+      T: 700
       waypoints: [0.0, 0.0, 5.0, 5.0, 5.0, 10.0] # [x1, y1, x2, y2, x3, y3]