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