kuavo-humanoid-sdk 1.1.2a924__py3-none-any.whl → 1.1.3a1239__py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/ros/tools.py

@@ -0,0 +1,158 @@
+#! /usr/bin/env python3
+# coding: utf-8
+
+import copy
+import time
+import numpy as np
+from typing import Tuple, Union
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+
+try:
+    import rospy
+    import tf2_ros
+    import tf.transformations as tf_trans
+    from std_msgs.msg import Float64
+    from nav_msgs.msg import Odometry
+    from sensor_msgs.msg import JointState
+    from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import sensorsData, lejuClawState, gaitTimeName, dexhandTouchState
+    from geometry_msgs.msg import TransformStamped
+except ImportError:
+    pass
+
+from kuavo_humanoid_sdk.interfaces.data_types import (PoseQuaternion, HomogeneousMatrix)
+from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param, EndEffectorType
+
+
+
+class KuavoRobotToolsCore:
+    """Provides core ROS tools for Kuavo humanoid robot transformations.
+    
+    Attributes:
+        tf_buffer (tf2_ros.Buffer): TF2 transform buffer for storing transforms
+        tf_listener (tf2_ros.TransformListener): TF2 transform listener
+    """
+    
+    def __init__(self):
+        """Initializes TF2 buffer and listener for coordinate transformations."""
+        if not hasattr(self, '_initialized'):
+            # 初始化TF2相关组件
+            self.tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(10))
+            self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
+
+    def _get_tf_tree_transform(self, target_frame: str, source_frame: str, 
+                     time=0.0, timeout=1.0,
+                     return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Gets transform between coordinate frames.
+        
+        Args:
+            target_frame (str): Target coordinate frame name
+            source_frame (str): Source coordinate frame name
+            time (rospy.Time, optional): Time of transform. Defaults to latest.
+            timeout (float, optional): Wait timeout in seconds. Defaults to 1.0.
+            return_type (str, optional): Return data format. Options: 
+                "pose_quaternion", "homogeneous". Defaults to "pose_quaternion".
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: Requested transform data
+                or None if failed
+        
+        Raises:
+            tf2_ros.LookupException: If transform is not available
+            tf2_ros.ConnectivityException: If transform connectivity issue
+            tf2_ros.ExtrapolationException: If transform time is out of range
+        """
+        time = rospy.Time(time)
+        try:
+            transform = self.tf_buffer.lookup_transform(
+                target_frame,
+                source_frame,
+                time,
+                rospy.Duration(timeout)
+            )
+            return self._parse_transform(transform.transform, return_type)
+        except Exception as e:
+            SDKLogger.error(f"Transform error: {str(e)}")
+            return None
+
+    def _parse_transform(self, transform, return_type: str) -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Parses transform data to specified format.
+        
+        Args:
+            transform (geometry_msgs/Transform): Input transform data
+            return_type (str): Output format type. Valid values: 
+                "pose_quaternion", "homogeneous"
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: Parsed transform data
+                in requested format, or None if invalid input
+        
+        Note:
+            Falls back to pose_quaternion format if invalid return_type specified
+        """
+        if return_type == "pose_quaternion":
+            return PoseQuaternion(
+                position=(transform.translation.x, 
+                         transform.translation.y,
+                         transform.translation.z),
+                orientation=(transform.rotation.x,
+                            transform.rotation.y,
+                            transform.rotation.z,
+                            transform.rotation.w)
+            )
+        elif return_type == "homogeneous":
+            return HomogeneousMatrix(
+                matrix=self._transform_to_homogeneous(transform)
+            )
+        else:
+            SDKLogger.warn(f"Invalid return_type: {return_type}, using default(pose_quaternion)")
+            return self._parse_transform(transform, "pose_quaternion")
+
+    def _transform_to_homogeneous(self, transform) -> np.ndarray:
+        """Converts geometry_msgs/Transform to homogeneous matrix.
+        
+        Args:
+            transform (geometry_msgs/Transform): Input transform message
+        
+        Returns:
+            np.ndarray: 4x4 homogeneous transformation matrix (numpy.float32)
+        
+        Example:
+            >>> matrix = _transform_to_homogeneous(transform_msg)
+            >>> print(matrix.shape)
+            (4, 4)
+        """
+        # 四元数转旋转矩阵
+        rotation = [
+            transform.rotation.x,
+            transform.rotation.y,
+            transform.rotation.z,
+            transform.rotation.w
+        ]
+        rot_matrix = tf_trans.quaternion_matrix(rotation)
+
+        # 设置平移分量
+        translation = [
+            transform.translation.x,
+            transform.translation.y,
+            transform.translation.z
+        ]
+        rot_matrix[:3, 3] = translation
+        
+        return rot_matrix.astype(np.float32)  # 确保矩阵数据类型一致
+
+# if __name__ == "__main__":
+#     robot_tools = KuavoRobotToolsCore()
+#     time.sleep(0.1)
+#     # 获取位姿信息
+#     pose = robot_tools._get_tf_tree_transform("odom", "base_link", return_type="pose_quaternion")
+#     print(f"Position: {pose.position}")
+#     print(f"Orientation: {pose.orientation}")
+
+#     # 获取齐次矩阵
+#     homogeneous = robot_tools._get_tf_tree_transform("odom", "base_link", return_type="homogeneous")
+#     print(f"Transformation matrix:\n{homogeneous.matrix}")
+
+#     # 矩阵运算示例
+#     transform_matrix = homogeneous.matrix
+#     inverse_matrix = np.linalg.inv(transform_matrix)  # 求逆变换
+#     print(f"Inverse matrix:\n{inverse_matrix}")

kuavo_humanoid_sdk/kuavo/core/ros/vision.py

@@ -0,0 +1,276 @@
+#! /usr/bin/env python3
+# coding: utf-8
+
+import copy
+import time
+from collections import deque
+from typing import Tuple, Optional
+
+from kuavo_humanoid_sdk.kuavo.core.ros.param import make_robot_param, EndEffectorType
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.interfaces.data_types import (AprilTagData)
+
+try:
+    import rospy
+    from std_msgs.msg import Float64
+    from nav_msgs.msg import Odometry
+    from sensor_msgs.msg import JointState
+    from apriltag_ros.msg import AprilTagDetectionArray
+    from geometry_msgs.msg import TransformStamped, PoseStamped
+    import tf2_ros
+    import tf2_geometry_msgs
+except ImportError:
+    pass
+
+class KuavoRobotVisionCore:
+    """Handles vision-related data processing for Kuavo humanoid robot.
+    
+    Attributes:
+        tf_buffer (tf2_ros.Buffer): TF2 transform buffer
+        tf_listener (tf2_ros.TransformListener): TF2 transform listener
+        tf_broadcaster (tf2_ros.TransformBroadcaster): TF2 transform broadcaster
+    """
+    
+    def __init__(self):
+        """Initializes vision system components including TF and AprilTag subscribers."""
+        if not hasattr(self, '_initialized'):
+            self.tf_buffer = tf2_ros.Buffer()
+            self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
+            self.tf_broadcaster = tf2_ros.TransformBroadcaster()
+            self._apriltag_data_camera_sub = rospy.Subscriber('/tag_detections', AprilTagDetectionArray, self._apriltag_data_callback_camera)
+            self._apriltag_data_base_sub = rospy.Subscriber('/robot_tag_info', AprilTagDetectionArray, self._apriltag_data_callback_base)
+
+            # 添加TF2监听器
+            self._tf_buffer = tf2_ros.Buffer()
+            self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
+
+            """ data """
+            self._apriltag_data_from_camera = AprilTagData(
+                id = [],
+                size = [],
+                pose = []
+            )
+
+            self._apriltag_data_from_base = AprilTagData(
+                id = [],
+                size = [],
+                pose = []
+            )
+
+            self._apriltag_data_from_odom = AprilTagData(
+                id = [],
+                size = [],
+                pose = []
+            )
+
+    def _apriltag_data_callback_camera(self, data):
+        """Callback for processing AprilTag detections from camera.
+        
+        Args:
+            data (AprilTagDetectionArray): Raw detection data from camera
+        """
+        # 清空之前的数据
+        self._apriltag_data_from_camera.id = []
+        self._apriltag_data_from_camera.size = []
+        self._apriltag_data_from_camera.pose = []
+        
+        # 处理每个标签检测
+        for detection in data.detections:
+            # 添加ID
+            for id in detection.id:
+                self._apriltag_data_from_camera.id.append(id)
+            
+            # 添加尺寸 (从size数组中获取)
+            if detection.size and len(detection.size) > 0:
+                self._apriltag_data_from_camera.size.append(detection.size[0])
+            else:
+                self._apriltag_data_from_camera.size.append(0.0)
+            
+            # 添加姿态
+            self._apriltag_data_from_camera.pose.append(detection.pose.pose.pose)
+        # # debug
+        # rospy.loginfo("Apriltag data from camera: %s", self._apriltag_data_from_camera)
+
+    def _apriltag_data_callback_base(self, data):
+        """Callback for processing AprilTag detections from base link.
+        
+        Args:
+            data (AprilTagDetectionArray): Raw detection data from base frame
+        """
+        # 清空之前的数据
+        self._apriltag_data_from_base.id = []
+        self._apriltag_data_from_base.size = []
+        self._apriltag_data_from_base.pose = []
+        
+        # 处理每个标签检测
+        for detection in data.detections:
+            # 添加ID
+            for id in detection.id:
+                self._apriltag_data_from_base.id.append(id)
+            
+            # 添加尺寸 (从size数组中获取)
+            if detection.size and len(detection.size) > 0:
+                self._apriltag_data_from_base.size.append(detection.size[0])
+            else:
+                self._apriltag_data_from_base.size.append(0.0)
+            
+            # 添加姿态
+            self._apriltag_data_from_base.pose.append(detection.pose.pose.pose)
+
+        # # debug
+        # rospy.loginfo("Apriltag data from base: %s", self._apriltag_data_from_base)
+
+        # 在基础数据处理完后，尝试进行odom坐标系的变换
+        self._transform_base_to_odom()
+
+    def _transform_base_to_odom(self):
+        """Transforms AprilTag poses from base_link to odom coordinate frame.
+        
+        Performs:
+            - Coordinate transformation using TF2
+            - TF broadcasting for transformed poses
+            - Data validation and error handling
+            
+        Raises:
+            tf2_ros.LookupException: If transform is not available
+            tf2_ros.ConnectivityException: If transform chain is broken
+            tf2_ros.ExtrapolationException: If transform time is out of range
+        """
+        # 添加节点状态检查
+        if rospy.is_shutdown():
+            return
+        
+        # 清空之前的数据
+        self._apriltag_data_from_odom.id = []
+        self._apriltag_data_from_odom.size = []
+        self._apriltag_data_from_odom.pose = []
+        
+        # 如果base数据为空，则不处理
+        if not self._apriltag_data_from_base.id:
+            SDKLogger.warn("No base tag data, skip transform")
+            return
+        
+        try:
+            # 获取从base_link到odom的变换
+            transform = self._tf_buffer.lookup_transform(
+                "odom",             
+                "base_link",        
+                rospy.Time(0),      
+                rospy.Duration(1.0) 
+            )
+            
+            # 复制ID和尺寸信息
+            self._apriltag_data_from_odom.id = self._apriltag_data_from_base.id.copy()
+            self._apriltag_data_from_odom.size = self._apriltag_data_from_base.size.copy()
+            
+            # 对每个姿态进行变换
+            for idx, (tag_id, pose) in enumerate(zip(self._apriltag_data_from_base.id, self._apriltag_data_from_base.pose)):
+                # 创建PoseStamped消息
+                pose_stamped = PoseStamped()
+                pose_stamped.header.frame_id = "base_link"
+                pose_stamped.header.stamp = rospy.Time.now()
+                pose_stamped.pose = pose
+                
+                # 执行变换
+                transformed_pose = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform)
+                
+                # 将变换后的姿态添加到odom数据中
+                self._apriltag_data_from_odom.pose.append(transformed_pose.pose)
+                
+                # 创建并广播TF
+                transform_stamped = TransformStamped()
+                transform_stamped.header.stamp = rospy.Time.now()
+                transform_stamped.header.frame_id = "odom"
+                transform_stamped.child_frame_id = f"tag_odom_{tag_id}"
+                transform_stamped.transform.translation.x = transformed_pose.pose.position.x
+                transform_stamped.transform.translation.y = transformed_pose.pose.position.y
+                transform_stamped.transform.translation.z = transformed_pose.pose.position.z
+                transform_stamped.transform.rotation = transformed_pose.pose.orientation
+                
+                # 发送变换前再次检查节点状态
+                if not rospy.is_shutdown():
+                    self.tf_broadcaster.sendTransform(transform_stamped)
+                
+        except (tf2_ros.LookupException, 
+               tf2_ros.ConnectivityException,
+               tf2_ros.ExtrapolationException,
+               rospy.ROSException) as e:  # 添加ROSException捕获
+            SDKLogger.warn(f"TF变换异常: {str(e)}")
+    
+    @property 
+    def apriltag_data_from_camera(self) -> AprilTagData:
+        """AprilTag detection data in camera coordinate frame.
+        
+        Returns:
+            AprilTagData: Contains lists of tag IDs, sizes and poses
+        """
+        return self._apriltag_data_from_camera
+    
+    @property
+    def apriltag_data_from_base(self) -> AprilTagData:
+        """AprilTag detection data in base_link coordinate frame.
+        
+        Returns:
+            AprilTagData: Contains lists of tag IDs, sizes and poses
+        """
+        return self._apriltag_data_from_base
+    
+    @property
+    def apriltag_data_from_odom(self) -> AprilTagData:
+        """AprilTag detection data in odom coordinate frame.
+        
+        Returns:
+            AprilTagData: Contains lists of tag IDs, sizes and transformed poses
+        """
+        return self._apriltag_data_from_odom
+
+    def _get_data_by_id(self, target_id: int, data_source: str = "base") -> Optional[dict]:
+        """Retrieves AprilTag data by specific ID from selected source.
+        
+        Args:
+            target_id (int): AprilTag ID to search for
+            data_source (str): Data source selector, valid options: 
+                "camera", "base", "odom"
+        
+        Returns:
+            Optional[dict]: Dictionary containing 'sizes' and 'poses' lists if found,
+                None if no matching data
+        
+        Raises:
+            ValueError: If invalid data_source is specified
+        """
+        data_map = {
+            "camera": self._apriltag_data_from_camera,
+            "base": self._apriltag_data_from_base,
+            "odom": self._apriltag_data_from_odom
+        }
+        
+        if data_source not in data_map:
+            SDKLogger.error(f"Invalid data source: {data_source}, must be one of {list(data_map.keys())}")
+            return None
+        
+        data = data_map[data_source]
+        
+        # 查找所有匹配的索引
+        indices = [i for i, tag_id in enumerate(data.id) if tag_id == target_id]
+        
+        if not indices:
+            SDKLogger.debug(f"No data found for tag ID {target_id} in {data_source} source")
+            return None
+        
+        return {
+            "sizes": [data.size[i] for i in indices],
+            "poses": [data.pose[i] for i in indices]
+        }
+
+# if __name__ == "__main__":
+
+#     kuavo_robot_vision_core = KuavoRobotVisionCore()
+#     time.sleep(5)
+#     print("apriltag_data_from_camera:")
+#     print(kuavo_robot_vision_core.apriltag_data_from_camera)
+#     print("apriltag_data_from_base:")
+#     print(kuavo_robot_vision_core.apriltag_data_from_base)
+#     print("apriltag_data_from_odom:")
+#     print(kuavo_robot_vision_core.apriltag_data_from_odom)
+#     rospy.spin()

kuavo_humanoid_sdk/kuavo/core/ros_env.py

@@ -2,10 +2,15 @@
 # coding: utf-8
 
 import os
-import rospy
+try:
+    import rospy
+except ImportError:
+    pass
 import subprocess
 import atexit
 from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
+import roslibpy
 
 class KuavoROSEnv:
     _instance = None
@@ -232,3 +237,226 @@ class KuavoROSEnv:
         except Exception as e:
             SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
             return False
+
+class KuavoROSEnvWebsocket:
+    _instance = None
+    _processes = []  # Store all subprocess instances
+
+    def __new__(cls):
+        if cls._instance is None:
+            cls._instance = super(KuavoROSEnvWebsocket, cls).__new__(cls)
+            cls._instance._initialized = False
+            cls._instance._init_success = False  # Add flag to track successful Init() call
+            # Register cleanup handler
+            atexit.register(cls._cleanup_processes)
+        return cls._instance
+
+    def __init__(self):
+        if not self._initialized:
+            self._initialized = True
+            self.websocket = None
+
+    @classmethod
+    def _cleanup_processes(cls):
+        """Cleanup all registered processes on exit"""
+        for process in cls._processes:
+            if process.poll() is None:  # Process is still running
+                process.terminate()
+                try:
+                    process.wait(timeout=3)  # Wait for process to terminate
+                except subprocess.TimeoutExpired:
+                    process.kill()  # Force kill if not terminated
+        cls._processes.clear()
+
+    def _get_kuavo_ws_root(self) -> str:
+        # For WebSocket version, we'll use environment variable instead of ROS param
+        model_path = os.environ.get('KUAVO_MODEL_PATH')
+        if not model_path:
+            raise Exception("KUAVO_MODEL_PATH environment variable not found")
+        
+        if not os.path.exists(model_path):
+            raise Exception(f"Model path {model_path} not found")
+            
+        # ws 
+        return model_path.replace('/src/kuavo_assets/models', '')
+    
+    def Init(self) -> bool:
+        """
+        Initialize the WebSocket environment.
+        Raises:
+            Exception: If the KUAVO_MODEL_PATH environment variable is not found or the path is not valid.
+        """
+        # if generate docs, skip init.
+        if 'GEN_KUAVO_HUMANOID_SDK_DOCS' in os.environ:
+            return True
+
+        # Return directly if already initialized successfully
+        if self._init_success:
+            return True
+        
+        # Check if WebSocket server is running
+        try:
+            
+            self.websocket = WebSocketKuavoSDK()
+            if not self.websocket.client.is_connected:
+                print(f"\033[31m\nError: Can't connect to WebSocket server. Please ensure the server is running.\033[0m"
+                      "\nMaybe manually launch the app first?"
+                      "\n - for example(sim): roslaunch humanoid_controller load_kuavo_mujoco_sim.launch, "
+                      "\n - for example(real): roslaunch humanoid_controller load_kuavo_real.launch\n")
+                exit(1)
+        except Exception as e:
+            print(f"\033[31m\nError: Failed to connect to WebSocket server: {e}\033[0m")
+            exit(1)
+        
+        # Only check nodes exist when Init SDK, if not, tips user manually launch nodes.
+        deps_nodes = ['/humanoid_gait_switch_by_name']
+        for node in deps_nodes:
+            if not self.check_rosnode_exists(node):
+                print(f"\033[31m\nError: Node {node} not found. Please launch it manually.\033[0m")
+                exit(1)
+                
+        self._init_success = True  # Set flag after successful initialization
+
+        return True
+    
+    def _launch_ros_node(self, node_name, launch_cmd, log_name):
+        """Launch a ROS node with the given command and log the output.
+        
+        Args:
+            node_name (str): Name of the node to launch
+            launch_cmd (str): Full launch command including source and roslaunch
+            log_name (str): Name for the log file
+            
+        Raises:
+            Exception: If node launch fails
+        """
+        # Launch in background and check if successful
+        try:
+            os.makedirs('/var/log/kuavo_humanoid_sdk', exist_ok=True)
+            log_path = f'/var/log/kuavo_humanoid_sdk/{log_name}.log'
+        except PermissionError:
+            os.makedirs('log/kuavo_humanoid_sdk', exist_ok=True)
+            log_path = f'log/kuavo_humanoid_sdk/{log_name}.log'
+            
+        with open(log_path, 'w') as log_file:
+            process = subprocess.Popen(launch_cmd, shell=True, executable='/bin/bash', stdout=log_file, stderr=log_file)
+            self._processes.append(process)  # Add process to tracking list
+            
+        if process.returncode is not None and process.returncode != 0:
+            raise Exception(f"Failed to launch {node_name}, return code: {process.returncode}")
+
+        SDKLogger.info(f"{node_name} launched successfully")
+
+    def _get_setup_file(self, ws_root=None):
+        """Get the appropriate ROS setup file path based on shell type.
+        
+        Args:
+            ws_root (str, optional): ROS workspace root path. If None, uses ROS_WORKSPACE.
+            
+        Returns:
+            str: Path to the setup file
+            
+        Raises:
+            Exception: If setup file not found
+        """
+        is_zsh = 'zsh' in os.environ.get('SHELL', '')
+        
+        if ws_root is None:
+            ws_root = os.environ['ROS_WORKSPACE']
+            
+        setup_files = {
+            'zsh': os.path.join(ws_root, 'devel/setup.zsh'),
+            'bash': os.path.join(ws_root, 'devel/setup.bash')
+        }
+        
+        setup_file = setup_files['zsh'] if is_zsh else setup_files['bash']
+        if not os.path.exists(setup_file):
+            setup_file = setup_file.replace('devel', 'install')
+            if not os.path.exists(setup_file):
+                raise Exception(f"Setup file not found in either devel or install: {setup_file}")
+                
+        return setup_file
+
+    def launch_ik_node(self):
+        # nodes: /arms_ik_node
+        # services: /ik/two_arm_hand_pose_cmd_srv, /ik/fk_srv
+        try:
+            if not self.websocket or not self.websocket.client.is_connected:
+                raise Exception("WebSocket server is not running")
+        except Exception as e:
+            raise Exception(f"WebSocket server is not running: {e}")
+
+        # Check if IK node and services exist
+        try:
+            # Check if arms_ik_node is running using roslibpy
+            service = roslibpy.Service(self.websocket.client, '/rosnode/list', 'rosapi/Nodes')
+            response = service.call({})
+            nodes = response.get('nodes', [])
+            
+            if '/arms_ik_node' not in nodes:
+                # Launch IK node if not running
+                kuavo_ws_root = self._get_kuavo_ws_root()
+                setup_file = self._get_setup_file(kuavo_ws_root)
+                source_cmd = f"source {setup_file}"
+                
+                # Get robot version from environment variable
+                robot_version = os.environ.get('ROBOT_VERSION')
+                if robot_version is None:
+                    raise Exception("Failed to get ROBOT_VERSION from environment variables")
+                
+                # Launch IK node
+                launch_cmd = f"roslaunch motion_capture_ik ik_node.launch robot_version:={robot_version}"
+                full_cmd = f"{source_cmd} && {launch_cmd}"
+                
+                self._launch_ros_node('IK node', full_cmd, 'launch_ik')
+
+            return True
+
+        except Exception as e:
+            raise Exception(f"Failed to verify IK node and services: {e}")
+        
+
+    def launch_gait_switch_node(self)-> bool:
+        """Verify that the gait switch node is running, launch if not."""
+        try:
+            # Check if node exists using roslibpy
+            service = roslibpy.Service(self.websocket.client, '/rosnode/list', 'rosapi/Nodes')
+            response = service.call({})
+            nodes = response.get('nodes', [])
+            
+            if '/humanoid_gait_switch_by_name' not in nodes:
+                kuavo_ws_root = self._get_kuavo_ws_root()
+                setup_file = self._get_setup_file(kuavo_ws_root)
+                source_cmd = f"source {setup_file}"
+    
+                # Launch gait switch node
+                launch_cmd = "roslaunch humanoid_interface_ros humanoid_switch_gait.launch"
+                full_cmd = f"{source_cmd} && {launch_cmd}"
+                
+                self._launch_ros_node('Gait switch node', full_cmd, 'launch_gait_switch')
+
+        except Exception as e:
+            raise Exception(f"Failed to launch gait_switch node: {e}")
+        
+    @staticmethod
+    def check_rosnode_exists(node_name):
+        """Check if a ROS node is running using roslibpy.
+        
+        Args:
+            node_name (str): Name of the node to check
+            
+        Returns:
+            bool: True if node is running, False otherwise
+        """
+        websocket = WebSocketKuavoSDK()
+        try:
+            if not websocket or not websocket.client.is_connected:
+                return False
+                
+            service = roslibpy.Service(websocket.client, '/rosapi/nodes', 'rosapi/Nodes')
+            response = service.call({})
+            nodes = response.get('nodes', [])
+            return node_name in nodes
+        except Exception as e:
+            SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
+            return False