Ronaldson Bellande
GitHub
commit
acfa565d0f
55
README.md
55
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
|
||||
|
||||
[](https://robotics-sensors.github.io)
|
||||
[](https://discord.gg/Yc72nd4w)
|
||||
[](https://github.com/sponsors/Robotics-Sensors)
|
||||
|
||||
## Mobile and ROS Control System
|
||||
- [](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_ios_api)
|
||||
- [](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_ios_api)
|
||||
- [](https://github.com/Application-UI-UX/bellande_functionality_mobile_ios_api)
|
||||
- [](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_android_api)
|
||||
- [](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_android_api)
|
||||
- [](https://github.com/Application-UI-UX/bellande_functionality_mobile_android_api)
|
||||
- [](https://github.com/Application-UI-UX/bellande_internal_sensor_web_api)
|
||||
- [](https://github.com/Application-UI-UX/bellande_external_sensor_web_api)
|
||||
- [](https://github.com/Application-UI-UX/bellande_functionality_web_api)
|
||||
- [](https://github.com/Robotics-Sensors/bellande_functionality_ros_api)
|
||||
- [](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
|
||||
|
||||
-  Starred by the community
|
||||
-  Forked by enthusiasts
|
||||
-  Watched by curious minds
|
||||
-  Starred by the community
|
||||
-  Forked by enthusiasts
|
||||
-  Watched by curious minds
|
||||
|
||||
-  Actively addressing issues
|
||||
-  Welcoming contributions
|
||||
-  Licensed under Apache 2.0
|
||||
-  Actively addressing issues
|
||||
-  Welcoming contributions
|
||||
-  Licensed under Apache 2.0
|
||||
|
||||
-  Latest updates
|
||||
-  Engaging with the community
|
||||
-  Latest updates
|
||||
-  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
|
||||
|
||||
[](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.
|
||||
[](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).
|
||||
|
||||
@ -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()
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
|
||||
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)
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
|
||||
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)
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
|
||||
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)
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
|
||||
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)
|
||||
@ -0,0 +1,41 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
-->
|
||||
<launch>
|
||||
<!-- Launch USB camera -->
|
||||
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
|
||||
<param name="video_device" value="/dev/video0" />
|
||||
<param name="image_width" value="640" />
|
||||
<param name="image_height" value="480" />
|
||||
<param name="pixel_format" value="yuyv" />
|
||||
<param name="camera_frame_id" value="usb_cam" />
|
||||
<param name="io_method" value="mmap"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch object detection node -->
|
||||
<node name="object_detection_node" pkg="ros_web_api_bellande_2d_computer_vision" type="bellande_2d_computer_vision_object_detection.py" output="screen">
|
||||
<remap from="camera/image_raw" to="/usb_cam/image_raw"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch object detection processor node -->
|
||||
<node name="object_detection_processor_node" pkg="humanoid_robot_intelligence_control_system_ball_detector" type="object_detection_processor.py" output="screen">
|
||||
<param name="yaml_path" value="$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch RViz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" />
|
||||
</launch>
|
||||
@ -0,0 +1,41 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
-->
|
||||
<launch>
|
||||
<!-- Launch USB camera -->
|
||||
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
|
||||
<param name="video_device" value="/dev/video0" />
|
||||
<param name="image_width" value="640" />
|
||||
<param name="image_height" value="480" />
|
||||
<param name="pixel_format" value="yuyv" />
|
||||
<param name="camera_frame_id" value="usb_cam" />
|
||||
<param name="io_method" value="mmap"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch object detection node -->
|
||||
<node name="object_detection_node" pkg="ros_web_api_bellande_2d_computer_vision" type="bellande_2d_computer_vision_object_detection.py" output="screen">
|
||||
<remap from="camera/image_raw" to="/usb_cam/image_raw"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch object detection processor node -->
|
||||
<node name="object_detection_processor_node" pkg="humanoid_robot_intelligence_control_system_ball_detector" type="object_detection_processor.py" output="screen">
|
||||
<param name="yaml_path" value="$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch RViz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" />
|
||||
</launch>
|
||||
@ -0,0 +1,41 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
-->
|
||||
<launch>
|
||||
<!-- Launch USB camera -->
|
||||
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
|
||||
<param name="video_device" value="/dev/video0" />
|
||||
<param name="image_width" value="640" />
|
||||
<param name="image_height" value="480" />
|
||||
<param name="pixel_format" value="yuyv" />
|
||||
<param name="camera_frame_id" value="usb_cam" />
|
||||
<param name="io_method" value="mmap"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch object detection node -->
|
||||
<node name="object_detection_node" pkg="ros_web_api_bellande_2d_computer_vision" type="bellande_2d_computer_vision_object_detection.py" output="screen">
|
||||
<remap from="camera/image_raw" to="/usb_cam/image_raw"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch object detection processor node -->
|
||||
<node name="object_detection_processor_node" pkg="humanoid_robot_intelligence_control_system_ball_detector" type="object_detection_processor.py" output="screen">
|
||||
<param name="yaml_path" value="$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch RViz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" />
|
||||
</launch>
|
||||
@ -0,0 +1,41 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
-->
|
||||
<launch>
|
||||
<!-- Launch USB camera -->
|
||||
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
|
||||
<param name="video_device" value="/dev/video0" />
|
||||
<param name="image_width" value="640" />
|
||||
<param name="image_height" value="480" />
|
||||
<param name="pixel_format" value="yuyv" />
|
||||
<param name="camera_frame_id" value="usb_cam" />
|
||||
<param name="io_method" value="mmap"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch object detection node -->
|
||||
<node name="object_detection_node" pkg="ros_web_api_bellande_2d_computer_vision" type="bellande_2d_computer_vision_object_detection.py" output="screen">
|
||||
<remap from="camera/image_raw" to="/usb_cam/image_raw"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch object detection processor node -->
|
||||
<node name="object_detection_processor_node" pkg="humanoid_robot_intelligence_control_system_ball_detector" type="object_detection_processor.py" output="screen">
|
||||
<param name="yaml_path" value="$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch RViz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" />
|
||||
</launch>
|
||||
@ -0,0 +1,61 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License"); you may not
|
||||
use this file except in compliance with the License. You may obtain a copy of
|
||||
the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
License for the specific language governing permissions and limitations under
|
||||
the License.
|
||||
-->
|
||||
|
||||
<package format="3">
|
||||
<name>humanoid_robot_intelligence_control_system_configure</name>
|
||||
<version>0.0.1</version>
|
||||
<description>
|
||||
This Package is for Configuration of Tracker and Follower related to it
|
||||
</description>
|
||||
<license>Apache 2.0</license>
|
||||
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
|
||||
|
||||
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
|
||||
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
|
||||
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
|
||||
|
||||
<!-- ROS 1 dependencies -->
|
||||
<depend condition="$ROS_VERSION == 1">rospy</depend>
|
||||
<depend condition="$ROS_VERSION == 1">std_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 1">sensor_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 1">geometry_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 1">cv_bridge</depend>
|
||||
<depend condition="$ROS_VERSION == 1">image_transport</depend>
|
||||
<depend condition="$ROS_VERSION == 1">dynamic_reconfigure</depend>
|
||||
<depend condition="$ROS_VERSION == 1">message_generation</depend>
|
||||
<depend condition="$ROS_VERSION == 1">message_runtime</depend>
|
||||
|
||||
<!-- ROS 2 dependencies -->
|
||||
<depend condition="$ROS_VERSION == 2">rclpy</depend>
|
||||
<depend condition="$ROS_VERSION == 2">std_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 2">sensor_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 2">geometry_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 2">cv_bridge</depend>
|
||||
<depend condition="$ROS_VERSION == 2">image_transport</depend>
|
||||
<depend condition="$ROS_VERSION == 2">rosidl_default_generators</depend>
|
||||
<depend condition="$ROS_VERSION == 2">rosidl_default_runtime</depend>
|
||||
|
||||
<!-- Common dependencies -->
|
||||
<depend>python3-opencv</depend>
|
||||
<depend>python3-yaml</depend>
|
||||
<depend>usb_cam</depend>
|
||||
|
||||
<export>
|
||||
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
|
||||
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,26 @@
|
||||
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
#
|
||||
# This program is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
|
||||
from distutils.core import setup
|
||||
from catkin_pkg.python_setup import generate_distutils_setup
|
||||
|
||||
# fetch values from package.xml
|
||||
setup_args = generate_distutils_setup(
|
||||
scripts=['src/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)
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
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)
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
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
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
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)
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
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)
|
||||
@ -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
|
||||
@ -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}
|
||||
)
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -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 <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <std_msgs/String.h>
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#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<int, std::string> action_sound_table_;
|
||||
std::vector<int> 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_ */
|
||||
@ -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 <geometry_msgs/Point.h>
|
||||
#include <math.h>
|
||||
#include <numeric>
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#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_ */
|
||||
@ -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 <geometry_msgs/Point.h>
|
||||
#include <math.h>
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#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_ */
|
||||
@ -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 <boost/thread.hpp>
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/String.h>
|
||||
|
||||
#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_ */
|
||||
@ -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 <geometry_msgs/Point.h>
|
||||
#include <math.h>
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <std_msgs/Int32.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
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_ */
|
||||
@ -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 <boost/thread.hpp>
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
#include <signal.h>
|
||||
#include <std_msgs/String.h>
|
||||
|
||||
#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_ */
|
||||
@ -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_ */
|
||||
@ -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 <boost/thread.hpp>
|
||||
#include <eigen3/Eigen/Eigen>
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#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<int, std::string> id_joint_table_;
|
||||
std::map<std::string, int> 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
|
||||
@ -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 <boost/thread.hpp>
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/Int32MultiArray.h>
|
||||
#include <std_msgs/String.h>
|
||||
|
||||
#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_ */
|
||||
@ -1,28 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- humanoid_robot_intelligence_control_system humanoid_robot_intelligence_control_system manager -->
|
||||
<include
|
||||
file="$(find humanoid_robot_intelligence_control_system_manager)/launch/humanoid_robot_intelligence_control_system_manager.launch" />
|
||||
|
||||
<!-- Camera and Ball detector -->
|
||||
<include
|
||||
file="$(find humanoid_robot_intelligence_control_system_ball_detector)/launch/ball_detector_from_usb_cam.launch" />
|
||||
|
||||
<!-- camera setting tool -->
|
||||
<include
|
||||
file="$(find humanoid_robot_intelligence_control_system_camera_setting_tool)/launch/humanoid_robot_intelligence_control_system_camera_setting_tool.launch" />
|
||||
|
||||
<!-- sound player -->
|
||||
<node pkg="humanoid_robot_intelligence_control_system_player"
|
||||
type="humanoid_robot_intelligence_control_system_player"
|
||||
name="humanoid_robot_intelligence_control_system_player" output="screen" />
|
||||
|
||||
<!-- humanoid_robot_intelligence_control_system humanoid_robot_intelligence_control_system demo -->
|
||||
<node pkg="humanoid_robot_intelligence_control_system_demo" type="op_demo_node"
|
||||
name="humanoid_robot_intelligence_control_system_demo" output="screen">
|
||||
<param name="grass_demo" type="bool" value="False" />
|
||||
<param name="p_gain" value="0.45" />
|
||||
<param name="d_gain" value="0.045" />
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
@ -1,28 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<arg name="face_cascade_name_0"
|
||||
default="$(find face_detection)/include/face_detection/HaarCascades/haarcascade_frontalface_alt.xml" />
|
||||
<arg name="face_cascade_name_1"
|
||||
default="$(find face_detection)/include/face_detection/HaarCascades/haarcascade_frontalface_alt2.xml" />
|
||||
<arg name="face_cascade_name_2"
|
||||
default="$(find face_detection)/include/face_detection/HaarCascades/haarcascade_frontalface_alt_tree.xml" />
|
||||
<arg name="face_cascade_name_3"
|
||||
default="$(find face_detection)/include/face_detection/HaarCascades/haarcascade_frontalface_default.xml" />
|
||||
<arg name="face_cascade_name_4"
|
||||
default="$(find face_detection)/include/face_detection/lbpCascades/lbpcascade_frontalface.xml" />
|
||||
|
||||
<node pkg="face_detection" type="face_tracking" name="face_tracking"
|
||||
args="$(arg face_cascade_name_0)
|
||||
$(arg face_cascade_name_1)
|
||||
$(arg face_cascade_name_2)
|
||||
$(arg face_cascade_name_3)
|
||||
$(arg face_cascade_name_4)"
|
||||
output="screen">
|
||||
<param name="imageInput" type="string" value="/usb_cam_node/image_raw" />
|
||||
<param name="displayed_Image" type="int" value="0" />
|
||||
|
||||
<!-- <param name="publish" type="int" value="2" /> -->
|
||||
<param name="publish" type="int" value="3" />
|
||||
<param name="start_condition" type="bool" value="false" />
|
||||
</node>
|
||||
</launch>
|
||||
@ -1,29 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- Launches an UVC camera, the ball detector and its visualization -->
|
||||
<launch>
|
||||
<!-- humanoid_robot_intelligence_control_system humanoid_robot_intelligence_control_system manager -->
|
||||
<include
|
||||
file="$(find humanoid_robot_intelligence_control_system_manager)/launch/humanoid_robot_intelligence_control_system_manager.launch" />
|
||||
|
||||
<!-- Camera and Ball detector -->
|
||||
<include
|
||||
file="$(find humanoid_robot_intelligence_control_system_ball_detector)/launch/ball_detector_from_usb_cam.launch" />
|
||||
|
||||
<!-- camera setting tool -->
|
||||
<include
|
||||
file="$(find humanoid_robot_intelligence_control_system_camera_setting_tool)/launch/humanoid_robot_intelligence_control_system_camera_setting_tool.launch" />
|
||||
|
||||
<!-- sound player -->
|
||||
<node pkg="humanoid_robot_intelligence_control_system_player"
|
||||
type="humanoid_robot_intelligence_control_system_player"
|
||||
name="humanoid_robot_intelligence_control_system_player" output="screen" />
|
||||
|
||||
<!-- humanoid_robot_intelligence_control_system humanoid_robot_intelligence_control_system self
|
||||
test demo -->
|
||||
<node pkg="humanoid_robot_intelligence_control_system_demo" type="self_test_node"
|
||||
name="humanoid_robot_intelligence_control_system_self_test" output="screen">
|
||||
<param name="grass_demo" type="bool" value="False" />
|
||||
<param name="p_gain" value="0.45" />
|
||||
<param name="d_gain" value="0.045" />
|
||||
</node>
|
||||
</launch>
|
||||
@ -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]
|
||||
@ -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]
|
||||
@ -1,180 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License"); you may not
|
||||
use this file except in compliance with the License. You may obtain a copy of
|
||||
the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
License for the specific language governing permissions and limitations under
|
||||
the License.
|
||||
-->
|
||||
|
||||
<package format="3">
|
||||
<name>humanoid_robot_intelligence_control_system_demo</name>
|
||||
<version>0.3.2</version>
|
||||
<description>
|
||||
HUMANOID_ROBOT default demo
|
||||
It includes three demontrations(soccer demo, vision demo, action script demo)
|
||||
</description>
|
||||
<license>Apache 2.0</license>
|
||||
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
|
||||
|
||||
|
||||
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
|
||||
|
||||
<build_depend condition="$ROS_VERSION == 1">roscpp</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">roslib</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">std_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">sensor_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">geometry_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_controller_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_walking_module_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_action_module_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">cmake_modules</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_math</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_ball_detector</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">boost</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">eigen</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">yaml-cpp</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_manager</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_camera_setting_tool</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_web_setting_tool</build_depend>
|
||||
|
||||
<build_export_depend condition="$ROS_VERSION == 1">roscpp</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">roslib</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">std_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">sensor_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">geometry_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_controller_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_walking_module_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_action_module_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">cmake_modules</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_math</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_ball_detector</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">boost</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">eigen</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">yaml-cpp</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_manager</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_camera_setting_tool</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_web_setting_tool</build_export_depend>
|
||||
|
||||
<exec_depend condition="$ROS_VERSION == 1">roscpp</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">roslib</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">std_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">sensor_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">geometry_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_controller_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_walking_module_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_action_module_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">cmake_modules</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_math</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_ball_detector</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">boost</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">eigen</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">yaml-cpp</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_manager</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_camera_setting_tool</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 1">
|
||||
humanoid_robot_intelligence_control_system_web_setting_tool</exec_depend>
|
||||
|
||||
|
||||
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
|
||||
|
||||
<build_depend condition="$ROS_VERSION == 2">roscpp</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">roslib</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">std_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">sensor_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">geometry_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_controller_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_walking_module_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_action_module_msgs</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">cmake_modules</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_math</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_ball_detector</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">boost</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">eigen</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">yaml-cpp</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_manager</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_camera_setting_tool</build_depend>
|
||||
<build_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_web_setting_tool</build_depend>
|
||||
|
||||
<build_export_depend condition="$ROS_VERSION == 2">roscpp</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">roslib</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">std_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">sensor_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">geometry_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_controller_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_walking_module_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_action_module_msgs</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">cmake_modules</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_math</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_ball_detector</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">boost</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">eigen</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">yaml-cpp</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_manager</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_camera_setting_tool</build_export_depend>
|
||||
<build_export_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_web_setting_tool</build_export_depend>
|
||||
|
||||
<exec_depend condition="$ROS_VERSION == 2">roscpp</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">roslib</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">std_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">sensor_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">geometry_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_controller_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_walking_module_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_action_module_msgs</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">cmake_modules</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_math</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_ball_detector</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">boost</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">eigen</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">yaml-cpp</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_manager</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_camera_setting_tool</exec_depend>
|
||||
<exec_depend condition="$ROS_VERSION == 2">
|
||||
humanoid_robot_intelligence_control_system_web_setting_tool</exec_depend>
|
||||
|
||||
</package>
|
||||
@ -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<std::string>("action_script", default_path);
|
||||
|
||||
std::string default_play_list = "default";
|
||||
play_list_name_ =
|
||||
nh.param<std::string>("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<std_msgs::String>("/humanoid_robot_intelligence_control_system/enable_ctrl_module", 0);
|
||||
motion_index_pub_ =
|
||||
nh.advertise<std_msgs::Int32>("/humanoid_robot_intelligence_control_system/action/page_num", 0);
|
||||
play_sound_pub_ = nh.advertise<std_msgs::String>("/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_module_msgs::IsRunning>(
|
||||
"/humanoid_robot_intelligence_control_system/action/is_running");
|
||||
set_joint_module_client_ =
|
||||
nh.serviceClient<humanoid_robot_intelligence_control_system_controller_msgs::SetModule>(
|
||||
"/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<int>();
|
||||
std::string mp3_path = yaml_it->second.as<std::string>();
|
||||
|
||||
action_sound_table_[action_index] = mp3_path;
|
||||
}
|
||||
|
||||
// default action set
|
||||
if (doc["default"])
|
||||
play_list_ = doc["default"].as<std::vector<int>>();
|
||||
}
|
||||
|
||||
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<std::vector<int>>();
|
||||
return true;
|
||||
} else
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ActionDemo::playActionWithSound(int motion_index) {
|
||||
std::map<int, std::string>::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 */
|
||||
@ -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 <ros/ros.h>
|
||||
#include <std_msgs/String.h>
|
||||
|
||||
#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<std_msgs::String>(
|
||||
"/humanoid_robot_intelligence_control_system/base/ini_pose", 0);
|
||||
play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
|
||||
led_pub =
|
||||
nh.advertise<humanoid_robot_intelligence_control_system_controller_msgs::
|
||||
SyncWriteItem>(
|
||||
"/humanoid_robot_intelligence_control_system/sync_write_item", 0);
|
||||
dxl_torque_pub = nh.advertise<std_msgs::String>(
|
||||
"/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<std::string> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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<std_msgs::String>("/humanoid_robot_intelligence_control_system/walking/command", 0);
|
||||
set_walking_param_pub_ =
|
||||
nh_.advertise<humanoid_robot_intelligence_control_system_walking_module_msgs::WalkingParam>(
|
||||
"/humanoid_robot_intelligence_control_system/walking/set_params", 0);
|
||||
get_walking_param_client_ =
|
||||
nh_.serviceClient<humanoid_robot_intelligence_control_system_walking_module_msgs::GetWalkingParam>(
|
||||
"/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
|
||||
@ -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<sensor_msgs::JointState>(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 0);
|
||||
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 0);
|
||||
head_scan_pub_ = nh_.advertise<std_msgs::String>(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/scan_command", 0);
|
||||
// error_pub_ =
|
||||
// nh_.advertise<std_msgs::Float64MultiArray>("/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
|
||||
@ -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<std::string>("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<bool>("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_controller_msgs::JointCtrlModule>(
|
||||
"/humanoid_robot_intelligence_control_system/set_joint_ctrl_modules", 0);
|
||||
motion_index_pub_ =
|
||||
nh.advertise<std_msgs::Int32>("/humanoid_robot_intelligence_control_system/action/page_num", 0);
|
||||
rgb_led_pub_ = nh.advertise<humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem>(
|
||||
"/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_module_msgs::IsRunning>(
|
||||
"/humanoid_robot_intelligence_control_system/action/is_running");
|
||||
set_joint_module_client_ =
|
||||
nh.serviceClient<humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule>(
|
||||
"/humanoid_robot_intelligence_control_system/set_present_joint_ctrl_modules");
|
||||
|
||||
test_pub_ = nh.advertise<std_msgs::String>("/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<int, std::string>::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<int, std::string>::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<int>();
|
||||
_joint_name = _it->second.as<std::string>();
|
||||
|
||||
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<int, std::string>::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<std::string, int>::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<std::string, int>::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
|
||||
@ -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_controller_msgs::SyncWriteItem>(
|
||||
"/humanoid_robot_intelligence_control_system/sync_write_item", 0);
|
||||
play_sound_pub_ = nh.advertise<std_msgs::String>("/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 */
|
||||
@ -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<std_msgs::String>("/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 */
|
||||
@ -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 <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/String.h>
|
||||
|
||||
#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<std_msgs::String>("/humanoid_robot_intelligence_control_system/base/ini_pose", 0);
|
||||
play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
|
||||
led_pub = nh.advertise<humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem>(
|
||||
"/humanoid_robot_intelligence_control_system/sync_write_item", 0);
|
||||
dxl_torque_pub =
|
||||
nh.advertise<std_msgs::String>("/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<std::string> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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<sensor_msgs::JointState>(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states_offset", 0);
|
||||
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>(
|
||||
"/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 0);
|
||||
head_scan_pub_ = nh_.advertise<std_msgs::String>(
|
||||
"/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
|
||||
@ -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<std_msgs::String>("/humanoid_robot_intelligence_control_system/enable_ctrl_module", 0);
|
||||
motion_index_pub_ =
|
||||
nh.advertise<std_msgs::Int32>("/humanoid_robot_intelligence_control_system/action/page_num", 0);
|
||||
rgb_led_pub_ = nh.advertise<humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem>(
|
||||
"/humanoid_robot_intelligence_control_system/sync_write_item", 0);
|
||||
face_tracking_command_pub_ =
|
||||
nh.advertise<std_msgs::Bool>("/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_controller_msgs::SetModule>(
|
||||
"/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 */
|
||||
@ -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()
|
||||
@ -0,0 +1,46 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License"); you may not
|
||||
use this file except in compliance with the License. You may obtain a copy of
|
||||
the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
License for the specific language governing permissions and limitations under
|
||||
the License.
|
||||
-->
|
||||
|
||||
<package format="3">
|
||||
<name>humanoid_robot_intelligence_control_system_detection</name>
|
||||
<version>0.0.1</version>
|
||||
<description>
|
||||
This Package is for Detection Math
|
||||
</description>
|
||||
<license>Apache 2.0</license>
|
||||
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
|
||||
|
||||
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
|
||||
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
|
||||
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
|
||||
|
||||
<!-- ROS 1 dependencies -->
|
||||
<depend condition="$ROS_VERSION == 1">rospy</depend>
|
||||
|
||||
<!-- ROS 2 dependencies -->
|
||||
<depend condition="$ROS_VERSION == 2">rclpy</depend>
|
||||
|
||||
<!-- Common dependencies -->
|
||||
<depend>python3-opencv</depend>
|
||||
<depend>python3-yaml</depend>
|
||||
<depend>usb_cam</depend>
|
||||
|
||||
<export>
|
||||
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
|
||||
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,26 @@
|
||||
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
#
|
||||
# This program is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
|
||||
from distutils.core import setup
|
||||
from catkin_pkg.python_setup import generate_distutils_setup
|
||||
|
||||
# fetch values from package.xml
|
||||
setup_args = generate_distutils_setup(
|
||||
scripts=['src/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)
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
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()
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
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
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
|
||||
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()
|
||||
@ -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 <https://www.gnu.org/licenses/>.
|
||||
|
||||
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
|
||||
@ -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()
|
||||
@ -0,0 +1,108 @@
|
||||
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
#
|
||||
# This program is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
|
||||
import os
|
||||
import sys
|
||||
import subprocess
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
|
||||
def ros1_launch_description():
|
||||
# Get command-line arguments
|
||||
args = sys.argv[1:]
|
||||
|
||||
# Construct the ROS 1 launch command
|
||||
roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_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)
|
||||
@ -0,0 +1,41 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
-->
|
||||
<launch>
|
||||
<!-- Launch USB camera -->
|
||||
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
|
||||
<param name="video_device" value="/dev/video0" />
|
||||
<param name="image_width" value="640" />
|
||||
<param name="image_height" value="480" />
|
||||
<param name="pixel_format" value="yuyv" />
|
||||
<param name="camera_frame_id" value="usb_cam" />
|
||||
<param name="io_method" value="mmap"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch face detection node -->
|
||||
<node name="face_detection_node" pkg="ros_web_api_bellande_2d_computer_vision" type="bellande_2d_computer_vision_face_detection.py" output="screen">
|
||||
<remap from="camera/image_raw" to="/usb_cam/image_raw"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch face detection processor node -->
|
||||
<node name="face_detection_processor_node" pkg="humanoid_robot_intelligence_control_system_ball_detector" type="face_detection_processor.py" output="screen">
|
||||
<param name="yaml_path" value="$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch RViz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" />
|
||||
</launch>
|
||||
@ -0,0 +1,61 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License"); you may not
|
||||
use this file except in compliance with the License. You may obtain a copy of
|
||||
the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
||||
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
||||
License for the specific language governing permissions and limitations under
|
||||
the License.
|
||||
-->
|
||||
|
||||
<package format="3">
|
||||
<name>humanoid_robot_intelligence_control_system_face_detector</name>
|
||||
<version>0.0.1</version>
|
||||
<description>
|
||||
This Package is for Object Detection, detecting faces like tools, or utilities
|
||||
</description>
|
||||
<license>Apache 2.0</license>
|
||||
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
|
||||
|
||||
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
|
||||
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
|
||||
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
|
||||
|
||||
<!-- ROS 1 dependencies -->
|
||||
<depend condition="$ROS_VERSION == 1">rospy</depend>
|
||||
<depend condition="$ROS_VERSION == 1">std_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 1">sensor_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 1">geometry_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 1">cv_bridge</depend>
|
||||
<depend condition="$ROS_VERSION == 1">image_transport</depend>
|
||||
<depend condition="$ROS_VERSION == 1">dynamic_reconfigure</depend>
|
||||
<depend condition="$ROS_VERSION == 1">message_generation</depend>
|
||||
<depend condition="$ROS_VERSION == 1">message_runtime</depend>
|
||||
|
||||
<!-- ROS 2 dependencies -->
|
||||
<depend condition="$ROS_VERSION == 2">rclpy</depend>
|
||||
<depend condition="$ROS_VERSION == 2">std_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 2">sensor_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 2">geometry_msgs</depend>
|
||||
<depend condition="$ROS_VERSION == 2">cv_bridge</depend>
|
||||
<depend condition="$ROS_VERSION == 2">image_transport</depend>
|
||||
<depend condition="$ROS_VERSION == 2">rosidl_default_generators</depend>
|
||||
<depend condition="$ROS_VERSION == 2">rosidl_default_runtime</depend>
|
||||
|
||||
<!-- Common dependencies -->
|
||||
<depend>python3-opencv</depend>
|
||||
<depend>python3-yaml</depend>
|
||||
<depend>usb_cam</depend>
|
||||
|
||||
<export>
|
||||
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
|
||||
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,26 @@
|
||||
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
#
|
||||
# This program is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
|
||||
from distutils.core import setup
|
||||
from catkin_pkg.python_setup import generate_distutils_setup
|
||||
|
||||
# fetch values from package.xml
|
||||
setup_args = generate_distutils_setup(
|
||||
scripts=['src/face_detection_processor.py'],
|
||||
packages=['humanoid_robot_intelligence_control_system_face_detector'],
|
||||
package_dir={'': 'src'},
|
||||
)
|
||||
|
||||
setup(**setup_args)
|
||||
@ -0,0 +1,111 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
|
||||
#
|
||||
# This program is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
|
||||
import rospy
|
||||
import cv2
|
||||
import numpy as np
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from std_msgs.msg import Bool, String
|
||||
from cv_bridge import CvBridge
|
||||
import yaml
|
||||
|
||||
class ObjectDetectionProcessor:
|
||||
def __init__(self):
|
||||
rospy.init_node('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
|
||||
@ -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}
|
||||
)
|
||||
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user