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

kuavo_humanoid_sdk/kuavo/core/ros/microphone.py

@@ -0,0 +1,38 @@
+import rospy
+import collections
+import threading
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+
+from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import AudioReceiverData
+
+
+class Microphone:
+    """
+    ROS-specific part for handling microphone data subscription.
+    It subscribes to an audio topic and buffers the received data.
+    """
+    def __init__(self, subscribe_topic="/micphone_data"):
+        self._buffer = collections.deque()
+        self._lock = threading.Lock()
+        self.subscriber = rospy.Subscriber(subscribe_topic, AudioReceiverData, self._audio_callback)
+        SDKLogger.debug(f"MicrophoneROSNode subscribed to topic: {subscribe_topic}")
+
+    def _audio_callback(self, msg: AudioReceiverData):
+        """
+        Callback function for the audio subscriber. Appends data to the buffer.
+        """
+        with self._lock:
+            self._buffer.append(msg.data)
+
+    def get_data(self):
+        """
+        Retrieves all data chunks from the buffer and clears it.
+        This is designed to be called by the processing layer.
+        """
+        with self._lock:
+            if not self._buffer:
+                return None
+            
+            data_batch = b''.join(self._buffer)
+            self._buffer.clear()
+            return data_batch

kuavo_humanoid_sdk/kuavo/core/ros/navigation.py

@@ -0,0 +1,217 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import rospy
+from std_msgs.msg import Bool,String
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import LoadMap, LoadMapRequest, LoadMapResponse
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import GetCurrentMap, GetCurrentMapRequest, GetCurrentMapResponse
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import GetAllMaps, GetAllMapsRequest, GetAllMapsResponse
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import TaskPointOperation, TaskPointOperationRequest, TaskPointOperationResponse
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import InitialPoseWithTaskPoint, InitialPoseWithTaskPointRequest, InitialPoseWithTaskPointResponse
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import NavigateToTaskPoint, NavigateToTaskPointRequest, NavigateToTaskPointResponse
+from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import TaskPoint as TaskPointMsg
+from geometry_msgs.msg import Pose, Point, Quaternion, PoseStamped, PoseWithCovarianceStamped
+from actionlib_msgs.msg import GoalStatusArray,GoalID
+from enum import Enum
+class NavigationStatus(Enum):
+    """Navigation status."""
+    PENDING = 0
+    ACTIVE = 1
+    PREEMPTED = 2
+    SUCCEEDED = 3
+    ABORTED = 4
+    REJECTED = 5
+    
+class Navigation:
+    """Navigation system interface for controlling navigation functionality of Kuavo humanoid robot.
+    
+    Provides functionality to play music files.
+    """
+    
+    def __init__(self):
+        """Initialize the navigation system."""
+        self.init_with_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=10, latch=True)
+        self.init_with_task_point_pub = rospy.Publisher('initialpose_with_taskpoint', String, queue_size=10)
+        self.goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10, latch=True)
+        self.move_base_cancel = rospy.Publisher('/move_base/cancel', GoalID, queue_size=10)
+        self.status_sub = rospy.Subscriber('/move_base/status', GoalStatusArray, self.status_callback)
+        self.status = NavigationStatus.PENDING
+         # Wait for publisher initialization
+
+    def status_callback(self, msg: GoalStatusArray):
+        """Callback for the status topic."""
+        if len(msg.status_list) > 0:
+            self.status = NavigationStatus(msg.status_list[0].status)
+
+    def srv_load_map(self, map_name: str):
+        """Load the map."""
+        try:
+            service_name = '/load_map'
+            rospy.wait_for_service(service_name,timeout=3.0)
+            load_map_client = rospy.ServiceProxy(service_name, LoadMap)
+
+            # request
+            req = LoadMapRequest()
+            req.map_name = map_name
+
+            # response  
+            res = load_map_client(req)
+            if res.success:
+                return True
+            else:
+                return False
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service `load_map` call failed: {e}")
+        except rospy.ROSException as e:
+            SDKLogger.error(f"Service `load_map` call failed: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Service `load_map` call failed: {e}")
+        return False
+    
+    def srv_get_current_map(self):
+        """Get the current map."""
+        try:
+            service_name = 'get_current_map'
+            rospy.wait_for_service(service_name,timeout=3.0)
+            get_current_map_client = rospy.ServiceProxy(service_name, GetCurrentMap)        
+
+            # request
+            req = GetCurrentMapRequest()
+
+            # response
+            res = get_current_map_client(req)
+            return res.current_map
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service `get_current_map` call failed: {e}")
+        except rospy.ROSException as e:
+            SDKLogger.error(f"Service `get_current_map` call failed: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Service `get_current_map` call failed: {e}")
+        return None
+    
+    def srv_get_all_maps(self):
+        """Get all maps."""
+        try:
+            service_name = 'get_all_maps'
+            rospy.wait_for_service(service_name,timeout=3.0)
+            get_all_maps_client = rospy.ServiceProxy(service_name, GetAllMaps)  
+            
+            # request   
+            req = GetAllMapsRequest()
+
+            # response
+            res = get_all_maps_client(req)
+            return res.maps
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service `get_all_maps` call failed: {e}")
+        except rospy.ROSException as e:
+            SDKLogger.error(f"Service `get_all_maps` call failed: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Service `get_all_maps` call failed: {e}")
+        return None
+    
+    def srv_navigate_to_task_point(self, task_point_name: String):
+        """Navigate to the task point."""
+        try:
+            service_name = 'navigate_to_task_point'
+            rospy.wait_for_service(service_name,timeout=3.0)
+            navigate_to_task_point_client = rospy.ServiceProxy(service_name, NavigateToTaskPoint)
+            
+            # request
+            req = NavigateToTaskPointRequest()
+            req.task_name = task_point_name
+            
+            # response
+            res = navigate_to_task_point_client(req)
+            return res.success
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service `navigate_to_task_point` call failed: {e}")
+        except rospy.ROSException as e:
+            SDKLogger.error(f"Service `navigate_to_task_point` call failed: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Service `navigate_to_task_point` call failed: {e}")
+        return False
+    
+    def srv_init_localization_by_task_point(self, task_point_name: str):
+        """Initialize the localization by task point."""
+        try:
+            service_name = '/initialpose_with_taskpoint'
+            rospy.wait_for_service(service_name,timeout=3.0)
+            init_localization_by_task_point_client = rospy.ServiceProxy(service_name, InitialPoseWithTaskPoint)
+            
+            # request
+            req = InitialPoseWithTaskPointRequest()
+            req.task_point_name = task_point_name
+            # response
+            res = init_localization_by_task_point_client(req)
+            return res.success
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service `/initialpose_with_taskpoint` call failed: {e}")
+        except rospy.ROSException as e:
+            SDKLogger.error(f"Service `/initialpose_with_taskpoint` call failed: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Service `/initialpose_with_taskpoint` call failed: {e}")
+        return False
+    
+    def pub_init_localization_by_pose(self, pose: Pose):
+        """Initialize the localization by pose."""
+        try:
+            # request
+            initialpose_msg = PoseWithCovarianceStamped()
+            initialpose_msg.header.frame_id = "map"
+            initialpose_msg.header.stamp = rospy.Time.now()
+            initialpose_msg.pose.pose = pose
+            
+            # 转发到/initialpose话题
+            self.init_with_pose_pub.publish(initialpose_msg)
+        except Exception as e:
+            SDKLogger.error(f"Failed to initialize localization by pose: {e}")
+            return False
+        return True
+            
+    def pub_init_localization_by_task_point(self, task_point_name: str):
+        """Initialize the localization by task point."""
+        try:
+            self.init_with_task_point_pub.publish(task_point_name)
+            rospy.sleep(3)
+        except Exception as e:
+            SDKLogger.error(f"Failed to initialize localization by task point: {e}")
+            return False
+        return True
+
+    def pub_navigate_to_goal(self, goal: Pose): 
+        """Navigate to the goal."""
+        try:
+            # request
+            goal_msg = PoseStamped()
+            goal_msg.header.frame_id = "map"
+            goal_msg.header.stamp = rospy.Time.now()
+            goal_msg.pose = goal
+
+            # 转发到/move_base/goal话题
+            self.goal_pub.publish(goal_msg)
+        except Exception as e:
+            SDKLogger.error(f"Failed to navigate to goal: {e}")
+            return False
+        return True
+
+    def get_current_status(self):
+        """Get the current status."""
+        return self.status
+
+    def pub_stop_navigation(self):
+        """Stop the navigation.Cancel all goal."""
+        try:
+            cancel_msg = GoalID()
+            cancel_msg.goal_id = self.goal_id
+            self.move_base_cancel.publish(cancel_msg)
+        except Exception as e:
+            SDKLogger.error(f"Failed to stop navigation: {e}")
+            return False
+        return True
+
+
+
+    
+    

kuavo_humanoid_sdk/kuavo/core/ros/observation.py

@@ -0,0 +1,94 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from kuavo_humanoid_sdk.interfaces.data_types import (
+    KuavoJointCommand, KuavoTwist)
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+import rospy
+from geometry_msgs.msg import Twist
+from kuavo_humanoid_sdk.msg.kuavo_msgs.msg import jointCmd
+
+class KuavoRobotObservationCore:
+    _instance = None
+
+    def __new__(cls, *args, **kwargs):
+        if not cls._instance:
+            cls._instance = super().__new__(cls)
+        return cls._instance
+
+    def __init__(self):
+        if not hasattr(self, '_initialized'):
+            rospy.Subscriber("/joint_cmd", jointCmd, self._joint_cmd_callback)
+            rospy.Subscriber("/cmd_vel", Twist, self._cmd_vel_callback)
+            rospy.Subscriber("/cmd_pose", Twist, self._cmd_pose_callback)
+            
+            """ data """
+            self._joint_cmd = KuavoJointCommand(
+                joint_q = [0.0] * 28,
+                joint_v = [0.0] * 28,
+                tau = [0.0] * 28,
+                tau_max = [0.0] * 28,
+                tau_ratio = [0.0] * 28,
+                joint_kp = [0.0] * 28,
+                joint_kd = [0.0] * 28,
+                control_modes = [0] * 28
+            )
+
+            self._cmd_vel = KuavoTwist(
+                linear = (0.0, 0.0, 0.0),
+                angular = (0.0, 0.0, 0.0)
+            )
+
+            self._cmd_pose = KuavoTwist(
+                linear = (0.0, 0.0, 0.0),
+                angular = (0.0, 0.0, 0.0)
+            )
+
+            self._initialized = True
+            
+    def _joint_cmd_callback(self, msg):
+        self._joint_cmd.joint_q = list(msg.joint_q)
+        self._joint_cmd.joint_v = list(msg.joint_v)
+        self._joint_cmd.tau = list(msg.tau)
+        self._joint_cmd.tau_max = list(msg.tau_max)
+        self._joint_cmd.tau_ratio = list(msg.tau_ratio)
+        self._joint_cmd.joint_kp = list(msg.joint_kp)
+        self._joint_cmd.joint_kd = list(msg.joint_kd)
+        self._joint_cmd.control_modes = list(msg.control_modes)
+        
+    def _cmd_vel_callback(self, msg):
+        self._cmd_vel.linear = (msg.linear.x, msg.linear.y, msg.linear.z)
+        self._cmd_vel.angular = (msg.angular.x, msg.angular.y, msg.angular.z)
+        
+    def _cmd_pose_callback(self, msg):
+        self._cmd_pose.linear = (msg.linear.x, msg.linear.y, msg.linear.z)
+        self._cmd_pose.angular = (msg.angular.x, msg.angular.y, msg.angular.z)
+        
+    @property
+    def joint_command(self) -> KuavoJointCommand:
+        return self._joint_cmd
+        
+    @property
+    def cmd_vel(self) -> KuavoTwist:
+        return self._cmd_vel
+        
+    @property
+    def cmd_pose(self) -> KuavoTwist:
+        return self._cmd_pose
+    
+    @property    
+    def arm_position_command(self) -> list:
+        """Return the position commands for the arm joints (indices 12-25).
+        
+        Returns:
+            list: Position commands for arm joints
+        """
+        return self._joint_cmd.joint_q[12:26]
+    
+    @property
+    def head_position_command(self) -> list:
+        """Return the position commands for the head joints (indices 26-27).
+        
+        Returns:
+            list: Position commands for head joints
+        """
+        return self._joint_cmd.joint_q[-2:]

kuavo_humanoid_sdk/kuavo/core/ros/param.py

@@ -0,0 +1,201 @@
+import rospy
+import json
+import xml.etree.ElementTree as ET
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+# End effector types
+class EndEffectorType:
+    QIANGNAO = "qiangnao"
+    QIANGNAO_TOUCH = "qiangnao_touch"
+    LEJUCLAW = "lejuclaw"
+class RosParameter:
+    def __init__(self):
+        pass
+    def robot_version(self)->str:
+        if not rospy.has_param('/robot_version'):
+            rospy.logerr("robot_version parameter not found")
+            return None
+        return rospy.get_param('/robot_version')
+    
+    def arm_dof(self)->int:
+        if not rospy.has_param('/armRealDof'):
+            rospy.logerr("armRealDof parameter not found")
+            return None
+        return rospy.get_param('/armRealDof')
+    
+    def head_dof(self)->int:
+        if not rospy.has_param('/headRealDof'):
+            rospy.logerr("headRealDof parameter not found")
+            return None
+        return rospy.get_param('/headRealDof')
+    
+    def leg_dof(self)->int:
+        if not rospy.has_param('/legRealDof'):
+            rospy.logerr("legRealDof parameter not found")
+            return None
+        return rospy.get_param('/legRealDof')
+    
+    def end_effector_type(self)->str:
+        if not rospy.has_param('/end_effector_type'):
+            return None
+        return rospy.get_param('/end_effector_type')
+    
+    def humanoid_description(self)->str:
+        if not rospy.has_param('/humanoid_description'):
+            rospy.logerr("humanoid_description parameter not found")
+            return None
+        return rospy.get_param('/humanoid_description')
+
+    def model_path(self)->str:
+        if not rospy.has_param('/modelPath'):
+            rospy.logerr("modelPath parameter not found")
+            return None
+        return rospy.get_param('/modelPath')
+
+    def kuavo_config(self)->str:
+        if not rospy.has_param('/kuavo_configuration'):
+            rospy.logerr("kuavo_configuration parameter not found")
+            return None
+        return rospy.get_param('/kuavo_configuration')
+
+    def initial_state(self)->str:
+        if not rospy.has_param('/initial_state'):
+            rospy.logerr("initial_state parameter not found")
+            return None
+        return rospy.get_param('/initial_state')
+    def init_stand_height(self)->float:
+        if not rospy.has_param('/com_height'):
+            rospy.logerr("com_height parameter not found")
+            # KUAVO-4PRO
+            return 0.8328437523948975
+        return rospy.get_param('/com_height')
+
+kuavo_ros_param = RosParameter()
+
+def joint_names()->dict:
+    if(kuavo_ros_param.robot_version() == 13):
+        leg_link_names = [
+            'leg_l1_link', 'leg_l2_link', 'leg_l3_link', 'leg_l4_link', 'leg_l5_link', 'leg_l6_link',
+            'leg_r1_link', 'leg_r2_link', 'leg_r3_link', 'leg_r4_link', 'leg_r5_link', 'leg_r6_link'
+        ]
+        arm_link_names = [
+            'zarm_l1_link', 'zarm_l2_link', 'zarm_l3_link', 'zarm_l4_link',
+            'zarm_r1_link', 'zarm_r2_link', 'zarm_r3_link', 'zarm_r4_link',
+        ]
+        head_link_names = [
+            'zhead_1_link', 'zhead_2_link'
+        ]
+    else:
+        leg_link_names = [
+            'leg_l1_link', 'leg_l2_link', 'leg_l3_link', 'leg_l4_link', 'leg_l5_link', 'leg_l6_link',
+            'leg_r1_link', 'leg_r2_link', 'leg_r3_link', 'leg_r4_link', 'leg_r5_link', 'leg_r6_link'
+        ]
+        arm_link_names = [
+            'zarm_l1_link', 'zarm_l2_link', 'zarm_l3_link', 'zarm_l4_link', 'zarm_l5_link', 'zarm_l6_link', 'zarm_l7_link',
+            'zarm_r1_link', 'zarm_r2_link', 'zarm_r3_link', 'zarm_r4_link', 'zarm_r5_link', 'zarm_r6_link', 'zarm_r7_link',
+        ]
+        head_link_names = [
+            'zhead_1_link', 'zhead_2_link'
+        ]
+    robot_desc = kuavo_ros_param.humanoid_description()
+    if robot_desc is None:
+        return None
+
+    """
+        <link name="leg_l1_link">
+        <inertial>
+        ....
+        </inertial>
+        <visual>
+        ...
+        <geometry>
+            <mesh filename="package://kuavo_assets/models/biped_s43/meshes/l_leg_roll.STL" />
+        </geometry>
+        ...
+        </visual>
+        </link>
+    """
+    root = ET.fromstring(robot_desc)
+    process_link_name = lambda link_name: (
+        (root.find(f".//link[@name='{link_name}']") is not None and
+        root.find(f".//link[@name='{link_name}']/visual") is not None and
+        root.find(f".//link[@name='{link_name}']/visual/geometry") is not None and
+        root.find(f".//link[@name='{link_name}']/visual/geometry/mesh") is not None and
+        root.find(f".//link[@name='{link_name}']/visual/geometry/mesh").get("filename") is not None)
+        and (
+            # Extract the basename (without path and extension)
+            root.find(f".//link[@name='{link_name}']/visual/geometry/mesh")
+            .get("filename")
+            .split("/")[-1]
+            .split(".")[0]
+        )
+        or (
+            SDKLogger.warn(f"Warning: {link_name} is not found or incomplete in robot_desc"),
+            None
+        )[1]  # Return None after printing the warning
+    )
+    leg_joint_names = [process_link_name(link_name) for link_name in leg_link_names if process_link_name(link_name) is not None]
+    arm_joint_names = [process_link_name(link_name) for link_name in arm_link_names if process_link_name(link_name) is not None]
+    head_joint_names = [process_link_name(link_name) for link_name in head_link_names if process_link_name(link_name) is not None]
+
+    if len(leg_link_names) != len(leg_joint_names):
+        SDKLogger.warn(f"leg_joint_names is not equal to leg_link_names, {len(leg_link_names)} != {len(leg_joint_names)}")
+        return None
+    if len(arm_link_names)!= len(arm_joint_names):
+        SDKLogger.warn(f"arm_joint_names is not equal to arm_link_names, {len(arm_link_names)}!= {len(arm_joint_names)}")
+        return None
+    if len(head_link_names)!= len(head_joint_names):
+        SDKLogger.warn(f"head_joint_names is not equal to head_link_names, {len(head_link_names)}!= {len(head_joint_names)}")
+        return None
+    
+    return leg_joint_names + arm_joint_names + head_joint_names
+
+kuavo_ros_info = None
+
+def end_frames_names()->dict:
+    default = ["torso", "zarm_l7_link", "zarm_r7_link", "zarm_l4_link", "zarm_r4_link"]
+
+    kuavo_ros_param = RosParameter()
+
+    kuavo_json = kuavo_ros_param.kuavo_config()
+    if kuavo_json is None:
+        return default
+    
+    try:
+        kuavo_config = json.loads(kuavo_json)
+        if kuavo_config.get('end_frames_names') is not None:
+            return kuavo_config.get('end_frames_names')
+        else:
+            return default
+    except Exception as e:
+        print(f"Failed to get end_frames_names from kuavo_json: {e}")
+        return default
+
+def make_robot_param()->dict:
+    global kuavo_ros_info
+    if kuavo_ros_info is not None:
+        return kuavo_ros_info
+    
+    kuavo_ros_param = RosParameter()
+
+    kuavo_ros_info = {
+        'robot_version': kuavo_ros_param.robot_version(),
+        'arm_dof': kuavo_ros_param.arm_dof(),
+        'head_dof': kuavo_ros_param.head_dof(),
+        'leg_dof': kuavo_ros_param.leg_dof(),
+        'end_effector_type': kuavo_ros_param.end_effector_type(),
+        'joint_names': joint_names(),
+        'end_frames_names': end_frames_names(),
+        'init_stand_height': kuavo_ros_param.init_stand_height()
+    }
+
+    for key, value in kuavo_ros_info.items():
+        if value is None and key != 'end_effector_type':
+            SDKLogger.debug(f"[Error]: Failed to get '{key}' from ROS.")
+            kuavo_ros_info = None
+            raise Exception(f"[Error]: Failed to get '{key}' from ROS.")
+
+    return kuavo_ros_info        
+
+if __name__ == "__main__":
+    rospy.init_node("kuavo_ros_param_test")
+    print(make_robot_param())

kuavo_humanoid_sdk/kuavo/core/ros/sat_utils.py

@@ -0,0 +1,103 @@
+import numpy as np
+
+class RotatingRectangle:
+    def __init__(self, center, width, height, angle):
+        self.center = center
+        self.width = width
+        self.height = height
+        self.angle = angle
+
+    def set_rotation(self, angle):
+        self.angle = angle
+    
+    def rotate_point(self, point):
+        """旋转点"""
+        px, py = point
+        cos_theta = np.cos(self.angle)
+        sin_theta = np.sin(self.angle)
+        return (cos_theta * px - sin_theta * py, sin_theta * px + cos_theta * py)
+    
+    def get_vertices(self):
+        """获取旋转长方形的顶点"""
+        cx, cy = self.center
+        half_w, half_h = self.width / 2, self.height / 2
+        vertices = [
+            (half_w, half_h),
+            (-half_w, half_h),
+            (-half_w, -half_h),
+            (half_w, -half_h)
+        ]
+        return [(cx + x, cy + y) for x, y in (self.rotate_point(v) for v in vertices)]
+
+    @staticmethod
+    def project(vertices, axis):
+        """将顶点投影到轴上"""
+        projections = [np.dot(v, axis) for v in vertices]
+        return min(projections), max(projections)
+
+    @staticmethod
+    def is_separating_axis(vertices1, vertices2, axis):
+        """检查是否为分离轴"""
+        min1, max1 = RotatingRectangle.project(vertices1, axis)
+        min2, max2 = RotatingRectangle.project(vertices2, axis)
+        return max1 < min2 or max2 < min1
+
+    def is_collision(self, other):
+        """检测两个旋转长方形是否发生碰撞"""
+        vertices1 = self.get_vertices()
+        vertices2 = other.get_vertices()
+        
+        # 获取所有可能的分离轴
+        axes = []
+        for i in range(4):
+            edge = (vertices1[i][0] - vertices1[i-1][0], vertices1[i][1] - vertices1[i-1][1])
+            axis = (-edge[1], edge[0])
+            axes.append(axis)
+        for i in range(4):
+            edge = (vertices2[i][0] - vertices2[i-1][0], vertices2[i][1] - vertices2[i-1][1])
+            axis = (-edge[1], edge[0])
+            axes.append(axis)
+        
+        # 检查所有轴上的投影是否重叠
+        for axis in axes:
+            if self.is_separating_axis(vertices1, vertices2, axis):
+                return False
+        return True
+
+    def show(self, color):
+        """绘制旋转长方形"""
+        vertices = self.get_vertices()
+        vertices.append(vertices[0])  # 闭合多边形
+        xs, ys = zip(*vertices)
+        plt.fill(xs, ys, color=color, alpha=0.5)
+
+
+# 示例
+if __name__ == "__main__":
+    rect1 = RotatingRectangle(center=(0, 0.1), width=0.2, height=0.1, angle=0)
+    rect2 = RotatingRectangle(center=(0, -0.1), width=0.2, height=0.1, angle=0)
+
+    collision = rect1.is_collision(rect2)
+    print("发生碰撞:", collision)
+
+    rect1.set_rotation(-np.pi / 4)
+    rect2.set_rotation(np.pi / 4)
+    collision = rect1.is_collision(rect2)
+    print("发生碰撞:", collision)
+
+    # visualize
+    import matplotlib.pyplot as plt
+    # 绘制长方形
+    plt.figure()
+    rect1.show(color='blue')
+    rect2.show(color='red')
+
+    # 设置显示区域
+    plt.xlim(-0.3, 0.3)
+    plt.ylim(-0.3, 0.3)
+    plt.xlabel('x')
+    plt.ylabel('y')
+    plt.gca().set_aspect('equal', adjustable='box')
+    plt.grid()
+    plt.title(f'Rotated Rectangle Collision Detection: {collision}')
+    plt.show()