kuavo-humanoid-sdk 1.1.2a922__py3-none-any.whl → 1.1.3a1226__py3-none-any.whl

kuavo_humanoid_sdk/kuavo/leju_claw.py

@@ -5,13 +5,17 @@ from typing import Tuple
 from kuavo_humanoid_sdk.interfaces.end_effector import EndEffector
 from kuavo_humanoid_sdk.interfaces.data_types import EndEffectorSide, EndEffectorState
 from kuavo_humanoid_sdk.kuavo.core.leju_claw_control import LejuClawControl
-from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
+from kuavo_humanoid_sdk.common.global_config import GlobalConfig
 
 class LejuClaw(EndEffector):
     def __init__(self):
         super().__init__(joint_names=['left_claw', 'right_claw'])
         self.leju_claw_control = LejuClawControl()
-        self._rb_state = KuavoRobotStateCore()
+        if GlobalConfig.use_websocket:
+            self._rb_state = KuavoRobotStateCoreWebsocket()
+        else:
+            self._rb_state = KuavoRobotStateCore()
 
     def control(self, target_positions: list, target_velocities:list=None, target_torques: list=None)->bool:
         """Control the claws to grip.

kuavo_humanoid_sdk/kuavo/robot.py

@@ -1,18 +1,17 @@
 #!/usr/bin/env python3
 # coding: utf-8
-from kuavo_humanoid_sdk.kuavo.core.ros_env import KuavoROSEnv
-kuavo_ros_env = KuavoROSEnv()
-if not kuavo_ros_env.Init():
-    raise RuntimeError("Failed to initialize ROS environment")
-else:
-    from typing import Tuple
-    from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
-    from kuavo_humanoid_sdk.kuavo.robot_arm import KuavoRobotArm 
-    from kuavo_humanoid_sdk.kuavo.robot_head import KuavoRobotHead
-    from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
-    from kuavo_humanoid_sdk.interfaces.robot import RobotBase
-    from kuavo_humanoid_sdk.interfaces.data_types import KuavoPose, KuavoIKParams
-    from kuavo_humanoid_sdk.common.logger import SDKLogger, disable_sdk_logging
+from kuavo_humanoid_sdk.common.global_config import GlobalConfig
+
+from kuavo_humanoid_sdk.kuavo.core.ros_env import KuavoROSEnv, KuavoROSEnvWebsocket
+from kuavo_humanoid_sdk.interfaces.robot import RobotBase
+from kuavo_humanoid_sdk.common.logger import SDKLogger, disable_sdk_logging
+from kuavo_humanoid_sdk.interfaces.data_types import KuavoPose, KuavoIKParams
+from kuavo_humanoid_sdk.kuavo.core.core import KuavoRobotCore
+
+from typing import Tuple
+from kuavo_humanoid_sdk.kuavo.robot_info import KuavoRobotInfo
+from kuavo_humanoid_sdk.kuavo.robot_arm import KuavoRobotArm 
+from kuavo_humanoid_sdk.kuavo.robot_head import KuavoRobotHead
 
 """
 Kuavo SDK - Python Interface for Kuavo Robot Control
@@ -45,19 +44,30 @@ class KuavoSDK:
        pass
 
     @staticmethod   
-    def Init(options:int=Options.Normal, log_level: str = "INFO")-> bool:
+    def Init(options:int=Options.Normal, log_level: str = "INFO", 
+             websocket_mode: bool = False, websocket_host='127.0.0.1', websocket_port=9090, websocket_timeout=5)-> bool:
         """Initialize the SDK.
-        
         Args:
             log_level (str): The logging level to use. Can be "ERROR", "WARN", "INFO", "DEBUG".
                 Defaults to "INFO".
-                
+            websocket_mode (bool): Whether to use websocket mode for ROS communication. If True,
+                the SDK will connect to ROS through websocket instead of direct ROS connection.
+                Defaults to False.
+            websocket_host (str): The host address of the rosbridge websocket server when using
+                websocket mode. Defaults to "127.0.0.1".
+            websocket_port (int): The port number of the rosbridge websocket server when using
+                websocket mode. Defaults to 9090.
+            websocket_timeout (int): The timeout for the websocket connection. Defaults to 5.
         Returns:
             bool: True if initialization is successful, False otherwise.
             
         Raises:
             RuntimeError: If the initialization fails.
         """
+        GlobalConfig.use_websocket = websocket_mode
+        GlobalConfig.websocket_host = websocket_host
+        GlobalConfig.websocket_port = websocket_port
+        GlobalConfig.websocket_timeout = websocket_timeout
         SDKLogger.setLevel(log_level.upper())
         # Initialize core components, connect ROS Topics...
         kuavo_core = KuavoRobotCore()
@@ -65,12 +75,18 @@ class KuavoSDK:
             debug = True
         else:
             debug = False   
-
         # Check if IK option is enabled
-        if options & KuavoSDK.Options.WithIK:
-            if not KuavoROSEnv.check_rosnode_exists('/arms_ik_node'):
-                print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
-                exit(1)
+        if GlobalConfig.use_websocket:
+            if options & KuavoSDK.Options.WithIK:
+                if not KuavoROSEnvWebsocket.check_rosnode_exists('/arms_ik_node'):
+                    print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
+                    exit(1)
+        else:
+            if options & KuavoSDK.Options.WithIK:
+                if not KuavoROSEnv.check_rosnode_exists('/arms_ik_node'):
+                    print("\033[31m\nError:WithIK option is enabled but ik_node is not running, please run `roslaunch motion_capture_ik ik_node.launch`\033[0m")      
+                    exit(1)
+
 
         if not kuavo_core.initialize(debug=debug):
             SDKLogger.error("[SDK] Failed to initialize core components.")
@@ -86,11 +102,16 @@ class KuavoSDK:
 class KuavoRobot(RobotBase):
     def __init__(self):
         super().__init__(robot_type="kuavo")
+        
+        if not GlobalConfig.use_websocket:
+            kuavo_ros_env = KuavoROSEnv()
+            if not kuavo_ros_env.Init():
+                raise RuntimeError("Failed to initialize ROS environment")
+
         self._robot_info = KuavoRobotInfo()
         self._robot_arm  = KuavoRobotArm()
         self._robot_head = KuavoRobotHead()
         self._kuavo_core = KuavoRobotCore()
-
     def stance(self)->bool:
         """Put the robot into 'stance' mode.
         

kuavo_humanoid_sdk/kuavo/robot_audio.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+# coding: utf-8
+from kuavo_humanoid_sdk.kuavo.core.audio import KuavoRobotAudioCore   
+
+class KuavoRobotAudio:
+    """Audio system interface for controlling audio playback functionality of Kuavo humanoid robot.
+    
+    Provides functionality to play music files.
+    """
+    
+    def __init__(self):
+        """Initialize the audio system."""
+        self.audio = KuavoRobotAudioCore()
+
+    def play_audio(self, file_name: str, volume: float = 0.5, speed: float = 1.0) -> bool:
+        """Play the specified audio file.
+        
+        Args:
+            file_name (str): Name of the audio file to play
+                
+        Returns:
+            bool: True if the play request was successfully sent, False otherwise
+        """
+        return self.audio.play_audio(file_name, volume, speed)
+    
+    def stop_music(self):
+        """Stop the currently playing audio."""
+        return self.audio.stop_music()
+    
+    def text_to_speech(self, text: str, volume: float = 0.5) -> bool:
+        """Synthesize and play the specified text.
+        
+        Args:
+            text (str): Text to be played
+                
+        Returns:
+            bool: True if the play request was successfully sent, False otherwise
+        """
+        return self.audio.text_to_speech(text, volume)

kuavo_humanoid_sdk/kuavo/robot_info.py

@@ -2,7 +2,8 @@
 # coding: utf-8
 from typing import Tuple
 from kuavo_humanoid_sdk.interfaces.robot_info import RobotInfoBase
-from kuavo_humanoid_sdk.kuavo.core.ros.param import RosParameter, make_robot_param
+from kuavo_humanoid_sdk.kuavo.core.ros.param import RosParameter, RosParamWebsocket, make_robot_param
+from kuavo_humanoid_sdk.common.global_config import GlobalConfig
 
 class KuavoRobotInfo(RobotInfoBase):
     def __init__(self, robot_type: str = "kuavo"):
@@ -10,7 +11,11 @@ class KuavoRobotInfo(RobotInfoBase):
         
         # Load robot parameters from ROS parameter server
         kuavo_ros_param = make_robot_param()
-        self._ros_param = RosParameter()
+        if GlobalConfig.use_websocket:
+            self._ros_param = RosParamWebsocket()
+        else:
+            self._ros_param = RosParameter()
+            
         self._robot_version = kuavo_ros_param['robot_version']
         self._end_effector_type = kuavo_ros_param['end_effector_type']
         self._arm_joint_dof = kuavo_ros_param['arm_dof']

kuavo_humanoid_sdk/kuavo/robot_state.py

@@ -4,11 +4,15 @@ import time
 from typing import Tuple
 from kuavo_humanoid_sdk.interfaces.data_types import (
     KuavoImuData, KuavoJointData, KuavoOdometry, KuavoArmCtrlMode,EndEffectorState)
-from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore
+from kuavo_humanoid_sdk.kuavo.core.ros.state import KuavoRobotStateCore, KuavoRobotStateCoreWebsocket
+from kuavo_humanoid_sdk.common.global_config import GlobalConfig
 
 class KuavoRobotState:
     def __init__(self, robot_type: str = "kuavo"):
-        self._rs_core = KuavoRobotStateCore()
+        if GlobalConfig.use_websocket:
+            self._rs_core = KuavoRobotStateCoreWebsocket()
+        else:
+            self._rs_core = KuavoRobotStateCore()
 
     @property
     def imu_data(self) -> KuavoImuData:

kuavo_humanoid_sdk/kuavo/robot_tool.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import math
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.interfaces.data_types import (PoseQuaternion, HomogeneousMatrix)
+from kuavo_humanoid_sdk.kuavo.core.ros.tools import KuavoRobotToolsCore
+from typing import Union
+
+class KuavoRobotTools:
+    """Robot tools class providing coordinate frame transformation interfaces.
+    
+    This class encapsulates coordinate transformation queries between different robot frames,
+    supporting multiple return data formats.
+    """
+    
+    def __init__(self):
+        """Initialize robot tools instance."""
+        self.tools_core = KuavoRobotToolsCore()
+
+    def get_tf_transform(self, target_frame: str, source_frame: str, 
+                       return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Get transformation between specified coordinate frames.
+        
+        Args:
+            target_frame (str): Name of target coordinate frame
+            source_frame (str): Name of source coordinate frame
+            return_type (str, optional): Return data format type. Valid values: 
+                "pose_quaternion" - quaternion pose format,
+                "homogeneous" - homogeneous matrix format. Defaults to "pose_quaternion".
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: 
+                Transformation data in specified format, or None if failed
+        
+        Raises:
+            ValueError: If invalid return_type is provided
+        """
+        return self.tools_core._get_tf_tree_transform(target_frame, source_frame, return_type=return_type)
+
+    def get_base_to_odom(self, return_type: str = "pose_quaternion") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Get transformation from base_link to odom frame.
+        
+        Args:
+            return_type (str, optional): Return format type. Same as get_tf_transform. 
+                Defaults to "pose_quaternion".
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: Transformation data or None
+        """
+        return self.get_tf_transform("odom", "base_link", return_type)
+
+    def get_camera_to_base(self, return_type: str = "homogeneous") -> Union[PoseQuaternion, HomogeneousMatrix, None]:
+        """Get transformation from camera_link to base_link frame.
+        
+        Args:
+            return_type (str, optional): Return format type. Same as get_tf_transform.
+                Defaults to "homogeneous".
+        
+        Returns:
+            Union[PoseQuaternion, HomogeneousMatrix, None]: Transformation data or None
+        """
+        return self.get_tf_transform("base_link", "camera_link", return_type)

kuavo_humanoid_sdk/kuavo/robot_vision.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+# coding: utf-8
+import math
+from kuavo_humanoid_sdk.common.logger import SDKLogger
+from kuavo_humanoid_sdk.interfaces.data_types import (AprilTagData)
+from kuavo_humanoid_sdk.kuavo.core.ros.vision import KuavoRobotVisionCore
+
+from apriltag_ros.msg import AprilTagDetectionArray
+
+class KuavoRobotVision:
+    """Vision system interface for Kuavo humanoid robot.
+    
+    Provides access to AprilTag detection data from different coordinate frames.
+    
+    """
+    
+    def __init__(self, robot_type: str = "kuavo"):
+        """Initialize the vision system.
+        
+        Args:
+            robot_type (str, optional): Robot type identifier. Defaults to "kuavo".
+        """
+        if not hasattr(self, '_initialized'):
+            self._vision_core = KuavoRobotVisionCore()
+
+    def get_data_by_id(self, target_id: int, data_source: str = "base") -> dict:
+        """Get AprilTag detection data for a specific ID from specified source.
+        
+        Args:
+            target_id (int): AprilTag ID to retrieve
+            data_source (str, optional): Data source frame. Can be "base", "camera", 
+                or "odom". Defaults to "base".
+                
+        Returns:
+            dict: Detection data containing position, orientation and metadata
+        """
+        return self._vision_core._get_data_by_id(target_id, data_source)
+    
+    def get_data_by_id_from_camera(self, target_id: int) -> dict:
+        """Get AprilTag data from camera coordinate frame.
+        
+        Args:
+            target_id (int): AprilTag ID to retrieve
+            
+        Returns:
+            dict: See get_data_by_id() for return format
+        """
+        return self._vision_core._get_data_by_id(target_id, "camera")
+    
+    def get_data_by_id_from_base(self, target_id: int) -> dict:
+        """Get AprilTag data from base coordinate frame.
+        
+        Args:
+            target_id (int): AprilTag ID to retrieve
+            
+        Returns:
+            dict: See get_data_by_id() for return format
+        """
+        return self._vision_core._get_data_by_id(target_id, "base")
+    
+    def get_data_by_id_from_odom(self, target_id: int) -> dict:
+        """Get AprilTag data from odom coordinate frame.
+        
+        Args:
+            target_id (int): AprilTag ID to retrieve
+            
+        Returns:
+            dict: See get_data_by_id() for return format
+        """
+        return self._vision_core._get_data_by_id(target_id, "odom")
+    
+    @property
+    def apriltag_data_from_camera(self) -> AprilTagData:
+        """AprilTagData: All detected AprilTags in camera frame (property)"""
+        return self._vision_core.apriltag_data_from_camera
+    
+    @property
+    def apriltag_data_from_base(self) -> AprilTagData:
+        """AprilTagData: All detected AprilTags in camera frame (property)"""
+        return self._vision_core.apriltag_data_from_base
+    
+    @property
+    def apriltag_data_from_odom(self) -> AprilTagData:
+        """AprilTagData: All detected AprilTags in camera frame (property)"""
+        return self._vision_core.apriltag_data_from_odom
+    
+

kuavo_humanoid_sdk/msg/kuavo_msgs/msg/_AprilTagDetection.py

@@ -0,0 +1,306 @@
+# This Python file uses the following encoding: utf-8
+"""autogenerated by genpy from kuavo_msgs/AprilTagDetection.msg. Do not edit."""
+import codecs
+import sys
+python3 = True if sys.hexversion > 0x03000000 else False
+import genpy
+import struct
+
+import geometry_msgs.msg
+import std_msgs.msg
+
+class AprilTagDetection(genpy.Message):
+  _md5sum = "090173a6e2b6c8fd96ce000fe9378b4e"
+  _type = "kuavo_msgs/AprilTagDetection"
+  _has_header = False  # flag to mark the presence of a Header object
+  _full_text = """# Tag ID(s). If a standalone tag, this is a vector of size 1. If a tag bundle,
+# this is a vector containing the IDs of each tag in the bundle.
+int32[] id
+
+# Tag size(s). If a standalone tag, this is a vector of size 1. If a tag bundle,
+# this is a vector containing the sizes of each tag in the bundle, in the same
+# order as the IDs above.
+float64[] size
+
+# Pose in the camera frame, obtained from homography transform. If a standalone
+# tag, the homography is from the four tag corners. If a tag bundle, the
+# homography is from at least the four corners of one member tag and at most the
+# four corners of all member tags.
+geometry_msgs/PoseWithCovarianceStamped pose
+================================================================================
+MSG: geometry_msgs/PoseWithCovarianceStamped
+# This expresses an estimated pose with a reference coordinate frame and timestamp
+
+Header header
+PoseWithCovariance pose
+
+================================================================================
+MSG: std_msgs/Header
+# Standard metadata for higher-level stamped data types.
+# This is generally used to communicate timestamped data 
+# in a particular coordinate frame.
+# 
+# sequence ID: consecutively increasing ID 
+uint32 seq
+#Two-integer timestamp that is expressed as:
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
+# time-handling sugar is provided by the client library
+time stamp
+#Frame this data is associated with
+string frame_id
+
+================================================================================
+MSG: geometry_msgs/PoseWithCovariance
+# This represents a pose in free space with uncertainty.
+
+Pose pose
+
+# Row-major representation of the 6x6 covariance matrix
+# The orientation parameters use a fixed-axis representation.
+# In order, the parameters are:
+# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
+float64[36] covariance
+
+================================================================================
+MSG: geometry_msgs/Pose
+# A representation of pose in free space, composed of position and orientation. 
+Point position
+Quaternion orientation
+
+================================================================================
+MSG: geometry_msgs/Point
+# This contains the position of a point in free space
+float64 x
+float64 y
+float64 z
+
+================================================================================
+MSG: geometry_msgs/Quaternion
+# This represents an orientation in free space in quaternion form.
+
+float64 x
+float64 y
+float64 z
+float64 w
+"""
+  __slots__ = ['id','size','pose']
+  _slot_types = ['int32[]','float64[]','geometry_msgs/PoseWithCovarianceStamped']
+
+  def __init__(self, *args, **kwds):
+    """
+    Constructor. Any message fields that are implicitly/explicitly
+    set to None will be assigned a default value. The recommend
+    use is keyword arguments as this is more robust to future message
+    changes.  You cannot mix in-order arguments and keyword arguments.
+
+    The available fields are:
+       id,size,pose
+
+    :param args: complete set of field values, in .msg order
+    :param kwds: use keyword arguments corresponding to message field names
+    to set specific fields.
+    """
+    if args or kwds:
+      super(AprilTagDetection, self).__init__(*args, **kwds)
+      # message fields cannot be None, assign default values for those that are
+      if self.id is None:
+        self.id = []
+      if self.size is None:
+        self.size = []
+      if self.pose is None:
+        self.pose = geometry_msgs.msg.PoseWithCovarianceStamped()
+    else:
+      self.id = []
+      self.size = []
+      self.pose = geometry_msgs.msg.PoseWithCovarianceStamped()
+
+  def _get_types(self):
+    """
+    internal API method
+    """
+    return self._slot_types
+
+  def serialize(self, buff):
+    """
+    serialize message into buffer
+    :param buff: buffer, ``StringIO``
+    """
+    try:
+      length = len(self.id)
+      buff.write(_struct_I.pack(length))
+      pattern = '<%si'%length
+      buff.write(struct.Struct(pattern).pack(*self.id))
+      length = len(self.size)
+      buff.write(_struct_I.pack(length))
+      pattern = '<%sd'%length
+      buff.write(struct.Struct(pattern).pack(*self.size))
+      _x = self
+      buff.write(_get_struct_3I().pack(_x.pose.header.seq, _x.pose.header.stamp.secs, _x.pose.header.stamp.nsecs))
+      _x = self.pose.header.frame_id
+      length = len(_x)
+      if python3 or type(_x) == unicode:
+        _x = _x.encode('utf-8')
+        length = len(_x)
+      buff.write(struct.Struct('<I%ss'%length).pack(length, _x))
+      _x = self
+      buff.write(_get_struct_7d().pack(_x.pose.pose.pose.position.x, _x.pose.pose.pose.position.y, _x.pose.pose.pose.position.z, _x.pose.pose.pose.orientation.x, _x.pose.pose.pose.orientation.y, _x.pose.pose.pose.orientation.z, _x.pose.pose.pose.orientation.w))
+      buff.write(_get_struct_36d().pack(*self.pose.pose.covariance))
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self)))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self)))))
+
+  def deserialize(self, str):
+    """
+    unpack serialized message in str into this message instance
+    :param str: byte array of serialized message, ``str``
+    """
+    if python3:
+      codecs.lookup_error("rosmsg").msg_type = self._type
+    try:
+      if self.pose is None:
+        self.pose = geometry_msgs.msg.PoseWithCovarianceStamped()
+      end = 0
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      pattern = '<%si'%length
+      start = end
+      s = struct.Struct(pattern)
+      end += s.size
+      self.id = s.unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      pattern = '<%sd'%length
+      start = end
+      s = struct.Struct(pattern)
+      end += s.size
+      self.size = s.unpack(str[start:end])
+      _x = self
+      start = end
+      end += 12
+      (_x.pose.header.seq, _x.pose.header.stamp.secs, _x.pose.header.stamp.nsecs,) = _get_struct_3I().unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      start = end
+      end += length
+      if python3:
+        self.pose.header.frame_id = str[start:end].decode('utf-8', 'rosmsg')
+      else:
+        self.pose.header.frame_id = str[start:end]
+      _x = self
+      start = end
+      end += 56
+      (_x.pose.pose.pose.position.x, _x.pose.pose.pose.position.y, _x.pose.pose.pose.position.z, _x.pose.pose.pose.orientation.x, _x.pose.pose.pose.orientation.y, _x.pose.pose.pose.orientation.z, _x.pose.pose.pose.orientation.w,) = _get_struct_7d().unpack(str[start:end])
+      start = end
+      end += 288
+      self.pose.pose.covariance = _get_struct_36d().unpack(str[start:end])
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e)  # most likely buffer underfill
+
+
+  def serialize_numpy(self, buff, numpy):
+    """
+    serialize message with numpy array types into buffer
+    :param buff: buffer, ``StringIO``
+    :param numpy: numpy python module
+    """
+    try:
+      length = len(self.id)
+      buff.write(_struct_I.pack(length))
+      pattern = '<%si'%length
+      buff.write(self.id.tostring())
+      length = len(self.size)
+      buff.write(_struct_I.pack(length))
+      pattern = '<%sd'%length
+      buff.write(self.size.tostring())
+      _x = self
+      buff.write(_get_struct_3I().pack(_x.pose.header.seq, _x.pose.header.stamp.secs, _x.pose.header.stamp.nsecs))
+      _x = self.pose.header.frame_id
+      length = len(_x)
+      if python3 or type(_x) == unicode:
+        _x = _x.encode('utf-8')
+        length = len(_x)
+      buff.write(struct.Struct('<I%ss'%length).pack(length, _x))
+      _x = self
+      buff.write(_get_struct_7d().pack(_x.pose.pose.pose.position.x, _x.pose.pose.pose.position.y, _x.pose.pose.pose.position.z, _x.pose.pose.pose.orientation.x, _x.pose.pose.pose.orientation.y, _x.pose.pose.pose.orientation.z, _x.pose.pose.pose.orientation.w))
+      buff.write(self.pose.pose.covariance.tostring())
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self)))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self)))))
+
+  def deserialize_numpy(self, str, numpy):
+    """
+    unpack serialized message in str into this message instance using numpy for array types
+    :param str: byte array of serialized message, ``str``
+    :param numpy: numpy python module
+    """
+    if python3:
+      codecs.lookup_error("rosmsg").msg_type = self._type
+    try:
+      if self.pose is None:
+        self.pose = geometry_msgs.msg.PoseWithCovarianceStamped()
+      end = 0
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      pattern = '<%si'%length
+      start = end
+      s = struct.Struct(pattern)
+      end += s.size
+      self.id = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      pattern = '<%sd'%length
+      start = end
+      s = struct.Struct(pattern)
+      end += s.size
+      self.size = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
+      _x = self
+      start = end
+      end += 12
+      (_x.pose.header.seq, _x.pose.header.stamp.secs, _x.pose.header.stamp.nsecs,) = _get_struct_3I().unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      start = end
+      end += length
+      if python3:
+        self.pose.header.frame_id = str[start:end].decode('utf-8', 'rosmsg')
+      else:
+        self.pose.header.frame_id = str[start:end]
+      _x = self
+      start = end
+      end += 56
+      (_x.pose.pose.pose.position.x, _x.pose.pose.pose.position.y, _x.pose.pose.pose.position.z, _x.pose.pose.pose.orientation.x, _x.pose.pose.pose.orientation.y, _x.pose.pose.pose.orientation.z, _x.pose.pose.pose.orientation.w,) = _get_struct_7d().unpack(str[start:end])
+      start = end
+      end += 288
+      self.pose.pose.covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=36)
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e)  # most likely buffer underfill
+
+_struct_I = genpy.struct_I
+def _get_struct_I():
+    global _struct_I
+    return _struct_I
+_struct_36d = None
+def _get_struct_36d():
+    global _struct_36d
+    if _struct_36d is None:
+        _struct_36d = struct.Struct("<36d")
+    return _struct_36d
+_struct_3I = None
+def _get_struct_3I():
+    global _struct_3I
+    if _struct_3I is None:
+        _struct_3I = struct.Struct("<3I")
+    return _struct_3I
+_struct_7d = None
+def _get_struct_7d():
+    global _struct_7d
+    if _struct_7d is None:
+        _struct_7d = struct.Struct("<7d")
+    return _struct_7d