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