diff --git a/README.md b/README.md index 3ee5208..a3dd60f 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,26 @@ -# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos +# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Architecture [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) +## Mobile and ROS Control System +- [![Bellande's Internal Sensor Mobile iOS API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_ios_api) +- [![Bellande's External Sensor Mobile iOS API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_ios_api) +- [![Bellande's Functionality Mobile iOS API](https://img.shields.io/badge/Bellande's%20Functionality%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_mobile_ios_api) +- [![Bellande's Internal Sensor Mobile Android API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_android_api) +- [![Bellande's External Sensor Mobile Android API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_android_api) +- [![Bellande's Functionality Mobile Android API](https://img.shields.io/badge/Bellande's%20Functionality%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_mobile_android_api) +- [![Bellande's Internal Sensor Web API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_web_api) +- [![Bellande's External Sensor Web API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_web_api) +- [![Bellande's Functionality Web API](https://img.shields.io/badge/Bellande's%20Functionality%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_web_api) +- [![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_functionality_ros_api) +- [![Bellande's Algorithm through Bellande MODELS](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20MODELS-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_ros_models) + + ## 🤖 Explore Humanoid Robot Intelligence with Us! -Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. +Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System architecture repository! Here, we invite you to delve into the fascinating world of humanoid robotics, showcasing the innovative capabilities of our intelligent control system. ## 💻 Functionality To Switch from ROS to ROS2 Checkout The Below Repository @@ -14,47 +28,34 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos reposit ### 🚀 Key Repository Stats -- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community -- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Forked by enthusiasts -- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Watched by curious minds +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Watched by curious minds -- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Actively addressing issues -- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Welcoming contributions -- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Licensed under Apache 2.0 +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Licensed under Apache 2.0 -- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Latest updates -- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic) Engaging with the community +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture/traffic) Engaging with the community --- ## 🌐 Repository Website -Explore our [repository website](https://robotics-sensors.github.io/bellande_humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. - ---- - -### 🔄 Updates and Versions - -- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_demos) -- **Old Version/Previous Used for Different Context:** [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) +Explore our [repository website](https://robotics-sensors.github.io/bellande_humanoid_robot_intelligence_control_system_architecture) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. --- # 🎉 Latest Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_demos/releases/) - ---- - -## 📢 Important Announcement - -The repository has recently undergone significant updates. Older commits and codes are covered under the previous license, while new commits and codes fall under the new license. +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture/releases/) --- 🚀 For the latest versions and updates, visit our organization: [Robotics-Sensors](https://github.com/Robotics-Sensors). -### 🧑‍💼 Maintainer +### 🧑‍💼 Maintainer & Author Meet our dedicated maintainer, **Ronaldson Bellande**. @@ -62,4 +63,4 @@ Meet our dedicated maintainer, **Ronaldson Bellande**. ## 📄 License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE). +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/blob/main/LICENSE). diff --git a/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt new file mode 100644 index 0000000..48ef842 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt @@ -0,0 +1,77 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_configure) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/face_tracker.py + src/face_follower.py + src/object_tracker.py + src/object_follower.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/face_tracker.py + src/face_follower.py + src/object_tracker.py + src/object_follower.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py new file mode 100644 index 0000000..44a7599 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py @@ -0,0 +1,112 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follower.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_face_detector", "face_detection_processor.py", "name:=face_detection_processor_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_configure", "face_follower.py", "name:=face_follower_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_face_detector', + executable='face_detection_processor.py', + name='face_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_configure', + executable='face_follower.py', + name='face_follower_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py new file mode 100644 index 0000000..d7d94dc --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py @@ -0,0 +1,112 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_tracker.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_face_tracker", "face_detection_processor.py", "name:=face_detection_processor_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_configure", "face_tracker.py", "name:=face_tracker_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_face_tracker', + executable='face_detection_processor.py', + name='face_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_configure', + executable='face_tracker.py', + name='face_tracker_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py new file mode 100644 index 0000000..2788682 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py @@ -0,0 +1,112 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follower.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_configure", "object_follower.py", "name:=object_follower_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_configure', + executable='object_follower.py', + name='object_follower_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py new file mode 100644 index 0000000..49307ae --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py @@ -0,0 +1,112 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_tracker.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_object_tracker", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_configure", "object_tracker.py", "name:=object_tracker_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_tracker', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_configure', + executable='object_tracker.py', + name='object_tracker_node', + output='screen', + parameters=[{ + 'p_gain': 0.4, + 'i_gain': 0.0, + 'd_gain': 0.0 + }] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/package.xml b/humanoid_robot_intelligence_control_system_configure/package.xml new file mode 100644 index 0000000..740f1f7 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/package.xml @@ -0,0 +1,61 @@ + + + + + humanoid_robot_intelligence_control_system_configure + 0.0.1 + + This Package is for Configuration of Tracker and Follower related to it + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime + + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_configure/setup.py b/humanoid_robot_intelligence_control_system_configure/setup.py new file mode 100644 index 0000000..690d947 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/face_tracker.py', "src/face_follower.py", "src/object_tracker.py", "src/object_follower.py"], + packages=['humanoid_robot_intelligence_control_system_configure'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py new file mode 100644 index 0000000..f12b5a4 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -0,0 +1,236 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String +from geometry_msgs.msg import Point +from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam +from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam + +from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to, calculate_footstep, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig + + +class FaceFollower: + def __init__(self, ros_version): + self.ros_version = ros_version + self.initialize_config = FollowerInitializeConfig() + self.config = FollowerConfig() + self.initialize_ros() + self.setup_ros_communication() + + def initialize_ros(self): + if self.ros_version == '1': + rospy.init_node('face_follower') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = rclpy.create_node('face_follower') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() + + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) + + def setup_ros_communication(self): + if self.ros_version == '1': + self.current_joint_states_sub = rospy.Subscriber( + "/humanoid_robot_intelligence_control_system/goal_joint_states", JointState, self.current_joint_states_callback) + self.set_walking_command_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/command", String, queue_size=1) + self.set_walking_param_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) + self.get_walking_param_client = rospy.ServiceProxy( + "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) + self.face_detection_sub = rospy.Subscriber( + "/face_detection_result", Point, self.face_detection_callback) + self.prev_time = rospy.Time.now() + else: + self.current_joint_states_sub = self.node.create_subscription( + JointState, "/humanoid_robot_intelligence_control_system/goal_joint_states", self.current_joint_states_callback, 10) + self.set_walking_command_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/walking/command", 1) + self.set_walking_param_pub = self.node.create_publisher( + WalkingParam, "/humanoid_robot_intelligence_control_system/walking/set_params", 1) + self.get_walking_param_client = self.node.create_client( + GetWalkingParam, "/humanoid_robot_intelligence_control_system/walking/get_params") + self.face_detection_sub = self.node.create_subscription( + Point, "/face_detection_result", self.face_detection_callback, 10) + self.prev_time = self.node.get_clock().now() + + def start_following(self): + self.initialize_config.on_tracking = True + self.logger.info("Start face following") + self.set_walking_command("start") + result = self.get_walking_param() + if result: + self.config.hip_pitch_offset = self.current_walking_param.hip_pitch_offset + self.config.curr_period_time = self.current_walking_param.period_time + else: + self.config.hip_pitch_offset = 7.0 * math.pi / 180 + self.config.curr_period_time = 0.6 + + def stop_following(self): + self.initialize_config.on_tracking = False + self.initialize_config.count_to_approach = 0 + self.logger.info("Stop face following") + self.set_walking_command("stop") + + def current_joint_states_callback(self, msg): + for i, name in enumerate(msg.name): + if name == "head_pan": + self.initialize_config.current_pan = msg.position[i] + elif name == "head_tilt": + self.initialize_config.current_tilt = msg.position[i] + + def process_following(self, x_angle, y_angle, face_size): + curr_time = rospy.Time.now() if self.ros_version == '1' else self.node.get_clock().now() + delta_time = calculate_delta_time(curr_time, self.prev_time) + self.prev_time = curr_time + + self.initialize_config.count_not_found = 0 + + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.logger.error("Failed to get current angle of head joints.") + self.set_walking_command("stop") + self.initialize_config.on_tracking = False + self.initialize_config.approach_face_position = "NotFound" + return False + + self.initialize_config.approach_face_position = "OutOfRange" + + distance_to_face = calculate_distance_to( + self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, face_size) + + distance_to_approach = 0.22 + + if (distance_to_face < distance_to_approach) and (abs(x_angle) < 25.0): + self.initialize_config.count_to_approach += 1 + if self.initialize_config.count_to_approach > 20: + self.set_walking_command("stop") + self.initialize_config.on_tracking = False + self.initialize_config.approach_face_position = "OnLeft" if x_angle > 0 else "OnRight" + return True + elif self.initialize_config.count_to_approach > 15: + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0) + return False + else: + self.initialize_config.count_to_approach = 0 + + distance_to_walk = distance_to_face - distance_to_approach + fb_move, rl_angle = calculate_footstep( + self.initialize_config.current_x_move, distance_to_walk, self.initialize_config.current_r_angle, self.initialize_config.current_pan, delta_time, self.config) + self.set_walking_param(fb_move, 0, rl_angle) + return False + + def decide_face_position(self, x_angle, y_angle): + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.initialize_config.approach_face_position = "NotFound" + return + face_x_angle = self.initialize_config.current_pan + x_angle + self.initialize_config.approach_face_position = "OnLeft" if face_x_angle > 0 else "OnRight" + + def wait_following(self): + self.initialize_config.count_not_found += 1 + if self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD * 0.5: + self.set_walking_param(0.0, 0.0, 0.0) + + def set_walking_command(self, command): + if command == "start": + self.get_walking_param() + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0, True) + msg = String() + msg.data = command + self.set_walking_command_pub.publish(msg) + + def set_walking_param(self, x_move, y_move, rotation_angle, balance=False): + self.current_walking_param.balance_enable = balance + self.current_walking_param.x_move_amplitude = x_move + self.config.SPOT_FB_OFFSET + self.current_walking_param.y_move_amplitude = y_move + self.config.SPOT_RL_OFFSET + self.current_walking_param.angle_move_amplitude = rotation_angle + self.config.SPOT_ANGLE_OFFSET + self.set_walking_param_pub.publish(self.current_walking_param) + self.initialize_config.current_x_move = x_move + self.initialize_config.current_r_angle = rotation_angle + + def get_walking_param(self): + if self.ros_version == '1': + try: + response = self.get_walking_param_client() + self.current_walking_param = response.parameters + return True + except rospy.ServiceException as e: + self.logger.error("Failed to get walking parameters: %s" % e) + return False + else: + future = self.get_walking_param_client.call_async(GetWalkingParam.Request()) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + self.current_walking_param = future.result().parameters + return True + else: + self.logger.error('Service call failed %r' % (future.exception(),)) + return False + + def face_detection_callback(self, msg): + if self.initialize_config.on_tracking: + x_angle = msg.x + y_angle = msg.y + face_size = msg.z + self.process_following(x_angle, y_angle, face_size) + else: + self.wait_following() + + def run(self): + if self.ros_version == '1': + rospy.spin() + else: + rclpy.spin(self.node) + +def main(ros_version): + try: + follower = FaceFollower(ros_version) + follower.start_following() + follower.run() + except Exception as e: + if ros_version == '1': + rospy.logerr(f"An error occurred: {e}") + else: + rclpy.shutdown() + print(f"An error occurred: {e}") + +if __name__ == '__main__': + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + import rospy + try: + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": + import rclpy + try: + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS2 main") + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py new file mode 100644 index 0000000..a7776ab --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py @@ -0,0 +1,194 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String, Float64MultiArray +from humanoid_robot_intelligence_control_system_ball_detector.msg import CircleSetStamped +from geometry_msgs.msg import Point + +class FaceTracker: + def __init__(self): + rospy.init_node('face_tracker') + + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.NOT_FOUND_THRESHOLD = 50 + self.WAITING_THRESHOLD = 5 + self.use_head_scan = True + self.count_not_found = 0 + self.on_tracking = False + self.current_face_pan = 0 + self.current_face_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + self.current_face_bottom = 0 + self.tracking_status = "NotFound" + self.DEBUG_PRINT = False + + param_nh = rospy.get_param("~") + self.p_gain = param_nh.get("p_gain", 0.4) + self.i_gain = param_nh.get("i_gain", 0.0) + self.d_gain = param_nh.get("d_gain", 0.0) + + rospy.loginfo(f"face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") + + self.head_joint_offset_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0) + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0) + + self.face_position_sub = rospy.Subscriber( + "/face_detector_node/face_position", Point, self.face_position_callback) + self.face_tracking_command_sub = rospy.Subscriber( + "/face_tracker/command", String, self.face_tracker_command_callback) + + self.face_position = Point() + self.prev_time = rospy.Time.now() + + def face_position_callback(self, msg): + self.face_position = msg + + def face_tracker_command_callback(self, msg): + if msg.data == "start": + self.start_tracking() + elif msg.data == "stop": + self.stop_tracking() + elif msg.data == "toggle_start": + if not self.on_tracking: + self.start_tracking() + else: + self.stop_tracking() + + def start_tracking(self): + self.on_tracking = True + rospy.loginfo("Start face tracking") if self.DEBUG_PRINT else None + + def stop_tracking(self): + self.go_init() + self.on_tracking = False + rospy.loginfo("Stop face tracking") if self.DEBUG_PRINT else None + self.current_face_pan = 0 + self.current_face_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + + def set_using_head_scan(self, use_scan): + self.use_head_scan = use_scan + + def process_tracking(self): + if not self.on_tracking: + self.face_position.z = 0 + self.count_not_found = 0 + return "NotFound" + + if self.face_position.z <= 0: + self.count_not_found += 1 + if self.count_not_found < self.WAITING_THRESHOLD: + if self.tracking_status in ["Found", "Waiting"]: + tracking_status = "Waiting" + else: + tracking_status = "NotFound" + elif self.count_not_found > self.NOT_FOUND_THRESHOLD: + self.scan_face() + self.count_not_found = 0 + tracking_status = "NotFound" + else: + tracking_status = "NotFound" + else: + self.count_not_found = 0 + tracking_status = "Found" + + if tracking_status != "Found": + self.tracking_status = tracking_status + self.current_face_pan = 0 + self.current_face_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + return tracking_status + + x_error = -math.atan(self.face_position.x * math.tan(self.FOV_WIDTH)) + y_error = -math.atan(self.face_position.y * math.tan(self.FOV_HEIGHT)) + face_size = self.face_position.z + + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + self.prev_time = curr_time + + x_error_diff = (x_error - self.current_face_pan) / delta_time + y_error_diff = (y_error - self.current_face_tilt) / delta_time + self.x_error_sum += x_error + self.y_error_sum += y_error + x_error_target = x_error * self.p_gain + x_error_diff * self.d_gain + self.x_error_sum * self.i_gain + y_error_target = y_error * self.p_gain + y_error_diff * self.d_gain + self.y_error_sum * self.i_gain + + if self.DEBUG_PRINT: + rospy.loginfo(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") + rospy.loginfo(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") + rospy.loginfo(f"Error sum: {self.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}") + rospy.loginfo(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") + + self.publish_head_joint(x_error_target, y_error_target) + + self.current_face_pan = x_error + self.current_face_tilt = y_error + self.current_face_bottom = face_size + + self.face_position.z = 0 + + self.tracking_status = tracking_status + return tracking_status + + def publish_head_joint(self, pan, tilt): + min_angle = 1 * math.pi / 180 + if abs(pan) < min_angle and abs(tilt) < min_angle: + return + + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [pan, tilt] + + self.head_joint_offset_pub.publish(head_angle_msg) + + def go_init(self): + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [0.0, 0.0] + + self.head_joint_pub.publish(head_angle_msg) + + def scan_face(self): + if not self.use_head_scan: + return + + scan_msg = String() + scan_msg.data = "scan" + + self.head_scan_pub.publish(scan_msg) + +if __name__ == '__main__': + try: + tracker = FaceTracker() + rate = rospy.Rate(30) + while not rospy.is_shutdown(): + tracker.process_tracking() + rate.sleep() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py new file mode 100644 index 0000000..ebe736f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py @@ -0,0 +1,236 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String +from geometry_msgs.msg import Point +from humanoid_robot_intelligence_control_system_walking_module_msgs.msg import WalkingParam +from humanoid_robot_intelligence_control_system_walking_module_msgs.srv import GetWalkingParam + +from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to, calculate_footstep, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig + + +class ObjectFollower: + def __init__(self, ros_version): + self.ros_version = ros_version + self.initialize_config = FollowerInitializeConfig() + self.config = FollowerConfig() + self.initialize_ros() + self.setup_ros_communication() + + def initialize_ros(self): + if self.ros_version == '1': + rospy.init_node('object_follower') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = rclpy.create_node('object_follower') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() + + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) + + def setup_ros_communication(self): + if self.ros_version == '1': + self.current_joint_states_sub = rospy.Subscriber( + "/humanoid_robot_intelligence_control_system/goal_joint_states", JointState, self.current_joint_states_callback) + self.set_walking_command_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/command", String, queue_size=1) + self.set_walking_param_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/walking/set_params", WalkingParam, queue_size=1) + self.get_walking_param_client = rospy.ServiceProxy( + "/humanoid_robot_intelligence_control_system/walking/get_params", GetWalkingParam) + self.object_detection_sub = rospy.Subscriber( + "/object_detection_result", Point, self.object_detection_callback) + self.prev_time = rospy.Time.now() + else: + self.current_joint_states_sub = self.node.create_subscription( + JointState, "/humanoid_robot_intelligence_control_system/goal_joint_states", self.current_joint_states_callback, 10) + self.set_walking_command_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/walking/command", 1) + self.set_walking_param_pub = self.node.create_publisher( + WalkingParam, "/humanoid_robot_intelligence_control_system/walking/set_params", 1) + self.get_walking_param_client = self.node.create_client( + GetWalkingParam, "/humanoid_robot_intelligence_control_system/walking/get_params") + self.object_detection_sub = self.node.create_subscription( + Point, "/object_detection_result", self.object_detection_callback, 10) + self.prev_time = self.node.get_clock().now() + + def start_following(self): + self.initialize_config.on_tracking = True + self.logger.info("Start Object following") + self.set_walking_command("start") + result = self.get_walking_param() + if result: + self.config.hip_pitch_offset = self.current_walking_param.hip_pitch_offset + self.config.curr_period_time = self.current_walking_param.period_time + else: + self.config.hip_pitch_offset = 7.0 * math.pi / 180 + self.config.curr_period_time = 0.6 + + def stop_following(self): + self.initialize_config.on_tracking = False + self.initialize_config.count_to_approach = 0 + self.logger.info("Stop Object following") + self.set_walking_command("stop") + + def current_joint_states_callback(self, msg): + for i, name in enumerate(msg.name): + if name == "head_pan": + self.initialize_config.current_pan = msg.position[i] + elif name == "head_tilt": + self.initialize_config.current_tilt = msg.position[i] + + def process_following(self, x_angle, y_angle, object_size): + curr_time = rospy.Time.now() if self.ros_version == '1' else self.node.get_clock().now() + delta_time = calculate_delta_time(curr_time, self.prev_time) + self.prev_time = curr_time + + self.initialize_config.count_not_found = 0 + + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.logger.error("Failed to get current angle of head joints.") + self.set_walking_command("stop") + self.initialize_config.on_tracking = False + self.initialize_config.approach_object_position = "NotFound" + return False + + self.initialize_config.approach_object_position = "OutOfRange" + + distance_to_object = calculate_distance_to( + self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, object_size) + + distance_to_approach = 0.22 + + if (distance_to_object < distance_to_approach) and (abs(x_angle) < 25.0): + self.initialize_config.count_to_approach += 1 + if self.initialize_config.count_to_approach > 20: + self.set_walking_command("stop") + self.initialize_config.on_tracking = False + self.initialize_config.approach_object_position = "OnLeft" if x_angle > 0 else "OnRight" + return True + elif self.initialize_config.count_to_approach > 15: + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0) + return False + else: + self.initialize_config.count_to_approach = 0 + + distance_to_walk = distance_to_object - distance_to_approach + fb_move, rl_angle = calculate_footstep( + self.initialize_config.current_x_move, distance_to_walk, self.initialize_config.current_r_angle, self.initialize_config.current_pan, delta_time, self.config) + self.set_walking_param(fb_move, 0, rl_angle) + return False + + def decide_object_position(self, x_angle, y_angle): + if self.initialize_config.current_tilt == -10 and self.initialize_config.current_pan == -10: + self.initialize_config.approach_object_position = "NotFound" + return + object_x_angle = self.initialize_config.current_pan + x_angle + self.initialize_config.approach_object_position = "OnLeft" if object_x_angle > 0 else "OnRight" + + def wait_following(self): + self.initialize_config.count_not_found += 1 + if self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD * 0.5: + self.set_walking_param(0.0, 0.0, 0.0) + + def set_walking_command(self, command): + if command == "start": + self.get_walking_param() + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0, True) + msg = String() + msg.data = command + self.set_walking_command_pub.publish(msg) + + def set_walking_param(self, x_move, y_move, rotation_angle, balance=False): + self.current_walking_param.balance_enable = balance + self.current_walking_param.x_move_amplitude = x_move + self.config.SPOT_FB_OFFSET + self.current_walking_param.y_move_amplitude = y_move + self.config.SPOT_RL_OFFSET + self.current_walking_param.angle_move_amplitude = rotation_angle + self.config.SPOT_ANGLE_OFFSET + self.set_walking_param_pub.publish(self.current_walking_param) + self.initialize_config.current_x_move = x_move + self.initialize_config.current_r_angle = rotation_angle + + def get_walking_param(self): + if self.ros_version == '1': + try: + response = self.get_walking_param_client() + self.current_walking_param = response.parameters + return True + except rospy.ServiceException as e: + self.logger.error("Failed to get walking parameters: %s" % e) + return False + else: + future = self.get_walking_param_client.call_async(GetWalkingParam.Request()) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + self.current_walking_param = future.result().parameters + return True + else: + self.logger.error('Service call failed %r' % (future.exception(),)) + return False + + def object_detection_callback(self, msg): + if self.initialize_config.on_tracking: + x_angle = msg.x + y_angle = msg.y + object_size = msg.z + self.process_following(x_angle, y_angle, object_size) + else: + self.wait_following() + + def run(self): + if self.ros_version == '1': + rospy.spin() + else: + rclpy.spin(self.node) + +def main(ros_version): + try: + follower = ObjectFollower(ros_version) + follower.start_following() + follower.run() + except Exception as e: + if ros_version == '1': + rospy.logerr(f"An error occurred: {e}") + else: + rclpy.shutdown() + print(f"An error occurred: {e}") + +if __name__ == '__main__': + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + import rospy + try: + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": + import rclpy + try: + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS2 main") + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py new file mode 100644 index 0000000..4f00592 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py @@ -0,0 +1,235 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String +from geometry_msgs.msg import Point + +from humanoid_robot_intelligence_control_system_detection.tracker import calculate_error, calculate_error_target, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.tracker_config import TrackerConfig, TrackerInitializeConfig + +class ObjectTracker: + def __init__(self, ros_version): + self.ros_version = ros_version + self.initialize_config = TrackerInitializeConfig() + self.config = TrackerConfig() + self.initialize_ros() + self.setup_ros_communication() + + def initialize_ros(self): + if self.ros_version == '1': + rospy.init_node('object_tracker') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = rclpy.create_node('object_tracker') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() + + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) + + def setup_ros_communication(self): + if self.ros_version == '1': + self.head_joint_offset_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", JointState, queue_size=0) + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=0) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=0) + self.object_position_sub = rospy.Subscriber( + "/object_detector_node/object_position", Point, self.object_position_callback) + self.object_tracking_command_sub = rospy.Subscriber( + "/object_tracker/command", String, self.object_tracker_command_callback) + self.prev_time = rospy.Time.now() + else: + self.head_joint_offset_pub = self.node.create_publisher( + JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 10) + self.head_joint_pub = self.node.create_publisher( + JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 10) + self.head_scan_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/head_control/scan_command", 10) + self.object_position_sub = self.node.create_subscription( + Point, "/object_detector_node/object_position", self.object_position_callback, 10) + self.object_tracking_command_sub = self.node.create_subscription( + String, "/object_tracker/command", self.object_tracker_command_callback, 10) + self.prev_time = self.node.get_clock().now() + + def object_position_callback(self, msg): + self.initialize_config.object_position = msg + if self.initialize_config.on_tracking: + self.process_tracking() + + def object_tracker_command_callback(self, msg): + if msg.data == "start": + self.start_tracking() + elif msg.data == "stop": + self.stop_tracking() + elif msg.data == "toggle_start": + if not self.initialize_config.on_tracking: + self.start_tracking() + else: + self.stop_tracking() + + def start_tracking(self): + self.initialize_config.on_tracking = True + self.logger.info("Start Object tracking") + + def stop_tracking(self): + self.go_init() + self.initialize_config.on_tracking = False + self.logger.info("Stop Object tracking") + self.initialize_config.current_object_pan = 0 + self.initialize_config.current_object_tilt = 0 + self.initialize_config.x_error_sum = 0 + self.initialize_config.y_error_sum = 0 + + def set_using_head_scan(self, use_scan): + self.config.use_head_scan = use_scan + + def process_tracking(self): + if not self.initialize_config.on_tracking: + self.initialize_config.object_position.z = 0 + self.initialize_config.count_not_found = 0 + return "NotFound" + + if self.initialize_config.object_position.z <= 0: + self.initialize_config.count_not_found += 1 + if self.initialize_config.count_not_found < self.config.WAITING_THRESHOLD: + if self.initialize_config.tracking_status in ["Found", "Waiting"]: + tracking_status = "Waiting" + else: + tracking_status = "NotFound" + elif self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD: + self.scan_object() + self.initialize_config.count_not_found = 0 + tracking_status = "NotFound" + else: + tracking_status = "NotFound" + else: + self.initialize_config.count_not_found = 0 + tracking_status = "Found" + + if tracking_status != "Found": + self.initialize_config.tracking_status = tracking_status + self.initialize_config.current_object_pan = 0 + self.initialize_config.current_object_tilt = 0 + self.initialize_config.x_error_sum = 0 + self.initialize_config.y_error_sum = 0 + return tracking_status + + x_error, y_error = calculate_error(self.initialize_config.object_position, self.config.FOV_WIDTH, self.config.FOV_HEIGHT) + object_size = self.initialize_config.object_position.z + + curr_time = rospy.Time.now() if self.ros_version == '1' else self.node.get_clock().now() + delta_time = calculate_delta_time(curr_time, self.prev_time) + self.prev_time = curr_time + + x_error_diff = (x_error - self.initialize_config.current_object_pan) / delta_time + y_error_diff = (y_error - self.initialize_config.current_object_tilt) / delta_time + self.initialize_config.x_error_sum += x_error + self.initialize_config.y_error_sum += y_error + + x_error_target = calculate_error_target(x_error, x_error_diff, self.initialize_config.x_error_sum, + self.config.p_gain, self.config.i_gain, self.config.d_gain) + y_error_target = calculate_error_target(y_error, y_error_diff, self.initialize_config.y_error_sum, + self.config.p_gain, self.config.i_gain, self.config.d_gain) + + if self.config.DEBUG_PRINT: + self.logger.info(f"Error: {x_error * 180 / math.pi} | {y_error * 180 / math.pi}") + self.logger.info(f"Error diff: {x_error_diff * 180 / math.pi} | {y_error_diff * 180 / math.pi} | {delta_time}") + self.logger.info(f"Error sum: {self.initialize_config.x_error_sum * 180 / math.pi} | {self.initialize_config.y_error_sum * 180 / math.pi}") + self.logger.info(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") + + self.publish_head_joint(x_error_target, y_error_target) + + self.initialize_config.current_object_pan = x_error + self.initialize_config.current_object_tilt = y_error + self.initialize_config.current_object_bottom = object_size + + self.initialize_config.object_position.z = 0 + + self.initialize_config.tracking_status = tracking_status + return tracking_status + + def publish_head_joint(self, pan, tilt): + min_angle = 1 * math.pi / 180 + if abs(pan) < min_angle and abs(tilt) < min_angle: + return + + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [pan, tilt] + + self.head_joint_offset_pub.publish(head_angle_msg) + + def go_init(self): + head_angle_msg = JointState() + head_angle_msg.name = ["head_pan", "head_tilt"] + head_angle_msg.position = [0.0, 0.0] + + self.head_joint_pub.publish(head_angle_msg) + + def scan_object(self): + if not self.config.use_head_scan: + return + + scan_msg = String() + scan_msg.data = "scan" + + self.head_scan_pub.publish(scan_msg) + + def run(self): + if self.ros_version == '1': + rospy.spin() + else: + rclpy.spin(self.node) + +def main(ros_version): + try: + tracker = ObjectTracker(ros_version) + tracker.start_tracking() + tracker.run() + except Exception as e: + if ros_version == '1': + rospy.logerr(f"An error occurred: {e}") + else: + rclpy.shutdown() + print(f"An error occurred: {e}") + +if __name__ == '__main__': + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + import rospy + try: + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": + import rclpy + try: + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS2 main") + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_demo/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_demo/CHANGELOG.rst deleted file mode 100644 index 39a2f89..0000000 --- a/humanoid_robot_intelligence_control_system_demo/CHANGELOG.rst +++ /dev/null @@ -1,34 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package humanoid_robot_intelligence_control_system_demo -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.2 (2023-10-03) ------------------- -* Make it compatible for ROS1/ROS2 -* Fix bugs -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.3.1 (2023-09-27) ------------------- -* Starting from this point it under a new license -* Fix errors and Issues -* Rename Repository for a completely different purpose -* Make it compatible with ROS/ROS2 -* Upgrade version of all builds and make it more compatible -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.3.0 (2021-05-05) ------------------- -* Update package.xml and CMakeList.txt for noetic branch -* Contributors: Ronaldson Bellande - -0.1.0 (2018-04-19) ------------------- -* first release for ROS Kinetic -* added launch files in order to move the camera setting to humanoid_robot_intelligence_control_system_camera_setting package -* added missing package in find_package() -* refacoring to release -* split repositoryfrom ROBOTIS-HUMANOID_ROBOT -* Contributors: Kayman, Zerom, Yoshimaru Tanaka, Pyo diff --git a/humanoid_robot_intelligence_control_system_demo/CMakeLists.txt b/humanoid_robot_intelligence_control_system_demo/CMakeLists.txt deleted file mode 100644 index 081329a..0000000 --- a/humanoid_robot_intelligence_control_system_demo/CMakeLists.txt +++ /dev/null @@ -1,153 +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.8) -project(humanoid_robot_intelligence_control_system_demo) - - -if($ENV{ROS_VERSION} EQUAL 1) - find_package( - catkin REQUIRED COMPONENTS - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - humanoid_robot_intelligence_control_system_controller_msgs - humanoid_robot_intelligence_control_system_walking_module_msgs - humanoid_robot_intelligence_control_system_action_module_msgs - cmake_modules - humanoid_robot_intelligence_control_system_math - humanoid_robot_intelligence_control_system_ball_detector - ) - find_package(Boost REQUIRED COMPONENTS thread) - find_package(Eigen3 REQUIRED) -else() - find_package(ament_cmake REQUIRED) -endif() - - -find_package(PkgConfig REQUIRED) -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 - CATKIN_DEPENDS - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - humanoid_robot_intelligence_control_system_controller_msgs - humanoid_robot_intelligence_control_system_walking_module_msgs - humanoid_robot_intelligence_control_system_action_module_msgs - cmake_modules - humanoid_robot_intelligence_control_system_math - humanoid_robot_intelligence_control_system_ball_detector - DEPENDS Boost EIGEN3 - ) -endif() - - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${YAML_CPP_INCLUDE_DIRS} -) - -add_executable( - op_demo_node - src/demo_node.cpp - src/soccer/soccer_demo.cpp - src/soccer/ball_tracker.cpp - src/soccer/ball_follower.cpp - src/action/action_demo.cpp - src/vision/vision_demo.cpp - src/vision/face_tracker.cpp -) - -add_dependencies( - op_demo_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -target_link_libraries( - op_demo_node - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${Eigen3_LIBRARIES} - ${YAML_CPP_LIBRARIES} -) - -add_executable( - self_test_node - src/test_node.cpp - src/soccer/soccer_demo.cpp - src/soccer/ball_tracker.cpp - src/soccer/ball_follower.cpp - src/action/action_demo.cpp - src/vision/vision_demo.cpp - src/vision/face_tracker.cpp - src/test/button_test.cpp - src/test/mic_test.cpp -) - -add_dependencies( - self_test_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -target_link_libraries( - self_test_node - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ${Eigen3_LIBRARIES} - ${YAML_CPP_LIBRARIES} -) - -install( - TARGETS op_demo_node self_test_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install( - DIRECTORY data launch list - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Autonomous soccer mode.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Autonomous soccer mode.mp3 deleted file mode 100644 index ef1f878..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Autonomous soccer mode.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Bye bye.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Bye bye.mp3 deleted file mode 100644 index c853dca..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Bye bye.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Clap please.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Clap please.mp3 deleted file mode 100644 index 5dcaf62..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Clap please.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Demonstration ready mode.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Demonstration ready mode.mp3 deleted file mode 100644 index 6f73a75..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Demonstration ready mode.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Headstand.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Headstand.mp3 deleted file mode 100644 index 6864057..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Headstand.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Interactive motion mode.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Interactive motion mode.mp3 deleted file mode 100644 index db2f2c9..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Interactive motion mode.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro01.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro01.mp3 deleted file mode 100644 index 60a5051..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro01.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro02.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro02.mp3 deleted file mode 100644 index b1c745c..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro02.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro03.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro03.mp3 deleted file mode 100644 index cc30740..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro03.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Introduction.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Introduction.mp3 deleted file mode 100644 index 87c699d..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Introduction.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Left kick.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Left kick.mp3 deleted file mode 100644 index 142b79c..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Left kick.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/No.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/No.mp3 deleted file mode 100644 index 44b4aab..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/No.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Oops.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Oops.mp3 deleted file mode 100644 index 3ee002c..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Oops.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Right kick.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Right kick.mp3 deleted file mode 100644 index a428de8..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Right kick.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Sensor calibration complete.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Sensor calibration complete.mp3 deleted file mode 100644 index 9f9ed9e..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Sensor calibration complete.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Sensor calibration fail.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Sensor calibration fail.mp3 deleted file mode 100644 index a0e3ff9..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Sensor calibration fail.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Shoot.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Shoot.mp3 deleted file mode 100644 index 1bd6c54..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Shoot.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Sit down.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Sit down.mp3 deleted file mode 100644 index d7f25da..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Sit down.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Stand up.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Stand up.mp3 deleted file mode 100644 index 821361e..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Stand up.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Start motion demonstration.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Start motion demonstration.mp3 deleted file mode 100644 index 3fb573b..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Start motion demonstration.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Start soccer demonstration.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Start soccer demonstration.mp3 deleted file mode 100644 index 8820520..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Start soccer demonstration.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Start vision processing demonstration.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Start vision processing demonstration.mp3 deleted file mode 100644 index 3daa867..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Start vision processing demonstration.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/System shutdown.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/System shutdown.mp3 deleted file mode 100644 index bdc2be4..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/System shutdown.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Thank you.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Thank you.mp3 deleted file mode 100644 index 6778cb1..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Thank you.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Vision processing mode.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Vision processing mode.mp3 deleted file mode 100644 index fd4d9bd..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Vision processing mode.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Wow.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Wow.mp3 deleted file mode 100644 index 92abee2..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Wow.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Yes go.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Yes go.mp3 deleted file mode 100644 index d3ebfb2..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Yes go.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/Yes.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/Yes.mp3 deleted file mode 100644 index 7a515d0..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/Yes.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/intro_test.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/intro_test.mp3 deleted file mode 100644 index 38a4d53..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/intro_test.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Announce mic test.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Announce mic test.mp3 deleted file mode 100644 index 4940e78..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Announce mic test.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Button test mode.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Button test mode.mp3 deleted file mode 100644 index d2d5ca0..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Button test mode.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Mic test mode.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Mic test mode.mp3 deleted file mode 100644 index 2e23402..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Mic test mode.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Mode button long pressed.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Mode button long pressed.mp3 deleted file mode 100644 index 01c0353..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Mode button long pressed.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Mode button pressed.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Mode button pressed.mp3 deleted file mode 100644 index fc56508..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Mode button pressed.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Self test ready mode.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Self test ready mode.mp3 deleted file mode 100644 index 0b3a9c1..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Self test ready mode.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start button long pressed.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start button long pressed.mp3 deleted file mode 100644 index 1562ae9..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start button long pressed.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start button pressed.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start button pressed.mp3 deleted file mode 100644 index a4348bd..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start button pressed.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start button test mode.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start button test mode.mp3 deleted file mode 100644 index 9bf71e9..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start button test mode.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start mic test mode.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start mic test mode.mp3 deleted file mode 100644 index fca8044..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start mic test mode.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start playing.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start playing.mp3 deleted file mode 100644 index f1c9693..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start playing.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start recording.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start recording.mp3 deleted file mode 100644 index 39d336c..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/Start recording.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/User button long pressed.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/User button long pressed.mp3 deleted file mode 100644 index 7994fde..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/User button long pressed.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/User button pressed.mp3 b/humanoid_robot_intelligence_control_system_demo/data/mp3/test/User button pressed.mp3 deleted file mode 100644 index f4e19d0..0000000 Binary files a/humanoid_robot_intelligence_control_system_demo/data/mp3/test/User button pressed.mp3 and /dev/null differ diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/action_demo.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/action_demo.h deleted file mode 100644 index b8ee165..0000000 --- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/action_demo.h +++ /dev/null @@ -1,114 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef ACTION_DEMO_H_ -#define ACTION_DEMO_H_ - -#include -#include -#include -#include - -#include -#include - -#include "humanoid_robot_intelligence_control_system_action_module_msgs/IsRunning.h" -#include "humanoid_robot_intelligence_control_system_demo/op_demo.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/SetModule.h" - -namespace humanoid_robot_intelligence_control_system_op { - -class ActionDemo : public OPDemo { -public: - ActionDemo(); - ~ActionDemo(); - - void setDemoEnable(); - void setDemoDisable(); - -protected: - enum ActionCommandIndex { - BrakeActionCommand = -2, - StopActionCommand = -1, - }; - - enum ActionStatus { - PlayAction = 1, - PauseAction = 2, - StopAction = 3, - ReadyAction = 4, - }; - - const int SPIN_RATE; - const bool DEBUG_PRINT; - - void processThread(); - void callbackThread(); - - void process(); - void startProcess(const std::string &set_name = "default"); - void resumeProcess(); - void pauseProcess(); - void stopProcess(); - - void handleStatus(); - - void parseActionScript(const std::string &path); - bool parseActionScriptSetName(const std::string &path, - const std::string &set_name); - - bool playActionWithSound(int motion_index); - - void playMP3(std::string &path); - void stopMP3(); - - void playAction(int motion_index); - void stopAction(); - void brakeAction(); - bool isActionRunning(); - - void setModuleToDemo(const std::string &module_name); - void callServiceSettingModule(const std::string &module_name); - void actionSetNameCallback(const std_msgs::String::ConstPtr &msg); - void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); - void demoCommandCallback(const std_msgs::String::ConstPtr &msg); - - ros::Publisher module_control_pub_; - ros::Publisher motion_index_pub_; - ros::Publisher play_sound_pub_; - - ros::Subscriber buttuon_sub_; - ros::Subscriber demo_command_sub_; - - ros::ServiceClient is_running_client_; - ros::ServiceClient set_joint_module_client_; - - std::map action_sound_table_; - std::vector play_list_; - - std::string script_path_; - std::string play_list_name_; - int play_index_; - - int play_status_; -}; - -} /* namespace humanoid_robot_intelligence_control_system_op */ - -#endif /* ACTION_DEMO_H_ */ diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_follower.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_follower.h deleted file mode 100644 index cb9ed78..0000000 --- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_follower.h +++ /dev/null @@ -1,129 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef BALL_FOLLOWER_H_ -#define BALL_FOLLOWER_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "humanoid_robot_intelligence_control_system_ball_detector/CircleSetStamped.h" -#include "humanoid_robot_intelligence_control_system_walking_module_msgs/GetWalkingParam.h" -#include "humanoid_robot_intelligence_control_system_walking_module_msgs/WalkingParam.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h" - -namespace humanoid_robot_intelligence_control_system_op { - -// following the ball using walking -class BallFollower { -public: - enum { - NotFound = 0, - OutOfRange = 1, - OnRight = 2, - OnLeft = 3, - }; - - BallFollower(); - ~BallFollower(); - - bool processFollowing(double x_angle, double y_angle, double ball_size); - void decideBallPositin(double x_angle, double y_angle); - void waitFollowing(); - void startFollowing(); - void stopFollowing(); - void clearBallPosition() { approach_ball_position_ = NotFound; } - - int getBallPosition() { return approach_ball_position_; } - - bool isBallInRange() { - return (approach_ball_position_ == OnRight || - approach_ball_position_ == OnLeft); - } - -protected: - const bool DEBUG_PRINT; - const double CAMERA_HEIGHT; - const int NOT_FOUND_THRESHOLD; - const double FOV_WIDTH; - const double FOV_HEIGHT; - const double MAX_FB_STEP; - const double MAX_RL_TURN; - const double IN_PLACE_FB_STEP; - const double MIN_FB_STEP; - const double MIN_RL_TURN; - const double UNIT_FB_STEP; - const double UNIT_RL_TURN; - - const double SPOT_FB_OFFSET; - const double SPOT_RL_OFFSET; - const double SPOT_ANGLE_OFFSET; - - void currentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void setWalkingCommand(const std::string &command); - void setWalkingParam(double x_move, double y_move, double rotation_angle, - bool balance = true); - bool getWalkingParam(); - void calcFootstep(double target_distance, double target_angle, - double delta_time, double &fb_move, double &rl_angle); - - // ros node handle - ros::NodeHandle nh_; - - // image publisher/subscriber - ros::Publisher module_control_pub_; - ros::Publisher head_joint_pub_; - ros::Publisher head_scan_pub_; - ros::Publisher set_walking_command_pub_; - ros::Publisher set_walking_param_pub_; - - ros::Publisher motion_index_pub_; - ros::ServiceClient get_walking_param_client_; - - ros::Subscriber ball_position_sub_; - ros::Subscriber ball_tracking_command_sub_; - ros::Subscriber current_joint_states_sub_; - - // (x, y) is the center position of the ball in image coordinates - // z is the ball radius - geometry_msgs::Point ball_position_; - humanoid_robot_intelligence_control_system_walking_module_msgs::WalkingParam current_walking_param_; - - int count_not_found_; - int count_to_kick_; - bool on_tracking_; - int approach_ball_position_; - double current_pan_, current_tilt_; - double current_x_move_, current_r_angle_; - int kick_motion_index_; - double hip_pitch_offset_; - ros::Time prev_time_; - - double curr_period_time_; - double accum_period_time_; -}; -} // namespace humanoid_robot_intelligence_control_system_op - -#endif /* BALL_FOLLOWER_H_ */ diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_tracker.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_tracker.h deleted file mode 100644 index 9ccceeb..0000000 --- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/ball_tracker.h +++ /dev/null @@ -1,113 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef BALL_TRACKING_H_ -#define BALL_TRACKING_H_ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "humanoid_robot_intelligence_control_system_ball_detector/CircleSetStamped.h" -#include "humanoid_robot_intelligence_control_system_walking_module_msgs/GetWalkingParam.h" -#include "humanoid_robot_intelligence_control_system_walking_module_msgs/WalkingParam.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h" - -namespace humanoid_robot_intelligence_control_system_op { - -// head tracking for looking the ball -class BallTracker { -public: - enum TrackingStatus { - NotFound = -1, - Waiting = 0, - Found = 1, - - }; - BallTracker(); - ~BallTracker(); - - int processTracking(); - - void startTracking(); - void stopTracking(); - - void setUsingHeadScan(bool use_scan); - void goInit(); - - double getPanOfBall() { - // left (+) ~ right (-) - return current_ball_pan_; - } - double getTiltOfBall() { - // top (+) ~ bottom (-) - return current_ball_tilt_; - } - double getBallSize() { return current_ball_bottom_; } - -protected: - const double FOV_WIDTH; - const double FOV_HEIGHT; - const int NOT_FOUND_THRESHOLD; - const int WAITING_THRESHOLD; - const bool DEBUG_PRINT; - - void ballPositionCallback( - const humanoid_robot_intelligence_control_system_ball_detector::CircleSetStamped::ConstPtr &msg); - void ballTrackerCommandCallback(const std_msgs::String::ConstPtr &msg); - void publishHeadJoint(double pan, double tilt); - void scanBall(); - - // ros node handle - ros::NodeHandle nh_; - - // image publisher/subscriber - ros::Publisher module_control_pub_; - ros::Publisher head_joint_offset_pub_; - ros::Publisher head_joint_pub_; - ros::Publisher head_scan_pub_; - - // ros::Publisher error_pub_; - - ros::Publisher motion_index_pub_; - - ros::Subscriber ball_position_sub_; - ros::Subscriber ball_tracking_command_sub_; - - // (x, y) is the center position of the ball in image coordinates - // z is the ball radius - geometry_msgs::Point ball_position_; - - int tracking_status_; - bool use_head_scan_; - int count_not_found_; - bool on_tracking_; - double current_ball_pan_, current_ball_tilt_; - double current_ball_bottom_; - double x_error_sum_, y_error_sum_; - ros::Time prev_time_; - double p_gain_, d_gain_, i_gain_; -}; -} // namespace humanoid_robot_intelligence_control_system_op - -#endif /* BALL_TRACKING_H_ */ diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/button_test.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/button_test.h deleted file mode 100644 index ead7c2b..0000000 --- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/button_test.h +++ /dev/null @@ -1,66 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef BUTTON_TEST_H_ -#define BUTTON_TEST_H_ - -#include -#include -#include -#include - -#include "humanoid_robot_intelligence_control_system_demo/op_demo.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h" - -namespace humanoid_robot_intelligence_control_system_op { - -class ButtonTest : public OPDemo { -public: - ButtonTest(); - ~ButtonTest(); - - void setDemoEnable(); - void setDemoDisable(); - -protected: - const int SPIN_RATE; - - void processThread(); - void callbackThread(); - - void process(); - - void setRGBLED(int blue, int green, int red); - void setLED(int led); - - void playSound(const std::string &path); - - void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); - - ros::Publisher rgb_led_pub_; - ros::Publisher play_sound_pub_; - ros::Subscriber buttuon_sub_; - - std::string default_mp3_path_; - int led_count_; - int rgb_led_count_; -}; - -} /* namespace humanoid_robot_intelligence_control_system_op */ - -#endif /* BUTTON_TEST_H_ */ diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/face_tracker.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/face_tracker.h deleted file mode 100644 index 6c2dbbc..0000000 --- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/face_tracker.h +++ /dev/null @@ -1,94 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef FACE_TRACKING_H_ -#define FACE_TRACKING_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace humanoid_robot_intelligence_control_system_op { - -// head tracking for looking the ball -class FaceTracker { -public: - enum TrackingStatus { - NotFound = -1, - Waiting = 0, - Found = 1, - - }; - - FaceTracker(); - ~FaceTracker(); - - int processTracking(); - - void startTracking(); - void stopTracking(); - - void setUsingHeadScan(bool use_scan); - void setFacePosition(geometry_msgs::Point &face_position); - void goInit(double init_pan, double init_tile); - - double getPanOfFace() { return current_face_pan_; } - double getTiltOfFace() { return current_face_tilt_; } - -protected: - const double FOV_WIDTH; - const double FOV_HEIGHT; - const int NOT_FOUND_THRESHOLD; - - void facePositionCallback(const geometry_msgs::Point::ConstPtr &msg); - void faceTrackerCommandCallback(const std_msgs::String::ConstPtr &msg); - void publishHeadJoint(double pan, double tilt); - void scanFace(); - - // ros node handle - ros::NodeHandle nh_; - - // image publisher/subscriber - ros::Publisher module_control_pub_; - ros::Publisher head_joint_offset_pub_; - ros::Publisher head_joint_pub_; - ros::Publisher head_scan_pub_; - - ros::Subscriber face_position_sub_; - ros::Subscriber face_tracking_command_sub_; - - // (x, y) is the center position of the ball in image coordinates - // z is the face size - geometry_msgs::Point face_position_; - - bool use_head_scan_; - int count_not_found_; - bool on_tracking_; - double current_face_pan_, current_face_tilt_; - - int dismissed_count_; -}; -} // namespace humanoid_robot_intelligence_control_system_op - -#endif /* FACE_TRACKING_H_ */ diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/mic_test.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/mic_test.h deleted file mode 100644 index 0477d8d..0000000 --- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/mic_test.h +++ /dev/null @@ -1,86 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef MIC_TEST_H_ -#define MIC_TEST_H_ - -#include -#include -#include -#include -#include - -#include "humanoid_robot_intelligence_control_system_demo/op_demo.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h" - -namespace humanoid_robot_intelligence_control_system_op { - -class MicTest : public OPDemo { -public: - enum Mic_Test_Status { - Ready = 0, - AnnounceRecording = 1, - MicRecording = 2, - PlayingSound = 3, - DeleteTempFile = 4, - DemoCount = 5 - }; - - MicTest(); - ~MicTest(); - - void setDemoEnable(); - void setDemoDisable(); - -protected: - const int SPIN_RATE; - - void processThread(); - void callbackThread(); - - void process(); - - void announceTest(); - void recordSound(int recording_time); - void recordSound(); - void playTestSound(const std::string &path); - void playSound(const std::string &file_path); - void deleteSoundFile(const std::string &file_path); - - void startTimer(double wait_time); - void finishTimer(); - - void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); - - std::string recording_file_name_; - std::string default_mp3_path_; - - ros::Publisher play_sound_pub_; - ros::Subscriber buttuon_sub_; - - ros::Time start_time_; - double wait_time_; - bool is_wait_; - int record_pid_; - int play_pid_; - int test_status_; -}; - -} /* namespace humanoid_robot_intelligence_control_system_op */ - -#endif /* MIC_TEST_H_ */ diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/op_demo.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/op_demo.h deleted file mode 100644 index 5872213..0000000 --- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/op_demo.h +++ /dev/null @@ -1,49 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef OP_DEMO_H_ -#define OP_DEMO_H_ - -namespace humanoid_robot_intelligence_control_system_op { - -class OPDemo { -public: - enum Motion_Index { - InitPose = 1, - WalkingReady = 9, - GetUpFront = 122, - GetUpBack = 123, - RightKick = 121, - LeftKick = 120, - Ceremony = 27, - ForGrass = 20, - }; - - OPDemo() {} - virtual ~OPDemo() {} - - virtual void setDemoEnable() {} - virtual void setDemoDisable() {} - -protected: - bool enable_; -}; - -} /* namespace humanoid_robot_intelligence_control_system_op */ - -#endif /* OP_DEMO_H_ */ diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/soccer_demo.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/soccer_demo.h deleted file mode 100644 index 4af5a5a..0000000 --- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/soccer_demo.h +++ /dev/null @@ -1,134 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef SOCCER_DEMO_H -#define SOCCER_DEMO_H - -#include -#include -#include -#include -#include -#include - -#include "humanoid_robot_intelligence_control_system_action_module_msgs/IsRunning.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/SetJointModule.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h" - -#include "humanoid_robot_intelligence_control_system_demo/ball_follower.h" -#include "humanoid_robot_intelligence_control_system_demo/ball_tracker.h" -#include "humanoid_robot_intelligence_control_system_demo/op_demo.h" -#include "humanoid_robot_intelligence_control_system_math/humanoid_robot_intelligence_control_system_linear_algebra.h" - -namespace humanoid_robot_intelligence_control_system_op { - -class SoccerDemo : public OPDemo { -public: - enum Stand_Status { - Stand = 0, - Fallen_Forward = 1, - Fallen_Behind = 2, - }; - - enum Robot_Status { - Waited = 0, - TrackingAndFollowing = 1, - ReadyToKick = 2, - ReadyToCeremony = 3, - ReadyToGetup = 4, - }; - - SoccerDemo(); - ~SoccerDemo(); - - void setDemoEnable(); - void setDemoDisable(); - -protected: - const double FALL_FORWARD_LIMIT; - const double FALL_BACK_LIMIT; - const int SPIN_RATE; - const bool DEBUG_PRINT; - - void processThread(); - void callbackThread(); - void trackingThread(); - - void setBodyModuleToDemo(const std::string &body_module, - bool with_head_control = true); - void setModuleToDemo(const std::string &module_name); - void callServiceSettingModule( - const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule &modules); - void parseJointNameFromYaml(const std::string &path); - bool getJointNameFromID(const int &id, std::string &joint_name); - bool getIDFromJointName(const std::string &joint_name, int &id); - int getJointCount(); - bool isHeadJoint(const int &id); - void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); - void demoCommandCallback(const std_msgs::String::ConstPtr &msg); - void imuDataCallback(const sensor_msgs::Imu::ConstPtr &msg); - - void startSoccerMode(); - void stopSoccerMode(); - - void process(); - void handleKick(int ball_position); - void handleKick(); - bool handleFallen(int fallen_status); - - void playMotion(int motion_index); - void setRGBLED(int blue, int green, int red); - bool isActionRunning(); - - void sendDebugTopic(const std::string &msgs); - - BallTracker ball_tracker_; - BallFollower ball_follower_; - - ros::Publisher module_control_pub_; - ros::Publisher motion_index_pub_; - ros::Publisher rgb_led_pub_; - ros::Subscriber buttuon_sub_; - ros::Subscriber demo_command_sub_; - ros::Subscriber imu_data_sub_; - - ros::Publisher test_pub_; - - ros::ServiceClient is_running_client_; - ros::ServiceClient set_joint_module_client_; - - std::map id_joint_table_; - std::map joint_id_table_; - - bool is_grass_; - int wait_count_; - bool on_following_ball_; - bool on_tracking_ball_; - bool restart_soccer_; - bool start_following_; - bool stop_following_; - bool stop_fallen_check_; - int robot_status_; - int tracking_status_; - int stand_state_; - double present_pitch_; -}; - -} // namespace humanoid_robot_intelligence_control_system_op -#endif // SOCCER_DEMO_H diff --git a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/vision_demo.h b/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/vision_demo.h deleted file mode 100644 index a42de50..0000000 --- a/humanoid_robot_intelligence_control_system_demo/include/humanoid_robot_intelligence_control_system_demo/vision_demo.h +++ /dev/null @@ -1,83 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#ifndef VISION_DEMO_H_ -#define VISION_DEMO_H_ - -#include -#include -#include -#include -#include - -#include "humanoid_robot_intelligence_control_system_controller_msgs/SetModule.h" -#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h" - -#include "humanoid_robot_intelligence_control_system_demo/face_tracker.h" -#include "humanoid_robot_intelligence_control_system_demo/op_demo.h" - -namespace humanoid_robot_intelligence_control_system_op { - -class VisionDemo : public OPDemo { -public: - VisionDemo(); - ~VisionDemo(); - - void setDemoEnable(); - void setDemoDisable(); - -protected: - const int SPIN_RATE; - const int TIME_TO_INIT; - - void processThread(); - void callbackThread(); - - void process(); - - void playMotion(int motion_index); - void setRGBLED(int blue, int green, int red); - - void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); - void facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg); - void demoCommandCallback(const std_msgs::String::ConstPtr &msg); - - void setModuleToDemo(const std::string &module_name); - void callServiceSettingModule(const std::string &module_name); - - FaceTracker face_tracker_; - - ros::Publisher module_control_pub_; - ros::Publisher motion_index_pub_; - ros::Publisher rgb_led_pub_; - ros::Publisher face_tracking_command_pub_; - - ros::Subscriber buttuon_sub_; - ros::Subscriber faceCoord_sub_; - - ros::ServiceClient set_joint_module_client_; - geometry_msgs::Point face_position_; - - ros::Time prev_time_; - - int tracking_status_; -}; - -} /* namespace humanoid_robot_intelligence_control_system_op */ - -#endif /* VISION_DEMO_H_ */ diff --git a/humanoid_robot_intelligence_control_system_demo/launch/demo.launch b/humanoid_robot_intelligence_control_system_demo/launch/demo.launch deleted file mode 100644 index 2734717..0000000 --- a/humanoid_robot_intelligence_control_system_demo/launch/demo.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/humanoid_robot_intelligence_control_system_demo/launch/face_detection.launch b/humanoid_robot_intelligence_control_system_demo/launch/face_detection.launch deleted file mode 100644 index 068d79e..0000000 --- a/humanoid_robot_intelligence_control_system_demo/launch/face_detection.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/humanoid_robot_intelligence_control_system_demo/launch/self_test.launch b/humanoid_robot_intelligence_control_system_demo/launch/self_test.launch deleted file mode 100644 index 4751a5f..0000000 --- a/humanoid_robot_intelligence_control_system_demo/launch/self_test.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/humanoid_robot_intelligence_control_system_demo/list/action_script.yaml b/humanoid_robot_intelligence_control_system_demo/list/action_script.yaml deleted file mode 100644 index 65c9e58..0000000 --- a/humanoid_robot_intelligence_control_system_demo/list/action_script.yaml +++ /dev/null @@ -1,23 +0,0 @@ -# combination action page number and mp3 file path -action_and_sound: - 4 : "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Thank you.mp3" - 41: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Introduction.mp3" - 24: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Wow.mp3" - 23: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Yes go.mp3" - 15: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Sit down.mp3" - 1: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Stand up.mp3" - 54: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Clap please.mp3" - 27: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Oops.mp3" - 38: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Bye bye.mp3" -# 101 : "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Oops.mp3" - 110 : "" - 111 : "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro01.mp3" - 115 : "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro02.mp3" - 118 : "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Intro03.mp3" - -# play list -prev_default: [4, 41, 24, 23, 15, 1, 54, 27, 38] -default: [4, 110, 111, 115, 118, 24, 54, 27, 38] - -# example of play list -#certification: [101] diff --git a/humanoid_robot_intelligence_control_system_demo/list/action_script_bk.yaml b/humanoid_robot_intelligence_control_system_demo/list/action_script_bk.yaml deleted file mode 100644 index 67c1389..0000000 --- a/humanoid_robot_intelligence_control_system_demo/list/action_script_bk.yaml +++ /dev/null @@ -1,18 +0,0 @@ -# combination action page number and mp3 file path -action_and_sound: - 4 : "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Thank you.mp3" - 41: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Introduction.mp3" - 24: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Wow.mp3" - 23: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Yes go.mp3" - 15: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Sit down.mp3" - 1: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Stand up.mp3" - 54: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Clap please.mp3" - 27: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Oops.mp3" - 38: "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Bye bye.mp3" - 101 : "/home/humanoid_robot_intelligence_control_system/catkin_ws/src/ROBOTIS-HUMANOID_ROBOT/ROBOTIS-HUMANOID_ROBOT-Demo/humanoid_robot_intelligence_control_system_demo/data/mp3/Oops.mp3" - -# play list -default: [4, 41, 24, 23, 15, 1, 54, 27, 38] - -# example of play list -certificatino: [101] diff --git a/humanoid_robot_intelligence_control_system_demo/package.xml b/humanoid_robot_intelligence_control_system_demo/package.xml deleted file mode 100644 index f0d81ce..0000000 --- a/humanoid_robot_intelligence_control_system_demo/package.xml +++ /dev/null @@ -1,180 +0,0 @@ - - - - - humanoid_robot_intelligence_control_system_demo - 0.3.2 - - HUMANOID_ROBOT default demo - It includes three demontrations(soccer demo, vision demo, action script demo) - - Apache 2.0 - Ronaldson Bellande - - - catkin - - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - - humanoid_robot_intelligence_control_system_walking_module_msgs - - humanoid_robot_intelligence_control_system_action_module_msgs - cmake_modules - humanoid_robot_intelligence_control_system_math - - humanoid_robot_intelligence_control_system_ball_detector - boost - eigen - yaml-cpp - humanoid_robot_intelligence_control_system_manager - - humanoid_robot_intelligence_control_system_camera_setting_tool - - humanoid_robot_intelligence_control_system_web_setting_tool - - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - - humanoid_robot_intelligence_control_system_walking_module_msgs - - humanoid_robot_intelligence_control_system_action_module_msgs - cmake_modules - humanoid_robot_intelligence_control_system_math - - humanoid_robot_intelligence_control_system_ball_detector - boost - eigen - yaml-cpp - - humanoid_robot_intelligence_control_system_manager - - humanoid_robot_intelligence_control_system_camera_setting_tool - - humanoid_robot_intelligence_control_system_web_setting_tool - - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - - humanoid_robot_intelligence_control_system_walking_module_msgs - - humanoid_robot_intelligence_control_system_action_module_msgs - cmake_modules - humanoid_robot_intelligence_control_system_math - - humanoid_robot_intelligence_control_system_ball_detector - boost - eigen - yaml-cpp - humanoid_robot_intelligence_control_system_manager - - humanoid_robot_intelligence_control_system_camera_setting_tool - - humanoid_robot_intelligence_control_system_web_setting_tool - - - ament_cmake - - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - - humanoid_robot_intelligence_control_system_walking_module_msgs - - humanoid_robot_intelligence_control_system_action_module_msgs - cmake_modules - humanoid_robot_intelligence_control_system_math - - humanoid_robot_intelligence_control_system_ball_detector - boost - eigen - yaml-cpp - humanoid_robot_intelligence_control_system_manager - - humanoid_robot_intelligence_control_system_camera_setting_tool - - humanoid_robot_intelligence_control_system_web_setting_tool - - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - - humanoid_robot_intelligence_control_system_walking_module_msgs - - humanoid_robot_intelligence_control_system_action_module_msgs - cmake_modules - humanoid_robot_intelligence_control_system_math - - humanoid_robot_intelligence_control_system_ball_detector - boost - eigen - yaml-cpp - - humanoid_robot_intelligence_control_system_manager - - humanoid_robot_intelligence_control_system_camera_setting_tool - - humanoid_robot_intelligence_control_system_web_setting_tool - - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - - humanoid_robot_intelligence_control_system_controller_msgs - - humanoid_robot_intelligence_control_system_walking_module_msgs - - humanoid_robot_intelligence_control_system_action_module_msgs - cmake_modules - humanoid_robot_intelligence_control_system_math - - humanoid_robot_intelligence_control_system_ball_detector - boost - eigen - yaml-cpp - humanoid_robot_intelligence_control_system_manager - - humanoid_robot_intelligence_control_system_camera_setting_tool - - humanoid_robot_intelligence_control_system_web_setting_tool - - diff --git a/humanoid_robot_intelligence_control_system_demo/src/action/action_demo.cpp b/humanoid_robot_intelligence_control_system_demo/src/action/action_demo.cpp deleted file mode 100644 index dcdec9f..0000000 --- a/humanoid_robot_intelligence_control_system_demo/src/action/action_demo.cpp +++ /dev/null @@ -1,346 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include "humanoid_robot_intelligence_control_system_demo/action_demo.h" - -namespace humanoid_robot_intelligence_control_system_op { - -ActionDemo::ActionDemo() - : SPIN_RATE(30), DEBUG_PRINT(false), play_index_(0), - play_status_(StopAction) { - enable_ = false; - - ros::NodeHandle nh(ros::this_node::getName()); - - std::string default_path = - ros::package::getPath("humanoid_robot_intelligence_control_system_demo") + "/list/action_script.yaml"; - script_path_ = nh.param("action_script", default_path); - - std::string default_play_list = "default"; - play_list_name_ = - nh.param("action_script_play_list", default_play_list); - - demo_command_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/demo_command", 1, - &ActionDemo::demoCommandCallback, this); - - parseActionScript(script_path_); - - boost::thread queue_thread = - boost::thread(boost::bind(&ActionDemo::callbackThread, this)); - boost::thread process_thread = - boost::thread(boost::bind(&ActionDemo::processThread, this)); -} - -ActionDemo::~ActionDemo() {} - -void ActionDemo::setDemoEnable() { - setModuleToDemo("action_module"); - - enable_ = true; - - ROS_INFO_COND(DEBUG_PRINT, "Start ActionScript Demo"); - - playAction(InitPose); - - startProcess(play_list_name_); -} - -void ActionDemo::setDemoDisable() { - stopProcess(); - - enable_ = false; - ROS_WARN("Set Action demo disable"); - play_list_.resize(0); -} - -void ActionDemo::process() { - switch (play_status_) { - case PlayAction: { - if (play_list_.size() == 0) { - ROS_WARN("Play List is empty."); - return; - } - - // action is not running - if (isActionRunning() == false) { - // play - bool result_play = playActionWithSound(play_list_.at(play_index_)); - - ROS_INFO_COND(!result_play, "Fail to play action script."); - - // add play index - int index_to_play = (play_index_ + 1) % play_list_.size(); - play_index_ = index_to_play; - } else { - // wait - return; - } - break; - } - - case PauseAction: { - stopMP3(); - brakeAction(); - - play_status_ = ReadyAction; - - break; - } - - case StopAction: { - stopMP3(); - stopAction(); - - play_status_ = ReadyAction; - - break; - } - - default: - break; - } -} - -void ActionDemo::startProcess(const std::string &set_name) { - parseActionScriptSetName(script_path_, set_name); - - play_status_ = PlayAction; -} - -void ActionDemo::resumeProcess() { play_status_ = PlayAction; } - -void ActionDemo::pauseProcess() { play_status_ = PauseAction; } - -void ActionDemo::stopProcess() { - play_index_ = 0; - play_status_ = StopAction; -} - -void ActionDemo::processThread() { - // set node loop rate - ros::Rate loop_rate(SPIN_RATE); - - // node loop - while (ros::ok()) { - if (enable_ == true) - process(); - - // relax to fit output rate - loop_rate.sleep(); - } -} - -void ActionDemo::callbackThread() { - ros::NodeHandle nh(ros::this_node::getName()); - - // subscriber & publisher - module_control_pub_ = - nh.advertise("/humanoid_robot_intelligence_control_system/enable_ctrl_module", 0); - motion_index_pub_ = - nh.advertise("/humanoid_robot_intelligence_control_system/action/page_num", 0); - play_sound_pub_ = nh.advertise("/play_sound_file", 0); - - buttuon_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1, - &ActionDemo::buttonHandlerCallback, this); - - is_running_client_ = - nh.serviceClient( - "/humanoid_robot_intelligence_control_system/action/is_running"); - set_joint_module_client_ = - nh.serviceClient( - "/humanoid_robot_intelligence_control_system/set_present_ctrl_modules"); - - while (nh.ok()) { - ros::spinOnce(); - - usleep(1000); - } -} - -void ActionDemo::parseActionScript(const std::string &path) { - YAML::Node doc; - - try { - // load yaml - doc = YAML::LoadFile(path.c_str()); - } catch (const std::exception &e) { - ROS_ERROR_STREAM("Fail to load action script yaml. - " << e.what()); - ROS_ERROR_STREAM("Script Path : " << path); - return; - } - - // parse action_sound table - YAML::Node sub_node = doc["action_and_sound"]; - for (YAML::iterator yaml_it = sub_node.begin(); yaml_it != sub_node.end(); - ++yaml_it) { - int action_index = yaml_it->first.as(); - std::string mp3_path = yaml_it->second.as(); - - action_sound_table_[action_index] = mp3_path; - } - - // default action set - if (doc["default"]) - play_list_ = doc["default"].as>(); -} - -bool ActionDemo::parseActionScriptSetName(const std::string &path, - const std::string &set_name) { - - YAML::Node doc; - - try { - // load yaml - doc = YAML::LoadFile(path.c_str()); - } catch (const std::exception &e) { - ROS_ERROR("Fail to load yaml."); - return false; - } - - // parse action_sound table - if (doc[set_name]) { - play_list_ = doc[set_name].as>(); - return true; - } else - return false; -} - -bool ActionDemo::playActionWithSound(int motion_index) { - std::map::iterator map_it = - action_sound_table_.find(motion_index); - if (map_it == action_sound_table_.end()) - return false; - - playAction(motion_index); - playMP3(map_it->second); - - ROS_INFO_STREAM_COND(DEBUG_PRINT, - "action : " << motion_index - << ", mp3 path : " << map_it->second); - - return true; -} - -void ActionDemo::playMP3(std::string &path) { - std_msgs::String sound_msg; - sound_msg.data = path; - - play_sound_pub_.publish(sound_msg); -} - -void ActionDemo::stopMP3() { - std_msgs::String sound_msg; - sound_msg.data = ""; - - play_sound_pub_.publish(sound_msg); -} - -void ActionDemo::playAction(int motion_index) { - std_msgs::Int32 motion_msg; - motion_msg.data = motion_index; - - motion_index_pub_.publish(motion_msg); -} - -void ActionDemo::stopAction() { - std_msgs::Int32 motion_msg; - motion_msg.data = StopActionCommand; - - motion_index_pub_.publish(motion_msg); -} - -void ActionDemo::brakeAction() { - std_msgs::Int32 motion_msg; - motion_msg.data = BrakeActionCommand; - - motion_index_pub_.publish(motion_msg); -} - -// check running of action -bool ActionDemo::isActionRunning() { - humanoid_robot_intelligence_control_system_action_module_msgs::IsRunning is_running_srv; - - if (is_running_client_.call(is_running_srv) == false) { - ROS_ERROR("Failed to get action status"); - return true; - } else { - if (is_running_srv.response.is_running == true) { - return true; - } - } - - return false; -} - -void ActionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { - if (enable_ == false) - return; - - if (msg->data == "start") { - switch (play_status_) { - case PlayAction: { - pauseProcess(); - break; - } - - case PauseAction: { - resumeProcess(); - break; - } - - case StopAction: { - resumeProcess(); - break; - } - - default: - break; - } - } else if (msg->data == "mode") { - } -} - -void ActionDemo::setModuleToDemo(const std::string &module_name) { - callServiceSettingModule(module_name); - ROS_INFO_STREAM("enable module : " << module_name); -} - -void ActionDemo::callServiceSettingModule(const std::string &module_name) { - humanoid_robot_intelligence_control_system_controller_msgs::SetModule set_module_srv; - set_module_srv.request.module_name = module_name; - - if (set_joint_module_client_.call(set_module_srv) == false) { - ROS_ERROR("Failed to set module"); - return; - } - - return; -} - -void ActionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) { - if (enable_ == false) - return; - - if (msg->data == "start") { - resumeProcess(); - } else if (msg->data == "stop") { - pauseProcess(); - } -} - -} /* namespace humanoid_robot_intelligence_control_system_op */ diff --git a/humanoid_robot_intelligence_control_system_demo/src/demo_node.cpp b/humanoid_robot_intelligence_control_system_demo/src/demo_node.cpp deleted file mode 100644 index 523b62a..0000000 --- a/humanoid_robot_intelligence_control_system_demo/src/demo_node.cpp +++ /dev/null @@ -1,351 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include -#include - -#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h" -#include "humanoid_robot_intelligence_control_system_demo/action_demo.h" -#include "humanoid_robot_intelligence_control_system_demo/soccer_demo.h" -#include "humanoid_robot_intelligence_control_system_demo/vision_demo.h" -#include "humanoid_robot_intelligence_control_system_math/humanoid_robot_intelligence_control_system_linear_algebra.h" - -enum Demo_Status { - Ready = 0, - SoccerDemo = 1, - VisionDemo = 2, - ActionDemo = 3, - DemoCount = 4, -}; - -void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); -void goInitPose(); -void playSound(const std::string &path); -void setLED(int led); -bool checkManagerRunning(std::string &manager_name); -void dxlTorqueChecker(); - -void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg); - -const int SPIN_RATE = 30; -const bool DEBUG_PRINT = false; - -ros::Publisher init_pose_pub; -ros::Publisher play_sound_pub; -ros::Publisher led_pub; -ros::Publisher dxl_torque_pub; - -std::string default_mp3_path = ""; -int current_status = Ready; -int desired_status = Ready; -bool apply_desired = false; - -// node main -int main(int argc, char **argv) { - // init ros - ros::init(argc, argv, "demo_node"); - - // create ros wrapper object - humanoid_robot_intelligence_control_system_op::OPDemo *current_demo = NULL; - humanoid_robot_intelligence_control_system_op::SoccerDemo *soccer_demo = - new humanoid_robot_intelligence_control_system_op::SoccerDemo(); - humanoid_robot_intelligence_control_system_op::ActionDemo *action_demo = - new humanoid_robot_intelligence_control_system_op::ActionDemo(); - humanoid_robot_intelligence_control_system_op::VisionDemo *vision_demo = - new humanoid_robot_intelligence_control_system_op::VisionDemo(); - - ros::NodeHandle nh(ros::this_node::getName()); - - init_pose_pub = nh.advertise( - "/humanoid_robot_intelligence_control_system/base/ini_pose", 0); - play_sound_pub = nh.advertise("/play_sound_file", 0); - led_pub = - nh.advertise( - "/humanoid_robot_intelligence_control_system/sync_write_item", 0); - dxl_torque_pub = nh.advertise( - "/humanoid_robot_intelligence_control_system/dxl_torque", 0); - ros::Subscriber buttuon_sub = - nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", - 1, buttonHandlerCallback); - ros::Subscriber mode_command_sub = - nh.subscribe("/humanoid_robot_intelligence_control_system/mode_command", - 1, demoModeCommandCallback); - - default_mp3_path = - ros::package::getPath("humanoid_robot_intelligence_control_system_demo") + - "/data/mp3/"; - - ros::start(); - - // set node loop rate - ros::Rate loop_rate(SPIN_RATE); - - // wait for starting of manager - std::string manager_name = - "/humanoid_robot_intelligence_control_system_manager"; - while (ros::ok()) { - ros::Duration(1.0).sleep(); - - if (checkManagerRunning(manager_name) == true) { - break; - ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect"); - } - ROS_WARN("Waiting for humanoid_robot_intelligence_control_system manager"); - } - - // init procedure - playSound(default_mp3_path + "Demonstration ready mode.mp3"); - // turn on R/G/B LED - setLED(0x01 | 0x02 | 0x04); - - // node loop - while (ros::ok()) { - // process - if (apply_desired == true) { - switch (desired_status) { - case Ready: { - - if (current_demo != NULL) - current_demo->setDemoDisable(); - - current_demo = NULL; - - goInitPose(); - - ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]"); - break; - } - - case SoccerDemo: { - if (current_demo != NULL) - current_demo->setDemoDisable(); - - current_demo = soccer_demo; - current_demo->setDemoEnable(); - - ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo"); - break; - } - - case VisionDemo: { - if (current_demo != NULL) - current_demo->setDemoDisable(); - - current_demo = vision_demo; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo"); - break; - } - case ActionDemo: { - if (current_demo != NULL) - current_demo->setDemoDisable(); - - current_demo = action_demo; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo"); - break; - } - - default: { - break; - } - } - - apply_desired = false; - current_status = desired_status; - } - - // execute pending callbacks - ros::spinOnce(); - - // relax to fit output rate - loop_rate.sleep(); - } - - // exit program - return 0; -} - -void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { - if (apply_desired == true) - return; - - // in the middle of playing demo - if (current_status != Ready) { - if (msg->data == "mode_long") { - // go to mode selection status - desired_status = Ready; - apply_desired = true; - - playSound(default_mp3_path + "Demonstration ready mode.mp3"); - setLED(0x01 | 0x02 | 0x04); - } else if (msg->data == "user_long") { - // it's using in humanoid_robot_intelligence_control_system_manager - // torque on and going to init pose - } - } - // ready to start demo - else { - if (msg->data == "start") { - // select current demo - desired_status = - (desired_status == Ready) ? desired_status + 1 : desired_status; - apply_desired = true; - - // sound out desired status - switch (desired_status) { - case SoccerDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + "Start soccer demonstration.mp3"); - break; - - case VisionDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + - "Start vision processing demonstration.mp3"); - break; - - case ActionDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + "Start motion demonstration.mp3"); - break; - - default: - break; - } - - ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } else if (msg->data == "mode") { - // change to next demo - desired_status = (desired_status + 1) % DemoCount; - desired_status = - (desired_status == Ready) ? desired_status + 1 : desired_status; - - // sound out desired status and changing LED - switch (desired_status) { - case SoccerDemo: - playSound(default_mp3_path + "Autonomous soccer mode.mp3"); - setLED(0x01); - break; - - case VisionDemo: - playSound(default_mp3_path + "Vision processing mode.mp3"); - setLED(0x02); - break; - - case ActionDemo: - playSound(default_mp3_path + "Interactive motion mode.mp3"); - setLED(0x04); - break; - - default: - break; - } - - ROS_INFO_COND(DEBUG_PRINT, "= Demo Mode : %d", desired_status); - } - } -} - -void goInitPose() { - std_msgs::String init_msg; - init_msg.data = "ini_pose"; - - init_pose_pub.publish(init_msg); -} - -void playSound(const std::string &path) { - std_msgs::String sound_msg; - sound_msg.data = path; - - play_sound_pub.publish(sound_msg); -} - -void setLED(int led) { - humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem - syncwrite_msg; - syncwrite_msg.item_name = "LED"; - syncwrite_msg.joint_name.push_back("open-cr"); - syncwrite_msg.value.push_back(led); - - led_pub.publish(syncwrite_msg); -} - -bool checkManagerRunning(std::string &manager_name) { - std::vector node_list; - ros::master::getNodes(node_list); - - for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); - node_list_idx++) { - if (node_list[node_list_idx] == manager_name) - return true; - } - - ROS_ERROR("Can't find humanoid_robot_intelligence_control_system_manager"); - return false; -} - -void dxlTorqueChecker() { - std_msgs::String check_msg; - check_msg.data = "check"; - - dxl_torque_pub.publish(check_msg); -} - -void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) { - // In demo mode - if (current_status != Ready) { - if (msg->data == "ready") { - // go to mode selection status - desired_status = Ready; - apply_desired = true; - - playSound(default_mp3_path + "Demonstration ready mode.mp3"); - setLED(0x01 | 0x02 | 0x04); - } - } - // In ready mode - else { - if (msg->data == "soccer") { - desired_status = SoccerDemo; - apply_desired = true; - - // play sound - dxlTorqueChecker(); - playSound(default_mp3_path + "Start soccer demonstration.mp3"); - ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } else if (msg->data == "vision") { - desired_status = VisionDemo; - apply_desired = true; - - // play sound - dxlTorqueChecker(); - playSound(default_mp3_path + "Start vision processing demonstration.mp3"); - ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } else if (msg->data == "action") { - desired_status = ActionDemo; - apply_desired = true; - - // play sound - dxlTorqueChecker(); - playSound(default_mp3_path + "Start motion demonstration.mp3"); - ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } - } -} diff --git a/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_follower.cpp b/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_follower.cpp deleted file mode 100644 index 98916b3..0000000 --- a/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_follower.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include "humanoid_robot_intelligence_control_system_demo/ball_follower.h" - -namespace humanoid_robot_intelligence_control_system_op { - -BallFollower::BallFollower() - : nh_(ros::this_node::getName()), FOV_WIDTH(35.2 * M_PI / 180), - FOV_HEIGHT(21.6 * M_PI / 180), count_not_found_(0), count_to_kick_(0), - on_tracking_(false), approach_ball_position_(NotFound), - kick_motion_index_(83), CAMERA_HEIGHT(0.46), NOT_FOUND_THRESHOLD(50), - MAX_FB_STEP(40.0 * 0.001), MAX_RL_TURN(15.0 * M_PI / 180), - IN_PLACE_FB_STEP(-3.0 * 0.001), MIN_FB_STEP(5.0 * 0.001), - MIN_RL_TURN(5.0 * M_PI / 180), UNIT_FB_STEP(1.0 * 0.001), - UNIT_RL_TURN(0.5 * M_PI / 180), SPOT_FB_OFFSET(0.0 * 0.001), - SPOT_RL_OFFSET(0.0 * 0.001), SPOT_ANGLE_OFFSET(0.0), - hip_pitch_offset_(7.0), current_pan_(-10), current_tilt_(-10), - current_x_move_(0.005), current_r_angle_(0), curr_period_time_(0.6), - accum_period_time_(0.0), DEBUG_PRINT(false) { - current_joint_states_sub_ = - nh_.subscribe("/humanoid_robot_intelligence_control_system/goal_joint_states", 10, - &BallFollower::currentJointStatesCallback, this); - - set_walking_command_pub_ = - nh_.advertise("/humanoid_robot_intelligence_control_system/walking/command", 0); - set_walking_param_pub_ = - nh_.advertise( - "/humanoid_robot_intelligence_control_system/walking/set_params", 0); - get_walking_param_client_ = - nh_.serviceClient( - "/humanoid_robot_intelligence_control_system/walking/get_params"); - - prev_time_ = ros::Time::now(); -} - -BallFollower::~BallFollower() {} - -void BallFollower::startFollowing() { - on_tracking_ = true; - ROS_INFO("Start Ball following"); - - setWalkingCommand("start"); - - bool result = getWalkingParam(); - if (result == true) { - hip_pitch_offset_ = current_walking_param_.hip_pitch_offset; - curr_period_time_ = current_walking_param_.period_time; - } else { - hip_pitch_offset_ = 7.0 * M_PI / 180; - curr_period_time_ = 0.6; - } -} - -void BallFollower::stopFollowing() { - on_tracking_ = false; - // approach_ball_position_ = NotFound; - count_to_kick_ = 0; - // accum_ball_position_ = 0; - ROS_INFO("Stop Ball following"); - - setWalkingCommand("stop"); -} - -void BallFollower::currentJointStatesCallback( - const sensor_msgs::JointState::ConstPtr &msg) { - double pan, tilt; - int get_count = 0; - - for (int ix = 0; ix < msg->name.size(); ix++) { - if (msg->name[ix] == "head_pan") { - pan = msg->position[ix]; - get_count += 1; - } else if (msg->name[ix] == "head_tilt") { - tilt = msg->position[ix]; - get_count += 1; - } - - if (get_count == 2) - break; - } - - // check variation - current_pan_ = pan; - current_tilt_ = tilt; -} - -void BallFollower::calcFootstep(double target_distance, double target_angle, - double delta_time, double &fb_move, - double &rl_angle) { - // clac fb - double next_movement = current_x_move_; - if (target_distance < 0) - target_distance = 0.0; - - double fb_goal = fmin(target_distance * 0.1, MAX_FB_STEP); - accum_period_time_ += delta_time; - if (accum_period_time_ > (curr_period_time_ / 4)) { - accum_period_time_ = 0.0; - if ((target_distance * 0.1 / 2) < current_x_move_) - next_movement -= UNIT_FB_STEP; - else - next_movement += UNIT_FB_STEP; - } - fb_goal = fmin(next_movement, fb_goal); - fb_move = fmax(fb_goal, MIN_FB_STEP); - ROS_INFO_COND(DEBUG_PRINT, - "distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", - target_distance, fb_move, delta_time); - ROS_INFO_COND(DEBUG_PRINT, "=============================================="); - - // calc rl angle - double rl_goal = 0.0; - if (fabs(target_angle) * 180 / M_PI > 5.0) { - double rl_offset = fabs(target_angle) * 0.2; - rl_goal = fmin(rl_offset, MAX_RL_TURN); - rl_goal = fmax(rl_goal, MIN_RL_TURN); - rl_angle = fmin(fabs(current_r_angle_) + UNIT_RL_TURN, rl_goal); - - if (target_angle < 0) - rl_angle *= (-1); - } -} - -// x_angle : ball position (pan), y_angle : ball position (tilt), ball_size : -// angle of ball radius -bool BallFollower::processFollowing(double x_angle, double y_angle, - double ball_size) { - ros::Time curr_time = ros::Time::now(); - ros::Duration dur = curr_time - prev_time_; - double delta_time = dur.nsec * 0.000000001 + dur.sec; - prev_time_ = curr_time; - - count_not_found_ = 0; - // int ball_position_sum = 0; - - // check of getting head joints angle - if (current_tilt_ == -10 && current_pan_ == -10) { - ROS_ERROR("Failed to get current angle of head joints."); - setWalkingCommand("stop"); - - on_tracking_ = false; - approach_ball_position_ = NotFound; - return false; - } - - ROS_INFO_COND(DEBUG_PRINT, " ============== Head | Ball ============== "); - ROS_INFO_STREAM_COND(DEBUG_PRINT, - "== Head Pan : " << (current_pan_ * 180 / M_PI) - << " | Ball X : " - << (x_angle * 180 / M_PI)); - ROS_INFO_STREAM_COND(DEBUG_PRINT, - "== Head Tilt : " << (current_tilt_ * 180 / M_PI) - << " | Ball Y : " - << (y_angle * 180 / M_PI)); - - approach_ball_position_ = OutOfRange; - - double distance_to_ball = CAMERA_HEIGHT * tan(M_PI * 0.5 + current_tilt_ - - hip_pitch_offset_ - ball_size); - - double ball_y_angle = (current_tilt_ + y_angle) * 180 / M_PI; - double ball_x_angle = (current_pan_ + x_angle) * 180 / M_PI; - - if (distance_to_ball < 0) - distance_to_ball *= (-1); - - // double distance_to_kick = 0.25; - double distance_to_kick = 0.22; - - // check whether ball is correct position. - if ((distance_to_ball < distance_to_kick) && (fabs(ball_x_angle) < 25.0)) { - count_to_kick_ += 1; - - ROS_INFO_STREAM_COND(DEBUG_PRINT, - "head pan : " << (current_pan_ * 180 / M_PI) - << " | ball pan : " - << (x_angle * 180 / M_PI)); - ROS_INFO_STREAM_COND(DEBUG_PRINT, - "head tilt : " << (current_tilt_ * 180 / M_PI) - << " | ball tilt : " - << (y_angle * 180 / M_PI)); - ROS_INFO_STREAM_COND(DEBUG_PRINT, "foot to kick : " << ball_x_angle); - - ROS_INFO("In range [%d | %f]", count_to_kick_, ball_x_angle); - - // ball queue - // if(ball_position_queue_.size() >= 5) - // ball_position_queue_.erase(ball_position_queue_.begin()); - - // ball_position_queue_.push_back((ball_x_angle > 0) ? 1 : -1); - - if (count_to_kick_ > 20) { - setWalkingCommand("stop"); - on_tracking_ = false; - - // check direction of the ball - // accum_ball_position_ = - // std::accumulate(ball_position_queue_.begin(), - // ball_position_queue_.end(), 0); - - // if (accum_ball_position_ > 0) - if (ball_x_angle > 0) { - ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : left"); // left - approach_ball_position_ = OnLeft; - } else { - ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : right"); // right - approach_ball_position_ = OnRight; - } - - return true; - } else if (count_to_kick_ > 15) { - // if (ball_x_angle > 0) - // accum_ball_position_ += 1; - // else - // accum_ball_position_ -= 1; - - // send message - setWalkingParam(IN_PLACE_FB_STEP, 0, 0); - - return false; - } - } else { - count_to_kick_ = 0; - // accum_ball_position_ = NotFound; - } - - double fb_move = 0.0, rl_angle = 0.0; - double distance_to_walk = distance_to_ball - distance_to_kick; - - calcFootstep(distance_to_walk, current_pan_, delta_time, fb_move, rl_angle); - - // send message - setWalkingParam(fb_move, 0, rl_angle); - - // for debug - // ROS_INFO("distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", - // distance_to_ball, fb_move, delta_time); - - return false; -} - -void BallFollower::decideBallPositin(double x_angle, double y_angle) { - // check of getting head joints angle - if (current_tilt_ == -10 && current_pan_ == -10) { - approach_ball_position_ = NotFound; - return; - } - - double ball_x_angle = current_pan_ + x_angle; - - if (ball_x_angle > 0) - approach_ball_position_ = OnLeft; - else - approach_ball_position_ = OnRight; -} - -void BallFollower::waitFollowing() { - count_not_found_++; - - if (count_not_found_ > NOT_FOUND_THRESHOLD * 0.5) - setWalkingParam(0.0, 0.0, 0.0); -} - -void BallFollower::setWalkingCommand(const std::string &command) { - // get param - if (command == "start") { - getWalkingParam(); - setWalkingParam(IN_PLACE_FB_STEP, 0, 0, true); - } - - std_msgs::String _command_msg; - _command_msg.data = command; - set_walking_command_pub_.publish(_command_msg); - - ROS_INFO_STREAM_COND(DEBUG_PRINT, "Send Walking command : " << command); -} - -void BallFollower::setWalkingParam(double x_move, double y_move, - double rotation_angle, bool balance) { - current_walking_param_.balance_enable = balance; - current_walking_param_.x_move_amplitude = x_move + SPOT_FB_OFFSET; - current_walking_param_.y_move_amplitude = y_move + SPOT_RL_OFFSET; - current_walking_param_.angle_move_amplitude = - rotation_angle + SPOT_ANGLE_OFFSET; - - set_walking_param_pub_.publish(current_walking_param_); - - current_x_move_ = x_move; - current_r_angle_ = rotation_angle; -} - -bool BallFollower::getWalkingParam() { - humanoid_robot_intelligence_control_system_walking_module_msgs::GetWalkingParam walking_param_msg; - - if (get_walking_param_client_.call(walking_param_msg)) { - current_walking_param_ = walking_param_msg.response.parameters; - - // update ui - ROS_INFO_COND(DEBUG_PRINT, "Get walking parameters"); - - return true; - } else { - ROS_ERROR("Fail to get walking parameters."); - - return false; - } -} - -} // namespace humanoid_robot_intelligence_control_system_op diff --git a/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_tracker.cpp b/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_tracker.cpp deleted file mode 100644 index 9cbeba4..0000000 --- a/humanoid_robot_intelligence_control_system_demo/src/soccer/ball_tracker.cpp +++ /dev/null @@ -1,267 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include "humanoid_robot_intelligence_control_system_demo/ball_tracker.h" - -namespace humanoid_robot_intelligence_control_system_op { - -BallTracker::BallTracker() - : nh_(ros::this_node::getName()), FOV_WIDTH(35.2 * M_PI / 180), - FOV_HEIGHT(21.6 * M_PI / 180), NOT_FOUND_THRESHOLD(50), - WAITING_THRESHOLD(5), use_head_scan_(true), count_not_found_(0), - on_tracking_(false), current_ball_pan_(0), current_ball_tilt_(0), - x_error_sum_(0), y_error_sum_(0), current_ball_bottom_(0), - tracking_status_(NotFound), DEBUG_PRINT(false) { - ros::NodeHandle param_nh("~"); - p_gain_ = param_nh.param("p_gain", 0.4); - i_gain_ = param_nh.param("i_gain", 0.0); - d_gain_ = param_nh.param("d_gain", 0.0); - - ROS_INFO_STREAM("Ball tracking Gain : " << p_gain_ << ", " << i_gain_ << ", " - << d_gain_); - - head_joint_offset_pub_ = nh_.advertise( - "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 0); - head_joint_pub_ = nh_.advertise( - "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 0); - head_scan_pub_ = nh_.advertise( - "/humanoid_robot_intelligence_control_system/head_control/scan_command", 0); - // error_pub_ = - // nh_.advertise("/ball_tracker/errors", 0); - - ball_position_sub_ = nh_.subscribe("/ball_detector_node/circle_set", 1, - &BallTracker::ballPositionCallback, this); - ball_tracking_command_sub_ = - nh_.subscribe("/ball_tracker/command", 1, - &BallTracker::ballTrackerCommandCallback, this); -} - -BallTracker::~BallTracker() {} - -void BallTracker::ballPositionCallback( - const humanoid_robot_intelligence_control_system_ball_detector::CircleSetStamped::ConstPtr &msg) { - for (int idx = 0; idx < msg->circles.size(); idx++) { - if (ball_position_.z >= msg->circles[idx].z) - continue; - - ball_position_ = msg->circles[idx]; - } -} - -void BallTracker::ballTrackerCommandCallback( - const std_msgs::String::ConstPtr &msg) { - if (msg->data == "start") { - startTracking(); - } else if (msg->data == "stop") { - stopTracking(); - } else if (msg->data == "toggle_start") { - if (on_tracking_ == false) - startTracking(); - else - stopTracking(); - } -} - -void BallTracker::startTracking() { - on_tracking_ = true; - ROS_INFO_COND(DEBUG_PRINT, "Start Ball tracking"); -} - -void BallTracker::stopTracking() { - goInit(); - - on_tracking_ = false; - ROS_INFO_COND(DEBUG_PRINT, "Stop Ball tracking"); - - current_ball_pan_ = 0; - current_ball_tilt_ = 0; - x_error_sum_ = 0; - y_error_sum_ = 0; -} - -void BallTracker::setUsingHeadScan(bool use_scan) { use_head_scan_ = use_scan; } - -int BallTracker::processTracking() { - int tracking_status = Found; - - if (on_tracking_ == false) { - ball_position_.z = 0; - count_not_found_ = 0; - return NotFound; - } - - // check ball position - if (ball_position_.z <= 0) { - count_not_found_++; - - if (count_not_found_ < WAITING_THRESHOLD) { - if (tracking_status_ == Found || tracking_status_ == Waiting) - tracking_status = Waiting; - else - tracking_status = NotFound; - } else if (count_not_found_ > NOT_FOUND_THRESHOLD) { - scanBall(); - count_not_found_ = 0; - tracking_status = NotFound; - } else { - tracking_status = NotFound; - } - } else { - count_not_found_ = 0; - } - - // if ball is found - // convert ball position to desired angle(rad) of head - // ball_position : top-left is (-1, -1), bottom-right is (+1, +1) - // offset_rad : top-left(+, +), bottom-right(-, -) - double x_error = 0.0, y_error = 0.0, ball_size = 0.0; - - switch (tracking_status) { - case NotFound: - tracking_status_ = tracking_status; - current_ball_pan_ = 0; - current_ball_tilt_ = 0; - x_error_sum_ = 0; - y_error_sum_ = 0; - return tracking_status; - - case Waiting: - tracking_status_ = tracking_status; - return tracking_status; - - case Found: - x_error = -atan(ball_position_.x * tan(FOV_WIDTH)); - y_error = -atan(ball_position_.y * tan(FOV_HEIGHT)); - ball_size = ball_position_.z; - break; - - default: - break; - } - - ROS_INFO_STREAM_COND( - DEBUG_PRINT, - "--------------------------------------------------------------"); - ROS_INFO_STREAM_COND(DEBUG_PRINT, "Ball position : " << ball_position_.x - << " | " - << ball_position_.y); - ROS_INFO_STREAM_COND(DEBUG_PRINT, - "Target angle : " << (x_error * 180 / M_PI) << " | " - << (y_error * 180 / M_PI)); - - ros::Time curr_time = ros::Time::now(); - ros::Duration dur = curr_time - prev_time_; - double delta_time = dur.nsec * 0.000000001 + dur.sec; - prev_time_ = curr_time; - - double x_error_diff = (x_error - current_ball_pan_) / delta_time; - double y_error_diff = (y_error - current_ball_tilt_) / delta_time; - x_error_sum_ += x_error; - y_error_sum_ += y_error; - double x_error_target = - x_error * p_gain_ + x_error_diff * d_gain_ + x_error_sum_ * i_gain_; - double y_error_target = - y_error * p_gain_ + y_error_diff * d_gain_ + y_error_sum_ * i_gain_; - - // std_msgs::Float64MultiArray x_error_msg; - // x_error_msg.data.push_back(x_error); - // x_error_msg.data.push_back(x_error_diff); - // x_error_msg.data.push_back(x_error_sum_); - // x_error_msg.data.push_back(x_error * p_gain_); - // x_error_msg.data.push_back(x_error_diff * d_gain_); - // x_error_msg.data.push_back(x_error_sum_ * i_gain_); - // x_error_msg.data.push_back(x_error_target); - // error_pub_.publish(x_error_msg); - - ROS_INFO_STREAM_COND(DEBUG_PRINT, - "------------------------ " - << tracking_status - << " --------------------------------------"); - ROS_INFO_STREAM_COND(DEBUG_PRINT, - "error : " << (x_error * 180 / M_PI) << " | " - << (y_error * 180 / M_PI)); - ROS_INFO_STREAM_COND(DEBUG_PRINT, "error_diff : " - << (x_error_diff * 180 / M_PI) << " | " - << (y_error_diff * 180 / M_PI) << " | " - << delta_time); - ROS_INFO_STREAM_COND(DEBUG_PRINT, - "error_sum : " << (x_error_sum_ * 180 / M_PI) << " | " - << (y_error_sum_ * 180 / M_PI)); - ROS_INFO_STREAM_COND( - DEBUG_PRINT, "error_target : " << (x_error_target * 180 / M_PI) << " | " - << (y_error_target * 180 / M_PI) - << " | P : " << p_gain_ << " | D : " - << d_gain_ << " | time : " << delta_time); - - // move head joint - publishHeadJoint(x_error_target, y_error_target); - - // args for following ball - current_ball_pan_ = x_error; - current_ball_tilt_ = y_error; - current_ball_bottom_ = ball_size; - - ball_position_.z = 0; - - tracking_status_ = tracking_status; - return tracking_status; -} - -void BallTracker::publishHeadJoint(double pan, double tilt) { - double min_angle = 1 * M_PI / 180; - if (fabs(pan) < min_angle && fabs(tilt) < min_angle) - return; - - sensor_msgs::JointState head_angle_msg; - - head_angle_msg.name.push_back("head_pan"); - head_angle_msg.name.push_back("head_tilt"); - - head_angle_msg.position.push_back(pan); - head_angle_msg.position.push_back(tilt); - - head_joint_offset_pub_.publish(head_angle_msg); -} - -void BallTracker::goInit() { - sensor_msgs::JointState head_angle_msg; - - head_angle_msg.name.push_back("head_pan"); - head_angle_msg.name.push_back("head_tilt"); - - head_angle_msg.position.push_back(0.0); - head_angle_msg.position.push_back(0.0); - - head_joint_pub_.publish(head_angle_msg); -} - -void BallTracker::scanBall() { - if (use_head_scan_ == false) - return; - - // check head control module enabled - // ... - - // send message to head control module - std_msgs::String scan_msg; - scan_msg.data = "scan"; - - head_scan_pub_.publish(scan_msg); -} - -} // namespace humanoid_robot_intelligence_control_system_op diff --git a/humanoid_robot_intelligence_control_system_demo/src/soccer/soccer_demo.cpp b/humanoid_robot_intelligence_control_system_demo/src/soccer/soccer_demo.cpp deleted file mode 100644 index 3a0bba6..0000000 --- a/humanoid_robot_intelligence_control_system_demo/src/soccer/soccer_demo.cpp +++ /dev/null @@ -1,619 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include "humanoid_robot_intelligence_control_system_demo/soccer_demo.h" - -namespace humanoid_robot_intelligence_control_system_op { - -SoccerDemo::SoccerDemo() - : FALL_FORWARD_LIMIT(60), FALL_BACK_LIMIT(-60), SPIN_RATE(30), - DEBUG_PRINT(false), wait_count_(0), on_following_ball_(false), - on_tracking_ball_(false), restart_soccer_(false), start_following_(false), - stop_following_(false), stop_fallen_check_(false), robot_status_(Waited), - stand_state_(Stand), tracking_status_(BallTracker::Waiting), - present_pitch_(0) { - // init ros - enable_ = false; - - ros::NodeHandle nh(ros::this_node::getName()); - - std::string default_path = ros::package::getPath("humanoid_robot_intelligence_control_system_gui_demo") + - "/config/gui_config.yaml"; - std::string path = nh.param("demo_config", default_path); - parseJointNameFromYaml(path); - - boost::thread queue_thread = - boost::thread(boost::bind(&SoccerDemo::callbackThread, this)); - boost::thread process_thread = - boost::thread(boost::bind(&SoccerDemo::processThread, this)); - boost::thread tracking_thread = - boost::thread(boost::bind(&SoccerDemo::trackingThread, this)); - - is_grass_ = nh.param("grass_demo", false); -} - -SoccerDemo::~SoccerDemo() {} - -void SoccerDemo::setDemoEnable() { - enable_ = true; - - startSoccerMode(); -} - -void SoccerDemo::setDemoDisable() { - // handle disable procedure - ball_tracker_.stopTracking(); - ball_follower_.stopFollowing(); - - enable_ = false; - wait_count_ = 0; - on_following_ball_ = false; - on_tracking_ball_ = false; - restart_soccer_ = false; - start_following_ = false; - stop_following_ = false; - stop_fallen_check_ = false; - - tracking_status_ = BallTracker::Waiting; -} - -void SoccerDemo::process() { - if (enable_ == false) - return; - - // check to start - if (start_following_ == true) { - ball_tracker_.startTracking(); - ball_follower_.startFollowing(); - start_following_ = false; - - wait_count_ = 1 * SPIN_RATE; - } - - // check to stop - if (stop_following_ == true) { - ball_tracker_.stopTracking(); - ball_follower_.stopFollowing(); - stop_following_ = false; - - wait_count_ = 0; - } - - if (wait_count_ <= 0) { - // ball following - if (on_following_ball_ == true) { - switch (tracking_status_) { - case BallTracker::Found: - ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), - ball_tracker_.getTiltOfBall(), 0.0); - break; - - case BallTracker::NotFound: - ball_follower_.waitFollowing(); - break; - - default: - break; - } - } - - // check fallen states - switch (stand_state_) { - case Stand: { - // check restart soccer - if (restart_soccer_ == true) { - restart_soccer_ = false; - startSoccerMode(); - break; - } - - // check states for kick - // int ball_position = ball_follower_.getBallPosition(); - bool in_range = ball_follower_.isBallInRange(); - - if (in_range == true) { - ball_follower_.stopFollowing(); - handleKick(); - } - break; - } - // fallen state : Fallen_Forward, Fallen_Behind - default: { - ball_follower_.stopFollowing(); - handleFallen(stand_state_); - break; - } - } - } else { - wait_count_ -= 1; - } -} - -void SoccerDemo::processThread() { - bool result = false; - - // set node loop rate - ros::Rate loop_rate(SPIN_RATE); - - ball_tracker_.startTracking(); - - // node loop - while (ros::ok()) { - if (enable_ == true) - process(); - - // relax to fit output rate - loop_rate.sleep(); - } -} - -void SoccerDemo::callbackThread() { - ros::NodeHandle nh(ros::this_node::getName()); - - // subscriber & publisher - module_control_pub_ = - nh.advertise( - "/humanoid_robot_intelligence_control_system/set_joint_ctrl_modules", 0); - motion_index_pub_ = - nh.advertise("/humanoid_robot_intelligence_control_system/action/page_num", 0); - rgb_led_pub_ = nh.advertise( - "/humanoid_robot_intelligence_control_system/sync_write_item", 0); - - buttuon_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1, - &SoccerDemo::buttonHandlerCallback, this); - demo_command_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/demo_command", 1, - &SoccerDemo::demoCommandCallback, this); - imu_data_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/imu", 1, - &SoccerDemo::imuDataCallback, this); - - is_running_client_ = - nh.serviceClient( - "/humanoid_robot_intelligence_control_system/action/is_running"); - set_joint_module_client_ = - nh.serviceClient( - "/humanoid_robot_intelligence_control_system/set_present_joint_ctrl_modules"); - - test_pub_ = nh.advertise("/debug_text", 0); - - while (nh.ok()) { - ros::spinOnce(); - - usleep(1000); - } -} - -void SoccerDemo::trackingThread() { - - // set node loop rate - ros::Rate loop_rate(SPIN_RATE); - - ball_tracker_.startTracking(); - - // node loop - while (ros::ok()) { - - if (enable_ == true && on_tracking_ball_ == true) { - // ball tracking - int tracking_status; - - tracking_status = ball_tracker_.processTracking(); - - // set led - switch (tracking_status) { - case BallTracker::Found: - if (tracking_status_ != tracking_status) - setRGBLED(0x1F, 0x1F, 0x1F); - break; - - case BallTracker::NotFound: - if (tracking_status_ != tracking_status) - setRGBLED(0, 0, 0); - break; - - default: - break; - } - - if (tracking_status != tracking_status_) - tracking_status_ = tracking_status; - } - // relax to fit output rate - loop_rate.sleep(); - } -} - -void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, - bool with_head_control) { - humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule control_msg; - - std::string head_module = "head_control_module"; - std::map::iterator joint_iter; - - for (joint_iter = id_joint_table_.begin(); - joint_iter != id_joint_table_.end(); ++joint_iter) { - // check whether joint name contains "head" - if (joint_iter->second.find("head") != std::string::npos) { - if (with_head_control == true) { - control_msg.joint_name.push_back(joint_iter->second); - control_msg.module_name.push_back(head_module); - } else - continue; - } else { - control_msg.joint_name.push_back(joint_iter->second); - control_msg.module_name.push_back(body_module); - } - } - - // no control - if (control_msg.joint_name.size() == 0) - return; - - callServiceSettingModule(control_msg); - std::cout << "enable module of body : " << body_module << std::endl; -} - -void SoccerDemo::setModuleToDemo(const std::string &module_name) { - if (enable_ == false) - return; - - humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule control_msg; - std::map::iterator joint_iter; - - for (joint_iter = id_joint_table_.begin(); - joint_iter != id_joint_table_.end(); ++joint_iter) { - control_msg.joint_name.push_back(joint_iter->second); - control_msg.module_name.push_back(module_name); - } - - // no control - if (control_msg.joint_name.size() == 0) - return; - - callServiceSettingModule(control_msg); - std::cout << "enable module : " << module_name << std::endl; -} - -void SoccerDemo::callServiceSettingModule( - const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule &modules) { - humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule set_joint_srv; - set_joint_srv.request.joint_name = modules.joint_name; - set_joint_srv.request.module_name = modules.module_name; - - if (set_joint_module_client_.call(set_joint_srv) == false) { - ROS_ERROR("Failed to set module"); - return; - } - - return; -} - -void SoccerDemo::parseJointNameFromYaml(const std::string &path) { - YAML::Node doc; - try { - // load yaml - doc = YAML::LoadFile(path.c_str()); - } catch (const std::exception &e) { - ROS_ERROR("Fail to load id_joint table yaml."); - return; - } - - // parse id_joint table - YAML::Node _id_sub_node = doc["id_joint"]; - for (YAML::iterator _it = _id_sub_node.begin(); _it != _id_sub_node.end(); - ++_it) { - int _id; - std::string _joint_name; - - _id = _it->first.as(); - _joint_name = _it->second.as(); - - id_joint_table_[_id] = _joint_name; - joint_id_table_[_joint_name] = _id; - } -} - -// joint id -> joint name -bool SoccerDemo::getJointNameFromID(const int &id, std::string &joint_name) { - std::map::iterator _iter; - - _iter = id_joint_table_.find(id); - if (_iter == id_joint_table_.end()) - return false; - - joint_name = _iter->second; - return true; -} - -// joint name -> joint id -bool SoccerDemo::getIDFromJointName(const std::string &joint_name, int &id) { - std::map::iterator _iter; - - _iter = joint_id_table_.find(joint_name); - if (_iter == joint_id_table_.end()) - return false; - - id = _iter->second; - return true; -} - -int SoccerDemo::getJointCount() { return joint_id_table_.size(); } - -bool SoccerDemo::isHeadJoint(const int &id) { - std::map::iterator _iter; - - for (_iter = joint_id_table_.begin(); _iter != joint_id_table_.end(); - ++_iter) { - if (_iter->first.find("head") != std::string::npos) - return true; - } - - return false; -} - -void SoccerDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { - if (enable_ == false) - return; - - if (msg->data == "start") { - if (on_following_ball_ == true) - stopSoccerMode(); - else - startSoccerMode(); - } -} - -void SoccerDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) { - if (enable_ == false) - return; - - if (msg->data == "start") { - if (on_following_ball_ == true) - stopSoccerMode(); - else - startSoccerMode(); - } else if (msg->data == "stop") { - stopSoccerMode(); - } -} - -// check fallen states -void SoccerDemo::imuDataCallback(const sensor_msgs::Imu::ConstPtr &msg) { - if (enable_ == false) - return; - - if (stop_fallen_check_ == true) - return; - - Eigen::Quaterniond orientation(msg->orientation.w, msg->orientation.x, - msg->orientation.y, msg->orientation.z); - Eigen::MatrixXd rpy_orientation = - humanoid_robot_intelligence_control_system_framework::convertQuaternionToRPY(orientation); - rpy_orientation *= (180 / M_PI); - - ROS_INFO_COND(DEBUG_PRINT, "Roll : %3.2f, Pitch : %2.2f", - rpy_orientation.coeff(0, 0), rpy_orientation.coeff(1, 0)); - - double pitch = rpy_orientation.coeff(1, 0); - - double alpha = 0.4; - if (present_pitch_ == 0) - present_pitch_ = pitch; - else - present_pitch_ = present_pitch_ * (1 - alpha) + pitch * alpha; - - if (present_pitch_ > FALL_FORWARD_LIMIT) - stand_state_ = Fallen_Forward; - else if (present_pitch_ < FALL_BACK_LIMIT) - stand_state_ = Fallen_Behind; - else - stand_state_ = Stand; -} - -void SoccerDemo::startSoccerMode() { - setModuleToDemo("action_module"); - - playMotion(WalkingReady); - - setBodyModuleToDemo("walking_module"); - - ROS_INFO("Start Soccer Demo"); - on_following_ball_ = true; - on_tracking_ball_ = true; - start_following_ = true; -} - -void SoccerDemo::stopSoccerMode() { - ROS_INFO("Stop Soccer Demo"); - on_following_ball_ = false; - on_tracking_ball_ = false; - stop_following_ = true; -} - -void SoccerDemo::handleKick(int ball_position) { - usleep(1500 * 1000); - - // change to motion module - setModuleToDemo("action_module"); - - if (handleFallen(stand_state_) == true || enable_ == false) - return; - - // kick motion - switch (ball_position) { - case humanoid_robot_intelligence_control_system_op::BallFollower::OnRight: - std::cout << "Kick Motion [R]: " << ball_position << std::endl; - playMotion(is_grass_ ? RightKick + ForGrass : RightKick); - break; - - case humanoid_robot_intelligence_control_system_op::BallFollower::OnLeft: - std::cout << "Kick Motion [L]: " << ball_position << std::endl; - playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick); - break; - - default: - break; - } - - on_following_ball_ = false; - restart_soccer_ = true; - tracking_status_ = BallTracker::NotFound; - ball_follower_.clearBallPosition(); - - usleep(2000 * 1000); - - if (handleFallen(stand_state_) == true) - return; - - // ceremony - // playMotion(Ceremony); -} - -void SoccerDemo::handleKick() { - usleep(1500 * 1000); - - // change to motion module - setModuleToDemo("action_module"); - - if (handleFallen(stand_state_) == true || enable_ == false) - return; - - // kick motion - ball_follower_.decideBallPositin(ball_tracker_.getPanOfBall(), - ball_tracker_.getTiltOfBall()); - int ball_position = ball_follower_.getBallPosition(); - if (ball_position == BallFollower::NotFound || - ball_position == BallFollower::OutOfRange) { - on_following_ball_ = false; - restart_soccer_ = true; - tracking_status_ = BallTracker::NotFound; - ball_follower_.clearBallPosition(); - return; - } - - switch (ball_position) { - case humanoid_robot_intelligence_control_system_op::BallFollower::OnRight: - std::cout << "Kick Motion [R]: " << ball_position << std::endl; - sendDebugTopic("Kick the ball using Right foot"); - playMotion(is_grass_ ? RightKick + ForGrass : RightKick); - break; - - case humanoid_robot_intelligence_control_system_op::BallFollower::OnLeft: - std::cout << "Kick Motion [L]: " << ball_position << std::endl; - sendDebugTopic("Kick the ball using Left foot"); - playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick); - break; - - default: - break; - } - - on_following_ball_ = false; - restart_soccer_ = true; - tracking_status_ = BallTracker::NotFound; - ball_follower_.clearBallPosition(); - - usleep(2000 * 1000); - - if (handleFallen(stand_state_) == true) - return; - - // ceremony - // playMotion(Ceremony); -} - -bool SoccerDemo::handleFallen(int fallen_status) { - if (fallen_status == Stand) { - return false; - } - - // change to motion module - setModuleToDemo("action_module"); - - // getup motion - switch (fallen_status) { - case Fallen_Forward: - std::cout << "Getup Motion [F]: " << std::endl; - playMotion(is_grass_ ? GetUpFront + ForGrass : GetUpFront); - break; - - case Fallen_Behind: - std::cout << "Getup Motion [B]: " << std::endl; - playMotion(is_grass_ ? GetUpBack + ForGrass : GetUpBack); - break; - - default: - break; - } - - while (isActionRunning() == true) - usleep(100 * 1000); - - usleep(650 * 1000); - - if (on_following_ball_ == true) - restart_soccer_ = true; - - // reset state - on_following_ball_ = false; - - return true; -} - -void SoccerDemo::playMotion(int motion_index) { - std_msgs::Int32 motion_msg; - motion_msg.data = motion_index; - - motion_index_pub_.publish(motion_msg); -} - -void SoccerDemo::setRGBLED(int blue, int green, int red) { - int led_full_unit = 0x1F; - int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | - (red & led_full_unit); - humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg; - syncwrite_msg.item_name = "LED_RGB"; - syncwrite_msg.joint_name.push_back("open-cr"); - syncwrite_msg.value.push_back(led_value); - - rgb_led_pub_.publish(syncwrite_msg); -} - -// check running of action -bool SoccerDemo::isActionRunning() { - humanoid_robot_intelligence_control_system_action_module_msgs::IsRunning is_running_srv; - - if (is_running_client_.call(is_running_srv) == false) { - ROS_ERROR("Failed to get action status"); - return true; - } else { - if (is_running_srv.response.is_running == true) { - return true; - } - } - - return false; -} - -void SoccerDemo::sendDebugTopic(const std::string &msgs) { - std_msgs::String debug_msg; - debug_msg.data = msgs; - - test_pub_.publish(debug_msg); -} - -} // namespace humanoid_robot_intelligence_control_system_op diff --git a/humanoid_robot_intelligence_control_system_demo/src/test/button_test.cpp b/humanoid_robot_intelligence_control_system_demo/src/test/button_test.cpp deleted file mode 100644 index d6ea5a0..0000000 --- a/humanoid_robot_intelligence_control_system_demo/src/test/button_test.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include "humanoid_robot_intelligence_control_system_demo/button_test.h" - -namespace humanoid_robot_intelligence_control_system_op { - -ButtonTest::ButtonTest() : SPIN_RATE(30), led_count_(0), rgb_led_count_(0) { - enable_ = false; - - ros::NodeHandle nh(ros::this_node::getName()); - - boost::thread queue_thread = - boost::thread(boost::bind(&ButtonTest::callbackThread, this)); - boost::thread process_thread = - boost::thread(boost::bind(&ButtonTest::processThread, this)); - - default_mp3_path_ = - ros::package::getPath("humanoid_robot_intelligence_control_system_demo") + "/data/mp3/test/"; -} - -ButtonTest::~ButtonTest() {} - -void ButtonTest::setDemoEnable() { - enable_ = true; - - ROS_INFO("Start Button Test"); -} - -void ButtonTest::setDemoDisable() { enable_ = false; } - -void ButtonTest::process() {} - -void ButtonTest::processThread() { - // set node loop rate - ros::Rate loop_rate(SPIN_RATE); - - // node loop - while (ros::ok()) { - if (enable_ == true) - process(); - - // relax to fit output rate - loop_rate.sleep(); - } -} - -void ButtonTest::callbackThread() { - ros::NodeHandle nh(ros::this_node::getName()); - - // subscriber & publisher - rgb_led_pub_ = nh.advertise( - "/humanoid_robot_intelligence_control_system/sync_write_item", 0); - play_sound_pub_ = nh.advertise("/play_sound_file", 0); - - buttuon_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1, - &ButtonTest::buttonHandlerCallback, this); - - while (nh.ok()) { - ros::spinOnce(); - - usleep(1 * 1000); - } -} - -// button test -void ButtonTest::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { - if (enable_ == false) - return; - - if (msg->data == "mode") { - playSound(default_mp3_path_ + "Mode button pressed.mp3"); - } else if (msg->data == "start") { - // RGB led color test - playSound(default_mp3_path_ + "Start button pressed.mp3"); - int rgb_selector[3] = {1, 0, 0}; - setRGBLED((0x1F * rgb_selector[rgb_led_count_ % 3]), - (0x1F * rgb_selector[(rgb_led_count_ + 1) % 3]), - (0x1F * rgb_selector[(rgb_led_count_ + 2) % 3])); - rgb_led_count_ += 1; - } else if (msg->data == "user") { - // Monochromatic led color test - playSound(default_mp3_path_ + "User button pressed.mp3"); - setLED(0x01 << (led_count_++ % 3)); - } else if (msg->data == "mode_long") { - playSound(default_mp3_path_ + "Mode button long pressed.mp3"); - } else if (msg->data == "start_long") { - playSound(default_mp3_path_ + "Start button long pressed.mp3"); - } else if (msg->data == "user_long") { - playSound(default_mp3_path_ + "User button long pressed.mp3"); - } -} - -void ButtonTest::setRGBLED(int blue, int green, int red) { - int led_full_unit = 0x1F; - int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | - (red & led_full_unit); - humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg; - syncwrite_msg.item_name = "LED_RGB"; - syncwrite_msg.joint_name.push_back("open-cr"); - syncwrite_msg.value.push_back(led_value); - - rgb_led_pub_.publish(syncwrite_msg); -} - -void ButtonTest::setLED(int led) { - humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg; - syncwrite_msg.item_name = "LED"; - syncwrite_msg.joint_name.push_back("open-cr"); - syncwrite_msg.value.push_back(led); - - rgb_led_pub_.publish(syncwrite_msg); -} - -void ButtonTest::playSound(const std::string &path) { - std_msgs::String sound_msg; - sound_msg.data = path; - - play_sound_pub_.publish(sound_msg); -} - -} /* namespace humanoid_robot_intelligence_control_system_op */ diff --git a/humanoid_robot_intelligence_control_system_demo/src/test/mic_test.cpp b/humanoid_robot_intelligence_control_system_demo/src/test/mic_test.cpp deleted file mode 100644 index 008755b..0000000 --- a/humanoid_robot_intelligence_control_system_demo/src/test/mic_test.cpp +++ /dev/null @@ -1,238 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include "humanoid_robot_intelligence_control_system_demo/mic_test.h" - -namespace humanoid_robot_intelligence_control_system_op { - -MicTest::MicTest() - : SPIN_RATE(30), is_wait_(false), wait_time_(-1), test_status_(Ready), - record_pid_(-1), play_pid_(-1) { - enable_ = false; - - ros::NodeHandle nh(ros::this_node::getName()); - - boost::thread queue_thread = - boost::thread(boost::bind(&MicTest::callbackThread, this)); - boost::thread process_thread = - boost::thread(boost::bind(&MicTest::processThread, this)); - - recording_file_name_ = ros::package::getPath("humanoid_robot_intelligence_control_system_demo") + - "/data/mp3/test/mic-test.wav"; - default_mp3_path_ = - ros::package::getPath("humanoid_robot_intelligence_control_system_demo") + "/data/mp3/test/"; - - start_time_ = ros::Time::now(); -} - -MicTest::~MicTest() {} - -void MicTest::setDemoEnable() { - wait_time_ = -1; - test_status_ = AnnounceRecording; - enable_ = true; - - ROS_INFO("Start Mic test Demo"); -} - -void MicTest::setDemoDisable() { - finishTimer(); - - test_status_ = Ready; - enable_ = false; -} - -void MicTest::process() { - // check status - // timer - if (wait_time_ > 0) { - ros::Duration dur = ros::Time::now() - start_time_; - - // check timer - if (dur.sec >= wait_time_) { - finishTimer(); - } - } else if (wait_time_ == -1.0) { - // handle test process - switch (test_status_) { - case Ready: - // do nothing - break; - - case AnnounceRecording: - announceTest(); - test_status_ = MicRecording; - break; - - case MicRecording: - recordSound(); - test_status_ = PlayingSound; - break; - - case PlayingSound: - playTestSound(recording_file_name_); - test_status_ = DeleteTempFile; - break; - - case DeleteTempFile: - deleteSoundFile(recording_file_name_); - test_status_ = Ready; - break; - - default: - break; - } - } -} - -void MicTest::processThread() { - // set node loop rate - ros::Rate loop_rate(SPIN_RATE); - - // node loop - while (ros::ok()) { - if (enable_ == true) - process(); - - // relax to fit output rate - loop_rate.sleep(); - } -} - -void MicTest::callbackThread() { - ros::NodeHandle nh(ros::this_node::getName()); - - // subscriber & publisher - buttuon_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1, - &MicTest::buttonHandlerCallback, this); - play_sound_pub_ = nh.advertise("/play_sound_file", 0); - - while (nh.ok()) { - ros::spinOnce(); - - usleep(1 * 1000); - } -} - -void MicTest::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { - if (enable_ == false) - return; - - if (msg->data == "start") { - // restart mic test - if (test_status_ != Ready) - return; - - test_status_ = AnnounceRecording; - } else if (msg->data == "user") { - is_wait_ = true; - } -} - -void MicTest::announceTest() { - // play mic test sound - playSound(default_mp3_path_ + "Announce mic test.mp3"); - - usleep(3.4 * 1000 * 1000); -} - -void MicTest::recordSound(int recording_time) { - ROS_INFO("Start to record"); - - playSound(default_mp3_path_ + "Start recording.mp3"); - - usleep(1.5 * 1000 * 1000); - - if (record_pid_ != -1) - kill(record_pid_, SIGKILL); - - record_pid_ = fork(); - - switch (record_pid_) { - case -1: - fprintf(stderr, "Fork Failed!! \n"); - ROS_WARN("Fork Failed!! \n"); - break; - - case 0: { - std::stringstream ss; - ss << "-d " << recording_time; - execl("/usr/bin/arecord", "arecord", "-Dplughw:1,0", "-fS16_LE", "-c1", - "-r22050", "-twav", ss.str().c_str(), recording_file_name_.c_str(), - (char *)0); - break; - } - - default: - break; - } - - startTimer(recording_time); -} - -void MicTest::recordSound() { recordSound(5); } - -void MicTest::playTestSound(const std::string &path) { - ROS_INFO("Start to play recording sound"); - - playSound(default_mp3_path_ + "Start playing.mp3"); - - usleep(1.3 * 1000 * 1000); - - if (play_pid_ != -1) - kill(play_pid_, SIGKILL); - - play_pid_ = fork(); - - switch (play_pid_) { - case -1: - fprintf(stderr, "Fork Failed!! \n"); - ROS_WARN("Fork Failed!! \n"); - break; - - case 0: - execl("/usr/bin/aplay", "aplay", path.c_str(), (char *)0); - break; - - default: - break; - } - - startTimer(5); -} - -void MicTest::playSound(const std::string &path) { - std_msgs::String sound_msg; - sound_msg.data = path; - - play_sound_pub_.publish(sound_msg); -} - -void MicTest::deleteSoundFile(const std::string &file_path) { - remove(file_path.c_str()); - ROS_INFO("Delete temporary file"); -} - -void MicTest::startTimer(double wait_time) { - start_time_ = ros::Time::now(); - wait_time_ = wait_time; -} - -void MicTest::finishTimer() { wait_time_ = -1; } - -} /* namespace humanoid_robot_intelligence_control_system_op */ diff --git a/humanoid_robot_intelligence_control_system_demo/src/test_node.cpp b/humanoid_robot_intelligence_control_system_demo/src/test_node.cpp deleted file mode 100644 index 53e1209..0000000 --- a/humanoid_robot_intelligence_control_system_demo/src/test_node.cpp +++ /dev/null @@ -1,393 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include -#include -#include - -#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h" -#include "humanoid_robot_intelligence_control_system_demo/action_demo.h" -#include "humanoid_robot_intelligence_control_system_demo/button_test.h" -#include "humanoid_robot_intelligence_control_system_demo/mic_test.h" -#include "humanoid_robot_intelligence_control_system_demo/soccer_demo.h" -#include "humanoid_robot_intelligence_control_system_demo/vision_demo.h" -#include "humanoid_robot_intelligence_control_system_math/humanoid_robot_intelligence_control_system_linear_algebra.h" - -enum Demo_Status { - Ready = 0, - ButtonTest = 1, - MicTest = 2, - SoccerDemo = 3, - VisionDemo = 4, - ActionDemo = 5, - DemoCount = 6, -}; - -void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); -void goInitPose(); -void playSound(const std::string &path); -void setLED(int led); -bool checkManagerRunning(std::string &manager_name); -void dxlTorqueChecker(); - -void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg); - -const int SPIN_RATE = 30; -const bool DEBUG_PRINT = true; - -ros::Publisher init_pose_pub; -ros::Publisher play_sound_pub; -ros::Publisher led_pub; -ros::Publisher dxl_torque_pub; - -std::string default_mp3_path = ""; -int current_status = Ready; -int desired_status = Ready; -bool apply_desired = false; - -// node main -int main(int argc, char **argv) { - // init ros - ros::init(argc, argv, "self_test_node"); - - // create ros wrapper object - humanoid_robot_intelligence_control_system_op::OPDemo *current_demo = NULL; - humanoid_robot_intelligence_control_system_op::SoccerDemo *soccer_demo = - new humanoid_robot_intelligence_control_system_op::SoccerDemo(); - humanoid_robot_intelligence_control_system_op::ActionDemo *action_demo = - new humanoid_robot_intelligence_control_system_op::ActionDemo(); - humanoid_robot_intelligence_control_system_op::VisionDemo *vision_demo = - new humanoid_robot_intelligence_control_system_op::VisionDemo(); - humanoid_robot_intelligence_control_system_op::ButtonTest *button_test = - new humanoid_robot_intelligence_control_system_op::ButtonTest(); - humanoid_robot_intelligence_control_system_op::MicTest *mic_test = new humanoid_robot_intelligence_control_system_op::MicTest(); - - ros::NodeHandle nh(ros::this_node::getName()); - - init_pose_pub = - nh.advertise("/humanoid_robot_intelligence_control_system/base/ini_pose", 0); - play_sound_pub = nh.advertise("/play_sound_file", 0); - led_pub = nh.advertise( - "/humanoid_robot_intelligence_control_system/sync_write_item", 0); - dxl_torque_pub = - nh.advertise("/humanoid_robot_intelligence_control_system/dxl_torque", 0); - ros::Subscriber buttuon_sub = - nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1, buttonHandlerCallback); - ros::Subscriber mode_command_sub = - nh.subscribe("/humanoid_robot_intelligence_control_system/mode_command", 1, demoModeCommandCallback); - - default_mp3_path = - ros::package::getPath("humanoid_robot_intelligence_control_system_demo") + "/data/mp3/"; - - ros::start(); - - // set node loop rate - ros::Rate loop_rate(SPIN_RATE); - - // wait for starting of manager - std::string manager_name = "/humanoid_robot_intelligence_control_system_manager"; - while (ros::ok()) { - ros::Duration(1.0).sleep(); - - if (checkManagerRunning(manager_name) == true) { - break; - ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect"); - } - ROS_WARN("Waiting for humanoid_robot_intelligence_control_system manager"); - } - - // init procedure - playSound(default_mp3_path + "test/Self test ready mode.mp3"); - setLED(0x01 | 0x02 | 0x04); - - // node loop - while (ros::ok()) { - // process - if (apply_desired == true) { - switch (desired_status) { - case Ready: { - - if (current_demo != NULL) - current_demo->setDemoDisable(); - - current_demo = NULL; - - goInitPose(); - - ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]"); - break; - } - - case SoccerDemo: { - if (current_demo != NULL) - current_demo->setDemoDisable(); - - current_demo = soccer_demo; - current_demo->setDemoEnable(); - - ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo"); - break; - } - - case VisionDemo: { - if (current_demo != NULL) - current_demo->setDemoDisable(); - - current_demo = vision_demo; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo"); - break; - } - case ActionDemo: { - if (current_demo != NULL) - current_demo->setDemoDisable(); - - current_demo = action_demo; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo"); - break; - } - case ButtonTest: { - if (current_demo != NULL) - current_demo->setDemoDisable(); - - current_demo = button_test; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Button Test"); - break; - } - case MicTest: { - if (current_demo != NULL) - current_demo->setDemoDisable(); - - current_demo = mic_test; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Mic Test"); - break; - } - - default: { - break; - } - } - - apply_desired = false; - current_status = desired_status; - } - - // execute pending callbacks - ros::spinOnce(); - - // relax to fit output rate - loop_rate.sleep(); - - // for debug - if (checkManagerRunning(manager_name) == false) - return 0; - } - - // exit program - return 0; -} - -void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { - if (apply_desired == true) - return; - - // in the middle of playing demo - if (current_status != Ready) { - if (msg->data == "mode_long") { - // go to mode selection status - desired_status = Ready; - apply_desired = true; - - playSound(default_mp3_path + "test/Self test ready mode.mp3"); - setLED(0x01 | 0x02 | 0x04); - } else if (msg->data == "user_long") { - // it's using in humanoid_robot_intelligence_control_system_manager - // torque on and going to init pose - } - } - // ready to start demo - else { - if (msg->data == "start") { - // select current demo - desired_status = - (desired_status == Ready) ? desired_status + 1 : desired_status; - apply_desired = true; - - // sound out desired status - switch (desired_status) { - case SoccerDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + "Start soccer demonstration.mp3"); - break; - - case VisionDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + - "Start vision processing demonstration.mp3"); - break; - - case ActionDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + "Start motion demonstration.mp3"); - break; - - case ButtonTest: - dxlTorqueChecker(); - playSound(default_mp3_path + "test/Start button test mode.mp3"); - break; - - case MicTest: - dxlTorqueChecker(); - playSound(default_mp3_path + "test/Start mic test mode.mp3"); - break; - - default: - break; - } - - ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } else if (msg->data == "mode") { - // change to next demo - desired_status = (desired_status + 1) % DemoCount; - desired_status = - (desired_status == Ready) ? desired_status + 1 : desired_status; - - // sound out desired status and changign LED - switch (desired_status) { - case SoccerDemo: - playSound(default_mp3_path + "Autonomous soccer mode.mp3"); - setLED(0x01); - break; - - case VisionDemo: - playSound(default_mp3_path + "Vision processing mode.mp3"); - setLED(0x02); - break; - - case ActionDemo: - playSound(default_mp3_path + "Interactive motion mode.mp3"); - setLED(0x04); - break; - - case ButtonTest: - playSound(default_mp3_path + "test/Button test mode.mp3"); - setLED(0x01 | 0x02); - break; - - case MicTest: - playSound(default_mp3_path + "test/Mic test mode.mp3"); - setLED(0x01 | 0x04); - break; - - default: - break; - } - - ROS_INFO_COND(DEBUG_PRINT, "= Demo Mode : %d", desired_status); - } - } -} - -void goInitPose() { - std_msgs::String init_msg; - init_msg.data = "ini_pose"; - - init_pose_pub.publish(init_msg); -} - -void playSound(const std::string &path) { - std_msgs::String sound_msg; - sound_msg.data = path; - - play_sound_pub.publish(sound_msg); -} - -void setLED(int led) { - humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg; - syncwrite_msg.item_name = "LED"; - syncwrite_msg.joint_name.push_back("open-cr"); - syncwrite_msg.value.push_back(led); - - led_pub.publish(syncwrite_msg); -} - -bool checkManagerRunning(std::string &manager_name) { - std::vector node_list; - ros::master::getNodes(node_list); - - for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); - node_list_idx++) { - if (node_list[node_list_idx] == manager_name) - return true; - } - - ROS_ERROR("Can't find humanoid_robot_intelligence_control_system_manager"); - return false; -} - -void dxlTorqueChecker() { - std_msgs::String check_msg; - check_msg.data = "check"; - - dxl_torque_pub.publish(check_msg); -} - -void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) { - // In demo mode - if (current_status != Ready) { - if (msg->data == "ready") { - // go to mode selection status - desired_status = Ready; - apply_desired = true; - - playSound(default_mp3_path + "Demonstration ready mode.mp3"); - setLED(0x01 | 0x02 | 0x04); - } - } - // In ready mode - else { - if (msg->data == "soccer") { - desired_status = SoccerDemo; - apply_desired = true; - - // play sound - dxlTorqueChecker(); - playSound(default_mp3_path + "Start soccer demonstration.mp3"); - ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } else if (msg->data == "vision") { - desired_status = VisionDemo; - apply_desired = true; - - // play sound - dxlTorqueChecker(); - playSound(default_mp3_path + "Start vision processing demonstration.mp3"); - ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } else if (msg->data == "action") { - desired_status = ActionDemo; - apply_desired = true; - - // play sound - dxlTorqueChecker(); - playSound(default_mp3_path + "Start motion demonstration.mp3"); - ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } - } -} diff --git a/humanoid_robot_intelligence_control_system_demo/src/vision/face_tracker.cpp b/humanoid_robot_intelligence_control_system_demo/src/vision/face_tracker.cpp deleted file mode 100644 index 20de21e..0000000 --- a/humanoid_robot_intelligence_control_system_demo/src/vision/face_tracker.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include "humanoid_robot_intelligence_control_system_demo/face_tracker.h" - -namespace humanoid_robot_intelligence_control_system_op { - -FaceTracker::FaceTracker() - : nh_(ros::this_node::getName()), FOV_WIDTH(35.2 * M_PI / 180), - FOV_HEIGHT(21.6 * M_PI / 180), NOT_FOUND_THRESHOLD(50), - use_head_scan_(false), count_not_found_(0), on_tracking_(false) { - head_joint_offset_pub_ = nh_.advertise( - "/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 0); - head_joint_pub_ = nh_.advertise( - "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 0); - head_scan_pub_ = nh_.advertise( - "/humanoid_robot_intelligence_control_system/head_control/scan_command", 0); - - face_position_sub_ = nh_.subscribe("/face_position", 1, - &FaceTracker::facePositionCallback, this); - // face_tracking_command_sub_ = nh_.subscribe("/humanoid_robot_intelligence_control_system/demo_command", - // 1, &FaceTracker::faceTrackerCommandCallback, this); -} - -FaceTracker::~FaceTracker() {} - -void FaceTracker::facePositionCallback( - const geometry_msgs::Point::ConstPtr &msg) { - if (msg->z < 0) - return; - - face_position_ = *msg; -} - -void FaceTracker::faceTrackerCommandCallback( - const std_msgs::String::ConstPtr &msg) { - if (msg->data == "start") { - startTracking(); - } else if (msg->data == "stop") { - stopTracking(); - } else if (msg->data == "toggle_start") { - if (on_tracking_ == false) - startTracking(); - else - stopTracking(); - } -} - -void FaceTracker::startTracking() { - on_tracking_ = true; - - ROS_INFO("Start Face tracking"); -} - -void FaceTracker::stopTracking() { - on_tracking_ = false; - - ROS_INFO("Stop Face tracking"); -} - -void FaceTracker::setUsingHeadScan(bool use_scan) { use_head_scan_ = use_scan; } - -void FaceTracker::setFacePosition(geometry_msgs::Point &face_position) { - if (face_position.z > 0) { - face_position_ = face_position; - } -} - -void FaceTracker::goInit(double init_pan, double init_tile) { - sensor_msgs::JointState head_angle_msg; - - head_angle_msg.name.push_back("head_pan"); - head_angle_msg.name.push_back("head_tilt"); - - head_angle_msg.position.push_back(init_pan); - head_angle_msg.position.push_back(init_tile); - - head_joint_pub_.publish(head_angle_msg); -} - -int FaceTracker::processTracking() { - if (on_tracking_ == false) { - face_position_.z = 0; - count_not_found_ = 0; - // return false; - return Waiting; - } - - // check ball position - if (face_position_.z <= 0) { - count_not_found_++; - - if (count_not_found_ == NOT_FOUND_THRESHOLD) { - scanFace(); - // count_not_found_ = 0; - return NotFound; - } else if (count_not_found_ > NOT_FOUND_THRESHOLD) { - return NotFound; - } else { - return Waiting; - } - - // return false; - } - - // if face is detected - double x_error = -atan(face_position_.x * tan(FOV_WIDTH)); - double y_error = -atan(face_position_.y * tan(FOV_HEIGHT)); - - face_position_.z = 0; - count_not_found_ = 0; - - double p_gain = 0.6, d_gain = 0.25; - double x_error_diff = x_error - current_face_pan_; - double y_error_diff = y_error - current_face_tilt_; - double x_error_target = x_error * p_gain + x_error_diff * d_gain; - double y_error_target = y_error * p_gain + y_error_diff * d_gain; - - // move head joint - publishHeadJoint(x_error_target, y_error_target); - - current_face_pan_ = x_error; - current_face_tilt_ = y_error; - - // return true; - return Found; -} - -void FaceTracker::publishHeadJoint(double pan, double tilt) { - double min_angle = 1 * M_PI / 180; - if (fabs(pan) < min_angle && fabs(tilt) < min_angle) { - dismissed_count_ += 1; - return; - } - std::cout << "Target angle[" << dismissed_count_ << "] : " << pan << " | " - << tilt << std::endl; - - dismissed_count_ = 0; - - sensor_msgs::JointState head_angle_msg; - - head_angle_msg.name.push_back("head_pan"); - head_angle_msg.name.push_back("head_tilt"); - - head_angle_msg.position.push_back(pan); - head_angle_msg.position.push_back(tilt); - - head_joint_offset_pub_.publish(head_angle_msg); -} - -void FaceTracker::scanFace() { - if (use_head_scan_ == false) - return; - - // check head control module enabled - // ... - - // send message to head control module - std_msgs::String scan_msg; - scan_msg.data = "scan"; - - head_scan_pub_.publish(scan_msg); - // ROS_INFO("Scan the ball"); -} - -} // namespace humanoid_robot_intelligence_control_system_op diff --git a/humanoid_robot_intelligence_control_system_demo/src/vision/vision_demo.cpp b/humanoid_robot_intelligence_control_system_demo/src/vision/vision_demo.cpp deleted file mode 100644 index 01e0047..0000000 --- a/humanoid_robot_intelligence_control_system_demo/src/vision/vision_demo.cpp +++ /dev/null @@ -1,227 +0,0 @@ -/******************************************************************************* - * Copyright 2017 ROBOTIS CO., LTD. - * - * 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. - *******************************************************************************/ - -/* Author: Kayman Jung */ - -#include "humanoid_robot_intelligence_control_system_demo/vision_demo.h" - -namespace humanoid_robot_intelligence_control_system_op { - -VisionDemo::VisionDemo() - : SPIN_RATE(30), TIME_TO_INIT(10), tracking_status_(FaceTracker::Waiting) { - enable_ = false; - - ros::NodeHandle nh(ros::this_node::getName()); - - boost::thread queue_thread = - boost::thread(boost::bind(&VisionDemo::callbackThread, this)); - boost::thread process_thread = - boost::thread(boost::bind(&VisionDemo::processThread, this)); -} - -VisionDemo::~VisionDemo() { - // TODO Auto-generated destructor stub -} - -void VisionDemo::setDemoEnable() { - // set prev time for timer - prev_time_ = ros::Time::now(); - - // change to motion module - setModuleToDemo("action_module"); - - playMotion(InitPose); - - usleep(1500 * 1000); - - setModuleToDemo("head_control_module"); - - enable_ = true; - - // send command to start face_tracking - std_msgs::Bool command; - command.data = enable_; - face_tracking_command_pub_.publish(command); - - face_tracker_.startTracking(); - - ROS_INFO("Start Vision Demo"); -} - -void VisionDemo::setDemoDisable() { - - face_tracker_.stopTracking(); - tracking_status_ = FaceTracker::Waiting; - enable_ = false; - - std_msgs::Bool command; - command.data = enable_; - face_tracking_command_pub_.publish(command); -} - -void VisionDemo::process() { - int tracking_status = face_tracker_.processTracking(); - - switch (tracking_status) { - case FaceTracker::Found: - if (tracking_status_ != tracking_status) - setRGBLED(0x1F, 0x1F, 0x1F); - prev_time_ = ros::Time::now(); - break; - - case FaceTracker::NotFound: { - if (tracking_status_ != tracking_status) - setRGBLED(0, 0, 0); - - ros::Time curr_time = ros::Time::now(); - ros::Duration dur = curr_time - prev_time_; - if (dur.sec > TIME_TO_INIT) { - face_tracker_.goInit(0, 0); - prev_time_ = curr_time; - } - break; - } - default: - break; - } - - if (tracking_status != FaceTracker::Waiting) - tracking_status_ = tracking_status; -} - -void VisionDemo::processThread() { - // set node loop rate - ros::Rate loop_rate(SPIN_RATE); - - // node loop - while (ros::ok()) { - if (enable_ == true) - process(); - - // relax to fit output rate - loop_rate.sleep(); - } -} - -void VisionDemo::callbackThread() { - ros::NodeHandle nh(ros::this_node::getName()); - - // subscriber & publisher - module_control_pub_ = - nh.advertise("/humanoid_robot_intelligence_control_system/enable_ctrl_module", 0); - motion_index_pub_ = - nh.advertise("/humanoid_robot_intelligence_control_system/action/page_num", 0); - rgb_led_pub_ = nh.advertise( - "/humanoid_robot_intelligence_control_system/sync_write_item", 0); - face_tracking_command_pub_ = - nh.advertise("/face_tracking/command", 0); - - buttuon_sub_ = nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1, - &VisionDemo::buttonHandlerCallback, this); - faceCoord_sub_ = - nh.subscribe("/faceCoord", 1, &VisionDemo::facePositionCallback, this); - - set_joint_module_client_ = - nh.serviceClient( - "/humanoid_robot_intelligence_control_system/set_present_ctrl_modules"); - - while (nh.ok()) { - ros::spinOnce(); - - usleep(1 * 1000); - } -} - -void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { - if (enable_ == false) - return; - - if (msg->data == "start") { - - } else if (msg->data == "mode") { - } -} - -void VisionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) { - if (enable_ == false) - return; - - if (msg->data == "start") { - - } else if (msg->data == "stop") { - } -} - -void VisionDemo::setModuleToDemo(const std::string &module_name) { - callServiceSettingModule(module_name); - ROS_INFO_STREAM("enable module : " << module_name); -} - -void VisionDemo::callServiceSettingModule(const std::string &module_name) { - humanoid_robot_intelligence_control_system_controller_msgs::SetModule set_module_srv; - set_module_srv.request.module_name = module_name; - - if (set_joint_module_client_.call(set_module_srv) == false) { - ROS_ERROR("Failed to set module"); - return; - } - - return; -} - -void VisionDemo::facePositionCallback( - const std_msgs::Int32MultiArray::ConstPtr &msg) { - if (enable_ == false) - return; - - // face is detected - if (msg->data.size() >= 10) { - // center of face - face_position_.x = - (msg->data[6] + msg->data[8] * 0.5) / msg->data[2] * 2 - 1; - face_position_.y = - (msg->data[7] + msg->data[9] * 0.5) / msg->data[3] * 2 - 1; - face_position_.z = msg->data[8] * 0.5 + msg->data[9] * 0.5; - - face_tracker_.setFacePosition(face_position_); - } else { - face_position_.x = 0; - face_position_.y = 0; - face_position_.z = 0; - return; - } -} - -void VisionDemo::playMotion(int motion_index) { - std_msgs::Int32 motion_msg; - motion_msg.data = motion_index; - - motion_index_pub_.publish(motion_msg); -} - -void VisionDemo::setRGBLED(int blue, int green, int red) { - int led_full_unit = 0x1F; - int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | - (red & led_full_unit); - humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg; - syncwrite_msg.item_name = "LED_RGB"; - syncwrite_msg.joint_name.push_back("open-cr"); - syncwrite_msg.value.push_back(led_value); - - rgb_led_pub_.publish(syncwrite_msg); -} - -} /* namespace humanoid_robot_intelligence_control_system_op */ diff --git a/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt b/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt new file mode 100644 index 0000000..c427fc3 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt @@ -0,0 +1,66 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_detection) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + ) + + catkin_package( + CATKIN_DEPENDS + rospy + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/follower.py + src/follower_config.py + src/tracker.py + src/tracker_config.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/follower.py + src/follower_config.py + src/tracker.py + src/tracker_config.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_detection/package.xml b/humanoid_robot_intelligence_control_system_detection/package.xml new file mode 100644 index 0000000..fa6c357 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/package.xml @@ -0,0 +1,46 @@ + + + + + humanoid_robot_intelligence_control_system_detection + 0.0.1 + + This Package is for Detection Math + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + + + rclpy + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_detection/setup.py b/humanoid_robot_intelligence_control_system_detection/setup.py new file mode 100644 index 0000000..ca93ba9 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/follower.py', "src/follower_config.py", 'src/tracker.py', "src/tracker_config.py"], + packages=['humanoid_robot_intelligence_control_system_detection'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower.py b/humanoid_robot_intelligence_control_system_detection/src/follower.py new file mode 100644 index 0000000..8d32487 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/follower.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + +def calculate_distance_to_object(camera_height, tilt, hip_pitch_offset, object_size): + distance = camera_height * math.tan(math.pi * 0.5 + tilt - hip_pitch_offset - object_size) + return abs(distance) + +def calculate_footstep(current_x_move, target_distance, current_r_angle, target_angle, delta_time, config): + next_movement = current_x_move + target_distance = max(0, target_distance) + fb_goal = min(target_distance * 0.1, config.MAX_FB_STEP) + config.accum_period_time += delta_time + if config.accum_period_time > (config.curr_period_time / 4): + config.accum_period_time = 0.0 + if (target_distance * 0.1 / 2) < current_x_move: + next_movement -= config.UNIT_FB_STEP + else: + next_movement += config.UNIT_FB_STEP + fb_goal = min(next_movement, fb_goal) + fb_move = max(fb_goal, config.MIN_FB_STEP) + + rl_goal = 0.0 + if abs(target_angle) * 180 / math.pi > 5.0: + rl_offset = abs(target_angle) * 0.2 + rl_goal = min(rl_offset, config.MAX_RL_TURN) + rl_goal = max(rl_goal, config.MIN_RL_TURN) + rl_angle = min(abs(current_r_angle) + config.UNIT_RL_TURN, rl_goal) + if target_angle < 0: + rl_angle *= -1 + else: + rl_angle = 0 + + return fb_move, rl_angle + +def calculate_delta_time(curr_time, prev_time): + return (curr_time - prev_time).to_sec() diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower_config.py b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py new file mode 100644 index 0000000..8bd53d1 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + + +class FollowerConfig: + def __init__(self): + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.CAMERA_HEIGHT = 0.46 + self.NOT_FOUND_THRESHOLD = 50 + self.MAX_FB_STEP = 40.0 * 0.001 + self.MAX_RL_TURN = 15.0 * math.pi / 180 + self.IN_PLACE_FB_STEP = -3.0 * 0.001 + self.MIN_FB_STEP = 5.0 * 0.001 + self.MIN_RL_TURN = 5.0 * math.pi / 180 + self.UNIT_FB_STEP = 1.0 * 0.001 + self.UNIT_RL_TURN = 0.5 * math.pi / 180 + self.SPOT_FB_OFFSET = 0.0 * 0.001 + self.SPOT_RL_OFFSET = 0.0 * 0.001 + self.SPOT_ANGLE_OFFSET = 0.0 + self.hip_pitch_offset = 7.0 + self.curr_period_time = 0.6 + self.accum_period_time = 0.0 + self.DEBUG_PRINT = False + + def update_from_params(self, get_param): + self.FOV_WIDTH = get_param('fov_width', self.FOV_WIDTH) + self.FOV_HEIGHT = get_param('fov_height', self.FOV_HEIGHT) + self.CAMERA_HEIGHT = get_param('camera_height', self.CAMERA_HEIGHT) + self.NOT_FOUND_THRESHOLD = get_param('not_found_threshold', self.NOT_FOUND_THRESHOLD) + self.MAX_FB_STEP = get_param('max_fb_step', self.MAX_FB_STEP) + self.MAX_RL_TURN = get_param('max_rl_turn', self.MAX_RL_TURN) + self.IN_PLACE_FB_STEP = get_param('in_place_fb_step', self.IN_PLACE_FB_STEP) + self.MIN_FB_STEP = get_param('min_fb_step', self.MIN_FB_STEP) + self.MIN_RL_TURN = get_param('min_rl_turn', self.MIN_RL_TURN) + self.UNIT_FB_STEP = get_param('unit_fb_step', self.UNIT_FB_STEP) + self.UNIT_RL_TURN = get_param('unit_rl_turn', self.UNIT_RL_TURN) + self.SPOT_FB_OFFSET = get_param('spot_fb_offset', self.SPOT_FB_OFFSET) + self.SPOT_RL_OFFSET = get_param('spot_rl_offset', self.SPOT_RL_OFFSET) + self.SPOT_ANGLE_OFFSET = get_param('spot_angle_offset', self.SPOT_ANGLE_OFFSET) + self.hip_pitch_offset = get_param('hip_pitch_offset', self.hip_pitch_offset) + self.curr_period_time = get_param('curr_period_time', self.curr_period_time) + self.DEBUG_PRINT = get_param('debug_print', self.DEBUG_PRINT) + + +class FollowerInitializeConfig: + def __init__(self): + self.count_not_found = 0 + self.count_to_approach = 0 + self.on_tracking = False + self.approach_object_position = "NotFound" + self.current_pan = -10 + self.current_tilt = -10 + self.current_x_move = 0.005 + self.current_r_angle = 0 + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + self.current_object_bottom = 0.0 + self.tracking_status = "NotFound" + self.object_position = None + + + def update_from_params(self, get_param): + self.count_not_found = get_param('initial_count_not_found', self.count_not_found) + self.count_to_approach = get_param('initial_count_to_approach', self.count_to_approach) + self.on_tracking = get_param('initial_on_tracking', self.on_tracking) + self.approach_object_position = get_param('initial_approach_object_position', self.approach_object_position) + self.current_pan = get_param('initial_current_pan', self.current_pan) + self.current_tilt = get_param('initial_current_tilt', self.current_tilt) + self.current_x_move = get_param('initial_current_x_move', self.current_x_move) + self.current_r_angle = get_param('initial_current_r_angle', self.current_r_angle) + self.x_error_sum = get_param('initial_x_error_sum', self.x_error_sum) + self.y_error_sum = get_param('initial_y_error_sum', self.y_error_sum) + self.current_object_bottom = get_param('initial_object_bottom', self.current_object_bottom) + self.tracking_status = get_param('initial_tracking_status', self.tracking_status) + + + def reset(self): + """Reset all values to their initial state.""" + self.__init__() + + def update_object_position(self, x, y, z): + """Update the object position.""" + self.object_position.x = x + self.object_position.y = y + self.object_position.z = z + + def increment_count_not_found(self): + """Increment the count of frames where object was not found.""" + self.count_not_found += 1 + + def reset_count_not_found(self): + """Reset the count of frames where object was not found.""" + self.count_not_found = 0 + + def increment_count_to_approach(self): + """Increment the count to approach.""" + self.count_to_approach += 1 + + def reset_count_to_approach(self): + """Reset the count to approach.""" + self.count_to_approach = 0 + + def set_tracking_status(self, status): + """Set the current tracking status.""" + self.tracking_status = status + + def update_error_sums(self, x_error, y_error): + """Update the cumulative error sums.""" + self.x_error_sum += x_error + self.y_error_sum += y_error + + def reset_error_sums(self): + """Reset the cumulative error sums.""" + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + + def update_current_object_position(self, pan, tilt, bottom): + """Update the current object position.""" + self.current_pan = pan + self.current_tilt = tilt + self.current_object_bottom = bottom + + def update_movement(self, x_move, r_angle): + """Update the current movement parameters.""" + self.current_x_move = x_move + self.current_r_angle = r_angle + + def is_tracking(self): + """Check if tracking is currently active.""" + return self.on_tracking + + def start_tracking(self): + """Start the tracking process.""" + self.on_tracking = True + + def stop_tracking(self): + """Stop the tracking process.""" + self.on_tracking = False + + def set_approach_position(self, position): + """Set the approach position of the object.""" + self.approach_object_position = position diff --git a/humanoid_robot_intelligence_control_system_detection/src/tracker.py b/humanoid_robot_intelligence_control_system_detection/src/tracker.py new file mode 100644 index 0000000..369d84e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/tracker.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +import math + +def calculate_error(object_position, FOV_WIDTH, FOV_HEIGHT): + x_error = -math.atan(object_position.x * math.tan(FOV_WIDTH)) + y_error = -math.atan(object_position.y * math.tan(FOV_HEIGHT)) + return x_error, y_error + +def calculate_error_target(error, error_diff, error_sum, p_gain, i_gain, d_gain): + return error * p_gain + error_diff * d_gain + error_sum * i_gain + +def calculate_delta_time(curr_time, prev_time): + return (curr_time - prev_time).to_sec() diff --git a/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py new file mode 100644 index 0000000..5310436 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + +class TrackerConfig: + def __init__(self): + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.NOT_FOUND_THRESHOLD = 50 + self.WAITING_THRESHOLD = 5 + self.use_head_scan = True + self.DEBUG_PRINT = False + self.p_gain = 0.4 + self.i_gain = 0.0 + self.d_gain = 0.0 + + def update_from_params(self, get_param): + self.FOV_WIDTH = get_param('fov_width', self.FOV_WIDTH) + self.FOV_HEIGHT = get_param('fov_height', self.FOV_HEIGHT) + self.NOT_FOUND_THRESHOLD = get_param('not_found_threshold', self.NOT_FOUND_THRESHOLD) + self.WAITING_THRESHOLD = get_param('waiting_threshold', self.WAITING_THRESHOLD) + self.use_head_scan = get_param('use_head_scan', self.use_head_scan) + self.DEBUG_PRINT = get_param('debug_print', self.DEBUG_PRINT) + self.p_gain = get_param('p_gain', self.p_gain) + self.i_gain = get_param('i_gain', self.i_gain) + self.d_gain = get_param('d_gain', self.d_gain) + + +class TrackerInitializeConfig: + def __init__(self): + self.count_not_found = 0 + self.on_tracking = False + self.current_object_pan = 0.0 + self.current_object_tilt = 0.0 + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + self.current_object_bottom = 0.0 + self.tracking_status = "NotFound" + self.object_position = None + + + def update_from_params(self, get_param): + self.count_not_found = get_param('initial_count_not_found', self.count_not_found) + self.on_tracking = get_param('initial_on_tracking', self.on_tracking) + self.current_object_pan = get_param('initial_object_pan', self.current_object_pan) + self.current_object_tilt = get_param('initial_object_tilt', self.current_object_tilt) + self.x_error_sum = get_param('initial_x_error_sum', self.x_error_sum) + self.y_error_sum = get_param('initial_y_error_sum', self.y_error_sum) + self.current_object_bottom = get_param('initial_object_bottom', self.current_object_bottom) + self.tracking_status = get_param('initial_tracking_status', self.tracking_status) + + + def reset(self): + """Reset all values to their initial state.""" + self.__init__() + + + def update_object_position(self, x, y, z): + """Update the object position.""" + self.object_position.x = x + self.object_position.y = y + self.object_position.z = z + + + def increment_count_not_found(self): + """Increment the count of frames where object was not found.""" + self.count_not_found += 1 + + + def reset_count_not_found(self): + """Reset the count of frames where object was not found.""" + self.count_not_found = 0 + + + def set_tracking_status(self, status): + """Set the current tracking status.""" + self.tracking_status = status + + + def update_error_sums(self, x_error, y_error): + """Update the cumulative error sums.""" + self.x_error_sum += x_error + self.y_error_sum += y_error + + + def reset_error_sums(self): + """Reset the cumulative error sums.""" + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + + + def update_current_object_position(self, pan, tilt, bottom): + """Update the current object position.""" + self.current_object_pan = pan + self.current_object_tilt = tilt + self.current_object_bottom = bottom + + + def is_tracking(self): + """Check if tracking is currently active.""" + return self.on_tracking + + + def start_tracking(self): + """Start the tracking process.""" + self.on_tracking = True + + + def stop_tracking(self): + """Stop the tracking process.""" + self.on_tracking = False diff --git a/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt new file mode 100644 index 0000000..2eb131f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt @@ -0,0 +1,71 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_face_detector) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/face_detection_processor.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/face_detection_processor.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py b/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py new file mode 100644 index 0000000..4a6456b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py @@ -0,0 +1,108 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_face_detector", "face_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_face_detection.py", "name:=face_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_ball_detector", "face_detection_processor.py", "name:=face_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_face_detection.py', + name='face_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_face_detector', + executable='face_detection_processor.py', + name='face_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch b/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch new file mode 100644 index 0000000..6c4507d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_face_detector/package.xml b/humanoid_robot_intelligence_control_system_face_detector/package.xml new file mode 100644 index 0000000..9e60a19 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/package.xml @@ -0,0 +1,61 @@ + + + + + humanoid_robot_intelligence_control_system_face_detector + 0.0.1 + + This Package is for Object Detection, detecting faces like tools, or utilities + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime + + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_face_detector/setup.py b/humanoid_robot_intelligence_control_system_face_detector/setup.py new file mode 100644 index 0000000..f391975 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/face_detection_processor.py'], + packages=['humanoid_robot_intelligence_control_system_face_detector'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py b/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py new file mode 100644 index 0000000..5484d2d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import cv2 +import numpy as np +from sensor_msgs.msg import Image, CameraInfo +from std_msgs.msg import Bool, String +from cv_bridge import CvBridge +import yaml + +class ObjectDetectionProcessor: + def __init__(self): + rospy.init_node('face_detection_processor') + self.bridge = CvBridge() + self.enable = True + self.new_image_flag = False + self.load_params() + self.setup_ros() + + def load_params(self): + param_path = rospy.get_param('~yaml_path', '') + if param_path: + with open(param_path, 'r') as file: + self.params = yaml.safe_load(file) + else: + self.set_default_params() + + def set_default_params(self): + self.params = { + 'debug': False, + 'ellipse_size': 2, + # Add other default parameters as needed + } + + def setup_ros(self): + self.image_pub = rospy.Publisher('image_out', Image, queue_size=10) + self.camera_info_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=10) + + rospy.Subscriber('enable', Bool, self.enable_callback) + rospy.Subscriber('image_in', Image, self.image_callback) + rospy.Subscriber('cameraInfo_in', CameraInfo, self.camera_info_callback) + rospy.Subscriber('face_detection_result', String, self.face_detection_callback) + + def enable_callback(self, msg): + self.enable = msg.data + + def image_callback(self, msg): + if not self.enable: + return + self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") + self.new_image_flag = True + self.image_header = msg.header + + def camera_info_callback(self, msg): + if not self.enable: + return + self.camera_info_msg = msg + + def face_detection_callback(self, msg): + if not self.enable or not hasattr(self, 'cv_image'): + return + + faces = eval(msg.data) # Assuming the data is a string representation of a list + self.process_detected_faces(faces) + + def process_detected_faces(self, faces): + output_image = self.cv_image.copy() + for obj in faces: + x, y, w, h = obj['bbox'] + cv2.rectangle(output_image, (int(x), int(y)), (int(x+w), int(y+h)), (0, 255, 0), 2) + cv2.putText(output_image, f"{obj['label']}: {obj['confidence']:.2f}", + (int(x), int(y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2) + + self.publish_image(output_image) + + def publish_image(self, image): + img_msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") + img_msg.header = self.image_header + self.image_pub.publish(img_msg) + if hasattr(self, 'camera_info_msg'): + self.camera_info_pub.publish(self.camera_info_msg) + + def run(self): + rate = rospy.Rate(30) # 30 Hz + while not rospy.is_shutdown(): + if self.new_image_flag: + # The processing is done in face_detection_callback + self.new_image_flag = False + rate.sleep() + +if __name__ == '__main__': + try: + processor = ObjectDetectionProcessor() + processor.run() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt index 7f45c8d..55bba24 100644 --- a/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt @@ -22,13 +22,6 @@ if($ENV{ROS_VERSION} EQUAL 1) sensor_msgs geometry_msgs cv_bridge - message_generation - ) - - generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs ) catkin_package( @@ -38,7 +31,6 @@ if($ENV{ROS_VERSION} EQUAL 1) sensor_msgs geometry_msgs cv_bridge - message_runtime ) else() @@ -55,7 +47,7 @@ if($ENV{ROS_VERSION} EQUAL 1) catkin_python_setup() catkin_install_python(PROGRAMS - scripts/object_detection_processor.py + src/object_detection_processor.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) @@ -67,7 +59,7 @@ else() ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS - scripts/object_detection_processor.py + src/object_detection_processor.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector.launch b/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector.launch deleted file mode 100644 index 564bebb..0000000 --- a/humanoid_robot_intelligence_control_system_object_detector/launch/object_detector.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/humanoid_robot_intelligence_control_system_object_detector/package.xml b/humanoid_robot_intelligence_control_system_object_detector/package.xml index 5f82887..706fe9e 100644 --- a/humanoid_robot_intelligence_control_system_object_detector/package.xml +++ b/humanoid_robot_intelligence_control_system_object_detector/package.xml @@ -24,106 +24,38 @@ the License. Apache 2.0 Ronaldson Bellande - catkin - - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - dynamic_reconfigure - cv_bridge - image_transport - boost - opencv3 - yaml-cpp - message_generation - message_runtime - message_runtime - usb_cam - - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - dynamic_reconfigure - cv_bridge - image_transport - boost - opencv3 - yaml-cpp - message_generation - message_runtime - message_runtime - usb_cam - - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - dynamic_reconfigure - cv_bridge - image_transport - boost - opencv3 - yaml-cpp - message_generation - message_runtime - message_runtime - usb_cam - - ament_cmake + ament_cmake_python - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - dynamic_reconfigure - cv_bridge - image_transport - boost - opencv3 - yaml-cpp - message_generation - message_runtime - message_runtime - usb_cam + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - dynamic_reconfigure - cv_bridge - image_transport - boost - opencv3 - yaml-cpp - message_generation - message_runtime - message_runtime - usb_cam + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - dynamic_reconfigure - cv_bridge - image_transport - boost - opencv3 - yaml-cpp - message_generation - message_runtime - message_runtime - usb_cam + + 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 new file mode 100644 index 0000000..a2eac39 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_object_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/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_read_write_demo/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_read_write_demo/CHANGELOG.rst deleted file mode 100644 index db641ea..0000000 --- a/humanoid_robot_intelligence_control_system_read_write_demo/CHANGELOG.rst +++ /dev/null @@ -1,26 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package humanoid_robot_intelligence_control_system_read_write_demo -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.2 (2023-10-03) ------------------- -* Make it compatible for ROS1/ROS2 -* Fix bugs -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.3.1 (2023-09-27) ------------------- -* Starting from this point it under a new license -* Fix errors and Issues -* Rename Repository for a completely different purpose -* Make it compatible with ROS/ROS2 -* Upgrade version of all builds and make it more compatible -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - - -0.3.0 (2021-05-05) ------------------- -* Update package.xml and CMakeList.txt for noetic branch -* Contributors: Ronaldson Bellande diff --git a/humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt new file mode 100644 index 0000000..78abc20 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_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_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 new file mode 100644 index 0000000..014521f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_detector/launch/ros1/speech_detector_processor.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 0000000..c11ca70 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_detector/launch/speech_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_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 new file mode 100644 index 0000000..416194b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_detector/package.xml @@ -0,0 +1,61 @@ + + + + + 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 new file mode 100644 index 0000000..f4a55e3 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_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/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 new file mode 100644 index 0000000..fcc60d2 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_speech_detector/src/speech_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('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