diff --git a/README.md b/README.md index 3ee5208..a3dd60f 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,26 @@ -# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos +# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Architecture [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) +## Mobile and ROS Control System +- [![Bellande's Internal Sensor Mobile iOS API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_ios_api) +- [![Bellande's External Sensor Mobile iOS API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_ios_api) +- [![Bellande's Functionality Mobile iOS API](https://img.shields.io/badge/Bellande's%20Functionality%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_mobile_ios_api) +- [![Bellande's Internal Sensor Mobile Android API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_android_api) +- [![Bellande's External Sensor Mobile Android API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_android_api) +- [![Bellande's Functionality Mobile Android API](https://img.shields.io/badge/Bellande's%20Functionality%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_mobile_android_api) +- [![Bellande's Internal Sensor Web API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_web_api) +- [![Bellande's External Sensor Web API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_web_api) +- [![Bellande's Functionality Web API](https://img.shields.io/badge/Bellande's%20Functionality%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_web_api) +- [![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_functionality_ros_api) +- [![Bellande's Algorithm through Bellande MODELS](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20MODELS-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_ros_models) + + ## 🤖 Explore Humanoid Robot Intelligence with Us! -Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System architecture repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. ## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository @@ -14,47 +28,34 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos reposit ### 🚀 Key Repository Stats -- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community -- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Forked by enthusiasts -- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Watched by curious minds +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Watched by curious minds -- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Actively addressing issues -- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Welcoming contributions -- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Licensed under Apache 2.0 +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Licensed under Apache 2.0 -- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Latest updates -- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic) Engaging with the community +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture/traffic) Engaging with the community --- ## 🌐 Repository Website -Explore our [repository website](https://robotics-sensors.github.io/bellande_humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. - ---- - -### 🔄 Updates and Versions - -- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_demos) -- **Old Version/Previous Used for Different Context:** [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) +Explore our [repository website](https://robotics-sensors.github.io/bellande_humanoid_robot_intelligence_control_system_architecture) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. --- # 🎉 Latest Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_demos/releases/) - ---- - -## 📢 Important Announcement - -The repository has recently undergone significant updates. Older commits and codes are covered under the previous license, while new commits and codes fall under the new license. +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture/releases/) --- 🚀 For the latest versions and updates, visit our organization: [Robotics-Sensors](https://github.com/Robotics-Sensors). -### 🧑‍💼 Maintainer +### 🧑‍💼 Maintainer & Author Meet our dedicated maintainer, **Ronaldson Bellande**. @@ -62,4 +63,4 @@ Meet our dedicated maintainer, **Ronaldson Bellande**. ## 📄 License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE). +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/blob/main/LICENSE). diff --git a/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt new file mode 100644 index 0000000..48ef842 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt @@ -0,0 +1,77 @@ +# 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_configure) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/face_tracker.py + src/face_follower.py + src/object_tracker.py + src/object_follower.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/face_tracker.py + src/face_follower.py + src/object_tracker.py + src/object_follower.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_configure/launch/face_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py new file mode 100644 index 0000000..44a7599 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py @@ -0,0 +1,112 @@ +# 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 os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follower.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_face_detector", "face_detection_processor.py", "name:=face_detection_processor_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_configure", "face_follower.py", "name:=face_follower_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_face_detector', + executable='face_detection_processor.py', + name='face_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_configure', + executable='face_follower.py', + name='face_follower_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py new file mode 100644 index 0000000..d7d94dc --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py @@ -0,0 +1,112 @@ +# 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 os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_tracker.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_face_tracker", "face_detection_processor.py", "name:=face_detection_processor_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_configure", "face_tracker.py", "name:=face_tracker_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_face_tracker', + executable='face_detection_processor.py', + name='face_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_configure', + executable='face_tracker.py', + name='face_tracker_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py new file mode 100644 index 0000000..2788682 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py @@ -0,0 +1,112 @@ +# 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 os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follower.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_configure", "object_follower.py", "name:=object_follower_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_configure', + executable='object_follower.py', + name='object_follower_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py new file mode 100644 index 0000000..49307ae --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py @@ -0,0 +1,112 @@ +# 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 os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_tracker.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_object_tracker", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_configure", "object_tracker.py", "name:=object_tracker_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_tracker', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_configure', + executable='object_tracker.py', + name='object_tracker_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/package.xml b/humanoid_robot_intelligence_control_system_configure/package.xml new file mode 100644 index 0000000..740f1f7 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/package.xml @@ -0,0 +1,61 @@ + + + + + humanoid_robot_intelligence_control_system_configure + 0.0.1 + + This Package is for Configuration of Tracker and Follower related to it + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime + + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_configure/setup.py b/humanoid_robot_intelligence_control_system_configure/setup.py new file mode 100644 index 0000000..690d947 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/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/face_tracker.py', "src/face_follower.py", "src/object_tracker.py", "src/object_follower.py"], + packages=['humanoid_robot_intelligence_control_system_configure'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py new file mode 100644 index 0000000..f12b5a4 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -0,0 +1,236 @@ +#!/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 os +import sys +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String +from geometry_msgs.msg import Point +from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam +from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam + +from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to, calculate_footstep, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig + + +class FaceFollower: + def __init__(self, ros_version): + self.ros_version = ros_version + self.initialize_config = FollowerInitializeConfig() + self.config = FollowerConfig() + self.initialize_ros() + self.setup_ros_communication() + + def initialize_ros(self): + if self.ros_version == '1': + rospy.init_node('face_follower') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = rclpy.create_node('face_follower') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() + + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) + + def setup_ros_communication(self): + if self.ros_version == '1': + self.current_joint_states_sub = rospy.Subscriber( + "/humanoid_robot_intelligence_control_system/goal_joint_states", JointState, self.current_joint_states_callback) + self.set_walking_command_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/command", String, queue_size=1) + self.set_walking_param_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) + self.get_walking_param_client = rospy.ServiceProxy( + "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) + self.face_detection_sub = rospy.Subscriber( + "/face_detection_result", Point, self.face_detection_callback) + self.prev_time = rospy.Time.now() + else: + self.current_joint_states_sub = self.node.create_subscription( + JointState, "/humanoid_robot_intelligence_control_system/goal_joint_states", self.current_joint_states_callback, 10) + self.set_walking_command_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/walking/command", 1) + self.set_walking_param_pub = self.node.create_publisher( + WalkingParam, "/humanoid_robot_intelligence_control_system/walking/set_params", 1) + self.get_walking_param_client = self.node.create_client( + GetWalkingParam, "/humanoid_robot_intelligence_control_system/walking/get_params") + self.face_detection_sub = self.node.create_subscription( + Point, "/face_detection_result", self.face_detection_callback, 10) + self.prev_time = self.node.get_clock().now() + + def start_following(self): + self.initialize_config.on_tracking = True + self.logger.info("Start face following") + self.set_walking_command("start") + result = self.get_walking_param() + if result: + self.config.hip_pitch_offset = self.current_walking_param.hip_pitch_offset + self.config.curr_period_time = self.current_walking_param.period_time + else: + self.config.hip_pitch_offset = 7.0 * math.pi / 180 + self.config.curr_period_time = 0.6 + + def stop_following(self): + self.initialize_config.on_tracking = False + self.initialize_config.count_to_approach = 0 + self.logger.info("Stop face following") + self.set_walking_command("stop") + + def current_joint_states_callback(self, msg): + for i, name in enumerate(msg.name): + if name == "head_pan": + self.initialize_config.current_pan = msg.position[i] + elif name == "head_tilt": + self.initialize_config.current_tilt = msg.position[i] + + def process_following(self, x_angle, y_angle, face_size): + curr_time = rospy.Time.now() if self.ros_version == '1' else self.node.get_clock().now() + delta_time = calculate_delta_time(curr_time, self.prev_time) + self.prev_time = curr_time + + self.initialize_config.count_not_found = 0 + + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.logger.error("Failed to get current angle of head joints.") + self.set_walking_command("stop") + self.initialize_config.on_tracking = False + self.initialize_config.approach_face_position = "NotFound" + return False + + self.initialize_config.approach_face_position = "OutOfRange" + + distance_to_face = calculate_distance_to( + self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, face_size) + + distance_to_approach = 0.22 + + if (distance_to_face < distance_to_approach) and (abs(x_angle) < 25.0): + self.initialize_config.count_to_approach += 1 + if self.initialize_config.count_to_approach > 20: + self.set_walking_command("stop") + self.initialize_config.on_tracking = False + self.initialize_config.approach_face_position = "OnLeft" if x_angle > 0 else "OnRight" + return True + elif self.initialize_config.count_to_approach > 15: + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0) + return False + else: + self.initialize_config.count_to_approach = 0 + + distance_to_walk = distance_to_face - distance_to_approach + fb_move, rl_angle = calculate_footstep( + self.initialize_config.current_x_move, distance_to_walk, self.initialize_config.current_r_angle, self.initialize_config.current_pan, delta_time, self.config) + self.set_walking_param(fb_move, 0, rl_angle) + return False + + def decide_face_position(self, x_angle, y_angle): + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.initialize_config.approach_face_position = "NotFound" + return + face_x_angle = self.initialize_config.current_pan + x_angle + self.initialize_config.approach_face_position = "OnLeft" if face_x_angle > 0 else "OnRight" + + def wait_following(self): + self.initialize_config.count_not_found += 1 + if self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD * 0.5: + self.set_walking_param(0.0, 0.0, 0.0) + + def set_walking_command(self, command): + if command == "start": + self.get_walking_param() + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0, True) + msg = String() + msg.data = command + self.set_walking_command_pub.publish(msg) + + def set_walking_param(self, x_move, y_move, rotation_angle, balance=False): + self.current_walking_param.balance_enable = balance + self.current_walking_param.x_move_amplitude = x_move + self.config.SPOT_FB_OFFSET + self.current_walking_param.y_move_amplitude = y_move + self.config.SPOT_RL_OFFSET + self.current_walking_param.angle_move_amplitude = rotation_angle + self.config.SPOT_ANGLE_OFFSET + self.set_walking_param_pub.publish(self.current_walking_param) + self.initialize_config.current_x_move = x_move + self.initialize_config.current_r_angle = rotation_angle + + def get_walking_param(self): + if self.ros_version == '1': + try: + response = self.get_walking_param_client() + self.current_walking_param = response.parameters + return True + except rospy.ServiceException as e: + self.logger.error("Failed to get walking parameters: %s" % e) + return False + else: + future = self.get_walking_param_client.call_async(GetWalkingParam.Request()) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + self.current_walking_param = future.result().parameters + return True + else: + self.logger.error('Service call failed %r' % (future.exception(),)) + return False + + def face_detection_callback(self, msg): + if self.initialize_config.on_tracking: + x_angle = msg.x + y_angle = msg.y + face_size = msg.z + self.process_following(x_angle, y_angle, face_size) + else: + self.wait_following() + + def run(self): + if self.ros_version == '1': + rospy.spin() + else: + rclpy.spin(self.node) + +def main(ros_version): + try: + follower = FaceFollower(ros_version) + follower.start_following() + follower.run() + except Exception as e: + if ros_version == '1': + rospy.logerr(f"An error occurred: {e}") + else: + rclpy.shutdown() + print(f"An error occurred: {e}") + +if __name__ == '__main__': + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + import rospy + try: + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": + import rclpy + try: + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS2 main") + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py new file mode 100644 index 0000000..a7776ab --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py @@ -0,0 +1,194 @@ +#!/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 rospy +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String, Float64MultiArray +from humanoid_robot_intelligence_control_system_ball_detector.msg import CircleSetStamped +from geometry_msgs.msg import Point + +class FaceTracker: + def __init__(self): + rospy.init_node('face_tracker') + + 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.count_not_found = 0 + self.on_tracking = False + self.current_face_pan = 0 + self.current_face_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + self.current_face_bottom = 0 + self.tracking_status = "NotFound" + self.DEBUG_PRINT = False + + param_nh = rospy.get_param("~") + self.p_gain = param_nh.get("p_gain", 0.4) + self.i_gain = param_nh.get("i_gain", 0.0) + self.d_gain = param_nh.get("d_gain", 0.0) + + rospy.loginfo(f"face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") + + self.head_joint_offset_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0) + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0) + + self.face_position_sub = rospy.Subscriber( + "/face_detector_node/face_position", Point, self.face_position_callback) + self.face_tracking_command_sub = rospy.Subscriber( + "/face_tracker/command", String, self.face_tracker_command_callback) + + self.face_position = Point() + self.prev_time = rospy.Time.now() + + def face_position_callback(self, msg): + self.face_position = msg + + def face_tracker_command_callback(self, msg): + if msg.data == "start": + self.start_tracking() + elif msg.data == "stop": + self.stop_tracking() + elif msg.data == "toggle_start": + if not self.on_tracking: + self.start_tracking() + else: + self.stop_tracking() + + def start_tracking(self): + self.on_tracking = True + rospy.loginfo("Start face tracking") if self.DEBUG_PRINT else None + + def stop_tracking(self): + self.go_init() + self.on_tracking = False + rospy.loginfo("Stop face tracking") if self.DEBUG_PRINT else None + self.current_face_pan = 0 + self.current_face_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + + def set_using_head_scan(self, use_scan): + self.use_head_scan = use_scan + + def process_tracking(self): + if not self.on_tracking: + self.face_position.z = 0 + self.count_not_found = 0 + return "NotFound" + + if self.face_position.z <= 0: + self.count_not_found += 1 + if self.count_not_found < self.WAITING_THRESHOLD: + if self.tracking_status in ["Found", "Waiting"]: + tracking_status = "Waiting" + else: + tracking_status = "NotFound" + elif self.count_not_found > self.NOT_FOUND_THRESHOLD: + self.scan_face() + self.count_not_found = 0 + tracking_status = "NotFound" + else: + tracking_status = "NotFound" + else: + self.count_not_found = 0 + tracking_status = "Found" + + if tracking_status != "Found": + self.tracking_status = tracking_status + self.current_face_pan = 0 + self.current_face_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + return tracking_status + + x_error = -math.atan(self.face_position.x * math.tan(self.FOV_WIDTH)) + y_error = -math.atan(self.face_position.y * math.tan(self.FOV_HEIGHT)) + face_size = self.face_position.z + + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + self.prev_time = curr_time + + x_error_diff = (x_error - self.current_face_pan) / delta_time + y_error_diff = (y_error - self.current_face_tilt) / delta_time + self.x_error_sum += x_error + self.y_error_sum += y_error + x_error_target = x_error * self.p_gain + x_error_diff * self.d_gain + self.x_error_sum * self.i_gain + y_error_target = y_error * self.p_gain + y_error_diff * self.d_gain + self.y_error_sum * self.i_gain + + if self.DEBUG_PRINT: + rospy.loginfo(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") + rospy.loginfo(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") + rospy.loginfo(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}") + rospy.loginfo(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") + + self.publish_head_joint(x_error_target, y_error_target) + + self.current_face_pan = x_error + self.current_face_tilt = y_error + self.current_face_bottom = face_size + + self.face_position.z = 0 + + self.tracking_status = tracking_status + return tracking_status + + def publish_head_joint(self, pan, tilt): + min_angle = 1 * math.pi / 180 + if abs(pan) < min_angle and abs(tilt) < min_angle: + return + + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [pan, tilt] + + self.head_joint_offset_pub.publish(head_angle_msg) + + def go_init(self): + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [0.0, 0.0] + + self.head_joint_pub.publish(head_angle_msg) + + def scan_face(self): + if not self.use_head_scan: + return + + scan_msg = String() + scan_msg.data = "scan" + + self.head_scan_pub.publish(scan_msg) + +if __name__ == '__main__': + try: + tracker = FaceTracker() + rate = rospy.Rate(30) + while not rospy.is_shutdown(): + tracker.process_tracking() + rate.sleep() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py new file mode 100644 index 0000000..ebe736f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py @@ -0,0 +1,236 @@ +#!/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 os +import sys +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String +from geometry_msgs.msg import Point +from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam +from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam + +from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to, calculate_footstep, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig + + +class ObjectFollower: + def __init__(self, ros_version): + self.ros_version = ros_version + self.initialize_config = FollowerInitializeConfig() + self.config = FollowerConfig() + self.initialize_ros() + self.setup_ros_communication() + + def initialize_ros(self): + if self.ros_version == '1': + rospy.init_node('object_follower') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = rclpy.create_node('object_follower') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() + + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) + + def setup_ros_communication(self): + if self.ros_version == '1': + self.current_joint_states_sub = rospy.Subscriber( + "/humanoid_robot_intelligence_control_system/goal_joint_states", JointState, self.current_joint_states_callback) + self.set_walking_command_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/command", String, queue_size=1) + self.set_walking_param_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) + self.get_walking_param_client = rospy.ServiceProxy( + "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) + self.object_detection_sub = rospy.Subscriber( + "/object_detection_result", Point, self.object_detection_callback) + self.prev_time = rospy.Time.now() + else: + self.current_joint_states_sub = self.node.create_subscription( + JointState, "/humanoid_robot_intelligence_control_system/goal_joint_states", self.current_joint_states_callback, 10) + self.set_walking_command_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/walking/command", 1) + self.set_walking_param_pub = self.node.create_publisher( + WalkingParam, "/humanoid_robot_intelligence_control_system/walking/set_params", 1) + self.get_walking_param_client = self.node.create_client( + GetWalkingParam, "/humanoid_robot_intelligence_control_system/walking/get_params") + self.object_detection_sub = self.node.create_subscription( + Point, "/object_detection_result", self.object_detection_callback, 10) + self.prev_time = self.node.get_clock().now() + + def start_following(self): + self.initialize_config.on_tracking = True + self.logger.info("Start Object following") + self.set_walking_command("start") + result = self.get_walking_param() + if result: + self.config.hip_pitch_offset = self.current_walking_param.hip_pitch_offset + self.config.curr_period_time = self.current_walking_param.period_time + else: + self.config.hip_pitch_offset = 7.0 * math.pi / 180 + self.config.curr_period_time = 0.6 + + def stop_following(self): + self.initialize_config.on_tracking = False + self.initialize_config.count_to_approach = 0 + self.logger.info("Stop Object following") + self.set_walking_command("stop") + + def current_joint_states_callback(self, msg): + for i, name in enumerate(msg.name): + if name == "head_pan": + self.initialize_config.current_pan = msg.position[i] + elif name == "head_tilt": + self.initialize_config.current_tilt = msg.position[i] + + def process_following(self, x_angle, y_angle, object_size): + curr_time = rospy.Time.now() if self.ros_version == '1' else self.node.get_clock().now() + delta_time = calculate_delta_time(curr_time, self.prev_time) + self.prev_time = curr_time + + self.initialize_config.count_not_found = 0 + + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.logger.error("Failed to get current angle of head joints.") + self.set_walking_command("stop") + self.initialize_config.on_tracking = False + self.initialize_config.approach_object_position = "NotFound" + return False + + self.initialize_config.approach_object_position = "OutOfRange" + + distance_to_object = calculate_distance_to( + self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, object_size) + + distance_to_approach = 0.22 + + if (distance_to_object < distance_to_approach) and (abs(x_angle) < 25.0): + self.initialize_config.count_to_approach += 1 + if self.initialize_config.count_to_approach > 20: + self.set_walking_command("stop") + self.initialize_config.on_tracking = False + self.initialize_config.approach_object_position = "OnLeft" if x_angle > 0 else "OnRight" + return True + elif self.initialize_config.count_to_approach > 15: + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0) + return False + else: + self.initialize_config.count_to_approach = 0 + + distance_to_walk = distance_to_object - distance_to_approach + fb_move, rl_angle = calculate_footstep( + self.initialize_config.current_x_move, distance_to_walk, self.initialize_config.current_r_angle, self.initialize_config.current_pan, delta_time, self.config) + self.set_walking_param(fb_move, 0, rl_angle) + return False + + def decide_object_position(self, x_angle, y_angle): + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.initialize_config.approach_object_position = "NotFound" + return + object_x_angle = self.initialize_config.current_pan + x_angle + self.initialize_config.approach_object_position = "OnLeft" if object_x_angle > 0 else "OnRight" + + def wait_following(self): + self.initialize_config.count_not_found += 1 + if self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD * 0.5: + self.set_walking_param(0.0, 0.0, 0.0) + + def set_walking_command(self, command): + if command == "start": + self.get_walking_param() + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0, True) + msg = String() + msg.data = command + self.set_walking_command_pub.publish(msg) + + def set_walking_param(self, x_move, y_move, rotation_angle, balance=False): + self.current_walking_param.balance_enable = balance + self.current_walking_param.x_move_amplitude = x_move + self.config.SPOT_FB_OFFSET + self.current_walking_param.y_move_amplitude = y_move + self.config.SPOT_RL_OFFSET + self.current_walking_param.angle_move_amplitude = rotation_angle + self.config.SPOT_ANGLE_OFFSET + self.set_walking_param_pub.publish(self.current_walking_param) + self.initialize_config.current_x_move = x_move + self.initialize_config.current_r_angle = rotation_angle + + def get_walking_param(self): + if self.ros_version == '1': + try: + response = self.get_walking_param_client() + self.current_walking_param = response.parameters + return True + except rospy.ServiceException as e: + self.logger.error("Failed to get walking parameters: %s" % e) + return False + else: + future = self.get_walking_param_client.call_async(GetWalkingParam.Request()) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + self.current_walking_param = future.result().parameters + return True + else: + self.logger.error('Service call failed %r' % (future.exception(),)) + return False + + def object_detection_callback(self, msg): + if self.initialize_config.on_tracking: + x_angle = msg.x + y_angle = msg.y + object_size = msg.z + self.process_following(x_angle, y_angle, object_size) + else: + self.wait_following() + + def run(self): + if self.ros_version == '1': + rospy.spin() + else: + rclpy.spin(self.node) + +def main(ros_version): + try: + follower = ObjectFollower(ros_version) + follower.start_following() + follower.run() + except Exception as e: + if ros_version == '1': + rospy.logerr(f"An error occurred: {e}") + else: + rclpy.shutdown() + print(f"An error occurred: {e}") + +if __name__ == '__main__': + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + import rospy + try: + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": + import rclpy + try: + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS2 main") + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py new file mode 100644 index 0000000..4f00592 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py @@ -0,0 +1,235 @@ +#!/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 os +import sys +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String +from geometry_msgs.msg import Point + +from humanoid_robot_intelligence_control_system_detection.tracker import calculate_error, calculate_error_target, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.tracker_config import TrackerConfig, TrackerInitializeConfig + +class ObjectTracker: + def __init__(self, ros_version): + self.ros_version = ros_version + self.initialize_config = TrackerInitializeConfig() + self.config = TrackerConfig() + self.initialize_ros() + self.setup_ros_communication() + + def initialize_ros(self): + if self.ros_version == '1': + rospy.init_node('object_tracker') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = rclpy.create_node('object_tracker') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() + + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) + + def setup_ros_communication(self): + if self.ros_version == '1': + self.head_joint_offset_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0) + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0) + self.object_position_sub = rospy.Subscriber( + "/object_detector_node/object_position", Point, self.object_position_callback) + self.object_tracking_command_sub = rospy.Subscriber( + "/object_tracker/command", String, self.object_tracker_command_callback) + self.prev_time = rospy.Time.now() + else: + self.head_joint_offset_pub = self.node.create_publisher( + JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 10) + self.head_joint_pub = self.node.create_publisher( + JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 10) + self.head_scan_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/head_control/scan_command", 10) + self.object_position_sub = self.node.create_subscription( + Point, "/object_detector_node/object_position", self.object_position_callback, 10) + self.object_tracking_command_sub = self.node.create_subscription( + String, "/object_tracker/command", self.object_tracker_command_callback, 10) + self.prev_time = self.node.get_clock().now() + + def object_position_callback(self, msg): + self.initialize_config.object_position = msg + if self.initialize_config.on_tracking: + self.process_tracking() + + def object_tracker_command_callback(self, msg): + if msg.data == "start": + self.start_tracking() + elif msg.data == "stop": + self.stop_tracking() + elif msg.data == "toggle_start": + if not self.initialize_config.on_tracking: + self.start_tracking() + else: + self.stop_tracking() + + def start_tracking(self): + self.initialize_config.on_tracking = True + self.logger.info("Start Object tracking") + + def stop_tracking(self): + self.go_init() + self.initialize_config.on_tracking = False + self.logger.info("Stop Object tracking") + self.initialize_config.current_object_pan = 0 + self.initialize_config.current_object_tilt = 0 + self.initialize_config.x_error_sum = 0 + self.initialize_config.y_error_sum = 0 + + def set_using_head_scan(self, use_scan): + self.config.use_head_scan = use_scan + + def process_tracking(self): + if not self.initialize_config.on_tracking: + self.initialize_config.object_position.z = 0 + self.initialize_config.count_not_found = 0 + return "NotFound" + + if self.initialize_config.object_position.z <= 0: + self.initialize_config.count_not_found += 1 + if self.initialize_config.count_not_found < self.config.WAITING_THRESHOLD: + if self.initialize_config.tracking_status in ["Found", "Waiting"]: + tracking_status = "Waiting" + else: + tracking_status = "NotFound" + elif self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD: + self.scan_object() + self.initialize_config.count_not_found = 0 + tracking_status = "NotFound" + else: + tracking_status = "NotFound" + else: + self.initialize_config.count_not_found = 0 + tracking_status = "Found" + + if tracking_status != "Found": + self.initialize_config.tracking_status = tracking_status + self.initialize_config.current_object_pan = 0 + self.initialize_config.current_object_tilt = 0 + self.initialize_config.x_error_sum = 0 + self.initialize_config.y_error_sum = 0 + return tracking_status + + x_error, y_error = calculate_error(self.initialize_config.object_position, self.config.FOV_WIDTH, self.config.FOV_HEIGHT) + object_size = self.initialize_config.object_position.z + + curr_time = rospy.Time.now() if self.ros_version == '1' else self.node.get_clock().now() + delta_time = calculate_delta_time(curr_time, self.prev_time) + self.prev_time = curr_time + + x_error_diff = (x_error - self.initialize_config.current_object_pan) / delta_time + y_error_diff = (y_error - self.initialize_config.current_object_tilt) / delta_time + self.initialize_config.x_error_sum += x_error + self.initialize_config.y_error_sum += y_error + + x_error_target = calculate_error_target(x_error, x_error_diff, self.initialize_config.x_error_sum, + self.config.p_gain, self.config.i_gain, self.config.d_gain) + y_error_target = calculate_error_target(y_error, y_error_diff, self.initialize_config.y_error_sum, + self.config.p_gain, self.config.i_gain, self.config.d_gain) + + if self.config.DEBUG_PRINT: + self.logger.info(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") + self.logger.info(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") + self.logger.info(f"Error sum: {self.initialize_config.x_error_sum * 180 / math.pi} | {self.initialize_config.y_error_sum * 180 / math.pi}") + self.logger.info(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") + + self.publish_head_joint(x_error_target, y_error_target) + + self.initialize_config.current_object_pan = x_error + self.initialize_config.current_object_tilt = y_error + self.initialize_config.current_object_bottom = object_size + + self.initialize_config.object_position.z = 0 + + self.initialize_config.tracking_status = tracking_status + return tracking_status + + def publish_head_joint(self, pan, tilt): + min_angle = 1 * math.pi / 180 + if abs(pan) < min_angle and abs(tilt) < min_angle: + return + + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [pan, tilt] + + self.head_joint_offset_pub.publish(head_angle_msg) + + def go_init(self): + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [0.0, 0.0] + + self.head_joint_pub.publish(head_angle_msg) + + def scan_object(self): + if not self.config.use_head_scan: + return + + scan_msg = String() + scan_msg.data = "scan" + + self.head_scan_pub.publish(scan_msg) + + def run(self): + if self.ros_version == '1': + rospy.spin() + else: + rclpy.spin(self.node) + +def main(ros_version): + try: + tracker = ObjectTracker(ros_version) + tracker.start_tracking() + tracker.run() + except Exception as e: + if ros_version == '1': + rospy.logerr(f"An error occurred: {e}") + else: + rclpy.shutdown() + print(f"An error occurred: {e}") + +if __name__ == '__main__': + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + import rospy + try: + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": + import rclpy + try: + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS2 main") + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) 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 diff --git a/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt new file mode 100644 index 0000000..2eb131f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt @@ -0,0 +1,71 @@ +# 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_face_detector) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/face_detection_processor.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/face_detection_processor.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_face_detector/launch/face_detector_processor.launch.py b/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py new file mode 100644 index 0000000..4a6456b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py @@ -0,0 +1,108 @@ +# 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 os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_face_detector", "face_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_face_detection.py", "name:=face_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_ball_detector", "face_detection_processor.py", "name:=face_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_face_detection.py', + name='face_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_face_detector', + executable='face_detection_processor.py', + name='face_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch b/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch new file mode 100644 index 0000000..6c4507d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_face_detector/package.xml b/humanoid_robot_intelligence_control_system_face_detector/package.xml new file mode 100644 index 0000000..9e60a19 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/package.xml @@ -0,0 +1,61 @@ + + + + + humanoid_robot_intelligence_control_system_face_detector + 0.0.1 + + This Package is for Object Detection, detecting faces like tools, or utilities + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime + + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_face_detector/setup.py b/humanoid_robot_intelligence_control_system_face_detector/setup.py new file mode 100644 index 0000000..f391975 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/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/face_detection_processor.py'], + packages=['humanoid_robot_intelligence_control_system_face_detector'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py b/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py new file mode 100644 index 0000000..5484d2d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py @@ -0,0 +1,111 @@ +#!/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 rospy +import cv2 +import numpy as np +from sensor_msgs.msg import Image, CameraInfo +from std_msgs.msg import Bool, String +from cv_bridge import CvBridge +import yaml + +class ObjectDetectionProcessor: + def __init__(self): + rospy.init_node('face_detection_processor') + self.bridge = CvBridge() + self.enable = True + self.new_image_flag = False + self.load_params() + self.setup_ros() + + def load_params(self): + param_path = rospy.get_param('~yaml_path', '') + if param_path: + with open(param_path, 'r') as file: + self.params = yaml.safe_load(file) + else: + self.set_default_params() + + def set_default_params(self): + self.params = { + 'debug': False, + 'ellipse_size': 2, + # Add other default parameters as needed + } + + def setup_ros(self): + self.image_pub = rospy.Publisher('image_out', Image, queue_size=10) + self.camera_info_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=10) + + rospy.Subscriber('enable', Bool, self.enable_callback) + rospy.Subscriber('image_in', Image, self.image_callback) + rospy.Subscriber('cameraInfo_in', CameraInfo, self.camera_info_callback) + rospy.Subscriber('face_detection_result', String, self.face_detection_callback) + + def enable_callback(self, msg): + self.enable = msg.data + + def image_callback(self, msg): + if not self.enable: + return + self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") + self.new_image_flag = True + self.image_header = msg.header + + def camera_info_callback(self, msg): + if not self.enable: + return + self.camera_info_msg = msg + + def face_detection_callback(self, msg): + if not self.enable or not hasattr(self, 'cv_image'): + return + + faces = eval(msg.data) # Assuming the data is a string representation of a list + self.process_detected_faces(faces) + + def process_detected_faces(self, faces): + output_image = self.cv_image.copy() + for obj in faces: + x, y, w, h = obj['bbox'] + cv2.rectangle(output_image, (int(x), int(y)), (int(x+w), int(y+h)), (0, 255, 0), 2) + cv2.putText(output_image, f"{obj['label']}: {obj['confidence']:.2f}", + (int(x), int(y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2) + + self.publish_image(output_image) + + def publish_image(self, image): + img_msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") + img_msg.header = self.image_header + self.image_pub.publish(img_msg) + if hasattr(self, 'camera_info_msg'): + self.camera_info_pub.publish(self.camera_info_msg) + + def run(self): + rate = rospy.Rate(30) # 30 Hz + while not rospy.is_shutdown(): + if self.new_image_flag: + # The processing is done in face_detection_callback + self.new_image_flag = False + rate.sleep() + +if __name__ == '__main__': + try: + processor = ObjectDetectionProcessor() + processor.run() + except rospy.ROSInterruptException: + pass