_SyncPlanningSceneGoal.py
Go to the documentation of this file.
00001 """autogenerated by genpy from arm_navigation_msgs/SyncPlanningSceneGoal.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006 
00007 import arm_navigation_msgs.msg
00008 import geometry_msgs.msg
00009 import std_msgs.msg
00010 import genpy
00011 import sensor_msgs.msg
00012 
00013 class SyncPlanningSceneGoal(genpy.Message):
00014   _md5sum = "285525c9abe002fbafa99af84a14b4cb"
00015   _type = "arm_navigation_msgs/SyncPlanningSceneGoal"
00016   _has_header = False #flag to mark the presence of a Header object
00017   _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00018 # THIS MESSAGE IS FOR INTERNAL COMMUNICATION BETWEEN
00019 # PLANNING ENVIRONMENT COMPONENTS ONLY
00020 
00021 #Full planning scene
00022 PlanningScene planning_scene
00023 
00024 ================================================================================
00025 MSG: arm_navigation_msgs/PlanningScene
00026 #full robot state
00027 arm_navigation_msgs/RobotState robot_state
00028 
00029 #additional frames for duplicating tf
00030 geometry_msgs/TransformStamped[] fixed_frame_transforms
00031 
00032 #full allowed collision matrix
00033 AllowedCollisionMatrix allowed_collision_matrix
00034 
00035 #allowed contacts
00036 arm_navigation_msgs/AllowedContactSpecification[] allowed_contacts
00037 
00038 #all link paddings
00039 arm_navigation_msgs/LinkPadding[] link_padding
00040 
00041 #collision objects
00042 arm_navigation_msgs/CollisionObject[] collision_objects
00043 arm_navigation_msgs/AttachedCollisionObject[] attached_collision_objects
00044 
00045 #the collision map
00046 arm_navigation_msgs/CollisionMap collision_map
00047 
00048 ================================================================================
00049 MSG: arm_navigation_msgs/RobotState
00050 # This message contains information about the robot state, i.e. the positions of its joints and links
00051 sensor_msgs/JointState joint_state
00052 arm_navigation_msgs/MultiDOFJointState multi_dof_joint_state
00053 
00054 ================================================================================
00055 MSG: sensor_msgs/JointState
00056 # This is a message that holds data to describe the state of a set of torque controlled joints. 
00057 #
00058 # The state of each joint (revolute or prismatic) is defined by:
00059 #  * the position of the joint (rad or m),
00060 #  * the velocity of the joint (rad/s or m/s) and 
00061 #  * the effort that is applied in the joint (Nm or N).
00062 #
00063 # Each joint is uniquely identified by its name
00064 # The header specifies the time at which the joint states were recorded. All the joint states
00065 # in one message have to be recorded at the same time.
00066 #
00067 # This message consists of a multiple arrays, one for each part of the joint state. 
00068 # The goal is to make each of the fields optional. When e.g. your joints have no
00069 # effort associated with them, you can leave the effort array empty. 
00070 #
00071 # All arrays in this message should have the same size, or be empty.
00072 # This is the only way to uniquely associate the joint name with the correct
00073 # states.
00074 
00075 
00076 Header header
00077 
00078 string[] name
00079 float64[] position
00080 float64[] velocity
00081 float64[] effort
00082 
00083 ================================================================================
00084 MSG: std_msgs/Header
00085 # Standard metadata for higher-level stamped data types.
00086 # This is generally used to communicate timestamped data 
00087 # in a particular coordinate frame.
00088 # 
00089 # sequence ID: consecutively increasing ID 
00090 uint32 seq
00091 #Two-integer timestamp that is expressed as:
00092 # * stamp.secs: seconds (stamp_secs) since epoch
00093 # * stamp.nsecs: nanoseconds since stamp_secs
00094 # time-handling sugar is provided by the client library
00095 time stamp
00096 #Frame this data is associated with
00097 # 0: no frame
00098 # 1: global frame
00099 string frame_id
00100 
00101 ================================================================================
00102 MSG: arm_navigation_msgs/MultiDOFJointState
00103 #A representation of a multi-dof joint state
00104 time stamp
00105 string[] joint_names
00106 string[] frame_ids
00107 string[] child_frame_ids
00108 geometry_msgs/Pose[] poses
00109 
00110 ================================================================================
00111 MSG: geometry_msgs/Pose
00112 # A representation of pose in free space, composed of postion and orientation. 
00113 Point position
00114 Quaternion orientation
00115 
00116 ================================================================================
00117 MSG: geometry_msgs/Point
00118 # This contains the position of a point in free space
00119 float64 x
00120 float64 y
00121 float64 z
00122 
00123 ================================================================================
00124 MSG: geometry_msgs/Quaternion
00125 # This represents an orientation in free space in quaternion form.
00126 
00127 float64 x
00128 float64 y
00129 float64 z
00130 float64 w
00131 
00132 ================================================================================
00133 MSG: geometry_msgs/TransformStamped
00134 # This expresses a transform from coordinate frame header.frame_id
00135 # to the coordinate frame child_frame_id
00136 #
00137 # This message is mostly used by the 
00138 # <a href="http://www.ros.org/wiki/tf">tf</a> package. 
00139 # See its documentation for more information.
00140 
00141 Header header
00142 string child_frame_id # the frame id of the child frame
00143 Transform transform
00144 
00145 ================================================================================
00146 MSG: geometry_msgs/Transform
00147 # This represents the transform between two coordinate frames in free space.
00148 
00149 Vector3 translation
00150 Quaternion rotation
00151 
00152 ================================================================================
00153 MSG: geometry_msgs/Vector3
00154 # This represents a vector in free space. 
00155 
00156 float64 x
00157 float64 y
00158 float64 z
00159 ================================================================================
00160 MSG: arm_navigation_msgs/AllowedCollisionMatrix
00161 # the list of link names in the matrix
00162 string[] link_names
00163 
00164 # the individual entries in the allowed collision matrix
00165 # symmetric, with same order as link_names
00166 AllowedCollisionEntry[] entries
00167 
00168 ================================================================================
00169 MSG: arm_navigation_msgs/AllowedCollisionEntry
00170 # whether or not collision checking is enabled
00171 bool[] enabled
00172 
00173 ================================================================================
00174 MSG: arm_navigation_msgs/AllowedContactSpecification
00175 # The names of the regions
00176 string name
00177 
00178 # The shape of the region in the environment
00179 arm_navigation_msgs/Shape shape
00180 
00181 # The pose of the space defining the region
00182 geometry_msgs/PoseStamped pose_stamped
00183 
00184 # The set of links that will be allowed to have penetration contact within this region
00185 string[] link_names
00186 
00187 # The maximum penetration depth allowed for every link
00188 float64 penetration_depth
00189 
00190 ================================================================================
00191 MSG: arm_navigation_msgs/Shape
00192 byte SPHERE=0
00193 byte BOX=1
00194 byte CYLINDER=2
00195 byte MESH=3
00196 
00197 byte type
00198 
00199 
00200 #### define sphere, box, cylinder ####
00201 # the origin of each shape is considered at the shape's center
00202 
00203 # for sphere
00204 # radius := dimensions[0]
00205 
00206 # for cylinder
00207 # radius := dimensions[0]
00208 # length := dimensions[1]
00209 # the length is along the Z axis
00210 
00211 # for box
00212 # size_x := dimensions[0]
00213 # size_y := dimensions[1]
00214 # size_z := dimensions[2]
00215 float64[] dimensions
00216 
00217 
00218 #### define mesh ####
00219 
00220 # list of triangles; triangle k is defined by tre vertices located
00221 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00222 int32[] triangles
00223 geometry_msgs/Point[] vertices
00224 
00225 ================================================================================
00226 MSG: geometry_msgs/PoseStamped
00227 # A Pose with reference coordinate frame and timestamp
00228 Header header
00229 Pose pose
00230 
00231 ================================================================================
00232 MSG: arm_navigation_msgs/LinkPadding
00233 #name for the link
00234 string link_name
00235 
00236 # padding to apply to the link
00237 float64 padding
00238 
00239 ================================================================================
00240 MSG: arm_navigation_msgs/CollisionObject
00241 # a header, used for interpreting the poses
00242 Header header
00243 
00244 # the id of the object
00245 string id
00246 
00247 # The padding used for filtering points near the object.
00248 # This does not affect collision checking for the object.  
00249 # Set to negative to get zero padding.
00250 float32 padding
00251 
00252 #This contains what is to be done with the object
00253 CollisionObjectOperation operation
00254 
00255 #the shapes associated with the object
00256 arm_navigation_msgs/Shape[] shapes
00257 
00258 #the poses associated with the shapes - will be transformed using the header
00259 geometry_msgs/Pose[] poses
00260 
00261 ================================================================================
00262 MSG: arm_navigation_msgs/CollisionObjectOperation
00263 #Puts the object into the environment
00264 #or updates the object if already added
00265 byte ADD=0
00266 
00267 #Removes the object from the environment entirely
00268 byte REMOVE=1
00269 
00270 #Only valid within the context of a CollisionAttachedObject message
00271 #Will be ignored if sent with an CollisionObject message
00272 #Takes an attached object, detaches from the attached link
00273 #But adds back in as regular object
00274 byte DETACH_AND_ADD_AS_OBJECT=2
00275 
00276 #Only valid within the context of a CollisionAttachedObject message
00277 #Will be ignored if sent with an CollisionObject message
00278 #Takes current object in the environment and removes it as
00279 #a regular object
00280 byte ATTACH_AND_REMOVE_AS_OBJECT=3
00281 
00282 # Byte code for operation
00283 byte operation
00284 
00285 ================================================================================
00286 MSG: arm_navigation_msgs/AttachedCollisionObject
00287 # The CollisionObject will be attached with a fixed joint to this link
00288 # If link name is set to REMOVE_ALL_ATTACHED_OBJECTS and object.operation 
00289 # is set to REMOVE will remove all attached bodies attached to any object
00290 string link_name
00291 
00292 #Reserved for indicating that all attached objects should be removed
00293 string REMOVE_ALL_ATTACHED_OBJECTS = "all"
00294 
00295 #This contains the actual shapes and poses for the CollisionObject
00296 #to be attached to the link
00297 #If action is remove and no object.id is set, all objects
00298 #attached to the link indicated by link_name will be removed
00299 CollisionObject object
00300 
00301 # The set of links that the attached objects are allowed to touch
00302 # by default - the link_name is included by default
00303 string[] touch_links
00304 
00305 ================================================================================
00306 MSG: arm_navigation_msgs/CollisionMap
00307 #header for interpreting box positions
00308 Header header
00309 
00310 #boxes for use in collision testing
00311 OrientedBoundingBox[] boxes
00312 
00313 ================================================================================
00314 MSG: arm_navigation_msgs/OrientedBoundingBox
00315 #the center of the box
00316 geometry_msgs/Point32 center
00317 
00318 #the extents of the box, assuming the center is at the point
00319 geometry_msgs/Point32 extents
00320 
00321 #the axis of the box
00322 geometry_msgs/Point32 axis
00323 
00324 #the angle of rotation around the axis
00325 float32 angle
00326 
00327 ================================================================================
00328 MSG: geometry_msgs/Point32
00329 # This contains the position of a point in free space(with 32 bits of precision).
00330 # It is recommeded to use Point wherever possible instead of Point32.  
00331 # 
00332 # This recommendation is to promote interoperability.  
00333 #
00334 # This message is designed to take up less space when sending
00335 # lots of points at once, as in the case of a PointCloud.  
00336 
00337 float32 x
00338 float32 y
00339 float32 z
00340 """
00341   __slots__ = ['planning_scene']
00342   _slot_types = ['arm_navigation_msgs/PlanningScene']
00343 
00344   def __init__(self, *args, **kwds):
00345     """
00346     Constructor. Any message fields that are implicitly/explicitly
00347     set to None will be assigned a default value. The recommend
00348     use is keyword arguments as this is more robust to future message
00349     changes.  You cannot mix in-order arguments and keyword arguments.
00350 
00351     The available fields are:
00352        planning_scene
00353 
00354     :param args: complete set of field values, in .msg order
00355     :param kwds: use keyword arguments corresponding to message field names
00356     to set specific fields.
00357     """
00358     if args or kwds:
00359       super(SyncPlanningSceneGoal, self).__init__(*args, **kwds)
00360       #message fields cannot be None, assign default values for those that are
00361       if self.planning_scene is None:
00362         self.planning_scene = arm_navigation_msgs.msg.PlanningScene()
00363     else:
00364       self.planning_scene = arm_navigation_msgs.msg.PlanningScene()
00365 
00366   def _get_types(self):
00367     """
00368     internal API method
00369     """
00370     return self._slot_types
00371 
00372   def serialize(self, buff):
00373     """
00374     serialize message into buffer
00375     :param buff: buffer, ``StringIO``
00376     """
00377     try:
00378       _x = self
00379       buff.write(_struct_3I.pack(_x.planning_scene.robot_state.joint_state.header.seq, _x.planning_scene.robot_state.joint_state.header.stamp.secs, _x.planning_scene.robot_state.joint_state.header.stamp.nsecs))
00380       _x = self.planning_scene.robot_state.joint_state.header.frame_id
00381       length = len(_x)
00382       if python3 or type(_x) == unicode:
00383         _x = _x.encode('utf-8')
00384         length = len(_x)
00385       buff.write(struct.pack('<I%ss'%length, length, _x))
00386       length = len(self.planning_scene.robot_state.joint_state.name)
00387       buff.write(_struct_I.pack(length))
00388       for val1 in self.planning_scene.robot_state.joint_state.name:
00389         length = len(val1)
00390         if python3 or type(val1) == unicode:
00391           val1 = val1.encode('utf-8')
00392           length = len(val1)
00393         buff.write(struct.pack('<I%ss'%length, length, val1))
00394       length = len(self.planning_scene.robot_state.joint_state.position)
00395       buff.write(_struct_I.pack(length))
00396       pattern = '<%sd'%length
00397       buff.write(struct.pack(pattern, *self.planning_scene.robot_state.joint_state.position))
00398       length = len(self.planning_scene.robot_state.joint_state.velocity)
00399       buff.write(_struct_I.pack(length))
00400       pattern = '<%sd'%length
00401       buff.write(struct.pack(pattern, *self.planning_scene.robot_state.joint_state.velocity))
00402       length = len(self.planning_scene.robot_state.joint_state.effort)
00403       buff.write(_struct_I.pack(length))
00404       pattern = '<%sd'%length
00405       buff.write(struct.pack(pattern, *self.planning_scene.robot_state.joint_state.effort))
00406       _x = self
00407       buff.write(_struct_2I.pack(_x.planning_scene.robot_state.multi_dof_joint_state.stamp.secs, _x.planning_scene.robot_state.multi_dof_joint_state.stamp.nsecs))
00408       length = len(self.planning_scene.robot_state.multi_dof_joint_state.joint_names)
00409       buff.write(_struct_I.pack(length))
00410       for val1 in self.planning_scene.robot_state.multi_dof_joint_state.joint_names:
00411         length = len(val1)
00412         if python3 or type(val1) == unicode:
00413           val1 = val1.encode('utf-8')
00414           length = len(val1)
00415         buff.write(struct.pack('<I%ss'%length, length, val1))
00416       length = len(self.planning_scene.robot_state.multi_dof_joint_state.frame_ids)
00417       buff.write(_struct_I.pack(length))
00418       for val1 in self.planning_scene.robot_state.multi_dof_joint_state.frame_ids:
00419         length = len(val1)
00420         if python3 or type(val1) == unicode:
00421           val1 = val1.encode('utf-8')
00422           length = len(val1)
00423         buff.write(struct.pack('<I%ss'%length, length, val1))
00424       length = len(self.planning_scene.robot_state.multi_dof_joint_state.child_frame_ids)
00425       buff.write(_struct_I.pack(length))
00426       for val1 in self.planning_scene.robot_state.multi_dof_joint_state.child_frame_ids:
00427         length = len(val1)
00428         if python3 or type(val1) == unicode:
00429           val1 = val1.encode('utf-8')
00430           length = len(val1)
00431         buff.write(struct.pack('<I%ss'%length, length, val1))
00432       length = len(self.planning_scene.robot_state.multi_dof_joint_state.poses)
00433       buff.write(_struct_I.pack(length))
00434       for val1 in self.planning_scene.robot_state.multi_dof_joint_state.poses:
00435         _v1 = val1.position
00436         _x = _v1
00437         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00438         _v2 = val1.orientation
00439         _x = _v2
00440         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00441       length = len(self.planning_scene.fixed_frame_transforms)
00442       buff.write(_struct_I.pack(length))
00443       for val1 in self.planning_scene.fixed_frame_transforms:
00444         _v3 = val1.header
00445         buff.write(_struct_I.pack(_v3.seq))
00446         _v4 = _v3.stamp
00447         _x = _v4
00448         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00449         _x = _v3.frame_id
00450         length = len(_x)
00451         if python3 or type(_x) == unicode:
00452           _x = _x.encode('utf-8')
00453           length = len(_x)
00454         buff.write(struct.pack('<I%ss'%length, length, _x))
00455         _x = val1.child_frame_id
00456         length = len(_x)
00457         if python3 or type(_x) == unicode:
00458           _x = _x.encode('utf-8')
00459           length = len(_x)
00460         buff.write(struct.pack('<I%ss'%length, length, _x))
00461         _v5 = val1.transform
00462         _v6 = _v5.translation
00463         _x = _v6
00464         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00465         _v7 = _v5.rotation
00466         _x = _v7
00467         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00468       length = len(self.planning_scene.allowed_collision_matrix.link_names)
00469       buff.write(_struct_I.pack(length))
00470       for val1 in self.planning_scene.allowed_collision_matrix.link_names:
00471         length = len(val1)
00472         if python3 or type(val1) == unicode:
00473           val1 = val1.encode('utf-8')
00474           length = len(val1)
00475         buff.write(struct.pack('<I%ss'%length, length, val1))
00476       length = len(self.planning_scene.allowed_collision_matrix.entries)
00477       buff.write(_struct_I.pack(length))
00478       for val1 in self.planning_scene.allowed_collision_matrix.entries:
00479         length = len(val1.enabled)
00480         buff.write(_struct_I.pack(length))
00481         pattern = '<%sB'%length
00482         buff.write(struct.pack(pattern, *val1.enabled))
00483       length = len(self.planning_scene.allowed_contacts)
00484       buff.write(_struct_I.pack(length))
00485       for val1 in self.planning_scene.allowed_contacts:
00486         _x = val1.name
00487         length = len(_x)
00488         if python3 or type(_x) == unicode:
00489           _x = _x.encode('utf-8')
00490           length = len(_x)
00491         buff.write(struct.pack('<I%ss'%length, length, _x))
00492         _v8 = val1.shape
00493         buff.write(_struct_b.pack(_v8.type))
00494         length = len(_v8.dimensions)
00495         buff.write(_struct_I.pack(length))
00496         pattern = '<%sd'%length
00497         buff.write(struct.pack(pattern, *_v8.dimensions))
00498         length = len(_v8.triangles)
00499         buff.write(_struct_I.pack(length))
00500         pattern = '<%si'%length
00501         buff.write(struct.pack(pattern, *_v8.triangles))
00502         length = len(_v8.vertices)
00503         buff.write(_struct_I.pack(length))
00504         for val3 in _v8.vertices:
00505           _x = val3
00506           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00507         _v9 = val1.pose_stamped
00508         _v10 = _v9.header
00509         buff.write(_struct_I.pack(_v10.seq))
00510         _v11 = _v10.stamp
00511         _x = _v11
00512         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00513         _x = _v10.frame_id
00514         length = len(_x)
00515         if python3 or type(_x) == unicode:
00516           _x = _x.encode('utf-8')
00517           length = len(_x)
00518         buff.write(struct.pack('<I%ss'%length, length, _x))
00519         _v12 = _v9.pose
00520         _v13 = _v12.position
00521         _x = _v13
00522         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00523         _v14 = _v12.orientation
00524         _x = _v14
00525         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00526         length = len(val1.link_names)
00527         buff.write(_struct_I.pack(length))
00528         for val2 in val1.link_names:
00529           length = len(val2)
00530           if python3 or type(val2) == unicode:
00531             val2 = val2.encode('utf-8')
00532             length = len(val2)
00533           buff.write(struct.pack('<I%ss'%length, length, val2))
00534         buff.write(_struct_d.pack(val1.penetration_depth))
00535       length = len(self.planning_scene.link_padding)
00536       buff.write(_struct_I.pack(length))
00537       for val1 in self.planning_scene.link_padding:
00538         _x = val1.link_name
00539         length = len(_x)
00540         if python3 or type(_x) == unicode:
00541           _x = _x.encode('utf-8')
00542           length = len(_x)
00543         buff.write(struct.pack('<I%ss'%length, length, _x))
00544         buff.write(_struct_d.pack(val1.padding))
00545       length = len(self.planning_scene.collision_objects)
00546       buff.write(_struct_I.pack(length))
00547       for val1 in self.planning_scene.collision_objects:
00548         _v15 = val1.header
00549         buff.write(_struct_I.pack(_v15.seq))
00550         _v16 = _v15.stamp
00551         _x = _v16
00552         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00553         _x = _v15.frame_id
00554         length = len(_x)
00555         if python3 or type(_x) == unicode:
00556           _x = _x.encode('utf-8')
00557           length = len(_x)
00558         buff.write(struct.pack('<I%ss'%length, length, _x))
00559         _x = val1.id
00560         length = len(_x)
00561         if python3 or type(_x) == unicode:
00562           _x = _x.encode('utf-8')
00563           length = len(_x)
00564         buff.write(struct.pack('<I%ss'%length, length, _x))
00565         buff.write(_struct_f.pack(val1.padding))
00566         _v17 = val1.operation
00567         buff.write(_struct_b.pack(_v17.operation))
00568         length = len(val1.shapes)
00569         buff.write(_struct_I.pack(length))
00570         for val2 in val1.shapes:
00571           buff.write(_struct_b.pack(val2.type))
00572           length = len(val2.dimensions)
00573           buff.write(_struct_I.pack(length))
00574           pattern = '<%sd'%length
00575           buff.write(struct.pack(pattern, *val2.dimensions))
00576           length = len(val2.triangles)
00577           buff.write(_struct_I.pack(length))
00578           pattern = '<%si'%length
00579           buff.write(struct.pack(pattern, *val2.triangles))
00580           length = len(val2.vertices)
00581           buff.write(_struct_I.pack(length))
00582           for val3 in val2.vertices:
00583             _x = val3
00584             buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00585         length = len(val1.poses)
00586         buff.write(_struct_I.pack(length))
00587         for val2 in val1.poses:
00588           _v18 = val2.position
00589           _x = _v18
00590           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00591           _v19 = val2.orientation
00592           _x = _v19
00593           buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00594       length = len(self.planning_scene.attached_collision_objects)
00595       buff.write(_struct_I.pack(length))
00596       for val1 in self.planning_scene.attached_collision_objects:
00597         _x = val1.link_name
00598         length = len(_x)
00599         if python3 or type(_x) == unicode:
00600           _x = _x.encode('utf-8')
00601           length = len(_x)
00602         buff.write(struct.pack('<I%ss'%length, length, _x))
00603         _v20 = val1.object
00604         _v21 = _v20.header
00605         buff.write(_struct_I.pack(_v21.seq))
00606         _v22 = _v21.stamp
00607         _x = _v22
00608         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00609         _x = _v21.frame_id
00610         length = len(_x)
00611         if python3 or type(_x) == unicode:
00612           _x = _x.encode('utf-8')
00613           length = len(_x)
00614         buff.write(struct.pack('<I%ss'%length, length, _x))
00615         _x = _v20.id
00616         length = len(_x)
00617         if python3 or type(_x) == unicode:
00618           _x = _x.encode('utf-8')
00619           length = len(_x)
00620         buff.write(struct.pack('<I%ss'%length, length, _x))
00621         buff.write(_struct_f.pack(_v20.padding))
00622         _v23 = _v20.operation
00623         buff.write(_struct_b.pack(_v23.operation))
00624         length = len(_v20.shapes)
00625         buff.write(_struct_I.pack(length))
00626         for val3 in _v20.shapes:
00627           buff.write(_struct_b.pack(val3.type))
00628           length = len(val3.dimensions)
00629           buff.write(_struct_I.pack(length))
00630           pattern = '<%sd'%length
00631           buff.write(struct.pack(pattern, *val3.dimensions))
00632           length = len(val3.triangles)
00633           buff.write(_struct_I.pack(length))
00634           pattern = '<%si'%length
00635           buff.write(struct.pack(pattern, *val3.triangles))
00636           length = len(val3.vertices)
00637           buff.write(_struct_I.pack(length))
00638           for val4 in val3.vertices:
00639             _x = val4
00640             buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00641         length = len(_v20.poses)
00642         buff.write(_struct_I.pack(length))
00643         for val3 in _v20.poses:
00644           _v24 = val3.position
00645           _x = _v24
00646           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00647           _v25 = val3.orientation
00648           _x = _v25
00649           buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00650         length = len(val1.touch_links)
00651         buff.write(_struct_I.pack(length))
00652         for val2 in val1.touch_links:
00653           length = len(val2)
00654           if python3 or type(val2) == unicode:
00655             val2 = val2.encode('utf-8')
00656             length = len(val2)
00657           buff.write(struct.pack('<I%ss'%length, length, val2))
00658       _x = self
00659       buff.write(_struct_3I.pack(_x.planning_scene.collision_map.header.seq, _x.planning_scene.collision_map.header.stamp.secs, _x.planning_scene.collision_map.header.stamp.nsecs))
00660       _x = self.planning_scene.collision_map.header.frame_id
00661       length = len(_x)
00662       if python3 or type(_x) == unicode:
00663         _x = _x.encode('utf-8')
00664         length = len(_x)
00665       buff.write(struct.pack('<I%ss'%length, length, _x))
00666       length = len(self.planning_scene.collision_map.boxes)
00667       buff.write(_struct_I.pack(length))
00668       for val1 in self.planning_scene.collision_map.boxes:
00669         _v26 = val1.center
00670         _x = _v26
00671         buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00672         _v27 = val1.extents
00673         _x = _v27
00674         buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00675         _v28 = val1.axis
00676         _x = _v28
00677         buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00678         buff.write(_struct_f.pack(val1.angle))
00679     except struct.error as se: self._check_types(se)
00680     except TypeError as te: self._check_types(te)
00681 
00682   def deserialize(self, str):
00683     """
00684     unpack serialized message in str into this message instance
00685     :param str: byte array of serialized message, ``str``
00686     """
00687     try:
00688       if self.planning_scene is None:
00689         self.planning_scene = arm_navigation_msgs.msg.PlanningScene()
00690       end = 0
00691       _x = self
00692       start = end
00693       end += 12
00694       (_x.planning_scene.robot_state.joint_state.header.seq, _x.planning_scene.robot_state.joint_state.header.stamp.secs, _x.planning_scene.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00695       start = end
00696       end += 4
00697       (length,) = _struct_I.unpack(str[start:end])
00698       start = end
00699       end += length
00700       if python3:
00701         self.planning_scene.robot_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
00702       else:
00703         self.planning_scene.robot_state.joint_state.header.frame_id = str[start:end]
00704       start = end
00705       end += 4
00706       (length,) = _struct_I.unpack(str[start:end])
00707       self.planning_scene.robot_state.joint_state.name = []
00708       for i in range(0, length):
00709         start = end
00710         end += 4
00711         (length,) = _struct_I.unpack(str[start:end])
00712         start = end
00713         end += length
00714         if python3:
00715           val1 = str[start:end].decode('utf-8')
00716         else:
00717           val1 = str[start:end]
00718         self.planning_scene.robot_state.joint_state.name.append(val1)
00719       start = end
00720       end += 4
00721       (length,) = _struct_I.unpack(str[start:end])
00722       pattern = '<%sd'%length
00723       start = end
00724       end += struct.calcsize(pattern)
00725       self.planning_scene.robot_state.joint_state.position = struct.unpack(pattern, str[start:end])
00726       start = end
00727       end += 4
00728       (length,) = _struct_I.unpack(str[start:end])
00729       pattern = '<%sd'%length
00730       start = end
00731       end += struct.calcsize(pattern)
00732       self.planning_scene.robot_state.joint_state.velocity = struct.unpack(pattern, str[start:end])
00733       start = end
00734       end += 4
00735       (length,) = _struct_I.unpack(str[start:end])
00736       pattern = '<%sd'%length
00737       start = end
00738       end += struct.calcsize(pattern)
00739       self.planning_scene.robot_state.joint_state.effort = struct.unpack(pattern, str[start:end])
00740       _x = self
00741       start = end
00742       end += 8
00743       (_x.planning_scene.robot_state.multi_dof_joint_state.stamp.secs, _x.planning_scene.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00744       start = end
00745       end += 4
00746       (length,) = _struct_I.unpack(str[start:end])
00747       self.planning_scene.robot_state.multi_dof_joint_state.joint_names = []
00748       for i in range(0, length):
00749         start = end
00750         end += 4
00751         (length,) = _struct_I.unpack(str[start:end])
00752         start = end
00753         end += length
00754         if python3:
00755           val1 = str[start:end].decode('utf-8')
00756         else:
00757           val1 = str[start:end]
00758         self.planning_scene.robot_state.multi_dof_joint_state.joint_names.append(val1)
00759       start = end
00760       end += 4
00761       (length,) = _struct_I.unpack(str[start:end])
00762       self.planning_scene.robot_state.multi_dof_joint_state.frame_ids = []
00763       for i in range(0, length):
00764         start = end
00765         end += 4
00766         (length,) = _struct_I.unpack(str[start:end])
00767         start = end
00768         end += length
00769         if python3:
00770           val1 = str[start:end].decode('utf-8')
00771         else:
00772           val1 = str[start:end]
00773         self.planning_scene.robot_state.multi_dof_joint_state.frame_ids.append(val1)
00774       start = end
00775       end += 4
00776       (length,) = _struct_I.unpack(str[start:end])
00777       self.planning_scene.robot_state.multi_dof_joint_state.child_frame_ids = []
00778       for i in range(0, length):
00779         start = end
00780         end += 4
00781         (length,) = _struct_I.unpack(str[start:end])
00782         start = end
00783         end += length
00784         if python3:
00785           val1 = str[start:end].decode('utf-8')
00786         else:
00787           val1 = str[start:end]
00788         self.planning_scene.robot_state.multi_dof_joint_state.child_frame_ids.append(val1)
00789       start = end
00790       end += 4
00791       (length,) = _struct_I.unpack(str[start:end])
00792       self.planning_scene.robot_state.multi_dof_joint_state.poses = []
00793       for i in range(0, length):
00794         val1 = geometry_msgs.msg.Pose()
00795         _v29 = val1.position
00796         _x = _v29
00797         start = end
00798         end += 24
00799         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00800         _v30 = val1.orientation
00801         _x = _v30
00802         start = end
00803         end += 32
00804         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00805         self.planning_scene.robot_state.multi_dof_joint_state.poses.append(val1)
00806       start = end
00807       end += 4
00808       (length,) = _struct_I.unpack(str[start:end])
00809       self.planning_scene.fixed_frame_transforms = []
00810       for i in range(0, length):
00811         val1 = geometry_msgs.msg.TransformStamped()
00812         _v31 = val1.header
00813         start = end
00814         end += 4
00815         (_v31.seq,) = _struct_I.unpack(str[start:end])
00816         _v32 = _v31.stamp
00817         _x = _v32
00818         start = end
00819         end += 8
00820         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00821         start = end
00822         end += 4
00823         (length,) = _struct_I.unpack(str[start:end])
00824         start = end
00825         end += length
00826         if python3:
00827           _v31.frame_id = str[start:end].decode('utf-8')
00828         else:
00829           _v31.frame_id = str[start:end]
00830         start = end
00831         end += 4
00832         (length,) = _struct_I.unpack(str[start:end])
00833         start = end
00834         end += length
00835         if python3:
00836           val1.child_frame_id = str[start:end].decode('utf-8')
00837         else:
00838           val1.child_frame_id = str[start:end]
00839         _v33 = val1.transform
00840         _v34 = _v33.translation
00841         _x = _v34
00842         start = end
00843         end += 24
00844         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00845         _v35 = _v33.rotation
00846         _x = _v35
00847         start = end
00848         end += 32
00849         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00850         self.planning_scene.fixed_frame_transforms.append(val1)
00851       start = end
00852       end += 4
00853       (length,) = _struct_I.unpack(str[start:end])
00854       self.planning_scene.allowed_collision_matrix.link_names = []
00855       for i in range(0, length):
00856         start = end
00857         end += 4
00858         (length,) = _struct_I.unpack(str[start:end])
00859         start = end
00860         end += length
00861         if python3:
00862           val1 = str[start:end].decode('utf-8')
00863         else:
00864           val1 = str[start:end]
00865         self.planning_scene.allowed_collision_matrix.link_names.append(val1)
00866       start = end
00867       end += 4
00868       (length,) = _struct_I.unpack(str[start:end])
00869       self.planning_scene.allowed_collision_matrix.entries = []
00870       for i in range(0, length):
00871         val1 = arm_navigation_msgs.msg.AllowedCollisionEntry()
00872         start = end
00873         end += 4
00874         (length,) = _struct_I.unpack(str[start:end])
00875         pattern = '<%sB'%length
00876         start = end
00877         end += struct.calcsize(pattern)
00878         val1.enabled = struct.unpack(pattern, str[start:end])
00879         val1.enabled = map(bool, val1.enabled)
00880         self.planning_scene.allowed_collision_matrix.entries.append(val1)
00881       start = end
00882       end += 4
00883       (length,) = _struct_I.unpack(str[start:end])
00884       self.planning_scene.allowed_contacts = []
00885       for i in range(0, length):
00886         val1 = arm_navigation_msgs.msg.AllowedContactSpecification()
00887         start = end
00888         end += 4
00889         (length,) = _struct_I.unpack(str[start:end])
00890         start = end
00891         end += length
00892         if python3:
00893           val1.name = str[start:end].decode('utf-8')
00894         else:
00895           val1.name = str[start:end]
00896         _v36 = val1.shape
00897         start = end
00898         end += 1
00899         (_v36.type,) = _struct_b.unpack(str[start:end])
00900         start = end
00901         end += 4
00902         (length,) = _struct_I.unpack(str[start:end])
00903         pattern = '<%sd'%length
00904         start = end
00905         end += struct.calcsize(pattern)
00906         _v36.dimensions = struct.unpack(pattern, str[start:end])
00907         start = end
00908         end += 4
00909         (length,) = _struct_I.unpack(str[start:end])
00910         pattern = '<%si'%length
00911         start = end
00912         end += struct.calcsize(pattern)
00913         _v36.triangles = struct.unpack(pattern, str[start:end])
00914         start = end
00915         end += 4
00916         (length,) = _struct_I.unpack(str[start:end])
00917         _v36.vertices = []
00918         for i in range(0, length):
00919           val3 = geometry_msgs.msg.Point()
00920           _x = val3
00921           start = end
00922           end += 24
00923           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00924           _v36.vertices.append(val3)
00925         _v37 = val1.pose_stamped
00926         _v38 = _v37.header
00927         start = end
00928         end += 4
00929         (_v38.seq,) = _struct_I.unpack(str[start:end])
00930         _v39 = _v38.stamp
00931         _x = _v39
00932         start = end
00933         end += 8
00934         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00935         start = end
00936         end += 4
00937         (length,) = _struct_I.unpack(str[start:end])
00938         start = end
00939         end += length
00940         if python3:
00941           _v38.frame_id = str[start:end].decode('utf-8')
00942         else:
00943           _v38.frame_id = str[start:end]
00944         _v40 = _v37.pose
00945         _v41 = _v40.position
00946         _x = _v41
00947         start = end
00948         end += 24
00949         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00950         _v42 = _v40.orientation
00951         _x = _v42
00952         start = end
00953         end += 32
00954         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00955         start = end
00956         end += 4
00957         (length,) = _struct_I.unpack(str[start:end])
00958         val1.link_names = []
00959         for i in range(0, length):
00960           start = end
00961           end += 4
00962           (length,) = _struct_I.unpack(str[start:end])
00963           start = end
00964           end += length
00965           if python3:
00966             val2 = str[start:end].decode('utf-8')
00967           else:
00968             val2 = str[start:end]
00969           val1.link_names.append(val2)
00970         start = end
00971         end += 8
00972         (val1.penetration_depth,) = _struct_d.unpack(str[start:end])
00973         self.planning_scene.allowed_contacts.append(val1)
00974       start = end
00975       end += 4
00976       (length,) = _struct_I.unpack(str[start:end])
00977       self.planning_scene.link_padding = []
00978       for i in range(0, length):
00979         val1 = arm_navigation_msgs.msg.LinkPadding()
00980         start = end
00981         end += 4
00982         (length,) = _struct_I.unpack(str[start:end])
00983         start = end
00984         end += length
00985         if python3:
00986           val1.link_name = str[start:end].decode('utf-8')
00987         else:
00988           val1.link_name = str[start:end]
00989         start = end
00990         end += 8
00991         (val1.padding,) = _struct_d.unpack(str[start:end])
00992         self.planning_scene.link_padding.append(val1)
00993       start = end
00994       end += 4
00995       (length,) = _struct_I.unpack(str[start:end])
00996       self.planning_scene.collision_objects = []
00997       for i in range(0, length):
00998         val1 = arm_navigation_msgs.msg.CollisionObject()
00999         _v43 = val1.header
01000         start = end
01001         end += 4
01002         (_v43.seq,) = _struct_I.unpack(str[start:end])
01003         _v44 = _v43.stamp
01004         _x = _v44
01005         start = end
01006         end += 8
01007         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01008         start = end
01009         end += 4
01010         (length,) = _struct_I.unpack(str[start:end])
01011         start = end
01012         end += length
01013         if python3:
01014           _v43.frame_id = str[start:end].decode('utf-8')
01015         else:
01016           _v43.frame_id = str[start:end]
01017         start = end
01018         end += 4
01019         (length,) = _struct_I.unpack(str[start:end])
01020         start = end
01021         end += length
01022         if python3:
01023           val1.id = str[start:end].decode('utf-8')
01024         else:
01025           val1.id = str[start:end]
01026         start = end
01027         end += 4
01028         (val1.padding,) = _struct_f.unpack(str[start:end])
01029         _v45 = val1.operation
01030         start = end
01031         end += 1
01032         (_v45.operation,) = _struct_b.unpack(str[start:end])
01033         start = end
01034         end += 4
01035         (length,) = _struct_I.unpack(str[start:end])
01036         val1.shapes = []
01037         for i in range(0, length):
01038           val2 = arm_navigation_msgs.msg.Shape()
01039           start = end
01040           end += 1
01041           (val2.type,) = _struct_b.unpack(str[start:end])
01042           start = end
01043           end += 4
01044           (length,) = _struct_I.unpack(str[start:end])
01045           pattern = '<%sd'%length
01046           start = end
01047           end += struct.calcsize(pattern)
01048           val2.dimensions = struct.unpack(pattern, str[start:end])
01049           start = end
01050           end += 4
01051           (length,) = _struct_I.unpack(str[start:end])
01052           pattern = '<%si'%length
01053           start = end
01054           end += struct.calcsize(pattern)
01055           val2.triangles = struct.unpack(pattern, str[start:end])
01056           start = end
01057           end += 4
01058           (length,) = _struct_I.unpack(str[start:end])
01059           val2.vertices = []
01060           for i in range(0, length):
01061             val3 = geometry_msgs.msg.Point()
01062             _x = val3
01063             start = end
01064             end += 24
01065             (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01066             val2.vertices.append(val3)
01067           val1.shapes.append(val2)
01068         start = end
01069         end += 4
01070         (length,) = _struct_I.unpack(str[start:end])
01071         val1.poses = []
01072         for i in range(0, length):
01073           val2 = geometry_msgs.msg.Pose()
01074           _v46 = val2.position
01075           _x = _v46
01076           start = end
01077           end += 24
01078           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01079           _v47 = val2.orientation
01080           _x = _v47
01081           start = end
01082           end += 32
01083           (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01084           val1.poses.append(val2)
01085         self.planning_scene.collision_objects.append(val1)
01086       start = end
01087       end += 4
01088       (length,) = _struct_I.unpack(str[start:end])
01089       self.planning_scene.attached_collision_objects = []
01090       for i in range(0, length):
01091         val1 = arm_navigation_msgs.msg.AttachedCollisionObject()
01092         start = end
01093         end += 4
01094         (length,) = _struct_I.unpack(str[start:end])
01095         start = end
01096         end += length
01097         if python3:
01098           val1.link_name = str[start:end].decode('utf-8')
01099         else:
01100           val1.link_name = str[start:end]
01101         _v48 = val1.object
01102         _v49 = _v48.header
01103         start = end
01104         end += 4
01105         (_v49.seq,) = _struct_I.unpack(str[start:end])
01106         _v50 = _v49.stamp
01107         _x = _v50
01108         start = end
01109         end += 8
01110         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01111         start = end
01112         end += 4
01113         (length,) = _struct_I.unpack(str[start:end])
01114         start = end
01115         end += length
01116         if python3:
01117           _v49.frame_id = str[start:end].decode('utf-8')
01118         else:
01119           _v49.frame_id = str[start:end]
01120         start = end
01121         end += 4
01122         (length,) = _struct_I.unpack(str[start:end])
01123         start = end
01124         end += length
01125         if python3:
01126           _v48.id = str[start:end].decode('utf-8')
01127         else:
01128           _v48.id = str[start:end]
01129         start = end
01130         end += 4
01131         (_v48.padding,) = _struct_f.unpack(str[start:end])
01132         _v51 = _v48.operation
01133         start = end
01134         end += 1
01135         (_v51.operation,) = _struct_b.unpack(str[start:end])
01136         start = end
01137         end += 4
01138         (length,) = _struct_I.unpack(str[start:end])
01139         _v48.shapes = []
01140         for i in range(0, length):
01141           val3 = arm_navigation_msgs.msg.Shape()
01142           start = end
01143           end += 1
01144           (val3.type,) = _struct_b.unpack(str[start:end])
01145           start = end
01146           end += 4
01147           (length,) = _struct_I.unpack(str[start:end])
01148           pattern = '<%sd'%length
01149           start = end
01150           end += struct.calcsize(pattern)
01151           val3.dimensions = struct.unpack(pattern, str[start:end])
01152           start = end
01153           end += 4
01154           (length,) = _struct_I.unpack(str[start:end])
01155           pattern = '<%si'%length
01156           start = end
01157           end += struct.calcsize(pattern)
01158           val3.triangles = struct.unpack(pattern, str[start:end])
01159           start = end
01160           end += 4
01161           (length,) = _struct_I.unpack(str[start:end])
01162           val3.vertices = []
01163           for i in range(0, length):
01164             val4 = geometry_msgs.msg.Point()
01165             _x = val4
01166             start = end
01167             end += 24
01168             (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01169             val3.vertices.append(val4)
01170           _v48.shapes.append(val3)
01171         start = end
01172         end += 4
01173         (length,) = _struct_I.unpack(str[start:end])
01174         _v48.poses = []
01175         for i in range(0, length):
01176           val3 = geometry_msgs.msg.Pose()
01177           _v52 = val3.position
01178           _x = _v52
01179           start = end
01180           end += 24
01181           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01182           _v53 = val3.orientation
01183           _x = _v53
01184           start = end
01185           end += 32
01186           (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01187           _v48.poses.append(val3)
01188         start = end
01189         end += 4
01190         (length,) = _struct_I.unpack(str[start:end])
01191         val1.touch_links = []
01192         for i in range(0, length):
01193           start = end
01194           end += 4
01195           (length,) = _struct_I.unpack(str[start:end])
01196           start = end
01197           end += length
01198           if python3:
01199             val2 = str[start:end].decode('utf-8')
01200           else:
01201             val2 = str[start:end]
01202           val1.touch_links.append(val2)
01203         self.planning_scene.attached_collision_objects.append(val1)
01204       _x = self
01205       start = end
01206       end += 12
01207       (_x.planning_scene.collision_map.header.seq, _x.planning_scene.collision_map.header.stamp.secs, _x.planning_scene.collision_map.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01208       start = end
01209       end += 4
01210       (length,) = _struct_I.unpack(str[start:end])
01211       start = end
01212       end += length
01213       if python3:
01214         self.planning_scene.collision_map.header.frame_id = str[start:end].decode('utf-8')
01215       else:
01216         self.planning_scene.collision_map.header.frame_id = str[start:end]
01217       start = end
01218       end += 4
01219       (length,) = _struct_I.unpack(str[start:end])
01220       self.planning_scene.collision_map.boxes = []
01221       for i in range(0, length):
01222         val1 = arm_navigation_msgs.msg.OrientedBoundingBox()
01223         _v54 = val1.center
01224         _x = _v54
01225         start = end
01226         end += 12
01227         (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01228         _v55 = val1.extents
01229         _x = _v55
01230         start = end
01231         end += 12
01232         (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01233         _v56 = val1.axis
01234         _x = _v56
01235         start = end
01236         end += 12
01237         (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01238         start = end
01239         end += 4
01240         (val1.angle,) = _struct_f.unpack(str[start:end])
01241         self.planning_scene.collision_map.boxes.append(val1)
01242       return self
01243     except struct.error as e:
01244       raise genpy.DeserializationError(e) #most likely buffer underfill
01245 
01246 
01247   def serialize_numpy(self, buff, numpy):
01248     """
01249     serialize message with numpy array types into buffer
01250     :param buff: buffer, ``StringIO``
01251     :param numpy: numpy python module
01252     """
01253     try:
01254       _x = self
01255       buff.write(_struct_3I.pack(_x.planning_scene.robot_state.joint_state.header.seq, _x.planning_scene.robot_state.joint_state.header.stamp.secs, _x.planning_scene.robot_state.joint_state.header.stamp.nsecs))
01256       _x = self.planning_scene.robot_state.joint_state.header.frame_id
01257       length = len(_x)
01258       if python3 or type(_x) == unicode:
01259         _x = _x.encode('utf-8')
01260         length = len(_x)
01261       buff.write(struct.pack('<I%ss'%length, length, _x))
01262       length = len(self.planning_scene.robot_state.joint_state.name)
01263       buff.write(_struct_I.pack(length))
01264       for val1 in self.planning_scene.robot_state.joint_state.name:
01265         length = len(val1)
01266         if python3 or type(val1) == unicode:
01267           val1 = val1.encode('utf-8')
01268           length = len(val1)
01269         buff.write(struct.pack('<I%ss'%length, length, val1))
01270       length = len(self.planning_scene.robot_state.joint_state.position)
01271       buff.write(_struct_I.pack(length))
01272       pattern = '<%sd'%length
01273       buff.write(self.planning_scene.robot_state.joint_state.position.tostring())
01274       length = len(self.planning_scene.robot_state.joint_state.velocity)
01275       buff.write(_struct_I.pack(length))
01276       pattern = '<%sd'%length
01277       buff.write(self.planning_scene.robot_state.joint_state.velocity.tostring())
01278       length = len(self.planning_scene.robot_state.joint_state.effort)
01279       buff.write(_struct_I.pack(length))
01280       pattern = '<%sd'%length
01281       buff.write(self.planning_scene.robot_state.joint_state.effort.tostring())
01282       _x = self
01283       buff.write(_struct_2I.pack(_x.planning_scene.robot_state.multi_dof_joint_state.stamp.secs, _x.planning_scene.robot_state.multi_dof_joint_state.stamp.nsecs))
01284       length = len(self.planning_scene.robot_state.multi_dof_joint_state.joint_names)
01285       buff.write(_struct_I.pack(length))
01286       for val1 in self.planning_scene.robot_state.multi_dof_joint_state.joint_names:
01287         length = len(val1)
01288         if python3 or type(val1) == unicode:
01289           val1 = val1.encode('utf-8')
01290           length = len(val1)
01291         buff.write(struct.pack('<I%ss'%length, length, val1))
01292       length = len(self.planning_scene.robot_state.multi_dof_joint_state.frame_ids)
01293       buff.write(_struct_I.pack(length))
01294       for val1 in self.planning_scene.robot_state.multi_dof_joint_state.frame_ids:
01295         length = len(val1)
01296         if python3 or type(val1) == unicode:
01297           val1 = val1.encode('utf-8')
01298           length = len(val1)
01299         buff.write(struct.pack('<I%ss'%length, length, val1))
01300       length = len(self.planning_scene.robot_state.multi_dof_joint_state.child_frame_ids)
01301       buff.write(_struct_I.pack(length))
01302       for val1 in self.planning_scene.robot_state.multi_dof_joint_state.child_frame_ids:
01303         length = len(val1)
01304         if python3 or type(val1) == unicode:
01305           val1 = val1.encode('utf-8')
01306           length = len(val1)
01307         buff.write(struct.pack('<I%ss'%length, length, val1))
01308       length = len(self.planning_scene.robot_state.multi_dof_joint_state.poses)
01309       buff.write(_struct_I.pack(length))
01310       for val1 in self.planning_scene.robot_state.multi_dof_joint_state.poses:
01311         _v57 = val1.position
01312         _x = _v57
01313         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01314         _v58 = val1.orientation
01315         _x = _v58
01316         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01317       length = len(self.planning_scene.fixed_frame_transforms)
01318       buff.write(_struct_I.pack(length))
01319       for val1 in self.planning_scene.fixed_frame_transforms:
01320         _v59 = val1.header
01321         buff.write(_struct_I.pack(_v59.seq))
01322         _v60 = _v59.stamp
01323         _x = _v60
01324         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01325         _x = _v59.frame_id
01326         length = len(_x)
01327         if python3 or type(_x) == unicode:
01328           _x = _x.encode('utf-8')
01329           length = len(_x)
01330         buff.write(struct.pack('<I%ss'%length, length, _x))
01331         _x = val1.child_frame_id
01332         length = len(_x)
01333         if python3 or type(_x) == unicode:
01334           _x = _x.encode('utf-8')
01335           length = len(_x)
01336         buff.write(struct.pack('<I%ss'%length, length, _x))
01337         _v61 = val1.transform
01338         _v62 = _v61.translation
01339         _x = _v62
01340         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01341         _v63 = _v61.rotation
01342         _x = _v63
01343         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01344       length = len(self.planning_scene.allowed_collision_matrix.link_names)
01345       buff.write(_struct_I.pack(length))
01346       for val1 in self.planning_scene.allowed_collision_matrix.link_names:
01347         length = len(val1)
01348         if python3 or type(val1) == unicode:
01349           val1 = val1.encode('utf-8')
01350           length = len(val1)
01351         buff.write(struct.pack('<I%ss'%length, length, val1))
01352       length = len(self.planning_scene.allowed_collision_matrix.entries)
01353       buff.write(_struct_I.pack(length))
01354       for val1 in self.planning_scene.allowed_collision_matrix.entries:
01355         length = len(val1.enabled)
01356         buff.write(_struct_I.pack(length))
01357         pattern = '<%sB'%length
01358         buff.write(val1.enabled.tostring())
01359       length = len(self.planning_scene.allowed_contacts)
01360       buff.write(_struct_I.pack(length))
01361       for val1 in self.planning_scene.allowed_contacts:
01362         _x = val1.name
01363         length = len(_x)
01364         if python3 or type(_x) == unicode:
01365           _x = _x.encode('utf-8')
01366           length = len(_x)
01367         buff.write(struct.pack('<I%ss'%length, length, _x))
01368         _v64 = val1.shape
01369         buff.write(_struct_b.pack(_v64.type))
01370         length = len(_v64.dimensions)
01371         buff.write(_struct_I.pack(length))
01372         pattern = '<%sd'%length
01373         buff.write(_v64.dimensions.tostring())
01374         length = len(_v64.triangles)
01375         buff.write(_struct_I.pack(length))
01376         pattern = '<%si'%length
01377         buff.write(_v64.triangles.tostring())
01378         length = len(_v64.vertices)
01379         buff.write(_struct_I.pack(length))
01380         for val3 in _v64.vertices:
01381           _x = val3
01382           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01383         _v65 = val1.pose_stamped
01384         _v66 = _v65.header
01385         buff.write(_struct_I.pack(_v66.seq))
01386         _v67 = _v66.stamp
01387         _x = _v67
01388         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01389         _x = _v66.frame_id
01390         length = len(_x)
01391         if python3 or type(_x) == unicode:
01392           _x = _x.encode('utf-8')
01393           length = len(_x)
01394         buff.write(struct.pack('<I%ss'%length, length, _x))
01395         _v68 = _v65.pose
01396         _v69 = _v68.position
01397         _x = _v69
01398         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01399         _v70 = _v68.orientation
01400         _x = _v70
01401         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01402         length = len(val1.link_names)
01403         buff.write(_struct_I.pack(length))
01404         for val2 in val1.link_names:
01405           length = len(val2)
01406           if python3 or type(val2) == unicode:
01407             val2 = val2.encode('utf-8')
01408             length = len(val2)
01409           buff.write(struct.pack('<I%ss'%length, length, val2))
01410         buff.write(_struct_d.pack(val1.penetration_depth))
01411       length = len(self.planning_scene.link_padding)
01412       buff.write(_struct_I.pack(length))
01413       for val1 in self.planning_scene.link_padding:
01414         _x = val1.link_name
01415         length = len(_x)
01416         if python3 or type(_x) == unicode:
01417           _x = _x.encode('utf-8')
01418           length = len(_x)
01419         buff.write(struct.pack('<I%ss'%length, length, _x))
01420         buff.write(_struct_d.pack(val1.padding))
01421       length = len(self.planning_scene.collision_objects)
01422       buff.write(_struct_I.pack(length))
01423       for val1 in self.planning_scene.collision_objects:
01424         _v71 = val1.header
01425         buff.write(_struct_I.pack(_v71.seq))
01426         _v72 = _v71.stamp
01427         _x = _v72
01428         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01429         _x = _v71.frame_id
01430         length = len(_x)
01431         if python3 or type(_x) == unicode:
01432           _x = _x.encode('utf-8')
01433           length = len(_x)
01434         buff.write(struct.pack('<I%ss'%length, length, _x))
01435         _x = val1.id
01436         length = len(_x)
01437         if python3 or type(_x) == unicode:
01438           _x = _x.encode('utf-8')
01439           length = len(_x)
01440         buff.write(struct.pack('<I%ss'%length, length, _x))
01441         buff.write(_struct_f.pack(val1.padding))
01442         _v73 = val1.operation
01443         buff.write(_struct_b.pack(_v73.operation))
01444         length = len(val1.shapes)
01445         buff.write(_struct_I.pack(length))
01446         for val2 in val1.shapes:
01447           buff.write(_struct_b.pack(val2.type))
01448           length = len(val2.dimensions)
01449           buff.write(_struct_I.pack(length))
01450           pattern = '<%sd'%length
01451           buff.write(val2.dimensions.tostring())
01452           length = len(val2.triangles)
01453           buff.write(_struct_I.pack(length))
01454           pattern = '<%si'%length
01455           buff.write(val2.triangles.tostring())
01456           length = len(val2.vertices)
01457           buff.write(_struct_I.pack(length))
01458           for val3 in val2.vertices:
01459             _x = val3
01460             buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01461         length = len(val1.poses)
01462         buff.write(_struct_I.pack(length))
01463         for val2 in val1.poses:
01464           _v74 = val2.position
01465           _x = _v74
01466           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01467           _v75 = val2.orientation
01468           _x = _v75
01469           buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01470       length = len(self.planning_scene.attached_collision_objects)
01471       buff.write(_struct_I.pack(length))
01472       for val1 in self.planning_scene.attached_collision_objects:
01473         _x = val1.link_name
01474         length = len(_x)
01475         if python3 or type(_x) == unicode:
01476           _x = _x.encode('utf-8')
01477           length = len(_x)
01478         buff.write(struct.pack('<I%ss'%length, length, _x))
01479         _v76 = val1.object
01480         _v77 = _v76.header
01481         buff.write(_struct_I.pack(_v77.seq))
01482         _v78 = _v77.stamp
01483         _x = _v78
01484         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01485         _x = _v77.frame_id
01486         length = len(_x)
01487         if python3 or type(_x) == unicode:
01488           _x = _x.encode('utf-8')
01489           length = len(_x)
01490         buff.write(struct.pack('<I%ss'%length, length, _x))
01491         _x = _v76.id
01492         length = len(_x)
01493         if python3 or type(_x) == unicode:
01494           _x = _x.encode('utf-8')
01495           length = len(_x)
01496         buff.write(struct.pack('<I%ss'%length, length, _x))
01497         buff.write(_struct_f.pack(_v76.padding))
01498         _v79 = _v76.operation
01499         buff.write(_struct_b.pack(_v79.operation))
01500         length = len(_v76.shapes)
01501         buff.write(_struct_I.pack(length))
01502         for val3 in _v76.shapes:
01503           buff.write(_struct_b.pack(val3.type))
01504           length = len(val3.dimensions)
01505           buff.write(_struct_I.pack(length))
01506           pattern = '<%sd'%length
01507           buff.write(val3.dimensions.tostring())
01508           length = len(val3.triangles)
01509           buff.write(_struct_I.pack(length))
01510           pattern = '<%si'%length
01511           buff.write(val3.triangles.tostring())
01512           length = len(val3.vertices)
01513           buff.write(_struct_I.pack(length))
01514           for val4 in val3.vertices:
01515             _x = val4
01516             buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01517         length = len(_v76.poses)
01518         buff.write(_struct_I.pack(length))
01519         for val3 in _v76.poses:
01520           _v80 = val3.position
01521           _x = _v80
01522           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01523           _v81 = val3.orientation
01524           _x = _v81
01525           buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01526         length = len(val1.touch_links)
01527         buff.write(_struct_I.pack(length))
01528         for val2 in val1.touch_links:
01529           length = len(val2)
01530           if python3 or type(val2) == unicode:
01531             val2 = val2.encode('utf-8')
01532             length = len(val2)
01533           buff.write(struct.pack('<I%ss'%length, length, val2))
01534       _x = self
01535       buff.write(_struct_3I.pack(_x.planning_scene.collision_map.header.seq, _x.planning_scene.collision_map.header.stamp.secs, _x.planning_scene.collision_map.header.stamp.nsecs))
01536       _x = self.planning_scene.collision_map.header.frame_id
01537       length = len(_x)
01538       if python3 or type(_x) == unicode:
01539         _x = _x.encode('utf-8')
01540         length = len(_x)
01541       buff.write(struct.pack('<I%ss'%length, length, _x))
01542       length = len(self.planning_scene.collision_map.boxes)
01543       buff.write(_struct_I.pack(length))
01544       for val1 in self.planning_scene.collision_map.boxes:
01545         _v82 = val1.center
01546         _x = _v82
01547         buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01548         _v83 = val1.extents
01549         _x = _v83
01550         buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01551         _v84 = val1.axis
01552         _x = _v84
01553         buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01554         buff.write(_struct_f.pack(val1.angle))
01555     except struct.error as se: self._check_types(se)
01556     except TypeError as te: self._check_types(te)
01557 
01558   def deserialize_numpy(self, str, numpy):
01559     """
01560     unpack serialized message in str into this message instance using numpy for array types
01561     :param str: byte array of serialized message, ``str``
01562     :param numpy: numpy python module
01563     """
01564     try:
01565       if self.planning_scene is None:
01566         self.planning_scene = arm_navigation_msgs.msg.PlanningScene()
01567       end = 0
01568       _x = self
01569       start = end
01570       end += 12
01571       (_x.planning_scene.robot_state.joint_state.header.seq, _x.planning_scene.robot_state.joint_state.header.stamp.secs, _x.planning_scene.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01572       start = end
01573       end += 4
01574       (length,) = _struct_I.unpack(str[start:end])
01575       start = end
01576       end += length
01577       if python3:
01578         self.planning_scene.robot_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
01579       else:
01580         self.planning_scene.robot_state.joint_state.header.frame_id = str[start:end]
01581       start = end
01582       end += 4
01583       (length,) = _struct_I.unpack(str[start:end])
01584       self.planning_scene.robot_state.joint_state.name = []
01585       for i in range(0, length):
01586         start = end
01587         end += 4
01588         (length,) = _struct_I.unpack(str[start:end])
01589         start = end
01590         end += length
01591         if python3:
01592           val1 = str[start:end].decode('utf-8')
01593         else:
01594           val1 = str[start:end]
01595         self.planning_scene.robot_state.joint_state.name.append(val1)
01596       start = end
01597       end += 4
01598       (length,) = _struct_I.unpack(str[start:end])
01599       pattern = '<%sd'%length
01600       start = end
01601       end += struct.calcsize(pattern)
01602       self.planning_scene.robot_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01603       start = end
01604       end += 4
01605       (length,) = _struct_I.unpack(str[start:end])
01606       pattern = '<%sd'%length
01607       start = end
01608       end += struct.calcsize(pattern)
01609       self.planning_scene.robot_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01610       start = end
01611       end += 4
01612       (length,) = _struct_I.unpack(str[start:end])
01613       pattern = '<%sd'%length
01614       start = end
01615       end += struct.calcsize(pattern)
01616       self.planning_scene.robot_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01617       _x = self
01618       start = end
01619       end += 8
01620       (_x.planning_scene.robot_state.multi_dof_joint_state.stamp.secs, _x.planning_scene.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01621       start = end
01622       end += 4
01623       (length,) = _struct_I.unpack(str[start:end])
01624       self.planning_scene.robot_state.multi_dof_joint_state.joint_names = []
01625       for i in range(0, length):
01626         start = end
01627         end += 4
01628         (length,) = _struct_I.unpack(str[start:end])
01629         start = end
01630         end += length
01631         if python3:
01632           val1 = str[start:end].decode('utf-8')
01633         else:
01634           val1 = str[start:end]
01635         self.planning_scene.robot_state.multi_dof_joint_state.joint_names.append(val1)
01636       start = end
01637       end += 4
01638       (length,) = _struct_I.unpack(str[start:end])
01639       self.planning_scene.robot_state.multi_dof_joint_state.frame_ids = []
01640       for i in range(0, length):
01641         start = end
01642         end += 4
01643         (length,) = _struct_I.unpack(str[start:end])
01644         start = end
01645         end += length
01646         if python3:
01647           val1 = str[start:end].decode('utf-8')
01648         else:
01649           val1 = str[start:end]
01650         self.planning_scene.robot_state.multi_dof_joint_state.frame_ids.append(val1)
01651       start = end
01652       end += 4
01653       (length,) = _struct_I.unpack(str[start:end])
01654       self.planning_scene.robot_state.multi_dof_joint_state.child_frame_ids = []
01655       for i in range(0, length):
01656         start = end
01657         end += 4
01658         (length,) = _struct_I.unpack(str[start:end])
01659         start = end
01660         end += length
01661         if python3:
01662           val1 = str[start:end].decode('utf-8')
01663         else:
01664           val1 = str[start:end]
01665         self.planning_scene.robot_state.multi_dof_joint_state.child_frame_ids.append(val1)
01666       start = end
01667       end += 4
01668       (length,) = _struct_I.unpack(str[start:end])
01669       self.planning_scene.robot_state.multi_dof_joint_state.poses = []
01670       for i in range(0, length):
01671         val1 = geometry_msgs.msg.Pose()
01672         _v85 = val1.position
01673         _x = _v85
01674         start = end
01675         end += 24
01676         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01677         _v86 = val1.orientation
01678         _x = _v86
01679         start = end
01680         end += 32
01681         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01682         self.planning_scene.robot_state.multi_dof_joint_state.poses.append(val1)
01683       start = end
01684       end += 4
01685       (length,) = _struct_I.unpack(str[start:end])
01686       self.planning_scene.fixed_frame_transforms = []
01687       for i in range(0, length):
01688         val1 = geometry_msgs.msg.TransformStamped()
01689         _v87 = val1.header
01690         start = end
01691         end += 4
01692         (_v87.seq,) = _struct_I.unpack(str[start:end])
01693         _v88 = _v87.stamp
01694         _x = _v88
01695         start = end
01696         end += 8
01697         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01698         start = end
01699         end += 4
01700         (length,) = _struct_I.unpack(str[start:end])
01701         start = end
01702         end += length
01703         if python3:
01704           _v87.frame_id = str[start:end].decode('utf-8')
01705         else:
01706           _v87.frame_id = str[start:end]
01707         start = end
01708         end += 4
01709         (length,) = _struct_I.unpack(str[start:end])
01710         start = end
01711         end += length
01712         if python3:
01713           val1.child_frame_id = str[start:end].decode('utf-8')
01714         else:
01715           val1.child_frame_id = str[start:end]
01716         _v89 = val1.transform
01717         _v90 = _v89.translation
01718         _x = _v90
01719         start = end
01720         end += 24
01721         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01722         _v91 = _v89.rotation
01723         _x = _v91
01724         start = end
01725         end += 32
01726         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01727         self.planning_scene.fixed_frame_transforms.append(val1)
01728       start = end
01729       end += 4
01730       (length,) = _struct_I.unpack(str[start:end])
01731       self.planning_scene.allowed_collision_matrix.link_names = []
01732       for i in range(0, length):
01733         start = end
01734         end += 4
01735         (length,) = _struct_I.unpack(str[start:end])
01736         start = end
01737         end += length
01738         if python3:
01739           val1 = str[start:end].decode('utf-8')
01740         else:
01741           val1 = str[start:end]
01742         self.planning_scene.allowed_collision_matrix.link_names.append(val1)
01743       start = end
01744       end += 4
01745       (length,) = _struct_I.unpack(str[start:end])
01746       self.planning_scene.allowed_collision_matrix.entries = []
01747       for i in range(0, length):
01748         val1 = arm_navigation_msgs.msg.AllowedCollisionEntry()
01749         start = end
01750         end += 4
01751         (length,) = _struct_I.unpack(str[start:end])
01752         pattern = '<%sB'%length
01753         start = end
01754         end += struct.calcsize(pattern)
01755         val1.enabled = numpy.frombuffer(str[start:end], dtype=numpy.bool, count=length)
01756         val1.enabled = map(bool, val1.enabled)
01757         self.planning_scene.allowed_collision_matrix.entries.append(val1)
01758       start = end
01759       end += 4
01760       (length,) = _struct_I.unpack(str[start:end])
01761       self.planning_scene.allowed_contacts = []
01762       for i in range(0, length):
01763         val1 = arm_navigation_msgs.msg.AllowedContactSpecification()
01764         start = end
01765         end += 4
01766         (length,) = _struct_I.unpack(str[start:end])
01767         start = end
01768         end += length
01769         if python3:
01770           val1.name = str[start:end].decode('utf-8')
01771         else:
01772           val1.name = str[start:end]
01773         _v92 = val1.shape
01774         start = end
01775         end += 1
01776         (_v92.type,) = _struct_b.unpack(str[start:end])
01777         start = end
01778         end += 4
01779         (length,) = _struct_I.unpack(str[start:end])
01780         pattern = '<%sd'%length
01781         start = end
01782         end += struct.calcsize(pattern)
01783         _v92.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01784         start = end
01785         end += 4
01786         (length,) = _struct_I.unpack(str[start:end])
01787         pattern = '<%si'%length
01788         start = end
01789         end += struct.calcsize(pattern)
01790         _v92.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01791         start = end
01792         end += 4
01793         (length,) = _struct_I.unpack(str[start:end])
01794         _v92.vertices = []
01795         for i in range(0, length):
01796           val3 = geometry_msgs.msg.Point()
01797           _x = val3
01798           start = end
01799           end += 24
01800           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01801           _v92.vertices.append(val3)
01802         _v93 = val1.pose_stamped
01803         _v94 = _v93.header
01804         start = end
01805         end += 4
01806         (_v94.seq,) = _struct_I.unpack(str[start:end])
01807         _v95 = _v94.stamp
01808         _x = _v95
01809         start = end
01810         end += 8
01811         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01812         start = end
01813         end += 4
01814         (length,) = _struct_I.unpack(str[start:end])
01815         start = end
01816         end += length
01817         if python3:
01818           _v94.frame_id = str[start:end].decode('utf-8')
01819         else:
01820           _v94.frame_id = str[start:end]
01821         _v96 = _v93.pose
01822         _v97 = _v96.position
01823         _x = _v97
01824         start = end
01825         end += 24
01826         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01827         _v98 = _v96.orientation
01828         _x = _v98
01829         start = end
01830         end += 32
01831         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01832         start = end
01833         end += 4
01834         (length,) = _struct_I.unpack(str[start:end])
01835         val1.link_names = []
01836         for i in range(0, length):
01837           start = end
01838           end += 4
01839           (length,) = _struct_I.unpack(str[start:end])
01840           start = end
01841           end += length
01842           if python3:
01843             val2 = str[start:end].decode('utf-8')
01844           else:
01845             val2 = str[start:end]
01846           val1.link_names.append(val2)
01847         start = end
01848         end += 8
01849         (val1.penetration_depth,) = _struct_d.unpack(str[start:end])
01850         self.planning_scene.allowed_contacts.append(val1)
01851       start = end
01852       end += 4
01853       (length,) = _struct_I.unpack(str[start:end])
01854       self.planning_scene.link_padding = []
01855       for i in range(0, length):
01856         val1 = arm_navigation_msgs.msg.LinkPadding()
01857         start = end
01858         end += 4
01859         (length,) = _struct_I.unpack(str[start:end])
01860         start = end
01861         end += length
01862         if python3:
01863           val1.link_name = str[start:end].decode('utf-8')
01864         else:
01865           val1.link_name = str[start:end]
01866         start = end
01867         end += 8
01868         (val1.padding,) = _struct_d.unpack(str[start:end])
01869         self.planning_scene.link_padding.append(val1)
01870       start = end
01871       end += 4
01872       (length,) = _struct_I.unpack(str[start:end])
01873       self.planning_scene.collision_objects = []
01874       for i in range(0, length):
01875         val1 = arm_navigation_msgs.msg.CollisionObject()
01876         _v99 = val1.header
01877         start = end
01878         end += 4
01879         (_v99.seq,) = _struct_I.unpack(str[start:end])
01880         _v100 = _v99.stamp
01881         _x = _v100
01882         start = end
01883         end += 8
01884         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01885         start = end
01886         end += 4
01887         (length,) = _struct_I.unpack(str[start:end])
01888         start = end
01889         end += length
01890         if python3:
01891           _v99.frame_id = str[start:end].decode('utf-8')
01892         else:
01893           _v99.frame_id = str[start:end]
01894         start = end
01895         end += 4
01896         (length,) = _struct_I.unpack(str[start:end])
01897         start = end
01898         end += length
01899         if python3:
01900           val1.id = str[start:end].decode('utf-8')
01901         else:
01902           val1.id = str[start:end]
01903         start = end
01904         end += 4
01905         (val1.padding,) = _struct_f.unpack(str[start:end])
01906         _v101 = val1.operation
01907         start = end
01908         end += 1
01909         (_v101.operation,) = _struct_b.unpack(str[start:end])
01910         start = end
01911         end += 4
01912         (length,) = _struct_I.unpack(str[start:end])
01913         val1.shapes = []
01914         for i in range(0, length):
01915           val2 = arm_navigation_msgs.msg.Shape()
01916           start = end
01917           end += 1
01918           (val2.type,) = _struct_b.unpack(str[start:end])
01919           start = end
01920           end += 4
01921           (length,) = _struct_I.unpack(str[start:end])
01922           pattern = '<%sd'%length
01923           start = end
01924           end += struct.calcsize(pattern)
01925           val2.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01926           start = end
01927           end += 4
01928           (length,) = _struct_I.unpack(str[start:end])
01929           pattern = '<%si'%length
01930           start = end
01931           end += struct.calcsize(pattern)
01932           val2.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01933           start = end
01934           end += 4
01935           (length,) = _struct_I.unpack(str[start:end])
01936           val2.vertices = []
01937           for i in range(0, length):
01938             val3 = geometry_msgs.msg.Point()
01939             _x = val3
01940             start = end
01941             end += 24
01942             (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01943             val2.vertices.append(val3)
01944           val1.shapes.append(val2)
01945         start = end
01946         end += 4
01947         (length,) = _struct_I.unpack(str[start:end])
01948         val1.poses = []
01949         for i in range(0, length):
01950           val2 = geometry_msgs.msg.Pose()
01951           _v102 = val2.position
01952           _x = _v102
01953           start = end
01954           end += 24
01955           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01956           _v103 = val2.orientation
01957           _x = _v103
01958           start = end
01959           end += 32
01960           (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01961           val1.poses.append(val2)
01962         self.planning_scene.collision_objects.append(val1)
01963       start = end
01964       end += 4
01965       (length,) = _struct_I.unpack(str[start:end])
01966       self.planning_scene.attached_collision_objects = []
01967       for i in range(0, length):
01968         val1 = arm_navigation_msgs.msg.AttachedCollisionObject()
01969         start = end
01970         end += 4
01971         (length,) = _struct_I.unpack(str[start:end])
01972         start = end
01973         end += length
01974         if python3:
01975           val1.link_name = str[start:end].decode('utf-8')
01976         else:
01977           val1.link_name = str[start:end]
01978         _v104 = val1.object
01979         _v105 = _v104.header
01980         start = end
01981         end += 4
01982         (_v105.seq,) = _struct_I.unpack(str[start:end])
01983         _v106 = _v105.stamp
01984         _x = _v106
01985         start = end
01986         end += 8
01987         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01988         start = end
01989         end += 4
01990         (length,) = _struct_I.unpack(str[start:end])
01991         start = end
01992         end += length
01993         if python3:
01994           _v105.frame_id = str[start:end].decode('utf-8')
01995         else:
01996           _v105.frame_id = str[start:end]
01997         start = end
01998         end += 4
01999         (length,) = _struct_I.unpack(str[start:end])
02000         start = end
02001         end += length
02002         if python3:
02003           _v104.id = str[start:end].decode('utf-8')
02004         else:
02005           _v104.id = str[start:end]
02006         start = end
02007         end += 4
02008         (_v104.padding,) = _struct_f.unpack(str[start:end])
02009         _v107 = _v104.operation
02010         start = end
02011         end += 1
02012         (_v107.operation,) = _struct_b.unpack(str[start:end])
02013         start = end
02014         end += 4
02015         (length,) = _struct_I.unpack(str[start:end])
02016         _v104.shapes = []
02017         for i in range(0, length):
02018           val3 = arm_navigation_msgs.msg.Shape()
02019           start = end
02020           end += 1
02021           (val3.type,) = _struct_b.unpack(str[start:end])
02022           start = end
02023           end += 4
02024           (length,) = _struct_I.unpack(str[start:end])
02025           pattern = '<%sd'%length
02026           start = end
02027           end += struct.calcsize(pattern)
02028           val3.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02029           start = end
02030           end += 4
02031           (length,) = _struct_I.unpack(str[start:end])
02032           pattern = '<%si'%length
02033           start = end
02034           end += struct.calcsize(pattern)
02035           val3.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02036           start = end
02037           end += 4
02038           (length,) = _struct_I.unpack(str[start:end])
02039           val3.vertices = []
02040           for i in range(0, length):
02041             val4 = geometry_msgs.msg.Point()
02042             _x = val4
02043             start = end
02044             end += 24
02045             (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02046             val3.vertices.append(val4)
02047           _v104.shapes.append(val3)
02048         start = end
02049         end += 4
02050         (length,) = _struct_I.unpack(str[start:end])
02051         _v104.poses = []
02052         for i in range(0, length):
02053           val3 = geometry_msgs.msg.Pose()
02054           _v108 = val3.position
02055           _x = _v108
02056           start = end
02057           end += 24
02058           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02059           _v109 = val3.orientation
02060           _x = _v109
02061           start = end
02062           end += 32
02063           (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02064           _v104.poses.append(val3)
02065         start = end
02066         end += 4
02067         (length,) = _struct_I.unpack(str[start:end])
02068         val1.touch_links = []
02069         for i in range(0, length):
02070           start = end
02071           end += 4
02072           (length,) = _struct_I.unpack(str[start:end])
02073           start = end
02074           end += length
02075           if python3:
02076             val2 = str[start:end].decode('utf-8')
02077           else:
02078             val2 = str[start:end]
02079           val1.touch_links.append(val2)
02080         self.planning_scene.attached_collision_objects.append(val1)
02081       _x = self
02082       start = end
02083       end += 12
02084       (_x.planning_scene.collision_map.header.seq, _x.planning_scene.collision_map.header.stamp.secs, _x.planning_scene.collision_map.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02085       start = end
02086       end += 4
02087       (length,) = _struct_I.unpack(str[start:end])
02088       start = end
02089       end += length
02090       if python3:
02091         self.planning_scene.collision_map.header.frame_id = str[start:end].decode('utf-8')
02092       else:
02093         self.planning_scene.collision_map.header.frame_id = str[start:end]
02094       start = end
02095       end += 4
02096       (length,) = _struct_I.unpack(str[start:end])
02097       self.planning_scene.collision_map.boxes = []
02098       for i in range(0, length):
02099         val1 = arm_navigation_msgs.msg.OrientedBoundingBox()
02100         _v110 = val1.center
02101         _x = _v110
02102         start = end
02103         end += 12
02104         (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
02105         _v111 = val1.extents
02106         _x = _v111
02107         start = end
02108         end += 12
02109         (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
02110         _v112 = val1.axis
02111         _x = _v112
02112         start = end
02113         end += 12
02114         (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
02115         start = end
02116         end += 4
02117         (val1.angle,) = _struct_f.unpack(str[start:end])
02118         self.planning_scene.collision_map.boxes.append(val1)
02119       return self
02120     except struct.error as e:
02121       raise genpy.DeserializationError(e) #most likely buffer underfill
02122 
02123 _struct_I = genpy.struct_I
02124 _struct_b = struct.Struct("<b")
02125 _struct_d = struct.Struct("<d")
02126 _struct_f = struct.Struct("<f")
02127 _struct_3f = struct.Struct("<3f")
02128 _struct_3I = struct.Struct("<3I")
02129 _struct_4d = struct.Struct("<4d")
02130 _struct_2I = struct.Struct("<2I")
02131 _struct_3d = struct.Struct("<3d")


arm_navigation_msgs
Author(s): Gil Jones
autogenerated on Mon Dec 2 2013 12:31:50