latest pushes

humanoid_robot_intelligence_control_system_detection/src/follower.py

@@ -17,7 +17,7 @@
 
 import math
 
-def calculate_distance_to_object(camera_height, tilt, hip_pitch_offset, object_size):
+def calculate_distance_to(camera_height, tilt, hip_pitch_offset, object_size):
     distance = camera_height * math.tan(math.pi * 0.5 + tilt - hip_pitch_offset - object_size)
     return abs(distance)
 

humanoid_robot_intelligence_control_system_detection/src/follower_config.py

@@ -58,6 +58,92 @@ class FollowerConfig:
         self.curr_period_time = get_param('curr_period_time', self.curr_period_time)
         self.DEBUG_PRINT = get_param('debug_print', self.DEBUG_PRINT)
 
+    def reset(self):
+        """Reset all values to their initial state."""
+        self.__init__()
+
+    def set_fov(self, width, height):
+        """Set the field of view."""
+        self.FOV_WIDTH = width
+        self.FOV_HEIGHT = height
+
+    def set_camera_height(self, height):
+        """Set the camera height."""
+        self.CAMERA_HEIGHT = height
+
+    def set_thresholds(self, not_found_threshold):
+        """Set the thresholds."""
+        self.NOT_FOUND_THRESHOLD = not_found_threshold
+
+    def set_step_parameters(self, max_fb, max_rl, in_place_fb, min_fb, min_rl, unit_fb, unit_rl):
+        """Set the step parameters."""
+        self.MAX_FB_STEP = max_fb
+        self.MAX_RL_TURN = max_rl
+        self.IN_PLACE_FB_STEP = in_place_fb
+        self.MIN_FB_STEP = min_fb
+        self.MIN_RL_TURN = min_rl
+        self.UNIT_FB_STEP = unit_fb
+        self.UNIT_RL_TURN = unit_rl
+
+    def set_spot_offsets(self, fb_offset, rl_offset, angle_offset):
+        """Set the spot offsets."""
+        self.SPOT_FB_OFFSET = fb_offset
+        self.SPOT_RL_OFFSET = rl_offset
+        self.SPOT_ANGLE_OFFSET = angle_offset
+
+    def set_hip_pitch_offset(self, offset):
+        """Set the hip pitch offset."""
+        self.hip_pitch_offset = offset
+
+    def set_period_time(self, period_time):
+        """Set the current period time."""
+        self.curr_period_time = period_time
+
+    def update_accum_period_time(self, delta_time):
+        """Update the accumulated period time."""
+        self.accum_period_time += delta_time
+        if self.accum_period_time > (self.curr_period_time / 4):
+            self.accum_period_time = 0.0
+        return self.accum_period_time
+
+    def reset_accum_period_time(self):
+        """Reset the accumulated period time."""
+        self.accum_period_time = 0.0
+
+    def set_debug_print(self, debug_print):
+        """Set the debug print flag."""
+        self.DEBUG_PRINT = debug_print
+
+    def get_config_dict(self):
+        """Return a dictionary of all configuration parameters."""
+        return {
+            'FOV_WIDTH': self.FOV_WIDTH,
+            'FOV_HEIGHT': self.FOV_HEIGHT,
+            'CAMERA_HEIGHT': self.CAMERA_HEIGHT,
+            'NOT_FOUND_THRESHOLD': self.NOT_FOUND_THRESHOLD,
+            'MAX_FB_STEP': self.MAX_FB_STEP,
+            'MAX_RL_TURN': self.MAX_RL_TURN,
+            'IN_PLACE_FB_STEP': self.IN_PLACE_FB_STEP,
+            'MIN_FB_STEP': self.MIN_FB_STEP,
+            'MIN_RL_TURN': self.MIN_RL_TURN,
+            'UNIT_FB_STEP': self.UNIT_FB_STEP,
+            'UNIT_RL_TURN': self.UNIT_RL_TURN,
+            'SPOT_FB_OFFSET': self.SPOT_FB_OFFSET,
+            'SPOT_RL_OFFSET': self.SPOT_RL_OFFSET,
+            'SPOT_ANGLE_OFFSET': self.SPOT_ANGLE_OFFSET,
+            'hip_pitch_offset': self.hip_pitch_offset,
+            'curr_period_time': self.curr_period_time,
+            'accum_period_time': self.accum_period_time,
+            'DEBUG_PRINT': self.DEBUG_PRINT
+        }
+
+    def load_config_dict(self, config_dict):
+        """Load configuration from a dictionary."""
+        for key, value in config_dict.items():
+            if hasattr(self, key):
+                setattr(self, key, value)
+
+
 
 class FollowerInitializeConfig:
     def __init__(self):

humanoid_robot_intelligence_control_system_detection/src/tracker_config.py

@@ -40,6 +40,104 @@ class TrackerConfig:
         self.i_gain = get_param('i_gain', self.i_gain)
         self.d_gain = get_param('d_gain', self.d_gain)
 
+    def reset(self):
+        """Reset all values to their initial state."""
+        self.__init__()
+
+    def set_fov(self, width, height):
+        """Set the field of view."""
+        self.FOV_WIDTH = width
+        self.FOV_HEIGHT = height
+
+    def set_thresholds(self, not_found_threshold, waiting_threshold):
+        """Set the thresholds."""
+        self.NOT_FOUND_THRESHOLD = not_found_threshold
+        self.WAITING_THRESHOLD = waiting_threshold
+
+    def set_use_head_scan(self, use_head_scan):
+        """Set whether to use head scan."""
+        self.use_head_scan = use_head_scan
+
+    def set_debug_print(self, debug_print):
+        """Set the debug print flag."""
+        self.DEBUG_PRINT = debug_print
+
+    def set_pid_gains(self, p_gain, i_gain, d_gain):
+        """Set the PID gains."""
+        self.p_gain = p_gain
+        self.i_gain = i_gain
+        self.d_gain = d_gain
+
+    def get_pid_gains(self):
+        """Get the PID gains."""
+        return self.p_gain, self.i_gain, self.d_gain
+
+    def adjust_pid_gains(self, p_adjust=0, i_adjust=0, d_adjust=0):
+        """Adjust the PID gains by the given amounts."""
+        self.p_gain += p_adjust
+        self.i_gain += i_adjust
+        self.d_gain += d_adjust
+
+    def get_config_dict(self):
+        """Return a dictionary of all configuration parameters."""
+        return {
+            'FOV_WIDTH': self.FOV_WIDTH,
+            'FOV_HEIGHT': self.FOV_HEIGHT,
+            'NOT_FOUND_THRESHOLD': self.NOT_FOUND_THRESHOLD,
+            'WAITING_THRESHOLD': self.WAITING_THRESHOLD,
+            'use_head_scan': self.use_head_scan,
+            'DEBUG_PRINT': self.DEBUG_PRINT,
+            'p_gain': self.p_gain,
+            'i_gain': self.i_gain,
+            'd_gain': self.d_gain
+        }
+
+    def load_config_dict(self, config_dict):
+        """Load configuration from a dictionary."""
+        for key, value in config_dict.items():
+            if hasattr(self, key):
+                setattr(self, key, value)
+
+    def validate_config(self):
+        """Validate the configuration parameters."""
+        assert 0 < self.FOV_WIDTH < math.pi, "FOV_WIDTH must be between 0 and pi"
+        assert 0 < self.FOV_HEIGHT < math.pi, "FOV_HEIGHT must be between 0 and pi"
+        assert self.NOT_FOUND_THRESHOLD > 0, "NOT_FOUND_THRESHOLD must be positive"
+        assert self.WAITING_THRESHOLD > 0, "WAITING_THRESHOLD must be positive"
+        assert isinstance(self.use_head_scan, bool), "use_head_scan must be a boolean"
+        assert isinstance(self.DEBUG_PRINT, bool), "DEBUG_PRINT must be a boolean"
+        assert self.p_gain >= 0, "p_gain must be non-negative"
+        assert self.i_gain >= 0, "i_gain must be non-negative"
+        assert self.d_gain >= 0, "d_gain must be non-negative"
+
+    def print_config(self):
+        """Print the current configuration."""
+        for key, value in self.get_config_dict().items():
+            print(f"{key}: {value}")
+
+    def to_ros_param(self):
+        """Convert the configuration to a format suitable for ROS parameters."""
+        return {
+            'tracker': {
+                'fov': {
+                    'width': self.FOV_WIDTH,
+                    'height': self.FOV_HEIGHT
+                },
+                'thresholds': {
+                    'not_found': self.NOT_FOUND_THRESHOLD,
+                    'waiting': self.WAITING_THRESHOLD
+                },
+                'use_head_scan': self.use_head_scan,
+                'debug_print': self.DEBUG_PRINT,
+                'pid': {
+                    'p': self.p_gain,
+                    'i': self.i_gain,
+                    'd': self.d_gain
+                }
+            }
+        }
+
+
 
 class TrackerInitializeConfig:
     def __init__(self):