Updated TestController

air_navigation_examples/__init__.py

 import air_navigation
 import sys
+from air_project_interfaces import Humans
+import rclpy
+from rclpy.node import Node
+import math
 
 class TestController(air_navigation.Controller):
   def __init__(self):
-    print("TestController constructor")
+    print("✅ TestController constructor START")
     super().__init__()
-  
+    print("✅ TestController constructor END")
+
+    # Use the controller's internal ROS 2 node methods
+    self.create_subscription(
+        Humans,
+        '/humans',
+        self.humans_callback,
+        10
+    )
+
+    self.closest_human_distance = float('inf')
+    print("✅ Subscribed to /humans using internal node")
+
+  def humans_callback(self, msg):
+    min_dist = float('inf')
+    for human in msg.humans:
+        dist = math.sqrt(human.position.x**2 + human.position.y**2)
+        min_dist = min(min_dist, dist)
+    self.closest_human_distance = min_dist
+    print(f"👥 Closest human at {min_dist:.2f} meters")
+
   def configure(self, name):
     print("TestController configure")
 
@@ -20,17 +44,28 @@ class TestController(air_navigation.Controller):
 
   def computeVelocityCommands(self, pose, velocity, goal):
     print("TestController computeVelocityCommands:", pose, velocity, goal)
+
+    base_speed = 1.0
+    slow_down_radius = 2.0
+    if self.closest_human_distance < slow_down_radius:
+      scale = self.closest_human_distance / slow_down_radius
+      speed = max(0.1, base_speed * scale)
+    else:
+      speed = base_speed
+
+    print(f"🚗 Adjusted speed: {speed:.2f}")
+
     t = air_navigation.TwistStamped()
-    t.twist.linear.x = 1.0
-    t.twist.angular.z = 2.0
+    t.twist.linear.x = speed
+    t.twist.angular.z = 0.0
     return t
 
   def setPlan(self, path):
     print("TestController setPlan:", path)    
 
   def setSpeedLimit(self, speed_limit, percentage):
-    print("TestController setSpeedLimit:", speed_limit, percentage)    
-
+    print("TestController setSpeedLimit:", speed_limit, percentage)
+    
 class TrajectoryCritic(air_navigation.TrajectoryCritic):
   def __init__(self):
     super().__init__()