kuavo-humanoid-sdk-ws 1.2.1__tar.gz → 1.2.1b1675__tar.gz

{kuavo_humanoid_sdk_ws-1.2.1 → kuavo_humanoid_sdk_ws-1.2.1b1675}/PKG-INFO

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

{kuavo_humanoid_sdk_ws-1.2.1 → kuavo_humanoid_sdk_ws-1.2.1b1675}/README.md

@@ -1,11 +1,11 @@
 # Kuavo Humanoid SDK
-[![Version](https://img.shields.io/pypi/v/kuavo-humanoid-sdk.svg)](https://pypi.org/project/kuavo-humanoid-sdk/)[![License](https://img.shields.io/pypi/l/kuavo-humanoid-sdk.svg)](#)[![Supported Python Versions](https://img.shields.io/pypi/pyversions/kuavo-humanoid-sdk.svg)](https://pypi.python.org/pypi/kuavo-humanoid-sdk)
+[![Version](https://img.shields.io/pypi/v/kuavo-humanoid-sdk-ws.svg)](https://pypi.org/project/kuavo-humanoid-sdk-ws/)[![License](https://img.shields.io/pypi/l/kuavo-humanoid-sdk-ws.svg)](#)[![Supported Python Versions](https://img.shields.io/pypi/pyversions/kuavo-humanoid-sdk-ws.svg)](https://pypi.python.org/pypi/kuavo-humanoid-sdk-ws)
 
 一个全面的 Python SDK，用于控制 Kuavo 人形机器人。该 SDK 提供了机器人状态管理、手臂和头部控制以及末端执行器操作的接口。它设计用于与 ROS（机器人操作系统）环境一起工作。
 
 **警告**：该 SDK 目前仅支持 **ROS1**。不支持 ROS2。
 
-PyPI 项目地址: https://pypi.org/project/kuavo-humanoid-sdk/
+PyPI 项目地址: https://pypi.org/project/kuavo-humanoid-sdk-ws/
 
 ## 安装
 **提示：对于本 SDK 目前存在两个版本，正式发布版与beta内测版, 他们的区别是：**

kuavo_humanoid_sdk_ws-1.2.1b1675/kuavo_humanoid_sdk/common/launch_robot_tool.py

@@ -0,0 +1,170 @@
+import roslibpy
+import time
+from typing import Tuple
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
+
+
+class LaunchRobotStatus:
+    # 与硬件节点保持一致
+    UNLAUNCH = 'unlaunch'
+    INITING = 'initing'
+    UNKNOWN = 'unknown'
+    READY_STANCE = 'ready_stance'
+    CALIBRATE = 'calibrate'
+    LAUNCHED = 'launched'
+
+class LaunchRobotTool:
+    def __init__(self):
+        self._websocket = WebSocketKuavoSDK()
+        self._stand_timeout = 10.0
+        self._srv_robot_start = roslibpy.Service(self._websocket.client, '/websocket_sdk_srv/start_robot', 'std_srvs/Trigger')
+        self._srv_robot_stand = roslibpy.Service(self._websocket.client, '/websocket_sdk_srv/stand_robot', 'std_srvs/Trigger')
+        self._srv_get_robot_launch_status = roslibpy.Service(self._websocket.client, '/websocket_sdk_srv/get_robot_launch_status', 'std_srvs/Trigger')
+        self._srv_robot_stop = roslibpy.Service(self._websocket.client, '/websocket_sdk_srv/stop_robot', 'std_srvs/Trigger')
+        
+    def is_unlaunch(self, status:str)->bool:
+        return status == LaunchRobotStatus.UNLAUNCH or status == LaunchRobotStatus.UNKNOWN
+    
+    def is_launched(self, status:str)->bool:
+        return status == LaunchRobotStatus.LAUNCHED
+
+    def is_launching(self, status:str)->bool:
+        return self.is_calibrate(status) or self.is_ready_stance(status)
+
+    def is_calibrate(self, status:str)->bool:
+        return status == LaunchRobotStatus.CALIBRATE
+    
+    def is_ready_stance(self, status:str)->bool:
+        return status == LaunchRobotStatus.READY_STANCE
+    
+    def robot_start(self) -> bool:
+        # launch robot
+        if not self.pub_start_command():
+            return False
+        try:
+            timeout = 120 # 120s 等待进入校准或准备姿态
+            start_time = time.time()
+            last_print = time.time()
+            print("\033[32m>_ 机器人启动中\033[0m", end="", flush=True)
+            while True:
+                if time.time() - start_time > timeout:
+                    SDKLogger.error("Robot launch timed out")
+                    return False
+                success, status = self.get_robot_launch_status()
+                # 如果机器人启动成功，已经进入校准或准备状态或者launched状态，则启动成功
+                if success and (self.is_launched(status) or self.is_launching(status)):
+                    print("\n")
+                    return True
+                
+                current_time = time.time()
+                if current_time - last_print >= 0.8:
+                    print("\033[32m.\033[0m", end="", flush=True)
+                    last_print = current_time
+                time.sleep(0.2)
+        except KeyboardInterrupt:
+            print("\n")
+            SDKLogger.info("Received keyboard interrupt, stopping robot launch")
+            return False
+    
+    def robot_stop(self)->bool:
+        """
+        停止机器人
+        """
+        try:
+            request = roslibpy.ServiceRequest({})
+            response = self._srv_robot_stop.call(request)
+            return response.get('success', False)
+        except Exception as e:
+            SDKLogger.error(f"[Error] calling stop robot service: {e}")
+            return False
+
+    def robot_stand(self)->bool:
+        try:
+            while True:
+                # 获取当前启动状态, 如果已启动则返回
+                success, status = self.get_robot_launch_status()
+                if not success:
+                    SDKLogger.error(f"[Error] Failed to get robot launch status: {status}")
+                    return False
+                if self.is_launched(status):
+                    return True
+                
+                # 从当前状态确认下一个状态
+                next_state = None
+                if self.is_calibrate(status):
+                    print("\033[32m>_ [校准状态]: 机器人当前处于校准状态，确认无误，请输入 y 键进入准备姿态\033[0m")
+                    next_state = LaunchRobotStatus.READY_STANCE
+                elif self.is_ready_stance(status):
+                    print("\033[32m>_ [准备姿态]: 请等待机器人缩腿到下蹲姿势，并将其下降到距离地面 2cm 处，确认无误后，请输入 y 键\033[0m")
+                    next_state = LaunchRobotStatus.LAUNCHED
+
+                if self.is_calibrate(status) or self.is_ready_stance(status):
+                    while True:
+                        try:
+                            choice = input().lower()
+                            if choice == 'y':
+                                break
+                            elif choice == 'n':
+                                return False
+                            else:
+                                SDKLogger.error("输入错误，请输入 y 键")
+                        except KeyboardInterrupt:
+                            return False
+                    
+                    # 发布站立命令
+                    if not self.pub_stand_command():
+                        return False
+
+                    # 等待进入下一个状态
+                    start_time = time.time()
+                    while True:
+                        if time.time() - start_time > self._stand_timeout: 
+                            break
+                        success, current_status = self.get_robot_launch_status()
+                        if not success:
+                            SDKLogger.error(f"[Error] Wait next state Failed to get robot launch status: {current_status}")
+                            return False
+                        if self.is_launched(current_status):
+                            return True
+                        elif current_status == next_state:
+                            # 成功进入下一个状态
+                            break
+                        time.sleep(0.2)
+                time.sleep(0.2)
+        except KeyboardInterrupt:
+            SDKLogger.info("Received keyboard interrupt, stopping robot stand")
+            return False
+
+    def pub_start_command(self) -> bool:
+        try:
+            request = roslibpy.ServiceRequest({})
+            response = self._srv_robot_start.call(request)
+            result = response.get('success', False)
+            return result
+        except Exception as e:
+            SDKLogger.error(f"[Error] calling start service: {e}")
+            return False
+
+    def pub_stand_command(self) -> bool:
+        try:
+            request = {}
+            response = self._srv_robot_stand.call(request)
+            if not response.get('success', False):
+                SDKLogger.error(f"Failed to make robot stand: {response.get('message', '')}")
+            return response.get('success', False)
+        except Exception as e:
+            SDKLogger.error(f"[Error] calling stand service: {e}")
+            return False
+        
+
+    def get_robot_launch_status(self)->Tuple[bool, str]:
+        try:
+            request = roslibpy.ServiceRequest({})
+            response = self._srv_get_robot_launch_status.call(request)
+            success = response.get('success', False)
+            status = response.get('message', 'unknown')
+            return success, status
+        except Exception as e:
+            SDKLogger.error(f"[Error] calling get robot launch status service: {e}")
+            return False, "unknown"

{kuavo_humanoid_sdk_ws-1.2.1 → kuavo_humanoid_sdk_ws-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/core/ros/state.py

@@ -340,7 +340,7 @@ class KuavoRobotStateCoreWebsocket:
     
     def _humanoid_mpc_observation_callback(self, msg) -> None:
         try:
-            SDKLogger.debug(f"[State] Received MPC observation message: {msg}")
+            # SDKLogger.debug(f"[State] Received MPC observation message: {msg}")
             self._mpc_observation_data = msg
             if hasattr(self, '_initialized'): 
                 curr_time = self._mpc_observation_data['time']

{kuavo_humanoid_sdk_ws-1.2.1 → kuavo_humanoid_sdk_ws-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/core/ros/tools.py

@@ -37,8 +37,9 @@ class KuavoRobotToolsCoreWebsocket:
                 self._transform_cache = {}
                 self._initialized = True
             except Exception as e:
-                SDKLogger.error(f"Failed to initialize tf2_web_republisher: {str(e)}")
-                SDKLogger.error("Please make sure tf2_web_republisher node is running")
+                SDKLogger.error(f"Failed to initialize kuavo_tf2_web_republisher: {str(e)}")
+                SDKLogger.error(f"kuavo_tf2_web_republisher 节点未运行")
+                SDKLogger.error("请运行 `cd <kuavo_ros_application> && source devel/setup.bash && rosrun kuavo_tf2_web_republisher kuavo_tf2_web_republisher` 启动 kuavo_tf2_web_republisher 节点")
                 raise
 
     def _get_tf_tree_transform(self, target_frame: str, source_frame: str, 

{kuavo_humanoid_sdk_ws-1.2.1 → kuavo_humanoid_sdk_ws-1.2.1b1675}/kuavo_humanoid_sdk/kuavo/robot.py

@@ -11,6 +11,7 @@ from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
 from kuavo_humanoid_sdk.kuavo.robot_arm import KuavoRobotArm 
 from kuavo_humanoid_sdk.kuavo.robot_head import KuavoRobotHead
 from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
+from kuavo_humanoid_sdk.common.launch_robot_tool import LaunchRobotTool
 
 """
 Kuavo SDK - Python Interface for Kuavo Robot Control
@@ -78,6 +79,59 @@ class KuavoSDK:
         SDKLogger.setLevel(log_level.upper())
         SDKLogger.debug(f" ================= Kuavo Humanoid Websocket SDK =================")
 
+        def launch_robot()->bool:
+            import time
+            import os
+            start_time = time.time()
+            launch_robot_tool = LaunchRobotTool()
+            
+            # 获取机器人启动状态
+            success, status = launch_robot_tool.get_robot_launch_status()
+            if not success:
+                # 可能websocket服务未启动或网络不通原因
+                SDKLogger.error(f"[SDK] Failed to get robot launch status: {status}")
+                return False
+
+            SDKLogger.info(f"[SDK] Robot launch status: {status}, time elapsed: {time.time() - start_time:.2f}s")
+
+            # 如果机器人已经启动，则直接返回True
+            if launch_robot_tool.is_launched(status):
+                return True
+            
+            # 未启动则启动
+            if launch_robot_tool.is_unlaunch(status):
+                print("\033[32m\n>_ 温馨提示: 机器人未启动，现在可以选择手动启动，如果需要请输入 y ，否则输入 n 退出\033[0m")
+                try:
+                    while True:
+                        choice = input().lower()
+                        if choice == 'y':
+                            if not launch_robot_tool.robot_start():
+                                SDKLogger.error("机器人启动失败，请检查机器人状态")
+                                os._exit(1)
+                            break
+                        elif choice == 'n':
+                            SDKLogger.warning("用户选择不启动机器人，程序退出。")
+                            os._exit(1)
+                        else:
+                            SDKLogger.error("输入错误，请输入 y 或 n")
+                except KeyboardInterrupt:
+                    os._exit(1)
+            
+            SDKLogger.debug(f"等待机器人启动，已耗时: {time.time() - start_time:.2f}s")
+           
+            if not launch_robot_tool.robot_stand():
+                SDKLogger.error("机器人站立失败，请检查机器人状态")
+                os._exit(1)
+
+            end_time = time.time()
+            SDKLogger.info(f"[SDK] Robot launch time: {end_time - start_time} seconds")
+            return True
+        
+        # Launch robot
+        if not launch_robot():
+            SDKLogger.error("[SDK] Failed to launch robot.")
+            return False
+
         # Initialize core components, connect ROS Topics...
         kuavo_core = KuavoRobotCore()
         if log_level.upper() == 'DEBUG':
@@ -90,8 +144,6 @@ class KuavoSDK:
                 print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
                 exit(1)
 
-
-
         if not kuavo_core.initialize(debug=debug):
             SDKLogger.error("[SDK] Failed to initialize core components.")
             return False
@@ -103,6 +155,18 @@ class KuavoSDK:
         """Disable all logging output from the SDK."""
         disable_sdk_logging()
 
+    @staticmethod
+    def StopRobot()->bool:
+        """Stop the robot."""
+        launch_robot_tool = LaunchRobotTool()
+        success, status = launch_robot_tool.get_robot_launch_status()
+        if not success:
+            SDKLogger.error(f"[SDK] Failed to get robot launch status: {status}")
+            return False
+        if launch_robot_tool.is_unlaunch(status):
+            return True
+        return launch_robot_tool.robot_stop()
+
 class KuavoRobot(RobotBase):
     def __init__(self):
         super().__init__(robot_type="kuavo")

{kuavo_humanoid_sdk_ws-1.2.1 → kuavo_humanoid_sdk_ws-1.2.1b1675}/kuavo_humanoid_sdk_ws.egg-info/PKG-INFO

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

{kuavo_humanoid_sdk_ws-1.2.1 → kuavo_humanoid_sdk_ws-1.2.1b1675}/kuavo_humanoid_sdk_ws.egg-info/SOURCES.txt

@@ -1,6 +1,7 @@
 README.md
 setup.py
 kuavo_humanoid_sdk/__init__.py
+kuavo_humanoid_sdk/common/launch_robot_tool.py
 kuavo_humanoid_sdk/common/logger.py
 kuavo_humanoid_sdk/common/websocket_kuavo_sdk.py
 kuavo_humanoid_sdk/interfaces/__init__.py