Ronaldson Bellande
GitHub
commit
74341b6854
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)
|
||||
@ -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
|
||||
Loading…
Reference in New Issue
Block a user