Added way to reinitialize waypoints through service call

vortexntnu · Jul 11, 2024 · 1cd14f8 · 1cd14f8
1 parent 7050c3b
commit 1cd14f8
Showing 1 changed file with 3 additions and 2 deletions.
diff --git 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: