From b73e3a23aec19876a131dfe8a6b22f87bc96dd32 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Mon, 22 Jul 2024 20:46:28 -0400 Subject: [PATCH 01/16] latest pushes --- .../CMakeLists.txt | 71 +++++++++++ .../launch/face_detector_processor.launch.py | 108 +++++++++++++++++ .../ros1/face_detector_processor.launch | 41 +++++++ .../package.xml | 61 ++++++++++ .../setup.py | 26 ++++ .../src/face_detection_processor.py | 111 ++++++++++++++++++ 6 files changed, 418 insertions(+) create mode 100644 humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py create mode 100644 humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch create mode 100644 humanoid_robot_intelligence_control_system_face_detector/package.xml create mode 100644 humanoid_robot_intelligence_control_system_face_detector/setup.py create mode 100644 humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py diff --git a/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt b/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt new file mode 100644 index 0000000..2eb131f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/CMakeLists.txt @@ -0,0 +1,71 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_face_detector) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/face_detection_processor.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/face_detection_processor.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py b/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py new file mode 100644 index 0000000..4a6456b --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/launch/face_detector_processor.launch.py @@ -0,0 +1,108 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_face_detector", "face_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_face_detection.py", "name:=face_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_ball_detector", "face_detection_processor.py", "name:=face_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_face_detection.py', + name='face_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_face_detector', + executable='face_detection_processor.py', + name='face_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/face_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch b/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch new file mode 100644 index 0000000..6c4507d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/launch/ros1/face_detector_processor.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_face_detector/package.xml b/humanoid_robot_intelligence_control_system_face_detector/package.xml new file mode 100644 index 0000000..9e60a19 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/package.xml @@ -0,0 +1,61 @@ + + + + + humanoid_robot_intelligence_control_system_face_detector + 0.0.1 + + This Package is for Object Detection, detecting faces like tools, or utilities + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime + + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_face_detector/setup.py b/humanoid_robot_intelligence_control_system_face_detector/setup.py new file mode 100644 index 0000000..f391975 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/face_detection_processor.py'], + packages=['humanoid_robot_intelligence_control_system_face_detector'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py b/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py new file mode 100644 index 0000000..5484d2d --- /dev/null +++ b/humanoid_robot_intelligence_control_system_face_detector/src/face_detection_processor.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import cv2 +import numpy as np +from sensor_msgs.msg import Image, CameraInfo +from std_msgs.msg import Bool, String +from cv_bridge import CvBridge +import yaml + +class ObjectDetectionProcessor: + def __init__(self): + rospy.init_node('face_detection_processor') + self.bridge = CvBridge() + self.enable = True + self.new_image_flag = False + self.load_params() + self.setup_ros() + + def load_params(self): + param_path = rospy.get_param('~yaml_path', '') + if param_path: + with open(param_path, 'r') as file: + self.params = yaml.safe_load(file) + else: + self.set_default_params() + + def set_default_params(self): + self.params = { + 'debug': False, + 'ellipse_size': 2, + # Add other default parameters as needed + } + + def setup_ros(self): + self.image_pub = rospy.Publisher('image_out', Image, queue_size=10) + self.camera_info_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=10) + + rospy.Subscriber('enable', Bool, self.enable_callback) + rospy.Subscriber('image_in', Image, self.image_callback) + rospy.Subscriber('cameraInfo_in', CameraInfo, self.camera_info_callback) + rospy.Subscriber('face_detection_result', String, self.face_detection_callback) + + def enable_callback(self, msg): + self.enable = msg.data + + def image_callback(self, msg): + if not self.enable: + return + self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") + self.new_image_flag = True + self.image_header = msg.header + + def camera_info_callback(self, msg): + if not self.enable: + return + self.camera_info_msg = msg + + def face_detection_callback(self, msg): + if not self.enable or not hasattr(self, 'cv_image'): + return + + faces = eval(msg.data) # Assuming the data is a string representation of a list + self.process_detected_faces(faces) + + def process_detected_faces(self, faces): + output_image = self.cv_image.copy() + for obj in faces: + x, y, w, h = obj['bbox'] + cv2.rectangle(output_image, (int(x), int(y)), (int(x+w), int(y+h)), (0, 255, 0), 2) + cv2.putText(output_image, f"{obj['label']}: {obj['confidence']:.2f}", + (int(x), int(y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2) + + self.publish_image(output_image) + + def publish_image(self, image): + img_msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8") + img_msg.header = self.image_header + self.image_pub.publish(img_msg) + if hasattr(self, 'camera_info_msg'): + self.camera_info_pub.publish(self.camera_info_msg) + + def run(self): + rate = rospy.Rate(30) # 30 Hz + while not rospy.is_shutdown(): + if self.new_image_flag: + # The processing is done in face_detection_callback + self.new_image_flag = False + rate.sleep() + +if __name__ == '__main__': + try: + processor = ObjectDetectionProcessor() + processor.run() + except rospy.ROSInterruptException: + pass From 8fb6ddc86e6a10a32c6217f847deefba7492c0a5 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 01:28:20 -0400 Subject: [PATCH 02/16] latest pushes --- .../CMakeLists.txt | 77 +++++++ .../launch/face_follower.launch.py | 114 +++++++++ .../launch/face_tracker.launch.py | 112 +++++++++ .../launch/object_follower.launch.py | 108 +++++++++ .../launch/object_tracker.launch.py | 108 +++++++++ .../launch/ros1/face_follower.launch | 41 ++++ .../launch/ros1/face_tracker.launch | 41 ++++ .../launch/ros1/object_follower.launch | 41 ++++ .../launch/ros1/object_tracker.launch | 41 ++++ .../package.xml | 61 +++++ .../setup.py | 26 +++ .../src/face_follower.py | 173 ++++++++++++++ .../src/face_tracker.py | 179 ++++++++++++++ .../src/object_follower.py | 218 ++++++++++++++++++ .../src/object_tracker.py | 194 ++++++++++++++++ 15 files changed, 1534 insertions(+) create mode 100644 humanoid_robot_intelligence_control_system_configure/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch create mode 100644 humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch create mode 100644 humanoid_robot_intelligence_control_system_configure/package.xml create mode 100644 humanoid_robot_intelligence_control_system_configure/setup.py create mode 100644 humanoid_robot_intelligence_control_system_configure/src/face_follower.py create mode 100644 humanoid_robot_intelligence_control_system_configure/src/face_tracker.py create mode 100644 humanoid_robot_intelligence_control_system_configure/src/object_follower.py create mode 100644 humanoid_robot_intelligence_control_system_configure/src/object_tracker.py diff --git a/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt new file mode 100644 index 0000000..0277ccd --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt @@ -0,0 +1,77 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_object_detector) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + + catkin_package( + CATKIN_DEPENDS + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(cv_bridge REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/face_tracker.py + src/face_follower.py + src/object_tracker.py + src/object_follower.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/face_tracker.py + src/face_follower.py + src/object_tracker.py + src/object_follower.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py new file mode 100644 index 0000000..051f56a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py @@ -0,0 +1,114 @@ +# 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 . + +#!/usr/bin/env python3 + +#!/usr/bin/env python3 + +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_object_detector", "object_follow.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_object_detector", "object_follower.py", "name:=object_follower_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_follow_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 humanoid_robot_intelligence_control_system_object_detector)/config/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + 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 humanoid_robot_intelligence_control_system_object_detector)/rviz/object_follow_visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py new file mode 100644 index 0000000..91a9896 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py @@ -0,0 +1,112 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +#!/usr/bin/env python3 + +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_detector", "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 humanoid_robot_intelligence_control_system_object_detector)/rviz/object_tracking_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 humanoid_robot_intelligence_control_system_object_detector)/config/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + 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 humanoid_robot_intelligence_control_system_object_detector)/rviz/object_tracking_visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py new file mode 100644 index 0000000..e61d87a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py @@ -0,0 +1,108 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_ball_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_object_detection.py', + name='object_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py new file mode 100644 index 0000000..e61d87a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py @@ -0,0 +1,108 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import sys +import subprocess +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def ros1_launch_description(): + # Get command-line arguments + args = sys.argv[1:] + + # Construct the ROS 1 launch command + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args + + roslaunch_command.extend([ + "usb_cam", "usb_cam_node", "name:=camera", + "video_device:=/dev/video0", + "image_width:=640", + "image_height:=480", + "pixel_format:=yuyv", + "camera_frame_id:=usb_cam", + "io_method:=mmap" + ]) + + roslaunch_command.extend([ + "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node" + ]) + + roslaunch_command.extend([ + "humanoid_robot_intelligence_control_system_ball_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + ]) + + roslaunch_command.extend([ + "rviz", "rviz", "name:=rviz", + "args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" + ]) + + # Execute the launch command + subprocess.call(roslaunch_command) + +def ros2_launch_description(): + nodes_to_launch = [] + + nodes_to_launch.append(Node( + package='usb_cam', + executable='usb_cam_node', + name='camera', + output='screen', + parameters=[{ + 'video_device': '/dev/video0', + 'image_width': 640, + 'image_height': 480, + 'pixel_format': 'yuyv', + 'camera_frame_id': 'usb_cam', + 'io_method': 'mmap' + }] + )) + + nodes_to_launch.append(Node( + package='ros_web_api_bellande_2d_computer_vision', + executable='bellande_2d_computer_vision_object_detection.py', + name='object_detection_node', + output='screen', + remappings=[('camera/image_raw', '/usb_cam/image_raw')] + )) + + nodes_to_launch.append(Node( + package='humanoid_robot_intelligence_control_system_object_detector', + executable='object_detection_processor.py', + name='object_detection_processor_node', + output='screen', + parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}] + )) + + nodes_to_launch.append(Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] + )) + + return LaunchDescription(nodes_to_launch) + +if __name__ == "__main__": + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + ros1_launch_description() + elif ros_version == "2": + ros2_launch_description() + else: + print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") + sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_follower.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/face_tracker.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_follower.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch new file mode 100644 index 0000000..6799e95 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/launch/ros1/object_tracker.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/humanoid_robot_intelligence_control_system_configure/package.xml b/humanoid_robot_intelligence_control_system_configure/package.xml new file mode 100644 index 0000000..740f1f7 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/package.xml @@ -0,0 +1,61 @@ + + + + + humanoid_robot_intelligence_control_system_configure + 0.0.1 + + This Package is for Configuration of Tracker and Follower related to it + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + dynamic_reconfigure + message_generation + message_runtime + + + rclpy + std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + rosidl_default_generators + rosidl_default_runtime + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_configure/setup.py b/humanoid_robot_intelligence_control_system_configure/setup.py new file mode 100644 index 0000000..690d947 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/face_tracker.py', "src/face_follower.py", "src/object_tracker.py", "src/object_follower.py"], + packages=['humanoid_robot_intelligence_control_system_configure'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py new file mode 100644 index 0000000..0f11ade --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String, Bool +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 + +class FaceFollower: + def __init__(self): + rospy.init_node('face_follower') + + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.count_not_found = 0 + self.count_to_approach = 0 + self.on_following = False + self.approach_face_position = "NotFound" + 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.current_pan = -10 + self.current_tilt = -10 + self.current_x_move = 0.005 + self.current_r_angle = 0 + self.curr_period_time = 0.6 + self.accum_period_time = 0.0 + self.DEBUG_PRINT = False + + 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_position_sub = rospy.Subscriber( + "/face_detector/face_position", Point, self.face_position_callback) + + self.prev_time = rospy.Time.now() + self.current_walking_param = WalkingParam() + + def start_following(self): + self.on_following = True + rospy.loginfo("Start Face following") + self.set_walking_command("start") + result = self.get_walking_param() + if result: + self.hip_pitch_offset = self.current_walking_param.hip_pitch_offset + self.curr_period_time = self.current_walking_param.period_time + else: + self.hip_pitch_offset = 7.0 * math.pi / 180 + self.curr_period_time = 0.6 + + def stop_following(self): + self.on_following = False + self.count_to_approach = 0 + rospy.loginfo("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.current_pan = msg.position[i] + elif name == "head_tilt": + self.current_tilt = msg.position[i] + + def face_position_callback(self, msg): + if self.on_following: + self.process_following(msg.x, msg.y, msg.z) + + def calc_footstep(self, target_distance, target_angle, delta_time): + next_movement = self.current_x_move + target_distance = max(0, target_distance) + fb_goal = min(target_distance * 0.1, self.MAX_FB_STEP) + self.accum_period_time += delta_time + if self.accum_period_time > (self.curr_period_time / 4): + self.accum_period_time = 0.0 + if (target_distance * 0.1 / 2) < self.current_x_move: + next_movement -= self.UNIT_FB_STEP + else: + next_movement += self.UNIT_FB_STEP + fb_goal = min(next_movement, fb_goal) + fb_move = max(fb_goal, self.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, self.MAX_RL_TURN) + rl_goal = max(rl_goal, self.MIN_RL_TURN) + rl_angle = min(abs(self.current_r_angle) + self.UNIT_RL_TURN, rl_goal) + if target_angle < 0: + rl_angle *= -1 + else: + rl_angle = 0 + + return fb_move, rl_angle + + def process_following(self, x_angle, y_angle, face_size): + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + self.prev_time = curr_time + + self.count_not_found = 0 + + if self.current_tilt == -10 and self.current_pan == -10: + rospy.logerr("Failed to get current angle of head joints.") + self.set_walking_command("stop") + self.on_following = False + self.approach_face_position = "NotFound" + return False + + self.approach_face_position = "OutOfRange" + + distance_to_face = self.CAMERA_HEIGHT * math.tan(math.pi * 0.5 + self.current_tilt - self.hip_pitch_offset - face_size) + distance_to_face = abs(distance_to_face) + + distance_to_approach = 0.5 # Adjust this value as needed + + if (distance_to_face < distance_to_approach) and (abs(x_angle) < 25.0): + self.count_to_approach += 1 + if self.count_to_approach > 20: + self.set_walking_command("stop") + self.on_following = False + self.approach_face_position = "OnLeft" if x_angle > 0 else "OnRight" + return True + elif self.count_to_approach > 15: + self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0) + return False + else: + self.count_to_approach = 0 + + distance_to_walk = distance_to_face - distance_to_approach + fb_move, rl_angle = self.calc_footstep(distance_to_walk, self.current_pan, delta_time) + self.set_walking_param(fb_move, 0, rl_angle) + return False + + def set_walking_command(self, command): + if command == "start": + self.get_walking_param() + self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0, True) + msg = String() + msg.data = command + self.set_walking_command_pub. diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py new file mode 100644 index 0000000..046f5b0 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +import rospy +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String +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.tracking_status = "NotFound" + self.DEBUG_PRINT = False + + self.p_gain = rospy.get_param('~p_gain', 0.4) + self.i_gain = rospy.get_param('~i_gain', 0.0) + self.d_gain = rospy.get_param('~d_gain', 0.0) + + rospy.loginfo(f"Face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") + + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=1) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=1) + + self.face_position_sub = rospy.Subscriber( + "/face_detector/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") + + def stop_tracking(self): + self.go_init() + self.on_tracking = False + rospy.loginfo("Stop Face tracking") + self.current_face_pan = 0 + self.current_face_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + + 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: + tracking_status = "Waiting" + 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 + 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)) + + 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.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_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) + + def run(self): + rate = rospy.Rate(30) # 30Hz + while not rospy.is_shutdown(): + self.process_tracking() + rate.sleep() + +if __name__ == '__main__': + try: + tracker = FaceTracker() + tracker.run() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py new file mode 100644 index 0000000..87c320a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String +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 geometry_msgs.msg import Point + + +class ObjectFollower: + def __init__(self): + rospy.init_node('object_follower') + + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.count_not_found = 0 + self.count_to_approach = 0 + self.on_tracking = False + self.approach_object_position = "NotFound" + 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.current_pan = -10 + self.current_tilt = -10 + self.current_x_move = 0.005 + self.current_r_angle = 0 + self.curr_period_time = 0.6 + self.accum_period_time = 0.0 + self.DEBUG_PRINT = False + + 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) + + # Subscribe to object detection results + self.object_detection_sub = rospy.Subscriber( + "/object_detection_result", Point, self.object_detection_callback) + + self.prev_time = rospy.Time.now() + self.current_walking_param = WalkingParam() + + def start_following(self): + self.on_tracking = True + rospy.loginfo("Start Object following") + self.set_walking_command("start") + result = self.get_walking_param() + if result: + self.hip_pitch_offset = self.current_walking_param.hip_pitch_offset + self.curr_period_time = self.current_walking_param.period_time + else: + self.hip_pitch_offset = 7.0 * math.pi / 180 + self.curr_period_time = 0.6 + + def stop_following(self): + self.on_tracking = False + self.count_to_approach = 0 + rospy.loginfo("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.current_pan = msg.position[i] + elif name == "head_tilt": + self.current_tilt = msg.position[i] + + def calc_footstep(self, target_distance, target_angle, delta_time): + next_movement = self.current_x_move + target_distance = max(0, target_distance) + fb_goal = min(target_distance * 0.1, self.MAX_FB_STEP) + self.accum_period_time += delta_time + if self.accum_period_time > (self.curr_period_time / 4): + self.accum_period_time = 0.0 + if (target_distance * 0.1 / 2) < self.current_x_move: + next_movement -= self.UNIT_FB_STEP + else: + next_movement += self.UNIT_FB_STEP + fb_goal = min(next_movement, fb_goal) + fb_move = max(fb_goal, self.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, self.MAX_RL_TURN) + rl_goal = max(rl_goal, self.MIN_RL_TURN) + rl_angle = min(abs(self.current_r_angle) + self.UNIT_RL_TURN, rl_goal) + if target_angle < 0: + rl_angle *= -1 + else: + rl_angle = 0 + + return fb_move, rl_angle + + def process_following(self, x_angle, y_angle, object_size): + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + self.prev_time = curr_time + + self.count_not_found = 0 + + if self.current_tilt == -10 and self.current_pan == -10: + rospy.logerr("Failed to get current angle of head joints.") + self.set_walking_command("stop") + self.on_tracking = False + self.approach_object_position = "NotFound" + return False + + self.approach_object_position = "OutOfRange" + + distance_to_object = self.CAMERA_HEIGHT * math.tan(math.pi * 0.5 + self.current_tilt - self.hip_pitch_offset - object_size) + distance_to_object = abs(distance_to_object) + + distance_to_approach = 0.22 + + if (distance_to_object < distance_to_approach) and (abs(x_angle) < 25.0): + self.count_to_approach += 1 + if self.count_to_approach > 20: + self.set_walking_command("stop") + self.on_tracking = False + self.approach_object_position = "OnLeft" if x_angle > 0 else "OnRight" + return True + elif self.count_to_approach > 15: + self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0) + return False + else: + self.count_to_approach = 0 + + distance_to_walk = distance_to_object - distance_to_approach + fb_move, rl_angle = self.calc_footstep(distance_to_walk, self.current_pan, delta_time) + self.set_walking_param(fb_move, 0, rl_angle) + return False + + def decide_object_position(self, x_angle, y_angle): + if self.current_tilt == -10 and self.current_pan == -10: + self.approach_object_position = "NotFound" + return + object_x_angle = self.current_pan + x_angle + self.approach_object_position = "OnLeft" if object_x_angle > 0 else "OnRight" + + def wait_following(self): + self.count_not_found += 1 + if self.count_not_found > self.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.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.SPOT_FB_OFFSET + self.current_walking_param.y_move_amplitude = y_move + self.SPOT_RL_OFFSET + self.current_walking_param.angle_move_amplitude = rotation_angle + self.SPOT_ANGLE_OFFSET + self.set_walking_param_pub.publish(self.current_walking_param) + self.current_x_move = x_move + self.current_r_angle = rotation_angle + + def get_walking_param(self): + try: + response = self.get_walking_param_client() + self.current_walking_param = response.parameters + return True + except rospy.ServiceException as e: + rospy.logerr("Failed to get walking parameters: %s" % e) + return False + + def object_detection_callback(self, msg): + if self.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() + +if __name__ == '__main__': + try: + follower = ObjectFollower() + follower.start_following() + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py new file mode 100644 index 0000000..2d54fba --- /dev/null +++ b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py @@ -0,0 +1,194 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +import math +from sensor_msgs.msg import JointState +from std_msgs.msg import String, Float64MultiArray +from humanoid_robot_intelligence_control_system_ball_detector.msg import CircleSetStamped +from geometry_msgs.msg import Point + +class ObjectTracker: + def __init__(self): + rospy.init_node('object_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_object_pan = 0 + self.current_object_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + self.current_object_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"Object 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.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.object_position = Point() + self.prev_time = rospy.Time.now() + + def object_position_callback(self, msg): + self.object_position = msg + + 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.on_tracking: + self.start_tracking() + else: + self.stop_tracking() + + def start_tracking(self): + self.on_tracking = True + rospy.loginfo("Start Object tracking") if self.DEBUG_PRINT else None + + def stop_tracking(self): + self.go_init() + self.on_tracking = False + rospy.loginfo("Stop Object tracking") if self.DEBUG_PRINT else None + self.current_object_pan = 0 + self.current_object_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.object_position.z = 0 + self.count_not_found = 0 + return "NotFound" + + if self.object_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_object() + 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_object_pan = 0 + self.current_object_tilt = 0 + self.x_error_sum = 0 + self.y_error_sum = 0 + return tracking_status + + x_error = -math.atan(self.object_position.x * math.tan(self.FOV_WIDTH)) + y_error = -math.atan(self.object_position.y * math.tan(self.FOV_HEIGHT)) + object_size = self.object_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_object_pan) / delta_time + y_error_diff = (y_error - self.current_object_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_object_pan = x_error + self.current_object_tilt = y_error + self.current_object_bottom = object_size + + self.object_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_object(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 = ObjectTracker() + rate = rospy.Rate(30) + while not rospy.is_shutdown(): + tracker.process_tracking() + rate.sleep() + except rospy.ROSInterruptException: + pass From b2271c014a38f8180a45942a934c94254622c887 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 12:52:27 -0400 Subject: [PATCH 03/16] latest pushes --- .../launch/face_follower.launch.py | 28 ++-- .../launch/object_follower.launch.py | 30 ++-- .../launch/object_tracker.launch.py | 32 ++-- .../src/face_follower.py | 143 +++++++++++++++--- 4 files changed, 168 insertions(+), 65 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py index 051f56a..917e490 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py @@ -13,9 +13,6 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -#!/usr/bin/env python3 - -#!/usr/bin/env python3 import os import sys @@ -28,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_follow.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follow.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", @@ -41,21 +38,22 @@ def ros1_launch_description(): ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + "humanoid_robot_intelligence_control_system_face_detector", "face_detection_processor.py", "name:=face_detection_processor_node" ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_object_detector", "object_follower.py", "name:=object_follower_node" + "humanoid_robot_intelligence_control_system_configure", "face_follower.py", "name:=face_follower_node" ]) roslaunch_command.extend([ "rviz", "rviz", "name:=rviz", - "args:=-d $(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_follow_visualization.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 = [] @@ -75,17 +73,17 @@ def ros2_launch_description(): )) nodes_to_launch.append(Node( - package='humanoid_robot_intelligence_control_system_object_detector', - executable='object_detection_processor.py', - name='object_detection_processor_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 humanoid_robot_intelligence_control_system_object_detector)/config/object_detection_params.yaml'}] + 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_object_detector', - executable='object_follower.py', - name='object_follower_node', + package='humanoid_robot_intelligence_control_system_configure', + executable='face_follower.py', + name='face_follower_node', output='screen', parameters=[{ 'p_gain': 0.4, @@ -98,7 +96,7 @@ def ros2_launch_description(): package='rviz2', executable='rviz2', name='rviz', - arguments=['-d', '$(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_follow_visualization.rviz'] + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] )) return LaunchDescription(nodes_to_launch) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py index e61d87a..dba557d 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py @@ -13,20 +13,19 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . + import os import sys import subprocess from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration def ros1_launch_description(): # Get command-line arguments args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follow.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", @@ -39,11 +38,11 @@ def ros1_launch_description(): ]) roslaunch_command.extend([ - "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node" + "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node" ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_ball_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + "humanoid_robot_intelligence_control_system_configure", "object_follower.py", "name:=object_follower_node" ]) roslaunch_command.extend([ @@ -54,6 +53,7 @@ def ros1_launch_description(): # Execute the launch command subprocess.call(roslaunch_command) + def ros2_launch_description(): nodes_to_launch = [] @@ -72,14 +72,6 @@ def ros2_launch_description(): }] )) - nodes_to_launch.append(Node( - package='ros_web_api_bellande_2d_computer_vision', - executable='bellande_2d_computer_vision_object_detection.py', - name='object_detection_node', - output='screen', - remappings=[('camera/image_raw', '/usb_cam/image_raw')] - )) - nodes_to_launch.append(Node( package='humanoid_robot_intelligence_control_system_object_detector', executable='object_detection_processor.py', @@ -88,6 +80,18 @@ def ros2_launch_description(): 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', diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py index e61d87a..9e0beef 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py @@ -13,20 +13,19 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . + import os import sys import subprocess from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration def ros1_launch_description(): # Get command-line arguments args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follow.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", @@ -39,11 +38,11 @@ def ros1_launch_description(): ]) roslaunch_command.extend([ - "ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node" + "humanoid_robot_intelligence_control_system_object_tracker", "object_detection_processor.py", "name:=object_detection_processor_node" ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_ball_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + "humanoid_robot_intelligence_control_system_configure", "object_follower.py", "name:=object_follower_node" ]) roslaunch_command.extend([ @@ -54,6 +53,7 @@ def ros1_launch_description(): # Execute the launch command subprocess.call(roslaunch_command) + def ros2_launch_description(): nodes_to_launch = [] @@ -73,21 +73,25 @@ def ros2_launch_description(): )) nodes_to_launch.append(Node( - package='ros_web_api_bellande_2d_computer_vision', - executable='bellande_2d_computer_vision_object_detection.py', - name='object_detection_node', - output='screen', - remappings=[('camera/image_raw', '/usb_cam/image_raw')] - )) - - nodes_to_launch.append(Node( - package='humanoid_robot_intelligence_control_system_object_detector', + 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_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', diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py index 0f11ade..3f923e4 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -15,18 +15,32 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import rospy +import os +import sys import math +from std_msgs.msg import String from sensor_msgs.msg import JointState -from std_msgs.msg import String, Bool 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 class FaceFollower: - def __init__(self): - rospy.init_node('face_follower') - + def __init__(self, ros_version): + self.ros_version = ros_version + 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.initialize_parameters() + self.setup_ros_communication() + + def initialize_parameters(self): self.FOV_WIDTH = 35.2 * math.pi / 180 self.FOV_HEIGHT = 21.6 * math.pi / 180 self.count_not_found = 0 @@ -54,24 +68,37 @@ class FaceFollower: self.accum_period_time = 0.0 self.DEBUG_PRINT = False - 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) + 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_position_sub = rospy.Subscriber( + "/face_detector/face_position", Point, self.face_position_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_position_sub = self.node.create_subscription( + Point, "/face_detector/face_position", self.face_position_callback, 10) + self.prev_time = self.node.get_clock().now() - self.face_position_sub = rospy.Subscriber( - "/face_detector/face_position", Point, self.face_position_callback) - - self.prev_time = rospy.Time.now() self.current_walking_param = WalkingParam() def start_following(self): self.on_following = True - rospy.loginfo("Start Face following") + self.logger.info("Start Face following") self.set_walking_command("start") result = self.get_walking_param() if result: @@ -84,7 +111,7 @@ class FaceFollower: def stop_following(self): self.on_following = False self.count_to_approach = 0 - rospy.loginfo("Stop Face following") + self.logger.info("Stop Face following") self.set_walking_command("stop") def current_joint_states_callback(self, msg): @@ -126,14 +153,18 @@ class FaceFollower: return fb_move, rl_angle def process_following(self, x_angle, y_angle, face_size): - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() + if self.ros_version == '1': + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + else: + curr_time = self.node.get_clock().now() + delta_time = (curr_time - self.prev_time).nanoseconds / 1e9 self.prev_time = curr_time self.count_not_found = 0 if self.current_tilt == -10 and self.current_pan == -10: - rospy.logerr("Failed to get current angle of head joints.") + self.logger.error("Failed to get current angle of head joints.") self.set_walking_command("stop") self.on_following = False self.approach_face_position = "NotFound" @@ -170,4 +201,70 @@ class FaceFollower: self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0, True) msg = String() msg.data = command - self.set_walking_command_pub. + 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.SPOT_FB_OFFSET + self.current_walking_param.y_move_amplitude = y_move + self.SPOT_RL_OFFSET + self.current_walking_param.angle_move_amplitude = rotation_angle + self.SPOT_ANGLE_OFFSET + self.set_walking_param_pub.publish(self.current_walking_param) + self.current_x_move = x_move + self.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 run(self): + if self.ros_version == '1': + rate = rospy.Rate(30) # 30Hz + while not rospy.is_shutdown(): + if self.on_following: + self.process_following(self.current_pan, self.current_tilt, 0.1) + rate.sleep() + else: + while rclpy.ok(): + if self.on_following: + self.process_following(self.current_pan, self.current_tilt, 0.1) + rclpy.spin_once(self.node, timeout_sec=0.03) + +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 + elif ros_version == "2": + import rclpy + from rclpy.node import Node + 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) + + main(ros_version) From d69617ee5a58ed20f525527e9e4b509705764353 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 13:50:25 -0400 Subject: [PATCH 04/16] latest pushes --- .../launch/face_tracker.launch.py | 26 +++++++++---------- .../launch/object_tracker.launch.py | 6 ++--- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py index 91a9896..6d4beeb 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py @@ -13,7 +13,6 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -#!/usr/bin/env python3 import os import sys @@ -26,7 +25,7 @@ def ros1_launch_description(): 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 = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follow.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", @@ -39,21 +38,22 @@ def ros1_launch_description(): ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_object_detector", "object_detection_processor.py", "name:=object_detection_processor_node" + "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", "object_tracker.py", "name:=object_tracker_node" + "humanoid_robot_intelligence_control_system_configure", "face_tracker.py", "name:=face_tracker_node" ]) roslaunch_command.extend([ "rviz", "rviz", "name:=rviz", - "args:=-d $(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_tracking_visualization.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 = [] @@ -73,17 +73,17 @@ def ros2_launch_description(): )) nodes_to_launch.append(Node( - package='humanoid_robot_intelligence_control_system_object_detector', - executable='object_detection_processor.py', - name='object_detection_processor_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 humanoid_robot_intelligence_control_system_object_detector)/config/object_detection_params.yaml'}] + 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_object_detector', - executable='object_tracker.py', - name='object_tracker_node', + package='humanoid_robot_intelligence_control_system_configure', + executable='face_tracker.py', + name='face_tracker_node', output='screen', parameters=[{ 'p_gain': 0.4, @@ -96,7 +96,7 @@ def ros2_launch_description(): package='rviz2', executable='rviz2', name='rviz', - arguments=['-d', '$(find humanoid_robot_intelligence_control_system_object_detector)/rviz/object_tracking_visualization.rviz'] + arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz'] )) return LaunchDescription(nodes_to_launch) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py index 9e0beef..892e0cc 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py @@ -42,7 +42,7 @@ def ros1_launch_description(): ]) roslaunch_command.extend([ - "humanoid_robot_intelligence_control_system_configure", "object_follower.py", "name:=object_follower_node" + "humanoid_robot_intelligence_control_system_configure", "object_tracker.py", "name:=object_tracker_node" ]) roslaunch_command.extend([ @@ -82,8 +82,8 @@ def ros2_launch_description(): nodes_to_launch.append(Node( package='humanoid_robot_intelligence_control_system_configure', - executable='object_follower.py', - name='object_follower_node', + executable='object_tracker.py', + name='object_tracker_node', output='screen', parameters=[{ 'p_gain': 0.4, From 4a9833b8f1ec12869cd11eb169e859f98dfdeacf Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 14:06:35 -0400 Subject: [PATCH 05/16] latest pushes --- .../launch/face_tracker.launch.py | 2 +- .../launch/object_follower.launch.py | 2 +- .../launch/object_tracker.launch.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py index 6d4beeb..d7d94dc 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_tracker.launch.py @@ -25,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follow.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_tracker.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py index dba557d..2788682 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_follower.launch.py @@ -25,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follow.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follower.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", diff --git a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py index 892e0cc..49307ae 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/object_tracker.launch.py @@ -25,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_follow.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "object_tracker.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", From e4f3617b86fc78d94dc077b7e5bfb9141c6d4a3d Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 14:19:45 -0400 Subject: [PATCH 06/16] latest pushes --- README.md | 33 ++++++++----------- .../launch/face_follower.launch.py | 2 +- 2 files changed, 14 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 3ee5208..8da2494 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos +# Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Architecture [![Website](https://img.shields.io/badge/Visit%20our-Website-0099cc?style=for-the-badge)](https://robotics-sensors.github.io) [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) @@ -6,7 +6,7 @@ ## 🤖 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,35 +14,28 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System Demos reposit ### 🚀 Key Repository Stats -- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Starred by the community -- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Forked by enthusiasts -- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg?style=social) Watched by curious minds +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Watched by curious minds -- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Actively addressing issues -- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Welcoming contributions -- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Licensed under Apache 2.0 +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Licensed under Apache 2.0 -- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos.svg) Latest updates -- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/traffic) Engaging with the community +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/traffic) Engaging with the community --- ## 🌐 Repository Website -Explore our [repository website](https://robotics-sensors.github.io/bellande_humanoid_robot_intelligence_control_system_demos) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. - ---- - -### 🔄 Updates and Versions - -- **Updated Version:** [humanoid_robot_intelligence_control_system_module](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_demos) -- **Old Version/Previous Used for Different Context:** [ROBOTIS-OP3-Demo](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo) +Explore our [repository website](https://robotics-sensors.github.io/bellande_humanoid_robot_intelligence_control_system_architecture) for detailed documentation, tutorials, and additional information about our humanoid robot intelligence control system. --- # 🎉 Latest Release -[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_demos/releases/) +[![Latest Release](https://img.shields.io/github/v/release/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_tools?style=for-the-badge&color=yellow)](https://github.com/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture/releases/) --- @@ -62,4 +55,4 @@ Meet our dedicated maintainer, **Ronaldson Bellande**. ## 📄 License -This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_demos/blob/main/LICENSE). +This SDK is distributed under the [Apache License, Version 2.0](https://www.apache.org/licenses/LICENSE-2.0). For detailed information, refer to [LICENSE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/blob/main/LICENSE) and [NOTICE](https://github.com/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/blob/main/LICENSE). diff --git a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py index 917e490..44a7599 100644 --- a/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py +++ b/humanoid_robot_intelligence_control_system_configure/launch/face_follower.launch.py @@ -25,7 +25,7 @@ def ros1_launch_description(): args = sys.argv[1:] # Construct the ROS 1 launch command - roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follow.launch"] + args + roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_configure", "face_follower.launch"] + args roslaunch_command.extend([ "usb_cam", "usb_cam_node", "name:=camera", From dce855f5153b32f60269b30e68e5221407f91bb1 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 23 Jul 2024 14:22:49 -0400 Subject: [PATCH 07/16] Update README.md --- README.md | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/README.md b/README.md index 8da2494..2f508a1 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,17 @@ [![Discord](https://img.shields.io/badge/Join%20our-Discord-7289DA?logo=discord&style=for-the-badge)](https://discord.gg/Yc72nd4w) [![Sponsor](https://img.shields.io/badge/Sponsor-Robotics%20Sensors%20Research-red?style=for-the-badge&logo=github)](https://github.com/sponsors/Robotics-Sensors) +## Mobile and ROS Control System +- [![Bellande's Internal Sensor Mobile iOS API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_ios_api) +- [![Bellande's External Sensor Mobile iOS API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_ios_api) +- [![Bellande's Functionality Mobile iOS API](https://img.shields.io/badge/Bellande's%20Functionality%20Mobile%20iOS%20API-Bellande%20API-blue?style=for-the-badge&logo=swift&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_mobile_ios_api) +- [![Bellande's Internal Sensor Mobile Android API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_mobile_android_api) +- [![Bellande's External Sensor Mobile Android API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_mobile_android_api) +- [![Bellande's Functionality Mobile Android API](https://img.shields.io/badge/Bellande's%20Functionality%20Mobile%20Android%20API-Bellande%20API-blue?style=for-the-badge&logo=android&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_mobile_android_api) +- [![Bellande's Internal Sensor Web API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_web_api) +- [![Bellande's External Sensor Web API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_web_api) +- [![Bellande's Functionality Web API](https://img.shields.io/badge/Bellande's%20Functionality%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_web_api) + ## 🤖 Explore Humanoid Robot Intelligence with Us! 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. From 34ceaf0e1395866290a8e477339913a14c547fcd Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 23 Jul 2024 14:26:19 -0400 Subject: [PATCH 08/16] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 2f508a1..b3b5c8e 100644 --- a/README.md +++ b/README.md @@ -58,7 +58,7 @@ The repository has recently undergone significant updates. Older commits and cod 🚀 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**. From acd84ed2d995aed78e330cd1265a116bcf38db09 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 23 Jul 2024 14:37:10 -0400 Subject: [PATCH 09/16] Update README.md --- README.md | 6 ------ 1 file changed, 6 deletions(-) diff --git a/README.md b/README.md index b3b5c8e..c214325 100644 --- a/README.md +++ b/README.md @@ -50,12 +50,6 @@ Explore our [repository website](https://robotics-sensors.github.io/bellande_hum --- -## 📢 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. - ---- - 🚀 For the latest versions and updates, visit our organization: [Robotics-Sensors](https://github.com/Robotics-Sensors). ### 🧑‍💼 Maintainer & Author From 04676b74b1af27e5bc9ac10b6cd3fe1a7b694bb4 Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Tue, 23 Jul 2024 14:38:20 -0400 Subject: [PATCH 10/16] Update README.md --- README.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index c214325..412d3e9 100644 --- a/README.md +++ b/README.md @@ -25,16 +25,16 @@ Welcome to the {BR&SRI} Humanoid Robot Intelligence Control System architecture ### 🚀 Key Repository Stats -- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Starred by the community -- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Forked by enthusiasts -- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg?style=social) Watched by curious minds +- ![GitHub stars](https://img.shields.io/github/stars/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Starred by the community +- ![GitHub forks](https://img.shields.io/github/forks/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Forked by enthusiasts +- ![GitHub watchers](https://img.shields.io/github/watchers/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg?style=social) Watched by curious minds -- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Actively addressing issues -- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Welcoming contributions -- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Licensed under Apache 2.0 +- ![GitHub issues](https://img.shields.io/github/issues/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Actively addressing issues +- ![GitHub pull requests](https://img.shields.io/github/issues-pr/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Welcoming contributions +- ![GitHub license](https://img.shields.io/github/license/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Licensed under Apache 2.0 -- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture.svg) Latest updates -- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/humanoid_robot_intelligence_control_system_architecture/traffic) Engaging with the community +- ![GitHub last commit](https://img.shields.io/github/last-commit/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture.svg) Latest updates +- ![Visitor & Github Clones](https://img.shields.io/badge/dynamic/json?color=2e8b57&label=Visitor%20%26%20GitHub%20Clones&query=$.count&url=https://api.github.com/repos/Robotics-Sensors/bellande_humanoid_robot_intelligence_control_system_architecture/traffic) Engaging with the community --- From 6774280085d7d527420f82065b70e2eb91ea25b2 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 19:44:16 -0400 Subject: [PATCH 11/16] latest pushes --- .../src/face_follower.py | 26 +++- .../src/face_tracker.py | 132 +++++++++++++----- 2 files changed, 115 insertions(+), 43 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py index 3f923e4..40fd0da 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -24,6 +24,7 @@ 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 + class FaceFollower: def __init__(self, ros_version): self.ros_version = ros_version @@ -40,6 +41,7 @@ class FaceFollower: self.initialize_parameters() self.setup_ros_communication() + def initialize_parameters(self): self.FOV_WIDTH = 35.2 * math.pi / 180 self.FOV_HEIGHT = 21.6 * math.pi / 180 @@ -233,7 +235,7 @@ class FaceFollower: def run(self): if self.ros_version == '1': - rate = rospy.Rate(30) # 30Hz + rate = rospy.Rate(30) while not rospy.is_shutdown(): if self.on_following: self.process_following(self.current_pan, self.current_tilt, 0.1) @@ -244,6 +246,7 @@ class FaceFollower: self.process_following(self.current_pan, self.current_tilt, 0.1) rclpy.spin_once(self.node, timeout_sec=0.03) + def main(ros_version): try: follower = FaceFollower(ros_version) @@ -256,15 +259,26 @@ def main(ros_version): rclpy.shutdown() print(f"An error occurred: {e}") + + if __name__ == '__main__': ros_version = os.getenv("ROS_VERSION") if ros_version == "1": - import rospy + try: + import rospy + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": - import rclpy - from rclpy.node import Node + try: + import rclpy + from rclpy.node import Node + main(ros_version) + except KeyboardInterrupt: + rclpy.shutdown() + print("Error in ROS1 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) - - main(ros_version) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py index 046f5b0..138d645 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py @@ -15,17 +15,31 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . - -import rospy +import os +import sys import math from sensor_msgs.msg import JointState from std_msgs.msg import String from geometry_msgs.msg import Point class FaceTracker: - def __init__(self): - rospy.init_node('face_tracker') - + def __init__(self, ros_version): + self.ros_version = ros_version + if self.ros_version == '1': + rospy.init_node('face_tracker') + self.get_param = rospy.get_param + self.logger = rospy + else: + rclpy.init() + self.node = Node('face_tracker') + self.get_param = self.node.get_parameter + self.logger = self.node.get_logger() + + self.initialize_parameters() + self.setup_ros_communication() + + + def initialize_parameters(self): self.FOV_WIDTH = 35.2 * math.pi / 180 self.FOV_HEIGHT = 21.6 * math.pi / 180 self.NOT_FOUND_THRESHOLD = 50 @@ -39,25 +53,36 @@ class FaceTracker: self.y_error_sum = 0 self.tracking_status = "NotFound" self.DEBUG_PRINT = False + + self.p_gain = self.get_param('p_gain', 0.4) + self.i_gain = self.get_param('i_gain', 0.0) + self.d_gain = self.get_param('d_gain', 0.0) - self.p_gain = rospy.get_param('~p_gain', 0.4) - self.i_gain = rospy.get_param('~i_gain', 0.0) - self.d_gain = rospy.get_param('~d_gain', 0.0) + self.logger.info(f"Face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") - rospy.loginfo(f"Face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") - - self.head_joint_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=1) - self.head_scan_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=1) - - self.face_position_sub = rospy.Subscriber( - "/face_detector/face_position", Point, self.face_position_callback) - self.face_tracking_command_sub = rospy.Subscriber( - "/face_tracker/command", String, self.face_tracker_command_callback) + def setup_ros_communication(self): + if self.ros_version == '1': + self.head_joint_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=1) + self.head_scan_pub = rospy.Publisher( + "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=1) + self.face_position_sub = rospy.Subscriber( + "/face_detector/face_position", Point, self.face_position_callback) + self.face_tracking_command_sub = rospy.Subscriber( + "/face_tracker/command", String, self.face_tracker_command_callback) + self.prev_time = rospy.Time.now() + else: + self.head_joint_pub = self.node.create_publisher( + JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 1) + self.head_scan_pub = self.node.create_publisher( + String, "/humanoid_robot_intelligence_control_system/head_control/scan_command", 1) + self.face_position_sub = self.node.create_subscription( + Point, "/face_detector/face_position", self.face_position_callback, 10) + self.face_tracking_command_sub = self.node.create_subscription( + String, "/face_tracker/command", self.face_tracker_command_callback, 10) + self.prev_time = self.node.get_clock().now() self.face_position = Point() - self.prev_time = rospy.Time.now() def face_position_callback(self, msg): self.face_position = msg @@ -75,12 +100,12 @@ class FaceTracker: def start_tracking(self): self.on_tracking = True - rospy.loginfo("Start Face tracking") + self.logger.info("Start Face tracking") def stop_tracking(self): self.go_init() self.on_tracking = False - rospy.loginfo("Stop Face tracking") + self.logger.info("Stop Face tracking") self.current_face_pan = 0 self.current_face_tilt = 0 self.x_error_sum = 0 @@ -113,8 +138,12 @@ class FaceTracker: 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)) - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() + if self.ros_version == '1': + curr_time = rospy.Time.now() + delta_time = (curr_time - self.prev_time).to_sec() + else: + curr_time = self.node.get_clock().now() + delta_time = (curr_time - self.prev_time).nanoseconds / 1e9 self.prev_time = curr_time x_error_diff = (x_error - self.current_face_pan) / delta_time @@ -125,10 +154,10 @@ class FaceTracker: 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.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.x_error_sum * 180 / math.pi} | {self.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) @@ -166,14 +195,43 @@ class FaceTracker: self.head_scan_pub.publish(scan_msg) def run(self): - rate = rospy.Rate(30) # 30Hz - while not rospy.is_shutdown(): - self.process_tracking() - rate.sleep() + if self.ros_version == '1': + rate = rospy.Rate(30) # 30Hz + while not rospy.is_shutdown(): + self.process_tracking() + rate.sleep() + else: + while rclpy.ok(): + self.process_tracking() + rclpy.spin_once(self.node, timeout_sec=0.03) # Approximately 30Hz + +def main(ros_version): + try: + tracker = FaceTracker(ros_version) + 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__': - try: - tracker = FaceTracker() - tracker.run() - except rospy.ROSInterruptException: - pass + ros_version = os.getenv("ROS_VERSION") + if ros_version == "1": + try: + import rospy + main(ros_version) + except rospy.ROSInterruptException: + print("Error in ROS1 main") + elif ros_version == "2": + try: + import rclpy + from rclpy.node import Node + 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) From 9f051b74d66f8c8da8421af626da3ac6a76112ab Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Tue, 23 Jul 2024 22:34:35 -0400 Subject: [PATCH 12/16] latest pushes --- .../src/face_follower.py | 205 +++++-------- .../src/object_follower.py | 269 ++++++++++-------- 2 files changed, 221 insertions(+), 253 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py index 40fd0da..e6d4b4f 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -18,16 +18,24 @@ import os import sys import math -from std_msgs.msg import String 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_face, 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 @@ -37,38 +45,9 @@ class FaceFollower: self.node = rclpy.create_node('face_follower') self.get_param = self.node.get_parameter self.logger = self.node.get_logger() - - self.initialize_parameters() - self.setup_ros_communication() - - - def initialize_parameters(self): - self.FOV_WIDTH = 35.2 * math.pi / 180 - self.FOV_HEIGHT = 21.6 * math.pi / 180 - self.count_not_found = 0 - self.count_to_approach = 0 - self.on_following = False - self.approach_face_position = "NotFound" - 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.current_pan = -10 - self.current_tilt = -10 - self.current_x_move = 0.005 - self.current_r_angle = 0 - self.curr_period_time = 0.6 - self.accum_period_time = 0.0 - self.DEBUG_PRINT = False + + 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': @@ -80,8 +59,8 @@ class FaceFollower: "/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_position_sub = rospy.Subscriber( - "/face_detector/face_position", Point, self.face_position_callback) + 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( @@ -92,127 +71,103 @@ class FaceFollower: 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_position_sub = self.node.create_subscription( - Point, "/face_detector/face_position", self.face_position_callback, 10) + 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() - self.current_walking_param = WalkingParam() - def start_following(self): - self.on_following = True - self.logger.info("Start Face following") + 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.hip_pitch_offset = self.current_walking_param.hip_pitch_offset - self.curr_period_time = self.current_walking_param.period_time + 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.hip_pitch_offset = 7.0 * math.pi / 180 - self.curr_period_time = 0.6 + self.config.hip_pitch_offset = 7.0 * math.pi / 180 + self.config.curr_period_time = 0.6 def stop_following(self): - self.on_following = False - self.count_to_approach = 0 - self.logger.info("Stop Face following") + 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.current_pan = msg.position[i] + self.initialize_config.current_pan = msg.position[i] elif name == "head_tilt": - self.current_tilt = msg.position[i] - - def face_position_callback(self, msg): - if self.on_following: - self.process_following(msg.x, msg.y, msg.z) - - def calc_footstep(self, target_distance, target_angle, delta_time): - next_movement = self.current_x_move - target_distance = max(0, target_distance) - fb_goal = min(target_distance * 0.1, self.MAX_FB_STEP) - self.accum_period_time += delta_time - if self.accum_period_time > (self.curr_period_time / 4): - self.accum_period_time = 0.0 - if (target_distance * 0.1 / 2) < self.current_x_move: - next_movement -= self.UNIT_FB_STEP - else: - next_movement += self.UNIT_FB_STEP - fb_goal = min(next_movement, fb_goal) - fb_move = max(fb_goal, self.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, self.MAX_RL_TURN) - rl_goal = max(rl_goal, self.MIN_RL_TURN) - rl_angle = min(abs(self.current_r_angle) + self.UNIT_RL_TURN, rl_goal) - if target_angle < 0: - rl_angle *= -1 - else: - rl_angle = 0 - - return fb_move, rl_angle + self.initialize_config.current_tilt = msg.position[i] def process_following(self, x_angle, y_angle, face_size): - if self.ros_version == '1': - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() - else: - curr_time = self.node.get_clock().now() - delta_time = (curr_time - self.prev_time).nanoseconds / 1e9 + 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.count_not_found = 0 + self.initialize_config.count_not_found = 0 - if self.current_tilt == -10 and self.current_pan == -10: + 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.on_following = False - self.approach_face_position = "NotFound" + self.initialize_config.on_tracking = False + self.initialize_config.approach_face_position = "NotFound" return False - self.approach_face_position = "OutOfRange" + self.initialize_config.approach_face_position = "OutOfRange" - distance_to_face = self.CAMERA_HEIGHT * math.tan(math.pi * 0.5 + self.current_tilt - self.hip_pitch_offset - face_size) - distance_to_face = abs(distance_to_face) + distance_to_face = calculate_distance_to_face( + self.config.CAMERA_HEIGHT, self.initialize_config.current_tilt, self.config.hip_pitch_offset, face_size) - distance_to_approach = 0.5 # Adjust this value as needed + distance_to_approach = 0.22 if (distance_to_face < distance_to_approach) and (abs(x_angle) < 25.0): - self.count_to_approach += 1 - if self.count_to_approach > 20: + self.initialize_config.count_to_approach += 1 + if self.initialize_config.count_to_approach > 20: self.set_walking_command("stop") - self.on_following = False - self.approach_face_position = "OnLeft" if x_angle > 0 else "OnRight" + self.initialize_config.on_tracking = False + self.initialize_config.approach_face_position = "OnLeft" if x_angle > 0 else "OnRight" return True - elif self.count_to_approach > 15: - self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0) + elif self.initialize_config.count_to_approach > 15: + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0) return False else: - self.count_to_approach = 0 + self.initialize_config.count_to_approach = 0 distance_to_walk = distance_to_face - distance_to_approach - fb_move, rl_angle = self.calc_footstep(distance_to_walk, self.current_pan, delta_time) + 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.IN_PLACE_FB_STEP, 0, 0, True) + 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.SPOT_FB_OFFSET - self.current_walking_param.y_move_amplitude = y_move + self.SPOT_RL_OFFSET - self.current_walking_param.angle_move_amplitude = rotation_angle + self.SPOT_ANGLE_OFFSET + 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.current_x_move = x_move - self.current_r_angle = rotation_angle + 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': @@ -233,19 +188,20 @@ class FaceFollower: 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': - rate = rospy.Rate(30) - while not rospy.is_shutdown(): - if self.on_following: - self.process_following(self.current_pan, self.current_tilt, 0.1) - rate.sleep() + rospy.spin() else: - while rclpy.ok(): - if self.on_following: - self.process_following(self.current_pan, self.current_tilt, 0.1) - rclpy.spin_once(self.node, timeout_sec=0.03) - + rclpy.spin(self.node) def main(ros_version): try: @@ -259,26 +215,21 @@ def main(ros_version): rclpy.shutdown() print(f"An error occurred: {e}") - - if __name__ == '__main__': ros_version = os.getenv("ROS_VERSION") if ros_version == "1": + import rospy try: - import rospy main(ros_version) except rospy.ROSInterruptException: print("Error in ROS1 main") - elif ros_version == "2": + import rclpy try: - import rclpy - from rclpy.node import Node main(ros_version) except KeyboardInterrupt: rclpy.shutdown() - print("Error in ROS1 main") - + print("Error in ROS2 main") else: print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.") sys.exit(1) diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py index 87c320a..5377ce5 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py @@ -15,193 +15,181 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import rospy +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 geometry_msgs.msg import Point +from humanoid_robot_intelligence_control_system_detection.follower import calculate_distance_to_object, calculate_footstep, calculate_delta_time +from humanoid_robot_intelligence_control_system_detection.follower_config import FollowerConfig, FollowerInitializeConfig class ObjectFollower: - def __init__(self): - rospy.init_node('object_follower') + 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.FOV_WIDTH = 35.2 * math.pi / 180 - self.FOV_HEIGHT = 21.6 * math.pi / 180 - self.count_not_found = 0 - self.count_to_approach = 0 - self.on_tracking = False - self.approach_object_position = "NotFound" - 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.current_pan = -10 - self.current_tilt = -10 - self.current_x_move = 0.005 - self.current_r_angle = 0 - self.curr_period_time = 0.6 - self.accum_period_time = 0.0 - self.DEBUG_PRINT = False + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) - 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) - - # Subscribe to object detection results - self.object_detection_sub = rospy.Subscriber( - "/object_detection_result", Point, self.object_detection_callback) - - self.prev_time = rospy.Time.now() - self.current_walking_param = WalkingParam() + 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.on_tracking = True - rospy.loginfo("Start Object following") + 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.hip_pitch_offset = self.current_walking_param.hip_pitch_offset - self.curr_period_time = self.current_walking_param.period_time + 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.hip_pitch_offset = 7.0 * math.pi / 180 - self.curr_period_time = 0.6 + self.config.hip_pitch_offset = 7.0 * math.pi / 180 + self.config.curr_period_time = 0.6 def stop_following(self): - self.on_tracking = False - self.count_to_approach = 0 - rospy.loginfo("Stop Object following") + 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.current_pan = msg.position[i] + self.initialize_config.current_pan = msg.position[i] elif name == "head_tilt": - self.current_tilt = msg.position[i] - - def calc_footstep(self, target_distance, target_angle, delta_time): - next_movement = self.current_x_move - target_distance = max(0, target_distance) - fb_goal = min(target_distance * 0.1, self.MAX_FB_STEP) - self.accum_period_time += delta_time - if self.accum_period_time > (self.curr_period_time / 4): - self.accum_period_time = 0.0 - if (target_distance * 0.1 / 2) < self.current_x_move: - next_movement -= self.UNIT_FB_STEP - else: - next_movement += self.UNIT_FB_STEP - fb_goal = min(next_movement, fb_goal) - fb_move = max(fb_goal, self.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, self.MAX_RL_TURN) - rl_goal = max(rl_goal, self.MIN_RL_TURN) - rl_angle = min(abs(self.current_r_angle) + self.UNIT_RL_TURN, rl_goal) - if target_angle < 0: - rl_angle *= -1 - else: - rl_angle = 0 - - return fb_move, rl_angle + self.initialize_config.current_tilt = msg.position[i] def process_following(self, x_angle, y_angle, object_size): - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() + 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.count_not_found = 0 + self.initialize_config.count_not_found = 0 - if self.current_tilt == -10 and self.current_pan == -10: - rospy.logerr("Failed to get current angle of head joints.") + 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.on_tracking = False - self.approach_object_position = "NotFound" + self.initialize_config.on_tracking = False + self.initialize_config.approach_object_position = "NotFound" return False - self.approach_object_position = "OutOfRange" + self.initialize_config.approach_object_position = "OutOfRange" - distance_to_object = self.CAMERA_HEIGHT * math.tan(math.pi * 0.5 + self.current_tilt - self.hip_pitch_offset - object_size) - distance_to_object = abs(distance_to_object) + distance_to_object = calculate_distance_to_object( + 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.count_to_approach += 1 - if self.count_to_approach > 20: + self.initialize_config.count_to_approach += 1 + if self.initialize_config.count_to_approach > 20: self.set_walking_command("stop") - self.on_tracking = False - self.approach_object_position = "OnLeft" if x_angle > 0 else "OnRight" + self.initialize_config.on_tracking = False + self.initialize_config.approach_object_position = "OnLeft" if x_angle > 0 else "OnRight" return True - elif self.count_to_approach > 15: - self.set_walking_param(self.IN_PLACE_FB_STEP, 0, 0) + elif self.initialize_config.count_to_approach > 15: + self.set_walking_param(self.config.IN_PLACE_FB_STEP, 0, 0) return False else: - self.count_to_approach = 0 + self.initialize_config.count_to_approach = 0 distance_to_walk = distance_to_object - distance_to_approach - fb_move, rl_angle = self.calc_footstep(distance_to_walk, self.current_pan, delta_time) + 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.current_tilt == -10 and self.current_pan == -10: - self.approach_object_position = "NotFound" + 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.current_pan + x_angle - self.approach_object_position = "OnLeft" if object_x_angle > 0 else "OnRight" + 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.count_not_found += 1 - if self.count_not_found > self.NOT_FOUND_THRESHOLD * 0.5: + 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.IN_PLACE_FB_STEP, 0, 0, True) + 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.SPOT_FB_OFFSET - self.current_walking_param.y_move_amplitude = y_move + self.SPOT_RL_OFFSET - self.current_walking_param.angle_move_amplitude = rotation_angle + self.SPOT_ANGLE_OFFSET + 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.current_x_move = x_move - self.current_r_angle = rotation_angle + self.initialize_config.current_x_move = x_move + self.initialize_config.current_r_angle = rotation_angle def get_walking_param(self): - try: - response = self.get_walking_param_client() - self.current_walking_param = response.parameters - return True - except rospy.ServiceException as e: - rospy.logerr("Failed to get walking parameters: %s" % e) - return False + 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.on_tracking: + if self.initialize_config.on_tracking: x_angle = msg.x y_angle = msg.y object_size = msg.z @@ -209,10 +197,39 @@ class ObjectFollower: else: self.wait_following() -if __name__ == '__main__': + def run(self): + if self.ros_version == '1': + rospy.spin() + else: + rclpy.spin(self.node) + +def main(ros_version): try: - follower = ObjectFollower() + follower = ObjectFollower(ros_version) follower.start_following() - rospy.spin() - except rospy.ROSInterruptException: - pass + 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) From 7e61eae008dc936f954772c26ee959c02ad4a1b2 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 24 Jul 2024 00:05:59 -0400 Subject: [PATCH 13/16] latest pushes --- .../src/face_follower.py | 5 +- .../src/face_tracker.py | 157 +++++------- .../src/object_follower.py | 5 +- .../src/object_tracker.py | 233 ++++++++++-------- 4 files changed, 200 insertions(+), 200 deletions(-) diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py index e6d4b4f..f12b5a4 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_follower.py @@ -24,9 +24,10 @@ 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_face, calculate_footstep, calculate_delta_time +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 @@ -116,7 +117,7 @@ class FaceFollower: self.initialize_config.approach_face_position = "OutOfRange" - distance_to_face = calculate_distance_to_face( + 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 diff --git a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py index 138d645..a7776ab 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py +++ b/humanoid_robot_intelligence_control_system_configure/src/face_tracker.py @@ -15,31 +15,17 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import os -import sys +import rospy import math from sensor_msgs.msg import JointState -from std_msgs.msg import String +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, ros_version): - self.ros_version = ros_version - if self.ros_version == '1': - rospy.init_node('face_tracker') - self.get_param = rospy.get_param - self.logger = rospy - else: - rclpy.init() - self.node = Node('face_tracker') - self.get_param = self.node.get_parameter - self.logger = self.node.get_logger() - - self.initialize_parameters() - self.setup_ros_communication() - - - def initialize_parameters(self): + 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 @@ -51,38 +37,31 @@ class FaceTracker: 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 - - self.p_gain = self.get_param('p_gain', 0.4) - self.i_gain = self.get_param('i_gain', 0.0) - self.d_gain = self.get_param('d_gain', 0.0) - self.logger.info(f"Face tracking Gain : {self.p_gain}, {self.i_gain}, {self.d_gain}") + 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) - def setup_ros_communication(self): - if self.ros_version == '1': - self.head_joint_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", JointState, queue_size=1) - self.head_scan_pub = rospy.Publisher( - "/humanoid_robot_intelligence_control_system/head_control/scan_command", String, queue_size=1) - self.face_position_sub = rospy.Subscriber( - "/face_detector/face_position", Point, self.face_position_callback) - self.face_tracking_command_sub = rospy.Subscriber( - "/face_tracker/command", String, self.face_tracker_command_callback) - self.prev_time = rospy.Time.now() - else: - self.head_joint_pub = self.node.create_publisher( - JointState, "/humanoid_robot_intelligence_control_system/head_control/set_joint_states", 1) - self.head_scan_pub = self.node.create_publisher( - String, "/humanoid_robot_intelligence_control_system/head_control/scan_command", 1) - self.face_position_sub = self.node.create_subscription( - Point, "/face_detector/face_position", self.face_position_callback, 10) - self.face_tracking_command_sub = self.node.create_subscription( - String, "/face_tracker/command", self.face_tracker_command_callback, 10) - self.prev_time = self.node.get_clock().now() + 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 @@ -100,17 +79,20 @@ class FaceTracker: def start_tracking(self): self.on_tracking = True - self.logger.info("Start Face tracking") + rospy.loginfo("Start face tracking") if self.DEBUG_PRINT else None def stop_tracking(self): self.go_init() self.on_tracking = False - self.logger.info("Stop Face tracking") + 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 @@ -120,7 +102,10 @@ class FaceTracker: if self.face_position.z <= 0: self.count_not_found += 1 if self.count_not_found < self.WAITING_THRESHOLD: - tracking_status = "Waiting" + 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 @@ -133,17 +118,18 @@ class FaceTracker: 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 - if self.ros_version == '1': - curr_time = rospy.Time.now() - delta_time = (curr_time - self.prev_time).to_sec() - else: - curr_time = self.node.get_clock().now() - delta_time = (curr_time - self.prev_time).nanoseconds / 1e9 + 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 @@ -154,15 +140,18 @@ class FaceTracker: 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: - 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.x_error_sum * 180 / math.pi} | {self.y_error_sum * 180 / math.pi}") - self.logger.info(f"Error target: {x_error_target * 180 / math.pi} | {y_error_target * 180 / math.pi}") + 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 @@ -176,7 +165,7 @@ class FaceTracker: head_angle_msg.name = ["head_pan", "head_tilt"] head_angle_msg.position = [pan, tilt] - self.head_joint_pub.publish(head_angle_msg) + self.head_joint_offset_pub.publish(head_angle_msg) def go_init(self): head_angle_msg = JointState() @@ -194,44 +183,12 @@ class FaceTracker: self.head_scan_pub.publish(scan_msg) - def run(self): - if self.ros_version == '1': - rate = rospy.Rate(30) # 30Hz - while not rospy.is_shutdown(): - self.process_tracking() - rate.sleep() - else: - while rclpy.ok(): - self.process_tracking() - rclpy.spin_once(self.node, timeout_sec=0.03) # Approximately 30Hz - -def main(ros_version): - try: - tracker = FaceTracker(ros_version) - 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": - try: - import rospy - main(ros_version) - except rospy.ROSInterruptException: - print("Error in ROS1 main") - elif ros_version == "2": - try: - import rclpy - from rclpy.node import Node - 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) + try: + tracker = FaceTracker() + rate = rospy.Rate(30) + while not rospy.is_shutdown(): + tracker.process_tracking() + rate.sleep() + except rospy.ROSInterruptException: + pass diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py index 5377ce5..ebe736f 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/object_follower.py +++ b/humanoid_robot_intelligence_control_system_configure/src/object_follower.py @@ -24,9 +24,10 @@ 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_object, calculate_footstep, calculate_delta_time +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 @@ -116,7 +117,7 @@ class ObjectFollower: self.initialize_config.approach_object_position = "OutOfRange" - distance_to_object = calculate_distance_to_object( + 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 diff --git a/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py index 2d54fba..4f00592 100644 --- a/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py +++ b/humanoid_robot_intelligence_control_system_configure/src/object_tracker.py @@ -15,56 +15,68 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -import rospy +import os +import sys 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 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): - rospy.init_node('object_tracker') + 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.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_object_pan = 0 - self.current_object_tilt = 0 - self.x_error_sum = 0 - self.y_error_sum = 0 - self.current_object_bottom = 0 - self.tracking_status = "NotFound" - self.DEBUG_PRINT = False + self.config.update_from_params(self.get_param) + self.initialize_config.update_from_params(self.get_param) - 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"Object 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.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.object_position = Point() - self.prev_time = rospy.Time.now() + 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.object_position = 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": @@ -72,88 +84,90 @@ class ObjectTracker: elif msg.data == "stop": self.stop_tracking() elif msg.data == "toggle_start": - if not self.on_tracking: + if not self.initialize_config.on_tracking: self.start_tracking() else: self.stop_tracking() def start_tracking(self): - self.on_tracking = True - rospy.loginfo("Start Object tracking") if self.DEBUG_PRINT else None + self.initialize_config.on_tracking = True + self.logger.info("Start Object tracking") def stop_tracking(self): self.go_init() - self.on_tracking = False - rospy.loginfo("Stop Object tracking") if self.DEBUG_PRINT else None - self.current_object_pan = 0 - self.current_object_tilt = 0 - self.x_error_sum = 0 - self.y_error_sum = 0 + 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.use_head_scan = use_scan + self.config.use_head_scan = use_scan def process_tracking(self): - if not self.on_tracking: - self.object_position.z = 0 - self.count_not_found = 0 + 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.object_position.z <= 0: - self.count_not_found += 1 - if self.count_not_found < self.WAITING_THRESHOLD: - if self.tracking_status in ["Found", "Waiting"]: + 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.count_not_found > self.NOT_FOUND_THRESHOLD: + elif self.initialize_config.count_not_found > self.config.NOT_FOUND_THRESHOLD: self.scan_object() - self.count_not_found = 0 + self.initialize_config.count_not_found = 0 tracking_status = "NotFound" else: tracking_status = "NotFound" else: - self.count_not_found = 0 + self.initialize_config.count_not_found = 0 tracking_status = "Found" if tracking_status != "Found": - self.tracking_status = tracking_status - self.current_object_pan = 0 - self.current_object_tilt = 0 - self.x_error_sum = 0 - self.y_error_sum = 0 + 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 = -math.atan(self.object_position.x * math.tan(self.FOV_WIDTH)) - y_error = -math.atan(self.object_position.y * math.tan(self.FOV_HEIGHT)) - object_size = self.object_position.z + 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() - delta_time = (curr_time - self.prev_time).to_sec() + 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.current_object_pan) / delta_time - y_error_diff = (y_error - self.current_object_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 + 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.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}") + 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.current_object_pan = x_error - self.current_object_tilt = y_error - self.current_object_bottom = object_size + 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.object_position.z = 0 + self.initialize_config.object_position.z = 0 - self.tracking_status = tracking_status + self.initialize_config.tracking_status = tracking_status return tracking_status def publish_head_joint(self, pan, tilt): @@ -175,7 +189,7 @@ class ObjectTracker: self.head_joint_pub.publish(head_angle_msg) def scan_object(self): - if not self.use_head_scan: + if not self.config.use_head_scan: return scan_msg = String() @@ -183,12 +197,39 @@ class ObjectTracker: self.head_scan_pub.publish(scan_msg) -if __name__ == '__main__': + def run(self): + if self.ros_version == '1': + rospy.spin() + else: + rclpy.spin(self.node) + +def main(ros_version): try: - tracker = ObjectTracker() - rate = rospy.Rate(30) - while not rospy.is_shutdown(): - tracker.process_tracking() - rate.sleep() - except rospy.ROSInterruptException: - pass + 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) From 52d1a300e112f9c2a1f98429e0ffeb1f57a8d15b Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 24 Jul 2024 01:51:43 -0400 Subject: [PATCH 14/16] latest pushes --- .../CMakeLists.txt | 66 ++++++++ .../package.xml | 46 +++++ .../setup.py | 26 +++ .../src/follower.py | 52 ++++++ .../src/follower_config.py | 159 ++++++++++++++++++ .../src/tracker.py | 30 ++++ .../src/tracker_config.py | 126 ++++++++++++++ 7 files changed, 505 insertions(+) create mode 100644 humanoid_robot_intelligence_control_system_detection/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_detection/package.xml create mode 100644 humanoid_robot_intelligence_control_system_detection/setup.py create mode 100644 humanoid_robot_intelligence_control_system_detection/src/follower.py create mode 100644 humanoid_robot_intelligence_control_system_detection/src/follower_config.py create mode 100644 humanoid_robot_intelligence_control_system_detection/src/tracker.py create mode 100644 humanoid_robot_intelligence_control_system_detection/src/tracker_config.py diff --git a/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt b/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt new file mode 100644 index 0000000..c427fc3 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/CMakeLists.txt @@ -0,0 +1,66 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_detection) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + ) + + catkin_package( + CATKIN_DEPENDS + rospy + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/follower.py + src/follower_config.py + src/tracker.py + src/tracker_config.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/follower.py + src/follower_config.py + src/tracker.py + src/tracker_config.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_detection/package.xml b/humanoid_robot_intelligence_control_system_detection/package.xml new file mode 100644 index 0000000..fa6c357 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/package.xml @@ -0,0 +1,46 @@ + + + + + humanoid_robot_intelligence_control_system_detection + 0.0.1 + + This Package is for Detection Math + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + + + rclpy + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_detection/setup.py b/humanoid_robot_intelligence_control_system_detection/setup.py new file mode 100644 index 0000000..ca93ba9 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/follower.py', "src/follower_config.py", 'src/tracker.py', "src/tracker_config.py"], + packages=['humanoid_robot_intelligence_control_system_detection'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower.py b/humanoid_robot_intelligence_control_system_detection/src/follower.py new file mode 100644 index 0000000..8d32487 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/follower.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + +def calculate_distance_to_object(camera_height, tilt, hip_pitch_offset, object_size): + distance = camera_height * math.tan(math.pi * 0.5 + tilt - hip_pitch_offset - object_size) + return abs(distance) + +def calculate_footstep(current_x_move, target_distance, current_r_angle, target_angle, delta_time, config): + next_movement = current_x_move + target_distance = max(0, target_distance) + fb_goal = min(target_distance * 0.1, config.MAX_FB_STEP) + config.accum_period_time += delta_time + if config.accum_period_time > (config.curr_period_time / 4): + config.accum_period_time = 0.0 + if (target_distance * 0.1 / 2) < current_x_move: + next_movement -= config.UNIT_FB_STEP + else: + next_movement += config.UNIT_FB_STEP + fb_goal = min(next_movement, fb_goal) + fb_move = max(fb_goal, config.MIN_FB_STEP) + + rl_goal = 0.0 + if abs(target_angle) * 180 / math.pi > 5.0: + rl_offset = abs(target_angle) * 0.2 + rl_goal = min(rl_offset, config.MAX_RL_TURN) + rl_goal = max(rl_goal, config.MIN_RL_TURN) + rl_angle = min(abs(current_r_angle) + config.UNIT_RL_TURN, rl_goal) + if target_angle < 0: + rl_angle *= -1 + else: + rl_angle = 0 + + return fb_move, rl_angle + +def calculate_delta_time(curr_time, prev_time): + return (curr_time - prev_time).to_sec() diff --git a/humanoid_robot_intelligence_control_system_detection/src/follower_config.py b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py new file mode 100644 index 0000000..8bd53d1 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/follower_config.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + + +class FollowerConfig: + def __init__(self): + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.CAMERA_HEIGHT = 0.46 + self.NOT_FOUND_THRESHOLD = 50 + self.MAX_FB_STEP = 40.0 * 0.001 + self.MAX_RL_TURN = 15.0 * math.pi / 180 + self.IN_PLACE_FB_STEP = -3.0 * 0.001 + self.MIN_FB_STEP = 5.0 * 0.001 + self.MIN_RL_TURN = 5.0 * math.pi / 180 + self.UNIT_FB_STEP = 1.0 * 0.001 + self.UNIT_RL_TURN = 0.5 * math.pi / 180 + self.SPOT_FB_OFFSET = 0.0 * 0.001 + self.SPOT_RL_OFFSET = 0.0 * 0.001 + self.SPOT_ANGLE_OFFSET = 0.0 + self.hip_pitch_offset = 7.0 + self.curr_period_time = 0.6 + self.accum_period_time = 0.0 + self.DEBUG_PRINT = False + + def update_from_params(self, get_param): + self.FOV_WIDTH = get_param('fov_width', self.FOV_WIDTH) + self.FOV_HEIGHT = get_param('fov_height', self.FOV_HEIGHT) + self.CAMERA_HEIGHT = get_param('camera_height', self.CAMERA_HEIGHT) + self.NOT_FOUND_THRESHOLD = get_param('not_found_threshold', self.NOT_FOUND_THRESHOLD) + self.MAX_FB_STEP = get_param('max_fb_step', self.MAX_FB_STEP) + self.MAX_RL_TURN = get_param('max_rl_turn', self.MAX_RL_TURN) + self.IN_PLACE_FB_STEP = get_param('in_place_fb_step', self.IN_PLACE_FB_STEP) + self.MIN_FB_STEP = get_param('min_fb_step', self.MIN_FB_STEP) + self.MIN_RL_TURN = get_param('min_rl_turn', self.MIN_RL_TURN) + self.UNIT_FB_STEP = get_param('unit_fb_step', self.UNIT_FB_STEP) + self.UNIT_RL_TURN = get_param('unit_rl_turn', self.UNIT_RL_TURN) + self.SPOT_FB_OFFSET = get_param('spot_fb_offset', self.SPOT_FB_OFFSET) + self.SPOT_RL_OFFSET = get_param('spot_rl_offset', self.SPOT_RL_OFFSET) + self.SPOT_ANGLE_OFFSET = get_param('spot_angle_offset', self.SPOT_ANGLE_OFFSET) + self.hip_pitch_offset = get_param('hip_pitch_offset', self.hip_pitch_offset) + self.curr_period_time = get_param('curr_period_time', self.curr_period_time) + self.DEBUG_PRINT = get_param('debug_print', self.DEBUG_PRINT) + + +class FollowerInitializeConfig: + def __init__(self): + self.count_not_found = 0 + self.count_to_approach = 0 + self.on_tracking = False + self.approach_object_position = "NotFound" + self.current_pan = -10 + self.current_tilt = -10 + self.current_x_move = 0.005 + self.current_r_angle = 0 + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + self.current_object_bottom = 0.0 + self.tracking_status = "NotFound" + self.object_position = None + + + def update_from_params(self, get_param): + self.count_not_found = get_param('initial_count_not_found', self.count_not_found) + self.count_to_approach = get_param('initial_count_to_approach', self.count_to_approach) + self.on_tracking = get_param('initial_on_tracking', self.on_tracking) + self.approach_object_position = get_param('initial_approach_object_position', self.approach_object_position) + self.current_pan = get_param('initial_current_pan', self.current_pan) + self.current_tilt = get_param('initial_current_tilt', self.current_tilt) + self.current_x_move = get_param('initial_current_x_move', self.current_x_move) + self.current_r_angle = get_param('initial_current_r_angle', self.current_r_angle) + self.x_error_sum = get_param('initial_x_error_sum', self.x_error_sum) + self.y_error_sum = get_param('initial_y_error_sum', self.y_error_sum) + self.current_object_bottom = get_param('initial_object_bottom', self.current_object_bottom) + self.tracking_status = get_param('initial_tracking_status', self.tracking_status) + + + def reset(self): + """Reset all values to their initial state.""" + self.__init__() + + def update_object_position(self, x, y, z): + """Update the object position.""" + self.object_position.x = x + self.object_position.y = y + self.object_position.z = z + + def increment_count_not_found(self): + """Increment the count of frames where object was not found.""" + self.count_not_found += 1 + + def reset_count_not_found(self): + """Reset the count of frames where object was not found.""" + self.count_not_found = 0 + + def increment_count_to_approach(self): + """Increment the count to approach.""" + self.count_to_approach += 1 + + def reset_count_to_approach(self): + """Reset the count to approach.""" + self.count_to_approach = 0 + + def set_tracking_status(self, status): + """Set the current tracking status.""" + self.tracking_status = status + + def update_error_sums(self, x_error, y_error): + """Update the cumulative error sums.""" + self.x_error_sum += x_error + self.y_error_sum += y_error + + def reset_error_sums(self): + """Reset the cumulative error sums.""" + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + + def update_current_object_position(self, pan, tilt, bottom): + """Update the current object position.""" + self.current_pan = pan + self.current_tilt = tilt + self.current_object_bottom = bottom + + def update_movement(self, x_move, r_angle): + """Update the current movement parameters.""" + self.current_x_move = x_move + self.current_r_angle = r_angle + + def is_tracking(self): + """Check if tracking is currently active.""" + return self.on_tracking + + def start_tracking(self): + """Start the tracking process.""" + self.on_tracking = True + + def stop_tracking(self): + """Stop the tracking process.""" + self.on_tracking = False + + def set_approach_position(self, position): + """Set the approach position of the object.""" + self.approach_object_position = position diff --git a/humanoid_robot_intelligence_control_system_detection/src/tracker.py b/humanoid_robot_intelligence_control_system_detection/src/tracker.py new file mode 100644 index 0000000..369d84e --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/tracker.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + + +import math + +def calculate_error(object_position, FOV_WIDTH, FOV_HEIGHT): + x_error = -math.atan(object_position.x * math.tan(FOV_WIDTH)) + y_error = -math.atan(object_position.y * math.tan(FOV_HEIGHT)) + return x_error, y_error + +def calculate_error_target(error, error_diff, error_sum, p_gain, i_gain, d_gain): + return error * p_gain + error_diff * d_gain + error_sum * i_gain + +def calculate_delta_time(curr_time, prev_time): + return (curr_time - prev_time).to_sec() diff --git a/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py new file mode 100644 index 0000000..5310436 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_detection/src/tracker_config.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import math + +class TrackerConfig: + def __init__(self): + self.FOV_WIDTH = 35.2 * math.pi / 180 + self.FOV_HEIGHT = 21.6 * math.pi / 180 + self.NOT_FOUND_THRESHOLD = 50 + self.WAITING_THRESHOLD = 5 + self.use_head_scan = True + self.DEBUG_PRINT = False + self.p_gain = 0.4 + self.i_gain = 0.0 + self.d_gain = 0.0 + + def update_from_params(self, get_param): + self.FOV_WIDTH = get_param('fov_width', self.FOV_WIDTH) + self.FOV_HEIGHT = get_param('fov_height', self.FOV_HEIGHT) + self.NOT_FOUND_THRESHOLD = get_param('not_found_threshold', self.NOT_FOUND_THRESHOLD) + self.WAITING_THRESHOLD = get_param('waiting_threshold', self.WAITING_THRESHOLD) + self.use_head_scan = get_param('use_head_scan', self.use_head_scan) + self.DEBUG_PRINT = get_param('debug_print', self.DEBUG_PRINT) + self.p_gain = get_param('p_gain', self.p_gain) + self.i_gain = get_param('i_gain', self.i_gain) + self.d_gain = get_param('d_gain', self.d_gain) + + +class TrackerInitializeConfig: + def __init__(self): + self.count_not_found = 0 + self.on_tracking = False + self.current_object_pan = 0.0 + self.current_object_tilt = 0.0 + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + self.current_object_bottom = 0.0 + self.tracking_status = "NotFound" + self.object_position = None + + + def update_from_params(self, get_param): + self.count_not_found = get_param('initial_count_not_found', self.count_not_found) + self.on_tracking = get_param('initial_on_tracking', self.on_tracking) + self.current_object_pan = get_param('initial_object_pan', self.current_object_pan) + self.current_object_tilt = get_param('initial_object_tilt', self.current_object_tilt) + self.x_error_sum = get_param('initial_x_error_sum', self.x_error_sum) + self.y_error_sum = get_param('initial_y_error_sum', self.y_error_sum) + self.current_object_bottom = get_param('initial_object_bottom', self.current_object_bottom) + self.tracking_status = get_param('initial_tracking_status', self.tracking_status) + + + def reset(self): + """Reset all values to their initial state.""" + self.__init__() + + + def update_object_position(self, x, y, z): + """Update the object position.""" + self.object_position.x = x + self.object_position.y = y + self.object_position.z = z + + + def increment_count_not_found(self): + """Increment the count of frames where object was not found.""" + self.count_not_found += 1 + + + def reset_count_not_found(self): + """Reset the count of frames where object was not found.""" + self.count_not_found = 0 + + + def set_tracking_status(self, status): + """Set the current tracking status.""" + self.tracking_status = status + + + def update_error_sums(self, x_error, y_error): + """Update the cumulative error sums.""" + self.x_error_sum += x_error + self.y_error_sum += y_error + + + def reset_error_sums(self): + """Reset the cumulative error sums.""" + self.x_error_sum = 0.0 + self.y_error_sum = 0.0 + + + def update_current_object_position(self, pan, tilt, bottom): + """Update the current object position.""" + self.current_object_pan = pan + self.current_object_tilt = tilt + self.current_object_bottom = bottom + + + def is_tracking(self): + """Check if tracking is currently active.""" + return self.on_tracking + + + def start_tracking(self): + """Start the tracking process.""" + self.on_tracking = True + + + def stop_tracking(self): + """Stop the tracking process.""" + self.on_tracking = False From 07ea79e04bbc1b802510d13cc07a9ae49b3db739 Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Wed, 24 Jul 2024 01:53:54 -0400 Subject: [PATCH 15/16] latest pushes --- .../CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt index 0277ccd..48ef842 100644 --- a/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_configure/CMakeLists.txt @@ -13,7 +13,7 @@ # the License. cmake_minimum_required(VERSION 3.0.2) -project(humanoid_robot_intelligence_control_system_object_detector) +project(humanoid_robot_intelligence_control_system_configure) if($ENV{ROS_VERSION} EQUAL 1) find_package(catkin REQUIRED COMPONENTS From b0e62683ac3550ff38bf1a06e78e2cb13b87381e Mon Sep 17 00:00:00 2001 From: Ronaldson Bellande <47253433+RonaldsonBellande@users.noreply.github.com> Date: Wed, 24 Jul 2024 10:15:33 -0400 Subject: [PATCH 16/16] Update README.md --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 412d3e9..a3dd60f 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,9 @@ - [![Bellande's Internal Sensor Web API](https://img.shields.io/badge/Bellande's%20Internal%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_internal_sensor_web_api) - [![Bellande's External Sensor Web API](https://img.shields.io/badge/Bellande's%20External%20Sensor%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_external_sensor_web_api) - [![Bellande's Functionality Web API](https://img.shields.io/badge/Bellande's%20Functionality%20Web%20API-Bellande%20API-blue?style=for-the-badge&logo=javascript&color=blue)](https://github.com/Application-UI-UX/bellande_functionality_web_api) +- [![Bellande's Algorithm through Bellande API](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20API-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_functionality_ros_api) +- [![Bellande's Algorithm through Bellande MODELS](https://img.shields.io/badge/Bellande's%20Algorithm%20through%20Bellande's%20API-Bellande%20MODELS-blue?style=for-the-badge&logo=ros&color=blue)](https://github.com/Robotics-Sensors/bellande_ros_models) + ## 🤖 Explore Humanoid Robot Intelligence with Us!