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


arm_navigation_msgs
Author(s): Gil Jones
autogenerated on Thu Dec 12 2013 11:08:10