Fixed guidance callback for DP

vortexntnu · Apr 28, 2024 · 1056e4f · 1056e4f
1 parent 0026c06
commit 1056e4f
Showing 1 changed file with 13 additions and 5 deletions.
diff --git a/simulation/asv_simulator/asv_simulator/asv_simulator_node.py b/simulation/asv_simulator/asv_simulator/asv_simulator_node.py
@@ -51,8 +51,9 @@ 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)
         self.posestamped_publisher_ = self.create_publisher(PoseStamped, "/sensor/seapath/posestamped/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')