From 660e951525f7799aa945cf4724f190ea338285ee Mon Sep 17 00:00:00 2001
From: Safak <safak.hazinedar@stud.fh-aachen.de>
Date: Mon, 24 Nov 2025 02:25:55 +0000
Subject: [PATCH] Optimierungen bzgl. sofortigem Stopp im gleichen Takt beim
 Erreichen des Ziels

---
 src/praktikum1/praktikum1/node_p1.py | 99 ++++++++++++++++++----------
 1 file changed, 64 insertions(+), 35 deletions(-)

diff --git a/src/praktikum1/praktikum1/node_p1.py b/src/praktikum1/praktikum1/node_p1.py
index ae00a7a..4be54a2 100644
--- a/src/praktikum1/praktikum1/node_p1.py
+++ b/src/praktikum1/praktikum1/node_p1.py
@@ -7,52 +7,81 @@ import math
 class CircleController(Node):
     def __init__(self):
         super().__init__('circle_controller')
+        
+        # Parameter
+        self.declare_parameter('linear_speed', 2.0)
+        self.declare_parameter('angular_speed', 1.0)
+        self.declare_parameter('goal_tolerance', 0.1) 
+        
+        self.linear_speed = self.get_parameter('linear_speed').value
+        self.angular_speed = self.get_parameter('angular_speed').value
+        self.goal_tolerance = self.get_parameter('goal_tolerance').value
+        
         self.publisher_ = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
         self.pose_subscriber = self.create_subscription(
             Pose, '/turtle1/pose', self.pose_callback, 10)
         
-        self.timer = self.create_timer(0.1, self.move_circle)
+        self.timer = self.create_timer(0.1, self.control_loop)
         
-        self.start_x = None
-        self.start_y = None
+        # Positionen
+        self.start_pose = None
         self.current_pose = None
-        self.circle_completed = False
-        self.moved = False  # Flag um Starttoleranz zu vermeiden
         
+        # Zustände
+        self.has_left_start_zone = False
+        self.finished = False
+
     def pose_callback(self, msg):
-        if self.start_x is None:
-            # Speichere Startposition
-            self.start_x = msg.x
-            self.start_y = msg.y
-            self.get_logger().info(f'Startposition: x={self.start_x:.2f}, y={self.start_y:.2f}')
-        
         self.current_pose = msg
-        
-        # Prüfe ob Schildkröte zum Start zurückgekehrt ist
-        if self.start_x is not None and self.moved:
-            distance = math.sqrt(
-                (msg.x - self.start_x)**2 + (msg.y - self.start_y)**2
-            )
-            # Anpassung auf 0.0098 von 0.01 beim wechsel auf ubuntu server mit x11 Forwarding
-            if distance < 0.0098:  
-                self.circle_completed = True
-                
-    def move_circle(self):
-        if self.circle_completed:
-            # Stoppe die Schildkröte
-            msg = Twist()
-            msg.linear.x = 0.0
-            msg.angular.z = 0.0
-            self.publisher_.publish(msg)
-            self.timer.cancel()
-            self.get_logger().info('Kreis beendet - zurück am Startpunkt!')
+        if self.start_pose is None:
+            self.start_pose = msg
+            self.get_logger().info(f'Start set at x={msg.x:.2f}, y={msg.y:.2f}')
+
+    def control_loop(self):
+        # Safety Check: Warten bis erste Pos da ist
+        if self.current_pose is None or self.start_pose is None:
             return
-            
+        
+        # Distanz zum Start (Euklidisch)
+        distance = math.sqrt(
+            (self.current_pose.x - self.start_pose.x)**2 + 
+            (self.current_pose.y - self.start_pose.y)**2
+        )
+        
+        self.check_goal_condition(distance)
+        
+        if self.finished:
+            self.stop_robot()   # ..() am Ende vergessen hahaha
+        else:
+            self.drive_circle()
+
+    def check_goal_condition(self, distance):
+        # 1. Startzone verlassen ?
+        if distance > 2 * self.goal_tolerance:
+            self.has_left_start_zone = True
+        
+        # 2. anschließend innerhalb des Toleranzbereichs ?
+        if self.has_left_start_zone and distance < self.goal_tolerance:
+            self.finished = True 
+            self.get_logger().info(f'Goal reached detected at x={self.current_pose.x:.2f}, y={self.current_pose.y:.2f}')           
+
+    def drive_circle(self):
+        # Drehung initieren und mit Werten versehen
         msg = Twist()
-        msg.linear.x = 2.0
-        msg.angular.z = 1.0
+        msg.linear.x = self.linear_speed
+        msg.angular.z = self.angular_speed
         self.publisher_.publish(msg)
-        self.moved = True
+
+    def stop_robot(self):
+        # einen leere Drehung übergeben
+        msg = Twist() 
+        self.publisher_.publish(msg)
+        
+        # Nur einmal loggen
+        if self.timer:
+            self.get_logger().info(f'Goal reached performed at x={self.current_pose.x:.2f}, y={self.current_pose.y:.2f}')
+            self.timer.cancel()
+            self.timer = None
 
 def main(args=None):
     rclpy.init(args=args)
@@ -62,4 +91,4 @@ def main(args=None):
     rclpy.shutdown()
 
 if __name__ == '__main__':
-    main()
+    main()
\ No newline at end of file