diff --git a/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt b/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt new file mode 100644 index 0000000..c427fc3 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt @@ -0,0 +1,66 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_detection) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + ) + + catkin_package( + CATKIN_DEPENDS + rospy + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/follower.py + src/follower_config.py + src/tracker.py + src/tracker_config.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/follower.py + src/follower_config.py + src/tracker.py + src/tracker_config.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_detection/package.xml b/humanoid_robot_intelligence_control_system_detection/package.xml new file mode 100644 index 0000000..fa6c357 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/package.xml @@ -0,0 +1,46 @@ + + + + + humanoid_robot_intelligence_control_system_detection + 0.0.1 + + This Package is for Detection Math + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + + + rclpy + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_detection/setup.py b/humanoid_robot_intelligence_control_system_detection/setup.py new file mode 100644 index 0000000..ca93ba9 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/follower.py', "src/follower_config.py", 'src/tracker.py', "src/tracker_config.py"], + packages=['humanoid_robot_intelligence_control_system_detection'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower.py b/humanoid_robot_intelligence_control_system_detection/src/follower.py new file mode 100644 index 0000000..8d32487 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/follower.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + +def calculate_distance_to_object(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) + +def calculate_footstep(current_x_move, target_distance, current_r_angle, target_angle, delta_time, config): + next_movement = current_x_move + target_distance = max(0, target_distance) + fb_goal = min(target_distance * 0.1, config.MAX_FB_STEP) + config.accum_period_time += delta_time + if config.accum_period_time > (config.curr_period_time / 4): + config.accum_period_time = 0.0 + if (target_distance * 0.1 / 2) < current_x_move: + next_movement -= config.UNIT_FB_STEP + else: + next_movement += config.UNIT_FB_STEP + fb_goal = min(next_movement, fb_goal) + fb_move = max(fb_goal, config.MIN_FB_STEP) + + rl_goal = 0.0 + if abs(target_angle) * 180 / math.pi > 5.0: + rl_offset = abs(target_angle) * 0.2 + rl_goal = min(rl_offset, config.MAX_RL_TURN) + rl_goal = max(rl_goal, config.MIN_RL_TURN) + rl_angle = min(abs(current_r_angle) + config.UNIT_RL_TURN, rl_goal) + if target_angle < 0: + rl_angle *= -1 + else: + rl_angle = 0 + + return fb_move, rl_angle + +def calculate_delta_time(curr_time, prev_time): + return (curr_time - prev_time).to_sec() diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower_config.py b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py new file mode 100644 index 0000000..8bd53d1 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + + +class FollowerConfig: + def __init__(self): + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.CAMERA_HEIGHT = 0.46 + self.NOT_FOUND_THRESHOLD = 50 + self.MAX_FB_STEP = 40.0 * 0.001 + self.MAX_RL_TURN = 15.0 * math.pi / 180 + self.IN_PLACE_FB_STEP = -3.0 * 0.001 + self.MIN_FB_STEP = 5.0 * 0.001 + self.MIN_RL_TURN = 5.0 * math.pi / 180 + self.UNIT_FB_STEP = 1.0 * 0.001 + self.UNIT_RL_TURN = 0.5 * math.pi / 180 + self.SPOT_FB_OFFSET = 0.0 * 0.001 + self.SPOT_RL_OFFSET = 0.0 * 0.001 + self.SPOT_ANGLE_OFFSET = 0.0 + self.hip_pitch_offset = 7.0 + self.curr_period_time = 0.6 + self.accum_period_time = 0.0 + self.DEBUG_PRINT = False + + def update_from_params(self, get_param): + self.FOV_WIDTH = get_param('fov_width', self.FOV_WIDTH) + self.FOV_HEIGHT = get_param('fov_height', self.FOV_HEIGHT) + self.CAMERA_HEIGHT = get_param('camera_height', self.CAMERA_HEIGHT) + self.NOT_FOUND_THRESHOLD = get_param('not_found_threshold', self.NOT_FOUND_THRESHOLD) + self.MAX_FB_STEP = get_param('max_fb_step', self.MAX_FB_STEP) + self.MAX_RL_TURN = get_param('max_rl_turn', self.MAX_RL_TURN) + self.IN_PLACE_FB_STEP = get_param('in_place_fb_step', self.IN_PLACE_FB_STEP) + self.MIN_FB_STEP = get_param('min_fb_step', self.MIN_FB_STEP) + self.MIN_RL_TURN = get_param('min_rl_turn', self.MIN_RL_TURN) + self.UNIT_FB_STEP = get_param('unit_fb_step', self.UNIT_FB_STEP) + self.UNIT_RL_TURN = get_param('unit_rl_turn', self.UNIT_RL_TURN) + self.SPOT_FB_OFFSET = get_param('spot_fb_offset', self.SPOT_FB_OFFSET) + self.SPOT_RL_OFFSET = get_param('spot_rl_offset', self.SPOT_RL_OFFSET) + self.SPOT_ANGLE_OFFSET = get_param('spot_angle_offset', self.SPOT_ANGLE_OFFSET) + self.hip_pitch_offset = get_param('hip_pitch_offset', self.hip_pitch_offset) + self.curr_period_time = get_param('curr_period_time', self.curr_period_time) + self.DEBUG_PRINT = get_param('debug_print', self.DEBUG_PRINT) + + +class FollowerInitializeConfig: + def __init__(self): + self.count_not_found = 0 + self.count_to_approach = 0 + self.on_tracking = False + self.approach_object_position = "NotFound" + self.current_pan = -10 + self.current_tilt = -10 + self.current_x_move = 0.005 + self.current_r_angle = 0 + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + self.current_object_bottom = 0.0 + self.tracking_status = "NotFound" + self.object_position = None + + + def update_from_params(self, get_param): + self.count_not_found = get_param('initial_count_not_found', self.count_not_found) + self.count_to_approach = get_param('initial_count_to_approach', self.count_to_approach) + self.on_tracking = get_param('initial_on_tracking', self.on_tracking) + self.approach_object_position = get_param('initial_approach_object_position', self.approach_object_position) + self.current_pan = get_param('initial_current_pan', self.current_pan) + self.current_tilt = get_param('initial_current_tilt', self.current_tilt) + self.current_x_move = get_param('initial_current_x_move', self.current_x_move) + self.current_r_angle = get_param('initial_current_r_angle', self.current_r_angle) + self.x_error_sum = get_param('initial_x_error_sum', self.x_error_sum) + self.y_error_sum = get_param('initial_y_error_sum', self.y_error_sum) + self.current_object_bottom = get_param('initial_object_bottom', self.current_object_bottom) + self.tracking_status = get_param('initial_tracking_status', self.tracking_status) + + + def reset(self): + """Reset all values to their initial state.""" + self.__init__() + + def update_object_position(self, x, y, z): + """Update the object position.""" + self.object_position.x = x + self.object_position.y = y + self.object_position.z = z + + def increment_count_not_found(self): + """Increment the count of frames where object was not found.""" + self.count_not_found += 1 + + def reset_count_not_found(self): + """Reset the count of frames where object was not found.""" + self.count_not_found = 0 + + def increment_count_to_approach(self): + """Increment the count to approach.""" + self.count_to_approach += 1 + + def reset_count_to_approach(self): + """Reset the count to approach.""" + self.count_to_approach = 0 + + def set_tracking_status(self, status): + """Set the current tracking status.""" + self.tracking_status = status + + def update_error_sums(self, x_error, y_error): + """Update the cumulative error sums.""" + self.x_error_sum += x_error + self.y_error_sum += y_error + + def reset_error_sums(self): + """Reset the cumulative error sums.""" + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + + def update_current_object_position(self, pan, tilt, bottom): + """Update the current object position.""" + self.current_pan = pan + self.current_tilt = tilt + self.current_object_bottom = bottom + + def update_movement(self, x_move, r_angle): + """Update the current movement parameters.""" + self.current_x_move = x_move + self.current_r_angle = r_angle + + def is_tracking(self): + """Check if tracking is currently active.""" + return self.on_tracking + + def start_tracking(self): + """Start the tracking process.""" + self.on_tracking = True + + def stop_tracking(self): + """Stop the tracking process.""" + self.on_tracking = False + + def set_approach_position(self, position): + """Set the approach position of the object.""" + self.approach_object_position = position diff --git a/humanoid_robot_intelligence_control_system_detection/src/tracker.py b/humanoid_robot_intelligence_control_system_detection/src/tracker.py new file mode 100644 index 0000000..369d84e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/tracker.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +import math + +def calculate_error(object_position, FOV_WIDTH, FOV_HEIGHT): + x_error = -math.atan(object_position.x * math.tan(FOV_WIDTH)) + y_error = -math.atan(object_position.y * math.tan(FOV_HEIGHT)) + return x_error, y_error + +def calculate_error_target(error, error_diff, error_sum, p_gain, i_gain, d_gain): + return error * p_gain + error_diff * d_gain + error_sum * i_gain + +def calculate_delta_time(curr_time, prev_time): + return (curr_time - prev_time).to_sec() diff --git a/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py new file mode 100644 index 0000000..5310436 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + +class TrackerConfig: + def __init__(self): + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.NOT_FOUND_THRESHOLD = 50 + self.WAITING_THRESHOLD = 5 + self.use_head_scan = True + self.DEBUG_PRINT = False + self.p_gain = 0.4 + self.i_gain = 0.0 + self.d_gain = 0.0 + + def update_from_params(self, get_param): + self.FOV_WIDTH = get_param('fov_width', self.FOV_WIDTH) + self.FOV_HEIGHT = get_param('fov_height', self.FOV_HEIGHT) + self.NOT_FOUND_THRESHOLD = get_param('not_found_threshold', self.NOT_FOUND_THRESHOLD) + self.WAITING_THRESHOLD = get_param('waiting_threshold', self.WAITING_THRESHOLD) + self.use_head_scan = get_param('use_head_scan', self.use_head_scan) + self.DEBUG_PRINT = get_param('debug_print', self.DEBUG_PRINT) + self.p_gain = get_param('p_gain', self.p_gain) + self.i_gain = get_param('i_gain', self.i_gain) + self.d_gain = get_param('d_gain', self.d_gain) + + +class TrackerInitializeConfig: + def __init__(self): + self.count_not_found = 0 + self.on_tracking = False + self.current_object_pan = 0.0 + self.current_object_tilt = 0.0 + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + self.current_object_bottom = 0.0 + self.tracking_status = "NotFound" + self.object_position = None + + + def update_from_params(self, get_param): + self.count_not_found = get_param('initial_count_not_found', self.count_not_found) + self.on_tracking = get_param('initial_on_tracking', self.on_tracking) + self.current_object_pan = get_param('initial_object_pan', self.current_object_pan) + self.current_object_tilt = get_param('initial_object_tilt', self.current_object_tilt) + self.x_error_sum = get_param('initial_x_error_sum', self.x_error_sum) + self.y_error_sum = get_param('initial_y_error_sum', self.y_error_sum) + self.current_object_bottom = get_param('initial_object_bottom', self.current_object_bottom) + self.tracking_status = get_param('initial_tracking_status', self.tracking_status) + + + def reset(self): + """Reset all values to their initial state.""" + self.__init__() + + + def update_object_position(self, x, y, z): + """Update the object position.""" + self.object_position.x = x + self.object_position.y = y + self.object_position.z = z + + + def increment_count_not_found(self): + """Increment the count of frames where object was not found.""" + self.count_not_found += 1 + + + def reset_count_not_found(self): + """Reset the count of frames where object was not found.""" + self.count_not_found = 0 + + + def set_tracking_status(self, status): + """Set the current tracking status.""" + self.tracking_status = status + + + def update_error_sums(self, x_error, y_error): + """Update the cumulative error sums.""" + self.x_error_sum += x_error + self.y_error_sum += y_error + + + def reset_error_sums(self): + """Reset the cumulative error sums.""" + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + + + def update_current_object_position(self, pan, tilt, bottom): + """Update the current object position.""" + self.current_object_pan = pan + self.current_object_tilt = tilt + self.current_object_bottom = bottom + + + def is_tracking(self): + """Check if tracking is currently active.""" + return self.on_tracking + + + def start_tracking(self): + """Start the tracking process.""" + self.on_tracking = True + + + def stop_tracking(self): + """Stop the tracking process.""" + self.on_tracking = False