From 1cd14f8bb4f0edd655a4928b0e5f78c44f3d612b Mon Sep 17 00:00:00 2001
From: Andeshog <157955984+Andeshog@users.noreply.github.com>
Date: Thu, 11 Jul 2024 18:22:31 +0200
Subject: [PATCH] Added way to reinitialize waypoints through service call

---
 guidance/dp_guidance/dp_guidance/dp_guidance_node.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py
index 483b507..b0d754f 100755
--- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py
+++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py
@@ -53,6 +53,7 @@ def __init__(self):
     def waypoint_callback(self, request, response):
         self.waypoints = request.waypoint
         self.get_logger().info(f'Received waypoints: {self.waypoints}')
+        self.xd = np.zeros(9)
         self.waypoints_received = True
         self.waiting_message_printed = False  # Reset this flag to handle multiple waypoint sets
         response.success = True
@@ -77,8 +78,8 @@ def guidance_callback(self):
             # 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(self.eta_ref)
+            odom_msg = state_to_odometrymsg(x_next[:3])
+            # odom_msg = state_to_odometrymsg(self.eta_ref)
             self.guidance_publisher.publish(odom_msg)
 
         else: