latest pushes

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()

humanoid_robot_intelligence_control_system_detection/package.xml

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<!--
+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.
+-->
+
+<package format="3">
+  <name>humanoid_robot_intelligence_control_system_detection</name>
+  <version>0.0.1</version>
+  <description>
+    This Package is for Detection Math
+  </description>
+  <license>Apache 2.0</license>
+  <maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
+
+  <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
+
+  <!-- ROS 1 dependencies -->
+  <depend condition="$ROS_VERSION == 1">rospy</depend>
+
+  <!-- ROS 2 dependencies -->
+  <depend condition="$ROS_VERSION == 2">rclpy</depend>
+
+  <!-- Common dependencies -->
+  <depend>python3-opencv</depend>
+  <depend>python3-yaml</depend>
+  <depend>usb_cam</depend>
+
+  <export>
+    <build_type condition="$ROS_VERSION == 1">catkin</build_type>
+    <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+  </export>
+</package>

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 <https://www.gnu.org/licenses/>.
+
+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)

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 <https://www.gnu.org/licenses/>.
+
+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()

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 <https://www.gnu.org/licenses/>.
+
+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

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 <https://www.gnu.org/licenses/>.
+
+
+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()

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 <https://www.gnu.org/licenses/>.
+
+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