diff --git a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt deleted file mode 100644 index e7de26d..0000000 --- a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt +++ /dev/null @@ -1,60 +0,0 @@ -# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande -# -# Licensed under the Apache License, Version 2.0 (the "License"); you may not -# use this file except in compliance with the License. You may obtain a copy of -# the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations under -# the License. - -cmake_minimum_required(VERSION 3.0.2) -project(humanoid_robot_intelligence_control_system_controller) - -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/PIDController.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/PIDController.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_controller/package.xml b/humanoid_robot_intelligence_control_system_controller/package.xml deleted file mode 100644 index 0ab0b27..0000000 --- a/humanoid_robot_intelligence_control_system_controller/package.xml +++ /dev/null @@ -1,46 +0,0 @@ - - - - - humanoid_robot_intelligence_control_system_controller - 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_controller/setup.py b/humanoid_robot_intelligence_control_system_controller/setup.py deleted file mode 100644 index cbd6630..0000000 --- a/humanoid_robot_intelligence_control_system_controller/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -# 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/PIDController.py'], - packages=['humanoid_robot_intelligence_control_system_controller'], - package_dir={'': 'src'}, -) - -setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_controller/src/PIDController.py b/humanoid_robot_intelligence_control_system_controller/src/PIDController.py deleted file mode 100644 index 0a87c58..0000000 --- a/humanoid_robot_intelligence_control_system_controller/src/PIDController.py +++ /dev/null @@ -1,50 +0,0 @@ -#!/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 time -from typing import Dict, List, Tuple - -class PIDController: - def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))): - self.kp, self.ki, self.kd = gains - self.name = name - self.output_limits = output_limits - self.reset() - - def reset(self): - self.last_error = 0 - self.integral = 0 - self.last_time = time.time() - - def compute(self, setpoint: float, process_variable: float) -> float: - current_time = time.time() - dt = current_time - self.last_time - error = setpoint - process_variable - - self.integral += error * dt - derivative = (error - self.last_error) / dt if dt > 0 else 0 - - output = self.kp * error + self.ki * self.integral + self.kd * derivative - output = max(min(output, self.output_limits[1]), self.output_limits[0]) - - self.last_error = error - self.last_time = current_time - - return output - - def update_config(self, gains: Tuple[float, float, float]): - self.kp, self.ki, self.kd = gains diff --git a/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt deleted file mode 100644 index 55bba24..0000000 --- a/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt +++ /dev/null @@ -1,71 +0,0 @@ -# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande -# -# Licensed under the Apache License, Version 2.0 (the "License"); you may not -# use this file except in compliance with the License. You may obtain a copy of -# the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations under -# the License. - -cmake_minimum_required(VERSION 3.0.2) -project(humanoid_robot_intelligence_control_system_object_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/object_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/object_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_object_detector/launch/object_detector_processor.launch.py b/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py deleted file mode 100644 index e61d87a..0000000 --- a/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py +++ /dev/null @@ -1,108 +0,0 @@ -# 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_object_detector", "object_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_object_detection.py", "name:=object_detection_node" - ]) - - roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_ball_detector", "object_detection_processor.py", "name:=object_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_object_detection.py', - name='object_detection_node', - output='screen', - remappings=[('camera/image_raw', '/usb_cam/image_raw')] - )) - - 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='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_object_detector/launch/ros1/object_detector_processor.launch b/humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch deleted file mode 100644 index 6799e95..0000000 --- a/humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/humanoid_robot_intelligence_control_system_object_detector/package.xml b/humanoid_robot_intelligence_control_system_object_detector/package.xml deleted file mode 100644 index 706fe9e..0000000 --- a/humanoid_robot_intelligence_control_system_object_detector/package.xml +++ /dev/null @@ -1,61 +0,0 @@ - - - - - humanoid_robot_intelligence_control_system_object_detector - 0.0.1 - - This Package is for Object Detection, detecting objects 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_object_detector/setup.py b/humanoid_robot_intelligence_control_system_object_detector/setup.py deleted file mode 100644 index a2eac39..0000000 --- a/humanoid_robot_intelligence_control_system_object_detector/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -# 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/object_detection_processor.py'], - packages=['humanoid_robot_intelligence_control_system_object_detector'], - package_dir={'': 'src'}, -) - -setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py b/humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py deleted file mode 100644 index 6f44d68..0000000 --- a/humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/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('object_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('object_detection_result', String, self.object_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 object_detection_callback(self, msg): - if not self.enable or not hasattr(self, 'cv_image'): - return - - objects = eval(msg.data) # Assuming the data is a string representation of a list - self.process_detected_objects(objects) - - def process_detected_objects(self, objects): - output_image = self.cv_image.copy() - for obj in objects: - 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 object_detection_callback - self.new_image_flag = False - rate.sleep() - -if __name__ == '__main__': - try: - processor = ObjectDetectionProcessor() - processor.run() - except rospy.ROSInterruptException: - pass diff --git a/humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt deleted file mode 100644 index 78abc20..0000000 --- a/humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt +++ /dev/null @@ -1,71 +0,0 @@ -# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande -# -# Licensed under the Apache License, Version 2.0 (the "License"); you may not -# use this file except in compliance with the License. You may obtain a copy of -# the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations under -# the License. - -cmake_minimum_required(VERSION 3.0.2) -project(humanoid_robot_intelligence_control_system_speech_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/speech_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/speech_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_speech_detector/launch/ros1/speech_detector_processor.launch b/humanoid_robot_intelligence_control_system_speech_detector/launch/ros1/speech_detector_processor.launch deleted file mode 100644 index 014521f..0000000 --- a/humanoid_robot_intelligence_control_system_speech_detector/launch/ros1/speech_detector_processor.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/humanoid_robot_intelligence_control_system_speech_detector/launch/speech_detector_processor.launch.py b/humanoid_robot_intelligence_control_system_speech_detector/launch/speech_detector_processor.launch.py deleted file mode 100644 index c11ca70..0000000 --- a/humanoid_robot_intelligence_control_system_speech_detector/launch/speech_detector_processor.launch.py +++ /dev/null @@ -1,108 +0,0 @@ -# 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_speech_detector", "speech_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_speech_detection.py", "name:=speech_detection_node" - ]) - - roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_ball_detector", "speech_detection_processor.py", "name:=speech_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_speech_detection.py', - name='speech_detection_node', - output='screen', - remappings=[('camera/image_raw', '/usb_cam/image_raw')] - )) - - nodes_to_launch.append(Node( - package='humanoid_robot_intelligence_control_system_speech_detector', - executable='speech_detection_processor.py', - name='speech_detection_processor_node', - output='screen', - parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/speech_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_speech_detector/package.xml b/humanoid_robot_intelligence_control_system_speech_detector/package.xml deleted file mode 100644 index 416194b..0000000 --- a/humanoid_robot_intelligence_control_system_speech_detector/package.xml +++ /dev/null @@ -1,61 +0,0 @@ - - - - - humanoid_robot_intelligence_control_system_speech_detector - 0.0.1 - - This Package is for Object Detection, detecting speechs 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_speech_detector/setup.py b/humanoid_robot_intelligence_control_system_speech_detector/setup.py deleted file mode 100644 index f4a55e3..0000000 --- a/humanoid_robot_intelligence_control_system_speech_detector/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -# 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/speech_detection_processor.py'], - packages=['humanoid_robot_intelligence_control_system_speech_detector'], - package_dir={'': 'src'}, -) - -setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_speech_detector/src/speech_detection_processor.py b/humanoid_robot_intelligence_control_system_speech_detector/src/speech_detection_processor.py deleted file mode 100644 index fcc60d2..0000000 --- a/humanoid_robot_intelligence_control_system_speech_detector/src/speech_detection_processor.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/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('speech_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('speech_detection_result', String, self.speech_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 speech_detection_callback(self, msg): - if not self.enable or not hasattr(self, 'cv_image'): - return - - speechs = eval(msg.data) # Assuming the data is a string representation of a list - self.process_detected_speechs(speechs) - - def process_detected_speechs(self, speechs): - output_image = self.cv_image.copy() - for obj in speechs: - 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 speech_detection_callback - self.new_image_flag = False - rate.sleep() - -if __name__ == '__main__': - try: - processor = ObjectDetectionProcessor() - processor.run() - except rospy.ROSInterruptException: - pass