diff --git a/led_adjust_on_object_detect_process_script.py b/led_adjust_on_object_detect_process_script.py index a668ed9..5c702d4 100644 --- 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 index 6bd1624..93f681f 100644 --- 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: