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