kuavo-humanoid-sdk 1.2.1b3279__20250911210455-py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/ros/tools.py

@@ -0,0 +1,220 @@
+#! /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
+import rospy
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import RepublishTFs
+from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import TFArray
+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
+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_service (rospy.ServiceProxy): Service proxy for tf2_web_republisher
+        _transform_cache (dict): Cache for storing recent transforms
+    """
+    
+    def __init__(self):
+        """Initializes TF2 web republisher service proxy."""
+        if not hasattr(self, '_initialized'):
+            try:
+                # 初始化TF2 web republisher服务
+                rospy.wait_for_service('/republish_tfs', timeout=5.0)
+                self.tf_service = rospy.ServiceProxy('/republish_tfs', RepublishTFs)
+                self._transform_cache = {}
+                self._initialized = True
+            except Exception as e:
+                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, 
+                     time=0.0, timeout=5.0,
+                     return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Gets transform between coordinate frames using tf2_web_republisher.
+        
+        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 5.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
+        """
+        try:
+            
+            # 调用服务
+            response = self.tf_service(
+                source_frames=[source_frame],
+                target_frame=target_frame,
+                angular_thres=0.01,  # 角度阈值
+                trans_thres=0.01,    # 平移阈值
+                rate=10.0,           # 更新频率
+                timeout=rospy.Duration(timeout)
+            )
+
+            if response.status == -1:
+                SDKLogger.error(f"{source_frame} or {target_frame} not exist")
+                return None
+            
+            # 检查话题是否发布
+            published_topics = rospy.get_published_topics()
+            if not any(topic_tuple[0] == response.topic_name for topic_tuple in published_topics):
+                SDKLogger.error(f"Topic {response.topic_name} not published")
+                return None
+                
+            # 创建订阅者
+            transform_received = False
+            transform_data = None
+            
+            def transform_callback(msg):
+                nonlocal transform_received, transform_data
+                transform_received = True
+                transform_data = msg
+                
+            sub = rospy.Subscriber(response.topic_name, TFArray, transform_callback)
+            
+            # 等待接收数据
+            start_time = rospy.Time.now()
+            while not transform_received or (rospy.Time.now() - start_time).to_sec() > timeout:
+                rospy.sleep(0.1)
+                
+            # 取消订阅
+            sub.unregister()
+                        
+            if not transform_received:
+                SDKLogger.error("No transform data received")
+                return None
+                
+            # 从TFArray中获取对应的变换
+            for tf_msg in transform_data.transforms:
+                if tf_msg.header.frame_id == target_frame and tf_msg.child_frame_id == source_frame:
+                    return self._parse_transform(tf_msg.transform, return_type)
+                    
+            SDKLogger.error(f"No matching transform found in TFArray")
+            return None
+            
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call failed: {str(e)}")
+            return None
+        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)  # 确保矩阵数据类型一致
+
+    def get_link_pose(self, link_name: str, target_frame: str = "base_link") -> Union[PoseQuaternion, None]:
+        """获取指定关节链接的位姿（核心实现）
+        
+        Args:
+            link_name (str): 关节链接名称
+            target_frame (str): 目标坐标系
+            
+        Returns:
+            PoseQuaternion: 包含位置和姿态的四元数表示
+        """
+        return self._get_tf_tree_transform(
+            target_frame,
+            link_name,
+            return_type="pose_quaternion"
+        )
+
+# 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,234 @@
+#! /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, AprilTagDetection)
+
+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
+
+
+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'):
+
+
+            # FIRST: Initialize data structures before creating subscribers
+            """ 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 = []
+            )
+            
+            # THEN: Create subscribers after all data structures are initialized
+            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)
+            self._apriltag_data_odom_sub = rospy.Subscriber('/robot_tag_info_odom', AprilTagDetectionArray, self._apriltag_data_callback_odom)
+            
+            # Mark as initialized
+            self._initialized = True
+
+    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(AprilTagDetection(
+                position=detection.pose.pose.pose.position,
+                orientation=detection.pose.pose.pose.orientation
+            ))
+        # # 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(AprilTagDetection(
+                position=detection.pose.pose.pose.position,
+                orientation=detection.pose.pose.pose.orientation
+            ))
+
+        # # debug
+        # rospy.loginfo("Apriltag data from base: %s", self._apriltag_data_from_base)
+
+    def _apriltag_data_callback_odom(self, data):
+        """Callback for processing AprilTag detections from odom.
+        
+        Args:
+            data (AprilTagDetectionArray): Raw detection data from odom frame
+        """
+        # 清空之前的数据
+        self._apriltag_data_from_odom.id = []
+        self._apriltag_data_from_odom.size = []
+        self._apriltag_data_from_odom.pose = []
+        
+        # 处理每个标签检测
+        for detection in data.detections:
+            # 添加ID
+            for id in detection.id:
+                self._apriltag_data_from_odom.id.append(id)
+            
+            # 添加尺寸 (从size数组中获取)
+            if detection.size and len(detection.size) > 0:
+                self._apriltag_data_from_odom.size.append(detection.size[0])
+            else:
+                self._apriltag_data_from_odom.size.append(0.0)
+
+            # 添加姿态
+            self._apriltag_data_from_odom.pose.append(AprilTagDetection(
+                position=detection.pose.pose.pose.position,
+                orientation=detection.pose.pose.pose.orientation
+            ))
+
+        # # debug
+        # rospy.loginfo("Apriltag data from odom: %s", self._apriltag_data_from_odom)
+        
+    @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

@@ -0,0 +1,238 @@
+#!/usr/bin/env python3
+# coding: utf-8
+
+import os
+try:
+    import rospy
+except ImportError:
+    pass
+import subprocess
+import atexit
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+
+class KuavoROSEnv:
+    _instance = None
+    _processes = []  # Store all subprocess instances
+
+    def __new__(cls):
+        if cls._instance is None:
+            cls._instance = super(KuavoROSEnv, 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
+
+    @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:
+        if not rospy.has_param('/modelPath'):
+            raise Exception("modelPath parameter not found")
+        model_path = rospy.get_param('/modelPath')
+        
+        if not os.path.exists(model_path):
+            raise Exception(f"modelPath f{model_path} not found")
+        # ws 
+        return model_path.replace('/src/kuavo_assets/models', '')
+    
+    def Init(self) -> bool:
+        """
+        Initialize the ROS environment.
+        Raises:
+            Exception: If the modelPath parameter is not found or the modelPath parameter is not a valid path.
+        """
+        # 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 ROS master is running
+        try:
+            rospy.get_master().getPid()
+        except Exception as e:
+            print(f"\033[31m\nError:Can't connect to ros master, Please start roscore first or check ROS_MASTER_URI.\nException:{e}\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)
+
+        """
+        # NOTE: We add kuavo_msgs/motion_capture_ik package in SDK integration.
+        kuavo_ws_root = self._get_kuavo_ws_root()
+        # Add kuavo_msgs package to Python path
+        dist_path = os.path.join(kuavo_ws_root, 'devel/lib/python3/dist-packages')
+        if not os.path.exists(dist_path):
+            dist_path = os.path.join(kuavo_ws_root, 'install/lib/python3/dist-packages')
+            if not os.path.exists(dist_path):
+                raise Exception(f"{dist_path} package not found in Python path")
+        if dist_path not in os.sys.path:
+            os.sys.path.append(dist_path)
+        """
+        
+        # Initialize the ROS node
+        if not rospy.get_node_uri():
+            rospy.init_node(f'kuavo_sdk_node', anonymous=True, disable_signals=True)
+        
+        # Only check nodes exist when Init SDK, if not, tips user manually launch nodes.
+        # self.launch_ik_node()
+        # self.launch_gait_switch_node()
+        deps_nodes = ['/humanoid_gait_switch_by_name']
+        for node in deps_nodes:
+            if not KuavoROSEnv.check_rosnode_exists(node):
+                print(f"\033[31m\nError:Node {node} not found. Please launch it manuallly.\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:
+            rospy.get_master().getPid()
+        except Exception as e:
+            raise Exception(f"ROS master is not running. Please start roscore first or check ROS_MASTER_URI, {e}")
+
+        # Check if IK node and services exist
+        try:
+            # Check if arms_ik_node is running
+            nodes = subprocess.check_output(['rosnode', 'list']).decode('utf-8').split('\n')
+            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 rosparam
+                robot_version = rospy.get_param('/robot_version', None)
+                if robot_version is None:
+                    raise Exception("Failed to get robot_version from rosparam")
+                
+                # 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
+             nodes = subprocess.check_output(['rosnode', 'list']).decode('utf-8').split('\n')
+             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.
+        
+        Args:
+            node_name (str): Name of the node to check
+            
+        Returns:
+            bool: True if node is running, False otherwise
+        """
+        try:
+            nodes = subprocess.check_output(['rosnode', 'list']).decode('utf-8').split('\n')
+            # print(f"Debug: nodes: {nodes}")
+            return node_name in nodes
+        except subprocess.CalledProcessError as e:
+            SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
+            return False
+        except Exception as e:
+            SDKLogger.error(f"Error checking if node {node_name} exists: {e}")
+            return False