From c4a68ec22e315aec6209ca2eff0a86e9423996a8 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Thu, 25 Jul 2024 10:07:05 -0400 Subject: [PATCH] latest pushes --- .../src/follower.py | 2 +- .../src/follower_config.py | 86 ++++++++++++++++ .../src/tracker_config.py | 98 +++++++++++++++++++ 3 files changed, 185 insertions(+), 1 deletion(-) diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower.py b/humanoid_robot_intelligence_control_system_detection/src/follower.py index 8d32487..b19ab4c 100644 --- a/humanoid_robot_intelligence_control_system_detection/src/follower.py +++ b/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) diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower_config.py b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py index 8bd53d1..ae09c01 100644 --- a/humanoid_robot_intelligence_control_system_detection/src/follower_config.py +++ b/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): diff --git a/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py index 5310436..43bd55a 100644 --- a/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py +++ b/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):