kuavo-humanoid-sdk 1.1.3a1252__py3-none-any.whl → 1.1.5__py3-none-any.whl

kuavo_humanoid_sdk/kuavo/core/ros/state.py

@@ -1,5 +1,4 @@
 import roslibpy
-import copy
 import time
 from typing import Tuple
 
@@ -14,12 +13,14 @@ try:
 except:
     pass
 
-from kuavo_humanoid_sdk.interfaces.data_types import (KuavoImuData, KuavoJointData, KuavoOdometry, 
-                                                      KuavoArmCtrlMode, EndEffectorState, KuavoDexHandTouchState)
+from kuavo_humanoid_sdk.interfaces.data_types import (KuavoImuData, KuavoJointData, KuavoOdometry, KuavoManipulationMpcFrame, 
+                                                      KuavoArmCtrlMode, EndEffectorState, KuavoDexHandTouchState,
+                                                      KuavoManipulationMpcCtrlMode, KuavoManipulationMpcControlFlow)
 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.common.websocket_kuavo_sdk import WebSocketKuavoSDK
-
+from kuavo_humanoid_sdk.msg.kuavo_msgs.srv import (changeArmCtrlMode, changeArmCtrlModeRequest,setMmCtrlFrame, setMmCtrlFrameRequest,
+                                                   changeTorsoCtrlMode, changeTorsoCtrlModeRequest)
 from collections import deque
 from typing import Tuple, Optional
 
@@ -157,6 +158,11 @@ class KuavoRobotStateCore:
             self._arm_ctrl_mode = self._srv_get_arm_ctrl_mode()
             self._initialized = True
 
+            # 获取manipulation mpc 相关参数
+            self._manipulation_mpc_frame = self._srv_get_manipulation_mpc_frame()
+            self._manipulation_mpc_ctrl_mode = self._srv_get_manipulation_mpc_ctrl_mode()
+            self._manipulation_mpc_control_flow = self._srv_get_manipulation_mpc_control_flow()
+
     @property
     def com_height(self)->float:
         # odom.position.z - terrain_height = com_height
@@ -181,6 +187,27 @@ class KuavoRobotStateCore:
             self._arm_ctrl_mode = mode
         return self._arm_ctrl_mode
     
+    @property
+    def manipulation_mpc_ctrl_mode(self)->KuavoManipulationMpcCtrlMode:
+        mode = self._srv_get_manipulation_mpc_ctrl_mode()
+        if mode is not None:
+            self._manipulation_mpc_ctrl_mode = mode
+        return self._manipulation_mpc_ctrl_mode
+    
+    @property
+    def manipulation_mpc_frame(self)->KuavoManipulationMpcFrame:
+        frame = self._srv_get_manipulation_mpc_frame()
+        if frame is not None:
+            self._manipulation_mpc_frame = frame
+        return self._manipulation_mpc_frame
+    
+    @property
+    def manipulation_mpc_control_flow(self)->KuavoManipulationMpcControlFlow:
+        flow = self._srv_get_manipulation_mpc_control_flow()
+        if flow is not None:
+            self._manipulation_mpc_control_flow = flow
+        return self._manipulation_mpc_control_flow
+    
     @property
     def eef_state(self)->Tuple[EndEffectorState, EndEffectorState]:
         return self._eef_state
@@ -387,6 +414,194 @@ class KuavoRobotStateCore:
             SDKLogger.error(f"Service call failed: {e}")
         return None
 
+    def _srv_get_manipulation_mpc_ctrl_mode(self, )->KuavoManipulationMpcCtrlMode:
+        try:
+            service_name = '/mobile_manipulator_get_mpc_control_mode'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeTorsoCtrlMode)
+            
+            req = changeTorsoCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc control mode: {resp.message}")
+                return KuavoManipulationMpcCtrlMode.ERROR
+            return KuavoManipulationMpcCtrlMode(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc control mode: {e}")
+        return KuavoManipulationMpcCtrlMode.ERROR
+
+    def _srv_get_manipulation_mpc_frame(self, )->KuavoManipulationMpcFrame:
+        try:
+            service_name = '/get_mm_ctrl_frame'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_frame_srv = rospy.ServiceProxy(service_name, setMmCtrlFrame)
+            
+            req = setMmCtrlFrameRequest()
+            
+            resp = get_frame_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc frame: {resp.message}")
+                return KuavoManipulationMpcFrame.ERROR
+            return KuavoManipulationMpcFrame(resp.currentFrame)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc frame: {e}")
+        return KuavoManipulationMpcFrame.ERROR
+
+    def _srv_get_manipulation_mpc_control_flow(self, )->KuavoManipulationMpcControlFlow:
+        try:
+            service_name = '/get_mm_wbc_arm_trajectory_control'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeArmCtrlMode)
+            
+            req = changeArmCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {resp.message}")
+                return KuavoManipulationMpcControlFlow.Error
+            return KuavoManipulationMpcControlFlow(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {e}")
+        return KuavoManipulationMpcControlFlow.Error
+
+    def _srv_get_manipulation_mpc_ctrl_mode(self, )->KuavoManipulationMpcCtrlMode:
+        try:
+            service_name = '/mobile_manipulator_get_mpc_control_mode'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeTorsoCtrlMode)
+            
+            req = changeTorsoCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc control mode: {resp.message}")
+                return KuavoManipulationMpcCtrlMode.ERROR
+            return KuavoManipulationMpcCtrlMode(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc control mode: {e}")
+        return KuavoManipulationMpcCtrlMode.ERROR
+
+    def _srv_get_manipulation_mpc_frame(self, )->KuavoManipulationMpcFrame:
+        try:
+            service_name = '/get_mm_ctrl_frame'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_frame_srv = rospy.ServiceProxy(service_name, setMmCtrlFrame)
+            
+            req = setMmCtrlFrameRequest()
+            
+            resp = get_frame_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc frame: {resp.message}")
+                return KuavoManipulationMpcFrame.ERROR
+            return KuavoManipulationMpcFrame(resp.currentFrame)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc frame: {e}")
+        return KuavoManipulationMpcFrame.ERROR
+
+    def _srv_get_manipulation_mpc_control_flow(self, )->KuavoManipulationMpcControlFlow:
+        try:
+            service_name = '/get_mm_wbc_arm_trajectory_control'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeArmCtrlMode)
+            
+            req = changeArmCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {resp.message}")
+                return KuavoManipulationMpcControlFlow.Error
+            return KuavoManipulationMpcControlFlow(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {e}")
+        return KuavoManipulationMpcControlFlow.Error
+
+    def _srv_get_manipulation_mpc_ctrl_mode(self, )->KuavoManipulationMpcCtrlMode:
+        try:
+            service_name = '/mobile_manipulator_get_mpc_control_mode'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeTorsoCtrlMode)
+            
+            req = changeTorsoCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc control mode: {resp.message}")
+                return KuavoManipulationMpcCtrlMode.ERROR
+            return KuavoManipulationMpcCtrlMode(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc control mode: {e}")
+        return KuavoManipulationMpcCtrlMode.ERROR
+
+    def _srv_get_manipulation_mpc_frame(self, )->KuavoManipulationMpcFrame:
+        try:
+            service_name = '/get_mm_ctrl_frame'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_frame_srv = rospy.ServiceProxy(service_name, setMmCtrlFrame)
+            
+            req = setMmCtrlFrameRequest()
+            
+            resp = get_frame_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc frame: {resp.message}")
+                return KuavoManipulationMpcFrame.ERROR
+            return KuavoManipulationMpcFrame(resp.currentFrame)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc frame: {e}")
+        return KuavoManipulationMpcFrame.ERROR
+
+    def _srv_get_manipulation_mpc_control_flow(self, )->KuavoManipulationMpcControlFlow:
+        try:
+            service_name = '/get_mm_wbc_arm_trajectory_control'
+            rospy.wait_for_service(service_name, timeout=2.0)
+            get_mode_srv = rospy.ServiceProxy(service_name, changeArmCtrlMode)
+            
+            req = changeArmCtrlModeRequest()
+            
+            resp = get_mode_srv(req)
+            if not resp.result:
+                SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {resp.message}")
+                return KuavoManipulationMpcControlFlow.Error
+            return KuavoManipulationMpcControlFlow(resp.mode)
+        except rospy.ServiceException as e:
+            SDKLogger.error(f"Service call to {service_name} failed: {e}")
+        except rospy.ROSException as e: # For timeout from wait_for_service
+            SDKLogger.error(f"Failed to connect to service {service_name}: {e}")
+        except Exception as e:
+            SDKLogger.error(f"Failed to get manipulation mpc wbc arm trajectory control mode: {e}")
+        return KuavoManipulationMpcControlFlow.Error
 class KuavoRobotStateCoreWebsocket:
     _instance = None
 
@@ -691,4 +906,12 @@ class KuavoRobotStateCoreWebsocket:
             return None
         except Exception as e:
             SDKLogger.error(f"Service call failed: {e}")
-        return None
+        return None
+
+
+if __name__ == "__main__":
+    state = KuavoRobotStateCore()
+    print(state.manipulation_mpc_frame)
+    print(state.manipulation_mpc_control_flow)
+    print(state.manipulation_mpc_ctrl_mode)
+    print(state.arm_control_mode)

kuavo_humanoid_sdk/kuavo/core/ros/vision.py

@@ -9,8 +9,6 @@ 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)
-import roslibpy
-from kuavo_humanoid_sdk.common.websocket_kuavo_sdk import WebSocketKuavoSDK
 
 try:
     import rospy
@@ -36,16 +34,16 @@ class KuavoRobotVisionCore:
     def __init__(self):
         """Initializes vision system components including TF and AprilTag subscribers."""
         if not hasattr(self, '_initialized'):
+            # Initialize TF components
             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)
 
+            # FIRST: Initialize data structures before creating subscribers
             """ data """
             self._apriltag_data_from_camera = AprilTagData(
                 id = [],
@@ -64,6 +62,13 @@ class KuavoRobotVisionCore:
                 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)
+            
+            # Mark as initialized
+            self._initialized = True
 
     def _apriltag_data_callback_camera(self, data):
         """Callback for processing AprilTag detections from camera.
@@ -149,7 +154,7 @@ class KuavoRobotVisionCore:
         
         # 如果base数据为空，则不处理
         if not self._apriltag_data_from_base.id:
-            SDKLogger.warn("No base tag data, skip transform")
+            # SDKLogger.warn("No base tag data, skip transform")
             return
         
         try:
@@ -248,7 +253,7 @@ class KuavoRobotVisionCore:
         }
         
         if data_source not in data_map:
-            SDKLogger.error(f"Invalid data source: {data_source}, must be one of {list(data_map.keys())}")
+            # SDKLogger.error(f"Invalid data source: {data_source}, must be one of {list(data_map.keys())}")
             return None
         
         data = data_map[data_source]
@@ -257,253 +262,9 @@ class KuavoRobotVisionCore:
         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]
-        }
-
-class KuavoRobotVisionCoreWebsocket:
-    _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'):
-            try:
-                self.websocket = WebSocketKuavoSDK()
-                if not self.websocket.client.is_connected:
-                    SDKLogger.error("Failed to connect to WebSocket server")
-                    raise ConnectionError("Failed to connect to WebSocket server")
-                
-                # Initialize subscribers for vision-related topics
-                self._sub_apriltag_camera = roslibpy.Topic(self.websocket.client, '/tag_detections', 'apriltag_ros/AprilTagDetectionArray')
-                self._sub_apriltag_base = roslibpy.Topic(self.websocket.client, '/robot_tag_info', 'apriltag_ros/AprilTagDetectionArray')
-                
-                # Initialize TF-related topics
-                self._sub_tf = roslibpy.Topic(self.websocket.client, '/tf', 'tf2_msgs/TFMessage')
-                self._sub_tf_static = roslibpy.Topic(self.websocket.client, '/tf_static', 'tf2_msgs/TFMessage')
-                
-                # Subscribe to topics
-                self._sub_apriltag_camera.subscribe(self._apriltag_camera_callback)
-                self._sub_apriltag_base.subscribe(self._apriltag_base_callback)
-                self._sub_tf.subscribe(self._tf_callback)
-                self._sub_tf_static.subscribe(self._tf_static_callback)
-                
-                # Initialize data structures
-                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 = []
-                )
-                
-                # TF buffer for storing transforms
-                self._tf_buffer = {}
-                self._tf_static_buffer = {}
-                
-                self._initialized = True
-            except Exception as e:
-                SDKLogger.error(f"Failed to initialize KuavoRobotVisionCoreWebsocket: {e}")
-                raise
-
-    def _tf_callback(self, msg):
-        """Callback for TF messages."""
-        for transform in msg['transforms']:
-            key = (transform['header']['frame_id'], transform['child_frame_id'])
-            self._tf_buffer[key] = transform
-
-    def _tf_static_callback(self, msg):
-        """Callback for static TF messages."""
-        for transform in msg['transforms']:
-            key = (transform['header']['frame_id'], transform['child_frame_id'])
-            self._tf_static_buffer[key] = transform
-
-    def _get_transform(self, target_frame, source_frame):
-        """Get transform between two frames.
-        
-        Args:
-            target_frame (str): Target frame ID
-            source_frame (str): Source frame ID
-            
-        Returns:
-            dict: Transform data if found, None otherwise
-        """
-        # Check both dynamic and static transforms
-        key = (source_frame, target_frame)
-        if key in self._tf_buffer:
-            return self._tf_buffer[key]
-        if key in self._tf_static_buffer:
-            return self._tf_static_buffer[key]
-        return None
-
-    def _transform_pose(self, pose, transform):
-        """Transform a pose using the given transform.
-        
-        Args:
-            pose (dict): Pose to transform
-            transform (dict): Transform to apply
-            
-        Returns:
-            dict: Transformed pose
-        """
-        # Extract transform components
-        t = transform['transform']
-        translation = t['translation']
-        rotation = t['rotation']
-        
-        # Extract pose components
-        p = pose['position']
-        o = pose['orientation']
-        
-        # TODO: Implement actual pose transformation
-        # This is a placeholder - actual implementation would involve
-        # proper quaternion and vector math
-        transformed_pose = {
-            'position': {
-                'x': p['x'] + translation['x'],
-                'y': p['y'] + translation['y'],
-                'z': p['z'] + translation['z']
-            },
-            'orientation': {
-                'x': o['x'],
-                'y': o['y'],
-                'z': o['z'],
-                'w': o['w']
-            }
-        }
-        
-        return transformed_pose
-
-    def _apriltag_camera_callback(self, msg):
-        """Callback for AprilTag detections in camera frame."""
-        # Clear previous data
-        self._apriltag_data_from_camera.id = []
-        self._apriltag_data_from_camera.size = []
-        self._apriltag_data_from_camera.pose = []
-        
-        # Process each detection
-        for detection in msg['detections']:
-            # Add ID
-            for tag_id in detection['id']:
-                self._apriltag_data_from_camera.id.append(tag_id)
-            
-            # Add size
-            if detection.get('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)
-            
-            # Add pose
-            self._apriltag_data_from_camera.pose.append(detection['pose']['pose']['pose'])
-
-    def _apriltag_base_callback(self, msg):
-        """Callback for AprilTag detections in base frame."""
-        # Clear previous data
-        self._apriltag_data_from_base.id = []
-        self._apriltag_data_from_base.size = []
-        self._apriltag_data_from_base.pose = []
-        
-        # Process each detection
-        for detection in msg['detections']:
-            # Add ID
-            for tag_id in detection['id']:
-                self._apriltag_data_from_base.id.append(tag_id)
-            
-            # Add size
-            if detection.get('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)
-            
-            # Add pose
-            self._apriltag_data_from_base.pose.append(detection['pose']['pose']['pose'])
-        
-        # Transform base data to odom frame
-        self._transform_base_to_odom()
-
-    def _transform_base_to_odom(self):
-        """Transform AprilTag poses from base_link to odom coordinate frame."""
-        # Clear previous odom data
-        self._apriltag_data_from_odom.id = []
-        self._apriltag_data_from_odom.size = []
-        self._apriltag_data_from_odom.pose = []
-        
-        # If no base data, skip transformation
-        if not self._apriltag_data_from_base.id:
-            SDKLogger.warn("No base tag data, skip transform")
-            return
-        
-        # Get transform from base_link to odom
-        transform = self._get_transform("odom", "base_link")
-        if not transform:
-            SDKLogger.warn("Transform from base_link to odom not available")
-            return
-        
-        # Copy ID and size information
-        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()
-        
-        # Transform each pose
-        for pose in self._apriltag_data_from_base.pose:
-            transformed_pose = self._transform_pose(pose, transform)
-            self._apriltag_data_from_odom.pose.append(transformed_pose)
-
-    @property
-    def apriltag_data_from_camera(self):
-        return self._apriltag_data_from_camera
-    
-    @property
-    def apriltag_data_from_base(self):
-        return self._apriltag_data_from_base
-    
-    @property
-    def apriltag_data_from_odom(self):
-        return self._apriltag_data_from_odom
-
-    def get_data_by_id(self, tag_id: int, frame: str = "odom"):
-        """Get AprilTag data for a specific tag ID from the specified frame.
-        
-        Args:
-            tag_id (int): The ID of the AprilTag to get data for
-            frame (str): The frame to get data from ("camera", "base", or "odom")
-            
-        Returns:
-            dict: The AprilTag data for the specified ID and frame, or None if not found
-        """
-        if frame == "camera":
-            data = self._apriltag_data_from_camera
-        elif frame == "base":
-            data = self._apriltag_data_from_base
-        elif frame == "odom":
-            data = self._apriltag_data_from_odom
-        else:
-            SDKLogger.error(f"Invalid frame: {frame}")
+            # SDKLogger.debug(f"No data found for tag ID {target_id} in {data_source} source")
             return None
-            
-        if not data or not data.id:
-            return None
-            
-        # Find all matching indices
-        indices = [i for i, id in enumerate(data.id) if id == tag_id]
         
-        if not indices:
-            return None
-            
         return {
             "sizes": [data.size[i] for i in indices],
             "poses": [data.pose[i] for i in indices]