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
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
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)
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)
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")