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)