BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #22 from RonaldsonBellande/main

Browse Source 
seperate
This commit is contained in:
Ronaldson Bellande
2024-07-26 17:19:14 -04:00
committed by GitHub
parent a1c79e6607 1e21ad479a
commit e2ca4fbcb0
23 changed files with 1303 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

157
humanoid_robot_intelligence_control_system_controller/CMakeLists.txt Executable file → Normal file
View File

@@ -1,97 +1,60 @@
# 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.8)
project(humanoid_robot_intelligence_control_system_controller)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_intelligence_control_system_controller_msgs
dynamixel_sdk
humanoid_robot_intelligence_control_system_device
humanoid_robot_intelligence_control_system_framework_common
cmake_modules
)
find_package(Boost REQUIRED)
find_package(PkgConfig REQUIRED)
else()
find_package(ament_cmake REQUIRED)
endif()
pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
find_path(YAML_CPP_INCLUDE_DIR
NAMES yaml_cpp.h
PATHS ${YAML_CPP_INCLUDE_DIRS}
)
find_library(YAML_CPP_LIBRARY
NAMES YAML_CPP
PATHS ${YAML_CPP_LIBRARY_DIRS}
)
link_directories(${YAML_CPP_LIBRARY_DIRS})
if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
add_definitions(-DHAVE_NEW_YAMLCPP)
endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package(
INCLUDE_DIRS include
LIBRARIES humanoid_robot_intelligence_control_system_controller
CATKIN_DEPENDS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_intelligence_control_system_controller_msgs
dynamixel_sdk
humanoid_robot_intelligence_control_system_device
humanoid_robot_intelligence_control_system_framework_common
cmake_modules
DEPENDS Boost
)
endif()
include_directories(
include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${YAML_CPP_INCLUDE_DIRS}
)
add_library(humanoid_robot_intelligence_control_system_controller src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp)
add_dependencies(humanoid_robot_intelligence_control_system_controller ${catkin_EXPORTED_TARGETS})
target_link_libraries(humanoid_robot_intelligence_control_system_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES})
install(TARGETS humanoid_robot_intelligence_control_system_controller
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
# 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()

100
humanoid_robot_intelligence_control_system_controller/package.xml Executable file → Normal file
View File

@@ -17,96 +17,30 @@ the License.
<package format="3">
<name>humanoid_robot_intelligence_control_system_controller</name>
<version>0.3.2</version>
<description>humanoid_robot_intelligence_control_system_controller package for ROBOTIS's platform
like Manipulator-H, THORMANG and OP series</description>
<version>0.0.1</version>
<description>
This Package is for Detection Math
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<build_depend condition="$ROS_VERSION == 1">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 1">roslib</build_depend>
<build_depend condition="$ROS_VERSION == 1">std_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">sensor_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_controller_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">dynamixel_sdk</build_depend>
<build_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_device</build_depend>
<build_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_framework_common</build_depend>
<build_depend condition="$ROS_VERSION == 1">cmake_modules</build_depend>
<build_depend condition="$ROS_VERSION == 1">yaml-cpp</build_depend>
<build_export_depend condition="$ROS_VERSION == 1">roscpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">roslib</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">std_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">sensor_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_controller_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">dynamixel_sdk</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_device</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_framework_common</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">cmake_modules</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">yaml-cpp</build_export_depend>
<exec_depend condition="$ROS_VERSION == 1">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">roslib</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">std_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">sensor_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_controller_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">dynamixel_sdk</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_device</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_framework_common</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">cmake_modules</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">yaml-cpp</exec_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<build_depend condition="$ROS_VERSION == 2">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 2">roslib</build_depend>
<build_depend condition="$ROS_VERSION == 2">std_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">sensor_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_controller_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">dynamixel_sdk</build_depend>
<build_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_device</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_framework_common</build_depend>
<build_depend condition="$ROS_VERSION == 2">cmake_modules</build_depend>
<build_depend condition="$ROS_VERSION == 2">yaml-cpp</build_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<build_export_depend condition="$ROS_VERSION == 2">roscpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">roslib</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">std_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">sensor_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_controller_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">dynamixel_sdk</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_device</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_framework_common</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">cmake_modules</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">yaml-cpp</build_export_depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<exec_depend condition="$ROS_VERSION == 2">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">roslib</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">std_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">sensor_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_controller_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">dynamixel_sdk</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_device</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_framework_common</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">cmake_modules</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">yaml-cpp</exec_depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

26
humanoid_robot_intelligence_control_system_controller/setup.py Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/PIDController.py'],
packages=['humanoid_robot_intelligence_control_system_controller'],
package_dir={'': 'src'},
)
setup(**setup_args)

50
humanoid_robot_intelligence_control_system_controller/src/PIDController.py Normal file
View File

@@ -0,0 +1,50 @@
#!/usr/bin/env python3
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
import 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

60
humanoid_robot_intelligence_control_system_external_sensors/CMakeLists.txt Normal file
View File

@@ -0,0 +1,60 @@
# 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()

46
humanoid_robot_intelligence_control_system_external_sensors/package.xml Normal file
View File

@@ -0,0 +1,46 @@
<?xml version="1.0"?>
<!--
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
Licensed under the Apache License, Version 2.0 (the "License"); you may not
use this file except in compliance with the License. You may obtain a copy of
the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
License for the specific language governing permissions and limitations under
the License.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_controller</name>
<version>0.0.1</version>
<description>
This Package is for Detection Math
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

26
humanoid_robot_intelligence_control_system_external_sensors/setup.py Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/PIDController.py'],
packages=['humanoid_robot_intelligence_control_system_controller'],
package_dir={'': 'src'},
)
setup(**setup_args)

50
humanoid_robot_intelligence_control_system_external_sensors/src/PIDController.py Normal file
View File

@@ -0,0 +1,50 @@
#!/usr/bin/env python3
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
import 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

60
humanoid_robot_intelligence_control_system_internal_sensors/CMakeLists.txt Normal file
View File

@@ -0,0 +1,60 @@
# 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()

46
humanoid_robot_intelligence_control_system_internal_sensors/package.xml Normal file
View File

@@ -0,0 +1,46 @@
<?xml version="1.0"?>
<!--
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
Licensed under the Apache License, Version 2.0 (the "License"); you may not
use this file except in compliance with the License. You may obtain a copy of
the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
License for the specific language governing permissions and limitations under
the License.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_controller</name>
<version>0.0.1</version>
<description>
This Package is for Detection Math
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

26
humanoid_robot_intelligence_control_system_internal_sensors/setup.py Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/PIDController.py'],
packages=['humanoid_robot_intelligence_control_system_controller'],
package_dir={'': 'src'},
)
setup(**setup_args)

71
humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt Normal file
View File

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

108
humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py Normal file
View File

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

41
humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch Normal file
View File

@@ -0,0 +1,41 @@
<?xml version="1.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 <https://www.gnu.org/licenses/>.
-->
<launch>
<!-- Launch USB camera -->
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
</node>
<!-- Launch object detection node -->
<node name="object_detection_node" pkg="ros_web_api_bellande_2d_computer_vision" type="bellande_2d_computer_vision_object_detection.py" output="screen">
<remap from="camera/image_raw" to="/usb_cam/image_raw"/>
</node>
<!-- Launch object detection processor node -->
<node name="object_detection_processor_node" pkg="humanoid_robot_intelligence_control_system_ball_detector" type="object_detection_processor.py" output="screen">
<param name="yaml_path" value="$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml"/>
</node>
<!-- Launch RViz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" />
</launch>

61
humanoid_robot_intelligence_control_system_object_detector/package.xml Normal file
View File

@@ -0,0 +1,61 @@
<?xml version="1.0"?>
<!--
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
Licensed under the Apache License, Version 2.0 (the "License"); you may not
use this file except in compliance with the License. You may obtain a copy of
the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
License for the specific language governing permissions and limitations under
the License.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_object_detector</name>
<version>0.0.1</version>
<description>
This Package is for Object Detection, detecting objects like tools, or utilities
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<depend condition="$ROS_VERSION == 1">std_msgs</depend>
<depend condition="$ROS_VERSION == 1">sensor_msgs</depend>
<depend condition="$ROS_VERSION == 1">geometry_msgs</depend>
<depend condition="$ROS_VERSION == 1">cv_bridge</depend>
<depend condition="$ROS_VERSION == 1">image_transport</depend>
<depend condition="$ROS_VERSION == 1">dynamic_reconfigure</depend>
<depend condition="$ROS_VERSION == 1">message_generation</depend>
<depend condition="$ROS_VERSION == 1">message_runtime</depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<depend condition="$ROS_VERSION == 2">std_msgs</depend>
<depend condition="$ROS_VERSION == 2">sensor_msgs</depend>
<depend condition="$ROS_VERSION == 2">geometry_msgs</depend>
<depend condition="$ROS_VERSION == 2">cv_bridge</depend>
<depend condition="$ROS_VERSION == 2">image_transport</depend>
<depend condition="$ROS_VERSION == 2">rosidl_default_generators</depend>
<depend condition="$ROS_VERSION == 2">rosidl_default_runtime</depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

26
humanoid_robot_intelligence_control_system_object_detector/setup.py Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/object_detection_processor.py'],
packages=['humanoid_robot_intelligence_control_system_object_detector'],
package_dir={'': 'src'},
)
setup(**setup_args)

111
humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py Normal file
View File

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

71
humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt Normal file
View File

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

41
humanoid_robot_intelligence_control_system_speech_detector/launch/ros1/speech_detector_processor.launch Normal file
View File

@@ -0,0 +1,41 @@
<?xml version="1.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 <https://www.gnu.org/licenses/>.
-->
<launch>
<!-- Launch USB camera -->
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
</node>
<!-- Launch speech detection node -->
<node name="speech_detection_node" pkg="ros_web_api_bellande_2d_computer_vision" type="bellande_2d_computer_vision_speech_detection.py" output="screen">
<remap from="camera/image_raw" to="/usb_cam/image_raw"/>
</node>
<!-- Launch speech detection processor node -->
<node name="speech_detection_processor_node" pkg="humanoid_robot_intelligence_control_system_ball_detector" type="speech_detection_processor.py" output="screen">
<param name="yaml_path" value="$(find ros_web_api_bellande_2d_computer_vision)/yaml/speech_detection_params.yaml"/>
</node>
<!-- Launch RViz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" />
</launch>

108
humanoid_robot_intelligence_control_system_speech_detector/launch/speech_detector_processor.launch.py Normal file
View File

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

61
humanoid_robot_intelligence_control_system_speech_detector/package.xml Normal file
View File

@@ -0,0 +1,61 @@
<?xml version="1.0"?>
<!--
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
Licensed under the Apache License, Version 2.0 (the "License"); you may not
use this file except in compliance with the License. You may obtain a copy of
the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
License for the specific language governing permissions and limitations under
the License.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_speech_detector</name>
<version>0.0.1</version>
<description>
This Package is for Object Detection, detecting speechs like tools, or utilities
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<depend condition="$ROS_VERSION == 1">std_msgs</depend>
<depend condition="$ROS_VERSION == 1">sensor_msgs</depend>
<depend condition="$ROS_VERSION == 1">geometry_msgs</depend>
<depend condition="$ROS_VERSION == 1">cv_bridge</depend>
<depend condition="$ROS_VERSION == 1">image_transport</depend>
<depend condition="$ROS_VERSION == 1">dynamic_reconfigure</depend>
<depend condition="$ROS_VERSION == 1">message_generation</depend>
<depend condition="$ROS_VERSION == 1">message_runtime</depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<depend condition="$ROS_VERSION == 2">std_msgs</depend>
<depend condition="$ROS_VERSION == 2">sensor_msgs</depend>
<depend condition="$ROS_VERSION == 2">geometry_msgs</depend>
<depend condition="$ROS_VERSION == 2">cv_bridge</depend>
<depend condition="$ROS_VERSION == 2">image_transport</depend>
<depend condition="$ROS_VERSION == 2">rosidl_default_generators</depend>
<depend condition="$ROS_VERSION == 2">rosidl_default_runtime</depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

26
humanoid_robot_intelligence_control_system_speech_detector/setup.py Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/speech_detection_processor.py'],
packages=['humanoid_robot_intelligence_control_system_speech_detector'],
package_dir={'': 'src'},
)
setup(**setup_args)

111
humanoid_robot_intelligence_control_system_speech_detector/src/speech_detection_processor.py Normal file
View File

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