Fixed a bug with the pt track speed calculation

nepi-engine · Mar 2, 2024 · fda2114 · fda2114
1 parent 3d05074
commit fda2114
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 9 deletions.
diff --git a/led_adjust_on_object_detect_process_script.py b/led_adjust_on_object_detect_process_script.py
@@ -52,7 +52,7 @@
 OBJECT_LABEL_OF_INTEREST = "person"
 LED_LEVEL_MAX = 0.3
 WD_TIMEOUT_SEC = 4
-AVG_LENGTH = 5
+AVG_LENGTH = 2
 
 #Set LED Control ROS Topic Name (or partial name)
 LED_CONTROL_TOPIC_NAME = "lsx/set_intensity"
@@ -160,7 +160,7 @@ def object_detected_callback(self,bounding_box_msg):
         center_ratios = [1-box_abs_error_x_ratio] # ignore vertical
         mean_center_ratio = statistics.mean(center_ratios)
         print("Target center ratio: " + "%.2f" % (mean_center_ratio))
-        intensity = self.led_level_max *  mean_center_ratio**2
+        intensity = self.led_level_max *  mean_center_ratio**4
         self.intensity_history = np.roll(self.intensity_history,1)
         self.intensity_history[0]=intensity
         avg_intensity = np.mean(self.intensity_history)

diff --git a/pantilt_object_tracker_action_script.py b/pantilt_object_tracker_action_script.py
@@ -67,7 +67,7 @@
 
 # Pan and Tilt tracking settings
 PTX_MAX_TRACK_SPEED_RATIO = 1.0
-PTX_MIN_TRACK_SPEED_RATIO = 0.05
+PTX_MIN_TRACK_SPEED_RATIO = 0.1
 PTX_OBJECT_TILT_OFFSET_RATIO = 0.15 # Adjust tilt center to lower or raise the calculated object center
 PTX_OBJ_CENTERED_BUFFER_RATIO = 0.15 # Hysteresis band about center of image for tracking purposes
 
@@ -284,7 +284,7 @@ def pt_track_box(self,object_error_x_ratio, object_error_y_ratio):
         #print("Object not centered in frame")
         # Now set the speed proportional to average error
         speed_control_value = PTX_MIN_TRACK_SPEED_RATIO + \
-                              (PTX_MAX_TRACK_SPEED_RATIO-PTX_MIN_TRACK_SPEED_RATIO) * max(object_error_x_ratio,object_error_y_ratio)
+                              (PTX_MAX_TRACK_SPEED_RATIO-PTX_MIN_TRACK_SPEED_RATIO) * max(abs(object_error_x_ratio),abs(object_error_y_ratio))
         #print("Current track speed ratio: " + "%.2f" % (speed_control_value))
         self.set_pt_speed_ratio_pub.publish(speed_control_value)
         # Per-axis adjustment
@@ -295,12 +295,13 @@ def pt_track_box(self,object_error_x_ratio, object_error_y_ratio):
           self.pan_scan_direction = - np.sign(object_error_x_ratio)
         else:
           self.pan_scan_direction = np.sign(object_error_x_ratio)
-        print("X Error: " + "%.2f" % (object_error_x_ratio))
-        print("New Scan Dir: " + str(self.pan_scan_direction))
+        #print("X Error: " + "%.2f" % (object_error_x_ratio))
+        #print("New Scan Dir: " + str(self.pan_scan_direction))
 
 
 
   def move_pan_rel_ratio(self,pan_rel_ratio):
+    print("Pan Track Info")
     print(self.current_pan_ratio)
     print(pan_rel_ratio)
     if self.status.reverse_yaw_control == False:
@@ -317,13 +318,14 @@ def move_pan_rel_ratio(self,pan_rel_ratio):
       self.set_pt_pan_ratio_pub.publish(pan_ratio)
 
   def move_tilt_rel_ratio(self,tilt_rel_ratio):
-    print(self.current_tilt_ratio)
-    print(tilt_rel_ratio)
+    #print("Tilt Track Info")
+    #print(self.current_tilt_ratio)
+    #print(tilt_rel_ratio)
     if self.status.reverse_pitch_control == True:
       tilt_ratio = self.current_tilt_ratio - tilt_rel_ratio
     else:
       tilt_ratio = self.current_tilt_ratio + tilt_rel_ratio
-    print(tilt_ratio)
+    #print(tilt_ratio)
     if tilt_ratio < 0:
       tilt_ratio = 0
     elif tilt_ratio > 1: