kuavo-humanoid-sdk 1.2.1b3284__tar.gz → 1.2.1b3295__tar.gz

{kuavo_humanoid_sdk-1.2.1b3284 → kuavo_humanoid_sdk-1.2.1b3295}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: kuavo_humanoid_sdk
-Version: 1.2.1b3284
+Version: 1.2.1b3295
 Summary: A Python SDK for kuavo humanoid robot.
 Home-page: https://gitee.com/leju-robot/kuavo-ros-opensource/
 Author: ['lejurobot']

kuavo_humanoid_sdk-1.2.1b3295/kuavo_humanoid_sdk/kuavo/core/ros/camera.py

@@ -0,0 +1,105 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import rospy
+from std_msgs.msg import Bool, Int16MultiArray
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+from kuavo_msgs.msg import yoloOutputData, yoloDetection
+import threading
+
+class CameraROSInterface:
+    def __init__(self):
+        self.bridge = CvBridge()
+        
+        self.cameras = {
+            'head': {
+                'topic': '/camera/color/image_raw',
+                'pub_topic': '/model_output_data',
+                'publisher': None
+            },
+            'chest': {
+                'topic': '/chest_cam',
+                'pub_topic': '/chest_detection_data',
+                'publisher': None
+            }
+        }
+
+        self.cv_image_shape = None
+
+    
+    def init_ros_node(self):
+        if not rospy.get_node_uri():
+            rospy.init_node('YOLO_detection', anonymous=True)
+        
+        for camera in self.cameras.values():
+            camera['publisher'] = rospy.Publisher(
+                camera['pub_topic'], 
+                yoloDetection, 
+                queue_size=1, 
+                tcp_nodelay=True
+            )
+
+    def get_camera_image(self, camera):
+        if camera not in self.cameras:
+            rospy.logerr(f"Unknown camera: {camera}")
+            return None
+            
+        try:
+            # 使用rospy.wait_for_message()获取一帧图像
+            msg = rospy.wait_for_message(self.cameras[camera]['topic'], Image, timeout=1.0)
+            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+            self.cv_image_shape = cv_image.shape
+            return cv_image
+        except rospy.ROSException as e:
+            print(f"Timeout waiting for {camera} camera image: {e}")
+            return None
+        except CvBridgeError as e:
+            rospy.logerr(f"{camera.capitalize()} Camera CvBridge Error: {e}")
+            return None
+
+    def node_spin(self):
+        rospy.spin()
+
+    def node_is_shutdown(self):
+        return rospy.is_shutdown()
+
+    def tensor_to_msg(self, results):
+        if not results:
+            return None
+        else:
+            yolo_detections = yoloDetection()
+            for result in results:
+                boxes = result.boxes.cpu().numpy()
+
+                xywh = boxes.xywh  # center-x(i,0), center-y(i,1), width(i,2), height(i,3)
+                class_ids = result.boxes.cls.int().cpu().numpy()
+                class_names = [result.names[cls.item()] for cls in result.boxes.cls.int()]
+                confs = boxes.conf
+
+                for i in range(len(xywh)):
+                    yolo_output_data = yoloOutputData(
+                        class_name=class_names[i],
+                        class_id=int(class_ids[i]),
+                        confidence=confs[i], 
+                        x_pos=xywh[i][0], 
+                        y_pos=xywh[i][1], 
+                        height=xywh[i][2], 
+                        width=xywh[i][3]
+                    )
+                    yolo_detections.data.append(yolo_output_data)
+            return yolo_detections
+
+    def publish_results(self, results, camera):
+        if not results:
+            rospy.loginfo(f"No results to publish for {camera} camera.")
+            return
+            
+        if camera not in self.cameras:
+            rospy.logerr(f"Unknown camera: {camera}")
+            return
+            
+        yolo_detections = self.tensor_to_msg(results)
+        if yolo_detections:
+            self.cameras[camera]['publisher'].publish(yolo_detections)

{kuavo_humanoid_sdk-1.2.1b3284 → kuavo_humanoid_sdk-1.2.1b3295}/kuavo_humanoid_sdk/kuavo/robot_blockly.py

@@ -28,9 +28,8 @@ from geometry_msgs.msg import Pose, Point, Quaternion
 from std_msgs.msg import Float64MultiArray
 
 import inspect
-from model_utils import *
-import model_utils as model_utils
-
+from kuavo_humanoid_sdk.kuavo.core.model_utils.model_utils import *
+from kuavo_humanoid_sdk.kuavo.core.model_utils import model_utils as model_utils
 
 from enum import IntEnum
 # Global flag for handling Ctrl+C
@@ -45,24 +44,6 @@ else:
     INIT_ARM_POS = [22.91831, 0, 0, -45.83662, 22.91831, 0, 0, -45.83662] # task.info: shoudler_center: 0.4rad, elbow_center: -0.8rad
 
 
-
-global ACTION_FILE_FOLDER, package_path, THRESHOLD_HEAD_CONTROL_COUNT, FLOAT_MAX
-THRESHOLD_HEAD_CONTROL_COUNT = 30
-FLOAT_MAX = float('inf')
-
-package_name = 'planarmwebsocketservice'
-package_path = rospkg.RosPack().get_path(package_name)
-sudo_user = os.environ.get("SUDO_USER")
-if sudo_user:
-    user_info = pwd.getpwnam(sudo_user)
-    home_path = user_info.pw_dir
-else:
-    home_path = os.path.expanduser("~")
-ACTION_FILE_FOLDER = os.path.join(home_path, '.config', 'lejuconfig', 'action_files')
-plan_arm_state_progress = 0
-plan_arm_state_status = False
-
-
 def signal_handler(sig, frame):
     """Signal handler for catching Ctrl+C and safely stopping the robot.
 
@@ -287,16 +268,29 @@ class RobotControlBlockly:
         g_robot_type = "ocs2"
         self.loop = None
         self.init_ros_publishers()
+        self.THRESHOLD_HEAD_CONTROL_COUNT = 30
+        self.FLOAT_MAX = float('inf')
         self.to_stance()
 
+        package_name = 'planarmwebsocketservice'
+        self.package_path = rospkg.RosPack().get_path(package_name)
+        sudo_user = os.environ.get("SUDO_USER")
+        if sudo_user:
+            user_info = pwd.getpwnam(sudo_user)
+            home_path = user_info.pw_dir
+        else:
+            home_path = os.path.expanduser("~")
+        self.ACTION_FILE_FOLDER = os.path.join(home_path, '.config', 'lejuconfig', 'action_files')
+        self.plan_arm_state_progress = 0
+        self.plan_arm_state_status = False
+
     def set_action_file_path(self, proj_name: str):
         """Set the action file path for a specific project.
 
         Args:
             proj_name (str): Project name.
         """
-        global ACTION_FILE_FOLDER
-        ACTION_FILE_FOLDER = package_path + "/upload_files/" + proj_name + "/action_files"
+        self.ACTION_FILE_FOLDER = self.package_path + "/upload_files/" + proj_name + "/action_files"
 
     def init_ros_publishers(self):
         """Initialize ROS publishers and subscribers for arm, hand, head control topics, sensor data, and trajectory state."""
@@ -318,7 +312,7 @@ class RobotControlBlockly:
         Only publishes when robot type is 'ocs2' and trajectory is not finished.
         """
         global kuavo_arm_traj_pub, control_hand_pub, control_head_pub, g_robot_type
-        if g_robot_type == "ocs2" and len(ocs2_joint_state.position) > 0 and plan_arm_state_status is False:
+        if g_robot_type == "ocs2" and len(ocs2_joint_state.position) > 0 and self.plan_arm_state_status is False:
             kuavo_arm_traj_pub.publish(ocs2_joint_state)
             control_hand_pub.publish(ocs2_hand_state)
             # control_head_pub.publish(ocs2_head_state)
@@ -349,9 +343,8 @@ class RobotControlBlockly:
         Args:
             msg (planArmState): Arm trajectory state message.
         """
-        global plan_arm_state_progress, plan_arm_state_status
-        plan_arm_state_progress = msg.progress
-        plan_arm_state_status = msg.is_finished
+        self.plan_arm_state_progress = msg.progress
+        self.plan_arm_state_status = msg.is_finished
 
     def traj_callback(self, msg):
         """Callback for trajectory messages, parses trajectory points and updates global arm, hand, and head state variables.
@@ -597,14 +590,13 @@ class RobotControlBlockly:
         """
         global g_robot_type
         g_robot_type = "ocs2"
-        global plan_arm_state_status
-        plan_arm_state_status = False
+        self.plan_arm_state_status = False
 
         action_filename = action_file + ".tact"
         if proj_name is None:
-            action_file_path = os.path.expanduser(f"{ACTION_FILE_FOLDER}/{action_filename}")
+            action_file_path = os.path.expanduser(f"{self.ACTION_FILE_FOLDER}/{action_filename}")
         else:
-            action_file_path = package_path + "/upload_files/" + proj_name + "/action_files/" + action_filename
+            action_file_path = self.package_path + "/upload_files/" + proj_name + "/action_files/" + action_filename
         if not os.path.exists(action_file_path):
             print(f"Action file not found: {action_file_path}")
             return
@@ -623,7 +615,7 @@ class RobotControlBlockly:
             self.play_music(music_file)
         self.plan_arm_trajectory_bezier_curve_client(req)
         time.sleep(0.5)
-        while plan_arm_state_status is False:
+        while self.plan_arm_state_status is False:
             time.sleep(0.01)
         print("action file executed")
 
@@ -635,13 +627,12 @@ class RobotControlBlockly:
             pitch (float): Pitch angle in degrees.
         """
         try:
-            global THRESHOLD_HEAD_CONTROL_COUNT, FLOAT_MAX
             self.robot.stance()
             yaw = math.radians(yaw)
             pitch = math.radians(pitch)
             self.robot.control_head(yaw, pitch)
-            diff_yaw_min = FLOAT_MAX
-            diff_pitch_min = FLOAT_MAX
+            diff_yaw_min = self.FLOAT_MAX
+            diff_pitch_min = self.FLOAT_MAX
             times_count = 0
             while True:
                 time.sleep(0.01)
@@ -657,7 +648,7 @@ class RobotControlBlockly:
                     diff_pitch_min = diff_pitch
                     continue
                 times_count += 1
-                if times_count > THRESHOLD_HEAD_CONTROL_COUNT:
+                if times_count > self.THRESHOLD_HEAD_CONTROL_COUNT:
                     break
 
         except Exception as e:
@@ -670,12 +661,11 @@ class RobotControlBlockly:
             yaw (float): Yaw angle in degrees.
         """
         try:
-            global THRESHOLD_HEAD_CONTROL_COUNT, FLOAT_MAX
             self.robot.stance()
             yaw = math.radians(yaw)
             self.robot.control_head(yaw, current_head_joint_state[1])
             times_count = 0
-            diff_yaw_min = FLOAT_MAX
+            diff_yaw_min = self.FLOAT_MAX
             while True:
                 time.sleep(0.01)
                 diff_yaw = abs(current_head_joint_state[0] - yaw)
@@ -684,7 +674,7 @@ class RobotControlBlockly:
                     diff_yaw_min = diff_yaw
                     continue
                 times_count += 1
-                if times_count > THRESHOLD_HEAD_CONTROL_COUNT:
+                if times_count > self.THRESHOLD_HEAD_CONTROL_COUNT:
                     break
         except Exception as e:
             print(f"Robot head control failed: {str(e)}")
@@ -696,12 +686,11 @@ class RobotControlBlockly:
             pitch (float): Pitch angle in degrees.
         """
         try:
-            global THRESHOLD_HEAD_CONTROL_COUNT, FLOAT_MAX
             self.robot.stance()
             pitch = math.radians(pitch)
             self.robot.control_head(current_head_joint_state[0], pitch)
             times_count = 0
-            diff_pitch_min = FLOAT_MAX
+            diff_pitch_min = self.FLOAT_MAX
             while True:
                 time.sleep(0.01)
                 diff_pitch = abs(current_head_joint_state[1] - pitch)
@@ -710,7 +699,7 @@ class RobotControlBlockly:
                     diff_pitch_min = diff_pitch
                     continue
                 times_count += 1
-                if times_count > THRESHOLD_HEAD_CONTROL_COUNT:
+                if times_count > self.THRESHOLD_HEAD_CONTROL_COUNT:
                     break
         except Exception as e:
             print(f"Robot head control failed: {str(e)}")
@@ -1086,14 +1075,18 @@ class RobotControlBlockly:
             if  not is_yolo_init:
                 yolo_detection = YOLO_detection()
                 yolo_detection.init_ros_node()
-                caller_file_path = inspect.stack()[1].filename
+                caller_file_path = self.package_path + "/upload_files/"
+                print(f"caller_file_path: {caller_file_path}")
                 model_path = os.path.join(os.path.dirname(caller_file_path), 'best.pt')
                 # print(f"函数被文件调用: {model_path}")
                 model = yolo_detection.load_model(model_path)
+                result = yolo_detection.camera_interface.get_camera_image("head")
+                if result is None:
+                    print("No head camera image...")
+                    return
 
-
-            IMAGE_CENTER_X = yolo_detection.cv_image_shape[1] / 2.0
-            IMAGE_CENTER_Y = yolo_detection.cv_image_shape[0] / 2.0
+            IMAGE_CENTER_X = yolo_detection.camera_interface.cv_image_shape[1] / 2.0
+            IMAGE_CENTER_Y = yolo_detection.camera_interface.cv_image_shape[0] / 2.0
             # rospy.loginfo(f"IMAGE_CENTER_X: {IMAGE_CENTER_X}")
             # rospy.loginfo(f"IMAGE_CENTER_Y: {IMAGE_CENTER_Y}")
             MOVE_SPEED_Y = 0.05  # Y方向移动速度
@@ -1102,7 +1095,6 @@ class RobotControlBlockly:
             while not rospy.is_shutdown():
 
                 results = yolo_detection.get_detections("head", model)
-
                 if not results:
                     continue
 

{kuavo_humanoid_sdk-1.2.1b3284 → kuavo_humanoid_sdk-1.2.1b3295}/kuavo_humanoid_sdk.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: kuavo-humanoid-sdk
-Version: 1.2.1b3284
+Version: 1.2.1b3295
 Summary: A Python SDK for kuavo humanoid robot.
 Home-page: https://gitee.com/leju-robot/kuavo-ros-opensource/
 Author: ['lejurobot']

{kuavo_humanoid_sdk-1.2.1b3284 → kuavo_humanoid_sdk-1.2.1b3295}/kuavo_humanoid_sdk.egg-info/SOURCES.txt

@@ -41,6 +41,7 @@ kuavo_humanoid_sdk/kuavo/core/navigation.py
 kuavo_humanoid_sdk/kuavo/core/ros_env.py
 kuavo_humanoid_sdk/kuavo/core/sdk_deprecated.py
 kuavo_humanoid_sdk/kuavo/core/ros/audio.py
+kuavo_humanoid_sdk/kuavo/core/ros/camera.py
 kuavo_humanoid_sdk/kuavo/core/ros/control.py
 kuavo_humanoid_sdk/kuavo/core/ros/microphone.py
 kuavo_humanoid_sdk/kuavo/core/ros/navigation.py