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
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 it's 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
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)
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)
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")