00001 """autogenerated by genpy from arm_navigation_msgs/MoveArmGoal.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006
00007 import arm_navigation_msgs.msg
00008 import geometry_msgs.msg
00009 import std_msgs.msg
00010 import genpy
00011 import sensor_msgs.msg
00012
00013 class MoveArmGoal(genpy.Message):
00014 _md5sum = "229373059043ad35d3ceeb2161f005d6"
00015 _type = "arm_navigation_msgs/MoveArmGoal"
00016 _has_header = False
00017 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00018 # Service name to call for getting a motion plan
00019 # Move arm will call a service on this service name
00020 # using the MotionPlanRequest specified here
00021 string planner_service_name
00022
00023 # A planning scene diff
00024 PlanningScene planning_scene_diff
00025
00026 # A motion planning request
00027 MotionPlanRequest motion_plan_request
00028
00029 # OPTIONAL: Diff uses ordered collision operations in addition to allowed_collision_matrix
00030 arm_navigation_msgs/OrderedCollisionOperations operations
00031
00032 # OPTIONAL flag
00033 # Setting this flag to true will allow move_arm to accept plans that do not go all the way to the goal
00034 bool accept_partial_plans
00035
00036 # OPTIONAL flag
00037 # Setting this flag to true will allow move_arm to accept invalid goals
00038 # This is useful if you are using a planner like CHOMP along with a noisy rapidly changing collision map
00039 # and you would like to plan to a goal near an object.
00040 bool accept_invalid_goals
00041
00042 # OPTIONAL flag
00043 # Setting this flag to true will disable the call to IK for a pose goal
00044 bool disable_ik
00045
00046 # OPTIONAL flag
00047 # Setting this flag to true will disable collision monitoring during execution of a trajectory
00048 bool disable_collision_monitoring
00049
00050 ================================================================================
00051 MSG: arm_navigation_msgs/PlanningScene
00052 #full robot state
00053 arm_navigation_msgs/RobotState robot_state
00054
00055 #additional frames for duplicating tf
00056 geometry_msgs/TransformStamped[] fixed_frame_transforms
00057
00058 #full allowed collision matrix
00059 AllowedCollisionMatrix allowed_collision_matrix
00060
00061 #allowed contacts
00062 arm_navigation_msgs/AllowedContactSpecification[] allowed_contacts
00063
00064 #all link paddings
00065 arm_navigation_msgs/LinkPadding[] link_padding
00066
00067 #collision objects
00068 arm_navigation_msgs/CollisionObject[] collision_objects
00069 arm_navigation_msgs/AttachedCollisionObject[] attached_collision_objects
00070
00071 #the collision map
00072 arm_navigation_msgs/CollisionMap collision_map
00073
00074 ================================================================================
00075 MSG: arm_navigation_msgs/RobotState
00076 # This message contains information about the robot state, i.e. the positions of its joints and links
00077 sensor_msgs/JointState joint_state
00078 arm_navigation_msgs/MultiDOFJointState multi_dof_joint_state
00079
00080 ================================================================================
00081 MSG: sensor_msgs/JointState
00082 # This is a message that holds data to describe the state of a set of torque controlled joints.
00083 #
00084 # The state of each joint (revolute or prismatic) is defined by:
00085 # * the position of the joint (rad or m),
00086 # * the velocity of the joint (rad/s or m/s) and
00087 # * the effort that is applied in the joint (Nm or N).
00088 #
00089 # Each joint is uniquely identified by its name
00090 # The header specifies the time at which the joint states were recorded. All the joint states
00091 # in one message have to be recorded at the same time.
00092 #
00093 # This message consists of a multiple arrays, one for each part of the joint state.
00094 # The goal is to make each of the fields optional. When e.g. your joints have no
00095 # effort associated with them, you can leave the effort array empty.
00096 #
00097 # All arrays in this message should have the same size, or be empty.
00098 # This is the only way to uniquely associate the joint name with the correct
00099 # states.
00100
00101
00102 Header header
00103
00104 string[] name
00105 float64[] position
00106 float64[] velocity
00107 float64[] effort
00108
00109 ================================================================================
00110 MSG: std_msgs/Header
00111 # Standard metadata for higher-level stamped data types.
00112 # This is generally used to communicate timestamped data
00113 # in a particular coordinate frame.
00114 #
00115 # sequence ID: consecutively increasing ID
00116 uint32 seq
00117 #Two-integer timestamp that is expressed as:
00118 # * stamp.secs: seconds (stamp_secs) since epoch
00119 # * stamp.nsecs: nanoseconds since stamp_secs
00120 # time-handling sugar is provided by the client library
00121 time stamp
00122 #Frame this data is associated with
00123 # 0: no frame
00124 # 1: global frame
00125 string frame_id
00126
00127 ================================================================================
00128 MSG: arm_navigation_msgs/MultiDOFJointState
00129 #A representation of a multi-dof joint state
00130 time stamp
00131 string[] joint_names
00132 string[] frame_ids
00133 string[] child_frame_ids
00134 geometry_msgs/Pose[] poses
00135
00136 ================================================================================
00137 MSG: geometry_msgs/Pose
00138 # A representation of pose in free space, composed of postion and orientation.
00139 Point position
00140 Quaternion orientation
00141
00142 ================================================================================
00143 MSG: geometry_msgs/Point
00144 # This contains the position of a point in free space
00145 float64 x
00146 float64 y
00147 float64 z
00148
00149 ================================================================================
00150 MSG: geometry_msgs/Quaternion
00151 # This represents an orientation in free space in quaternion form.
00152
00153 float64 x
00154 float64 y
00155 float64 z
00156 float64 w
00157
00158 ================================================================================
00159 MSG: geometry_msgs/TransformStamped
00160 # This expresses a transform from coordinate frame header.frame_id
00161 # to the coordinate frame child_frame_id
00162 #
00163 # This message is mostly used by the
00164 # <a href="http://www.ros.org/wiki/tf">tf</a> package.
00165 # See it's documentation for more information.
00166
00167 Header header
00168 string child_frame_id # the frame id of the child frame
00169 Transform transform
00170
00171 ================================================================================
00172 MSG: geometry_msgs/Transform
00173 # This represents the transform between two coordinate frames in free space.
00174
00175 Vector3 translation
00176 Quaternion rotation
00177
00178 ================================================================================
00179 MSG: geometry_msgs/Vector3
00180 # This represents a vector in free space.
00181
00182 float64 x
00183 float64 y
00184 float64 z
00185 ================================================================================
00186 MSG: arm_navigation_msgs/AllowedCollisionMatrix
00187 # the list of link names in the matrix
00188 string[] link_names
00189
00190 # the individual entries in the allowed collision matrix
00191 # symmetric, with same order as link_names
00192 AllowedCollisionEntry[] entries
00193
00194 ================================================================================
00195 MSG: arm_navigation_msgs/AllowedCollisionEntry
00196 # whether or not collision checking is enabled
00197 bool[] enabled
00198
00199 ================================================================================
00200 MSG: arm_navigation_msgs/AllowedContactSpecification
00201 # The names of the regions
00202 string name
00203
00204 # The shape of the region in the environment
00205 arm_navigation_msgs/Shape shape
00206
00207 # The pose of the space defining the region
00208 geometry_msgs/PoseStamped pose_stamped
00209
00210 # The set of links that will be allowed to have penetration contact within this region
00211 string[] link_names
00212
00213 # The maximum penetration depth allowed for every link
00214 float64 penetration_depth
00215
00216 ================================================================================
00217 MSG: arm_navigation_msgs/Shape
00218 byte SPHERE=0
00219 byte BOX=1
00220 byte CYLINDER=2
00221 byte MESH=3
00222
00223 byte type
00224
00225
00226 #### define sphere, box, cylinder ####
00227 # the origin of each shape is considered at the shape's center
00228
00229 # for sphere
00230 # radius := dimensions[0]
00231
00232 # for cylinder
00233 # radius := dimensions[0]
00234 # length := dimensions[1]
00235 # the length is along the Z axis
00236
00237 # for box
00238 # size_x := dimensions[0]
00239 # size_y := dimensions[1]
00240 # size_z := dimensions[2]
00241 float64[] dimensions
00242
00243
00244 #### define mesh ####
00245
00246 # list of triangles; triangle k is defined by tre vertices located
00247 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00248 int32[] triangles
00249 geometry_msgs/Point[] vertices
00250
00251 ================================================================================
00252 MSG: geometry_msgs/PoseStamped
00253 # A Pose with reference coordinate frame and timestamp
00254 Header header
00255 Pose pose
00256
00257 ================================================================================
00258 MSG: arm_navigation_msgs/LinkPadding
00259 #name for the link
00260 string link_name
00261
00262 # padding to apply to the link
00263 float64 padding
00264
00265 ================================================================================
00266 MSG: arm_navigation_msgs/CollisionObject
00267 # a header, used for interpreting the poses
00268 Header header
00269
00270 # the id of the object
00271 string id
00272
00273 # The padding used for filtering points near the object.
00274 # This does not affect collision checking for the object.
00275 # Set to negative to get zero padding.
00276 float32 padding
00277
00278 #This contains what is to be done with the object
00279 CollisionObjectOperation operation
00280
00281 #the shapes associated with the object
00282 arm_navigation_msgs/Shape[] shapes
00283
00284 #the poses associated with the shapes - will be transformed using the header
00285 geometry_msgs/Pose[] poses
00286
00287 ================================================================================
00288 MSG: arm_navigation_msgs/CollisionObjectOperation
00289 #Puts the object into the environment
00290 #or updates the object if already added
00291 byte ADD=0
00292
00293 #Removes the object from the environment entirely
00294 byte REMOVE=1
00295
00296 #Only valid within the context of a CollisionAttachedObject message
00297 #Will be ignored if sent with an CollisionObject message
00298 #Takes an attached object, detaches from the attached link
00299 #But adds back in as regular object
00300 byte DETACH_AND_ADD_AS_OBJECT=2
00301
00302 #Only valid within the context of a CollisionAttachedObject message
00303 #Will be ignored if sent with an CollisionObject message
00304 #Takes current object in the environment and removes it as
00305 #a regular object
00306 byte ATTACH_AND_REMOVE_AS_OBJECT=3
00307
00308 # Byte code for operation
00309 byte operation
00310
00311 ================================================================================
00312 MSG: arm_navigation_msgs/AttachedCollisionObject
00313 # The CollisionObject will be attached with a fixed joint to this link
00314 # If link name is set to REMOVE_ALL_ATTACHED_OBJECTS and object.operation
00315 # is set to REMOVE will remove all attached bodies attached to any object
00316 string link_name
00317
00318 #Reserved for indicating that all attached objects should be removed
00319 string REMOVE_ALL_ATTACHED_OBJECTS = "all"
00320
00321 #This contains the actual shapes and poses for the CollisionObject
00322 #to be attached to the link
00323 #If action is remove and no object.id is set, all objects
00324 #attached to the link indicated by link_name will be removed
00325 CollisionObject object
00326
00327 # The set of links that the attached objects are allowed to touch
00328 # by default - the link_name is included by default
00329 string[] touch_links
00330
00331 ================================================================================
00332 MSG: arm_navigation_msgs/CollisionMap
00333 #header for interpreting box positions
00334 Header header
00335
00336 #boxes for use in collision testing
00337 OrientedBoundingBox[] boxes
00338
00339 ================================================================================
00340 MSG: arm_navigation_msgs/OrientedBoundingBox
00341 #the center of the box
00342 geometry_msgs/Point32 center
00343
00344 #the extents of the box, assuming the center is at the point
00345 geometry_msgs/Point32 extents
00346
00347 #the axis of the box
00348 geometry_msgs/Point32 axis
00349
00350 #the angle of rotation around the axis
00351 float32 angle
00352
00353 ================================================================================
00354 MSG: geometry_msgs/Point32
00355 # This contains the position of a point in free space(with 32 bits of precision).
00356 # It is recommeded to use Point wherever possible instead of Point32.
00357 #
00358 # This recommendation is to promote interoperability.
00359 #
00360 # This message is designed to take up less space when sending
00361 # lots of points at once, as in the case of a PointCloud.
00362
00363 float32 x
00364 float32 y
00365 float32 z
00366 ================================================================================
00367 MSG: arm_navigation_msgs/MotionPlanRequest
00368 # This service contains the definition for a request to the motion
00369 # planner and the output it provides
00370
00371 # Parameters for the workspace that the planner should work inside
00372 arm_navigation_msgs/WorkspaceParameters workspace_parameters
00373
00374 # Starting state updates. If certain joints should be considered
00375 # at positions other than the current ones, these positions should
00376 # be set here
00377 arm_navigation_msgs/RobotState start_state
00378
00379 # The goal state for the model to plan for. The goal is achieved
00380 # if all constraints are satisfied
00381 arm_navigation_msgs/Constraints goal_constraints
00382
00383 # No state at any point along the path in the produced motion plan will violate these constraints
00384 arm_navigation_msgs/Constraints path_constraints
00385
00386 # The name of the motion planner to use. If no name is specified,
00387 # a default motion planner will be used
00388 string planner_id
00389
00390 # The name of the group of joints on which this planner is operating
00391 string group_name
00392
00393 # The number of times this plan is to be computed. Shortest solution
00394 # will be reported.
00395 int32 num_planning_attempts
00396
00397 # The maximum amount of time the motion planner is allowed to plan for
00398 duration allowed_planning_time
00399
00400 # An expected path duration (in seconds) along with an expected discretization of the path allows the planner to determine the discretization of the trajectory that it returns
00401 duration expected_path_duration
00402 duration expected_path_dt
00403
00404 ================================================================================
00405 MSG: arm_navigation_msgs/WorkspaceParameters
00406 # This message contains a set of parameters useful in
00407 # setting up the workspace for planning
00408 arm_navigation_msgs/Shape workspace_region_shape
00409 geometry_msgs/PoseStamped workspace_region_pose
00410
00411
00412 ================================================================================
00413 MSG: arm_navigation_msgs/Constraints
00414 # This message contains a list of motion planning constraints.
00415
00416 arm_navigation_msgs/JointConstraint[] joint_constraints
00417 arm_navigation_msgs/PositionConstraint[] position_constraints
00418 arm_navigation_msgs/OrientationConstraint[] orientation_constraints
00419 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints
00420
00421 ================================================================================
00422 MSG: arm_navigation_msgs/JointConstraint
00423 # Constrain the position of a joint to be within a certain bound
00424 string joint_name
00425
00426 # the bound to be achieved is [position - tolerance_below, position + tolerance_above]
00427 float64 position
00428 float64 tolerance_above
00429 float64 tolerance_below
00430
00431 # A weighting factor for this constraint
00432 float64 weight
00433 ================================================================================
00434 MSG: arm_navigation_msgs/PositionConstraint
00435 # This message contains the definition of a position constraint.
00436 Header header
00437
00438 # The robot link this constraint refers to
00439 string link_name
00440
00441 # The offset (in the link frame) for the target point on the link we are planning for
00442 geometry_msgs/Point target_point_offset
00443
00444 # The nominal/target position for the point we are planning for
00445 geometry_msgs/Point position
00446
00447 # The shape of the bounded region that constrains the position of the end-effector
00448 # This region is always centered at the position defined above
00449 arm_navigation_msgs/Shape constraint_region_shape
00450
00451 # The orientation of the bounded region that constrains the position of the end-effector.
00452 # This allows the specification of non-axis aligned constraints
00453 geometry_msgs/Quaternion constraint_region_orientation
00454
00455 # Constraint weighting factor - a weight for this constraint
00456 float64 weight
00457
00458 ================================================================================
00459 MSG: arm_navigation_msgs/OrientationConstraint
00460 # This message contains the definition of an orientation constraint.
00461 Header header
00462
00463 # The robot link this constraint refers to
00464 string link_name
00465
00466 # The type of the constraint
00467 int32 type
00468 int32 LINK_FRAME=0
00469 int32 HEADER_FRAME=1
00470
00471 # The desired orientation of the robot link specified as a quaternion
00472 geometry_msgs/Quaternion orientation
00473
00474 # optional RPY error tolerances specified if
00475 float64 absolute_roll_tolerance
00476 float64 absolute_pitch_tolerance
00477 float64 absolute_yaw_tolerance
00478
00479 # Constraint weighting factor - a weight for this constraint
00480 float64 weight
00481
00482 ================================================================================
00483 MSG: arm_navigation_msgs/VisibilityConstraint
00484 # This message contains the definition of a visibility constraint.
00485 Header header
00486
00487 # The point stamped target that needs to be kept within view of the sensor
00488 geometry_msgs/PointStamped target
00489
00490 # The local pose of the frame in which visibility is to be maintained
00491 # The frame id should represent the robot link to which the sensor is attached
00492 # The visual axis of the sensor is assumed to be along the X axis of this frame
00493 geometry_msgs/PoseStamped sensor_pose
00494
00495 # The deviation (in radians) that will be tolerated
00496 # Constraint error will be measured as the solid angle between the
00497 # X axis of the frame defined above and the vector between the origin
00498 # of the frame defined above and the target location
00499 float64 absolute_tolerance
00500
00501
00502 ================================================================================
00503 MSG: geometry_msgs/PointStamped
00504 # This represents a Point with reference coordinate frame and timestamp
00505 Header header
00506 Point point
00507
00508 ================================================================================
00509 MSG: arm_navigation_msgs/OrderedCollisionOperations
00510 # A set of collision operations that will be performed in the order they are specified
00511 CollisionOperation[] collision_operations
00512 ================================================================================
00513 MSG: arm_navigation_msgs/CollisionOperation
00514 # A definition of a collision operation
00515 # E.g. ("gripper",COLLISION_SET_ALL,ENABLE) will enable collisions
00516 # between the gripper and all objects in the collision space
00517
00518 string object1
00519 string object2
00520 string COLLISION_SET_ALL="all"
00521 string COLLISION_SET_OBJECTS="objects"
00522 string COLLISION_SET_ATTACHED_OBJECTS="attached"
00523
00524 # The penetration distance to which collisions are allowed. This is 0.0 by default.
00525 float64 penetration_distance
00526
00527 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above
00528 int32 operation
00529 int32 DISABLE=0
00530 int32 ENABLE=1
00531
00532 """
00533 __slots__ = ['planner_service_name','planning_scene_diff','motion_plan_request','operations','accept_partial_plans','accept_invalid_goals','disable_ik','disable_collision_monitoring']
00534 _slot_types = ['string','arm_navigation_msgs/PlanningScene','arm_navigation_msgs/MotionPlanRequest','arm_navigation_msgs/OrderedCollisionOperations','bool','bool','bool','bool']
00535
00536 def __init__(self, *args, **kwds):
00537 """
00538 Constructor. Any message fields that are implicitly/explicitly
00539 set to None will be assigned a default value. The recommend
00540 use is keyword arguments as this is more robust to future message
00541 changes. You cannot mix in-order arguments and keyword arguments.
00542
00543 The available fields are:
00544 planner_service_name,planning_scene_diff,motion_plan_request,operations,accept_partial_plans,accept_invalid_goals,disable_ik,disable_collision_monitoring
00545
00546 :param args: complete set of field values, in .msg order
00547 :param kwds: use keyword arguments corresponding to message field names
00548 to set specific fields.
00549 """
00550 if args or kwds:
00551 super(MoveArmGoal, self).__init__(*args, **kwds)
00552
00553 if self.planner_service_name is None:
00554 self.planner_service_name = ''
00555 if self.planning_scene_diff is None:
00556 self.planning_scene_diff = arm_navigation_msgs.msg.PlanningScene()
00557 if self.motion_plan_request is None:
00558 self.motion_plan_request = arm_navigation_msgs.msg.MotionPlanRequest()
00559 if self.operations is None:
00560 self.operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
00561 if self.accept_partial_plans is None:
00562 self.accept_partial_plans = False
00563 if self.accept_invalid_goals is None:
00564 self.accept_invalid_goals = False
00565 if self.disable_ik is None:
00566 self.disable_ik = False
00567 if self.disable_collision_monitoring is None:
00568 self.disable_collision_monitoring = False
00569 else:
00570 self.planner_service_name = ''
00571 self.planning_scene_diff = arm_navigation_msgs.msg.PlanningScene()
00572 self.motion_plan_request = arm_navigation_msgs.msg.MotionPlanRequest()
00573 self.operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
00574 self.accept_partial_plans = False
00575 self.accept_invalid_goals = False
00576 self.disable_ik = False
00577 self.disable_collision_monitoring = False
00578
00579 def _get_types(self):
00580 """
00581 internal API method
00582 """
00583 return self._slot_types
00584
00585 def serialize(self, buff):
00586 """
00587 serialize message into buffer
00588 :param buff: buffer, ``StringIO``
00589 """
00590 try:
00591 _x = self.planner_service_name
00592 length = len(_x)
00593 if python3 or type(_x) == unicode:
00594 _x = _x.encode('utf-8')
00595 length = len(_x)
00596 buff.write(struct.pack('<I%ss'%length, length, _x))
00597 _x = self
00598 buff.write(_struct_3I.pack(_x.planning_scene_diff.robot_state.joint_state.header.seq, _x.planning_scene_diff.robot_state.joint_state.header.stamp.secs, _x.planning_scene_diff.robot_state.joint_state.header.stamp.nsecs))
00599 _x = self.planning_scene_diff.robot_state.joint_state.header.frame_id
00600 length = len(_x)
00601 if python3 or type(_x) == unicode:
00602 _x = _x.encode('utf-8')
00603 length = len(_x)
00604 buff.write(struct.pack('<I%ss'%length, length, _x))
00605 length = len(self.planning_scene_diff.robot_state.joint_state.name)
00606 buff.write(_struct_I.pack(length))
00607 for val1 in self.planning_scene_diff.robot_state.joint_state.name:
00608 length = len(val1)
00609 if python3 or type(val1) == unicode:
00610 val1 = val1.encode('utf-8')
00611 length = len(val1)
00612 buff.write(struct.pack('<I%ss'%length, length, val1))
00613 length = len(self.planning_scene_diff.robot_state.joint_state.position)
00614 buff.write(_struct_I.pack(length))
00615 pattern = '<%sd'%length
00616 buff.write(struct.pack(pattern, *self.planning_scene_diff.robot_state.joint_state.position))
00617 length = len(self.planning_scene_diff.robot_state.joint_state.velocity)
00618 buff.write(_struct_I.pack(length))
00619 pattern = '<%sd'%length
00620 buff.write(struct.pack(pattern, *self.planning_scene_diff.robot_state.joint_state.velocity))
00621 length = len(self.planning_scene_diff.robot_state.joint_state.effort)
00622 buff.write(_struct_I.pack(length))
00623 pattern = '<%sd'%length
00624 buff.write(struct.pack(pattern, *self.planning_scene_diff.robot_state.joint_state.effort))
00625 _x = self
00626 buff.write(_struct_2I.pack(_x.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.secs, _x.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.nsecs))
00627 length = len(self.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names)
00628 buff.write(_struct_I.pack(length))
00629 for val1 in self.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names:
00630 length = len(val1)
00631 if python3 or type(val1) == unicode:
00632 val1 = val1.encode('utf-8')
00633 length = len(val1)
00634 buff.write(struct.pack('<I%ss'%length, length, val1))
00635 length = len(self.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids)
00636 buff.write(_struct_I.pack(length))
00637 for val1 in self.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids:
00638 length = len(val1)
00639 if python3 or type(val1) == unicode:
00640 val1 = val1.encode('utf-8')
00641 length = len(val1)
00642 buff.write(struct.pack('<I%ss'%length, length, val1))
00643 length = len(self.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids)
00644 buff.write(_struct_I.pack(length))
00645 for val1 in self.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids:
00646 length = len(val1)
00647 if python3 or type(val1) == unicode:
00648 val1 = val1.encode('utf-8')
00649 length = len(val1)
00650 buff.write(struct.pack('<I%ss'%length, length, val1))
00651 length = len(self.planning_scene_diff.robot_state.multi_dof_joint_state.poses)
00652 buff.write(_struct_I.pack(length))
00653 for val1 in self.planning_scene_diff.robot_state.multi_dof_joint_state.poses:
00654 _v1 = val1.position
00655 _x = _v1
00656 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00657 _v2 = val1.orientation
00658 _x = _v2
00659 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00660 length = len(self.planning_scene_diff.fixed_frame_transforms)
00661 buff.write(_struct_I.pack(length))
00662 for val1 in self.planning_scene_diff.fixed_frame_transforms:
00663 _v3 = val1.header
00664 buff.write(_struct_I.pack(_v3.seq))
00665 _v4 = _v3.stamp
00666 _x = _v4
00667 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00668 _x = _v3.frame_id
00669 length = len(_x)
00670 if python3 or type(_x) == unicode:
00671 _x = _x.encode('utf-8')
00672 length = len(_x)
00673 buff.write(struct.pack('<I%ss'%length, length, _x))
00674 _x = val1.child_frame_id
00675 length = len(_x)
00676 if python3 or type(_x) == unicode:
00677 _x = _x.encode('utf-8')
00678 length = len(_x)
00679 buff.write(struct.pack('<I%ss'%length, length, _x))
00680 _v5 = val1.transform
00681 _v6 = _v5.translation
00682 _x = _v6
00683 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00684 _v7 = _v5.rotation
00685 _x = _v7
00686 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00687 length = len(self.planning_scene_diff.allowed_collision_matrix.link_names)
00688 buff.write(_struct_I.pack(length))
00689 for val1 in self.planning_scene_diff.allowed_collision_matrix.link_names:
00690 length = len(val1)
00691 if python3 or type(val1) == unicode:
00692 val1 = val1.encode('utf-8')
00693 length = len(val1)
00694 buff.write(struct.pack('<I%ss'%length, length, val1))
00695 length = len(self.planning_scene_diff.allowed_collision_matrix.entries)
00696 buff.write(_struct_I.pack(length))
00697 for val1 in self.planning_scene_diff.allowed_collision_matrix.entries:
00698 length = len(val1.enabled)
00699 buff.write(_struct_I.pack(length))
00700 pattern = '<%sB'%length
00701 buff.write(struct.pack(pattern, *val1.enabled))
00702 length = len(self.planning_scene_diff.allowed_contacts)
00703 buff.write(_struct_I.pack(length))
00704 for val1 in self.planning_scene_diff.allowed_contacts:
00705 _x = val1.name
00706 length = len(_x)
00707 if python3 or type(_x) == unicode:
00708 _x = _x.encode('utf-8')
00709 length = len(_x)
00710 buff.write(struct.pack('<I%ss'%length, length, _x))
00711 _v8 = val1.shape
00712 buff.write(_struct_b.pack(_v8.type))
00713 length = len(_v8.dimensions)
00714 buff.write(_struct_I.pack(length))
00715 pattern = '<%sd'%length
00716 buff.write(struct.pack(pattern, *_v8.dimensions))
00717 length = len(_v8.triangles)
00718 buff.write(_struct_I.pack(length))
00719 pattern = '<%si'%length
00720 buff.write(struct.pack(pattern, *_v8.triangles))
00721 length = len(_v8.vertices)
00722 buff.write(_struct_I.pack(length))
00723 for val3 in _v8.vertices:
00724 _x = val3
00725 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00726 _v9 = val1.pose_stamped
00727 _v10 = _v9.header
00728 buff.write(_struct_I.pack(_v10.seq))
00729 _v11 = _v10.stamp
00730 _x = _v11
00731 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00732 _x = _v10.frame_id
00733 length = len(_x)
00734 if python3 or type(_x) == unicode:
00735 _x = _x.encode('utf-8')
00736 length = len(_x)
00737 buff.write(struct.pack('<I%ss'%length, length, _x))
00738 _v12 = _v9.pose
00739 _v13 = _v12.position
00740 _x = _v13
00741 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00742 _v14 = _v12.orientation
00743 _x = _v14
00744 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00745 length = len(val1.link_names)
00746 buff.write(_struct_I.pack(length))
00747 for val2 in val1.link_names:
00748 length = len(val2)
00749 if python3 or type(val2) == unicode:
00750 val2 = val2.encode('utf-8')
00751 length = len(val2)
00752 buff.write(struct.pack('<I%ss'%length, length, val2))
00753 buff.write(_struct_d.pack(val1.penetration_depth))
00754 length = len(self.planning_scene_diff.link_padding)
00755 buff.write(_struct_I.pack(length))
00756 for val1 in self.planning_scene_diff.link_padding:
00757 _x = val1.link_name
00758 length = len(_x)
00759 if python3 or type(_x) == unicode:
00760 _x = _x.encode('utf-8')
00761 length = len(_x)
00762 buff.write(struct.pack('<I%ss'%length, length, _x))
00763 buff.write(_struct_d.pack(val1.padding))
00764 length = len(self.planning_scene_diff.collision_objects)
00765 buff.write(_struct_I.pack(length))
00766 for val1 in self.planning_scene_diff.collision_objects:
00767 _v15 = val1.header
00768 buff.write(_struct_I.pack(_v15.seq))
00769 _v16 = _v15.stamp
00770 _x = _v16
00771 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00772 _x = _v15.frame_id
00773 length = len(_x)
00774 if python3 or type(_x) == unicode:
00775 _x = _x.encode('utf-8')
00776 length = len(_x)
00777 buff.write(struct.pack('<I%ss'%length, length, _x))
00778 _x = val1.id
00779 length = len(_x)
00780 if python3 or type(_x) == unicode:
00781 _x = _x.encode('utf-8')
00782 length = len(_x)
00783 buff.write(struct.pack('<I%ss'%length, length, _x))
00784 buff.write(_struct_f.pack(val1.padding))
00785 _v17 = val1.operation
00786 buff.write(_struct_b.pack(_v17.operation))
00787 length = len(val1.shapes)
00788 buff.write(_struct_I.pack(length))
00789 for val2 in val1.shapes:
00790 buff.write(_struct_b.pack(val2.type))
00791 length = len(val2.dimensions)
00792 buff.write(_struct_I.pack(length))
00793 pattern = '<%sd'%length
00794 buff.write(struct.pack(pattern, *val2.dimensions))
00795 length = len(val2.triangles)
00796 buff.write(_struct_I.pack(length))
00797 pattern = '<%si'%length
00798 buff.write(struct.pack(pattern, *val2.triangles))
00799 length = len(val2.vertices)
00800 buff.write(_struct_I.pack(length))
00801 for val3 in val2.vertices:
00802 _x = val3
00803 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00804 length = len(val1.poses)
00805 buff.write(_struct_I.pack(length))
00806 for val2 in val1.poses:
00807 _v18 = val2.position
00808 _x = _v18
00809 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00810 _v19 = val2.orientation
00811 _x = _v19
00812 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00813 length = len(self.planning_scene_diff.attached_collision_objects)
00814 buff.write(_struct_I.pack(length))
00815 for val1 in self.planning_scene_diff.attached_collision_objects:
00816 _x = val1.link_name
00817 length = len(_x)
00818 if python3 or type(_x) == unicode:
00819 _x = _x.encode('utf-8')
00820 length = len(_x)
00821 buff.write(struct.pack('<I%ss'%length, length, _x))
00822 _v20 = val1.object
00823 _v21 = _v20.header
00824 buff.write(_struct_I.pack(_v21.seq))
00825 _v22 = _v21.stamp
00826 _x = _v22
00827 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00828 _x = _v21.frame_id
00829 length = len(_x)
00830 if python3 or type(_x) == unicode:
00831 _x = _x.encode('utf-8')
00832 length = len(_x)
00833 buff.write(struct.pack('<I%ss'%length, length, _x))
00834 _x = _v20.id
00835 length = len(_x)
00836 if python3 or type(_x) == unicode:
00837 _x = _x.encode('utf-8')
00838 length = len(_x)
00839 buff.write(struct.pack('<I%ss'%length, length, _x))
00840 buff.write(_struct_f.pack(_v20.padding))
00841 _v23 = _v20.operation
00842 buff.write(_struct_b.pack(_v23.operation))
00843 length = len(_v20.shapes)
00844 buff.write(_struct_I.pack(length))
00845 for val3 in _v20.shapes:
00846 buff.write(_struct_b.pack(val3.type))
00847 length = len(val3.dimensions)
00848 buff.write(_struct_I.pack(length))
00849 pattern = '<%sd'%length
00850 buff.write(struct.pack(pattern, *val3.dimensions))
00851 length = len(val3.triangles)
00852 buff.write(_struct_I.pack(length))
00853 pattern = '<%si'%length
00854 buff.write(struct.pack(pattern, *val3.triangles))
00855 length = len(val3.vertices)
00856 buff.write(_struct_I.pack(length))
00857 for val4 in val3.vertices:
00858 _x = val4
00859 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00860 length = len(_v20.poses)
00861 buff.write(_struct_I.pack(length))
00862 for val3 in _v20.poses:
00863 _v24 = val3.position
00864 _x = _v24
00865 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00866 _v25 = val3.orientation
00867 _x = _v25
00868 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00869 length = len(val1.touch_links)
00870 buff.write(_struct_I.pack(length))
00871 for val2 in val1.touch_links:
00872 length = len(val2)
00873 if python3 or type(val2) == unicode:
00874 val2 = val2.encode('utf-8')
00875 length = len(val2)
00876 buff.write(struct.pack('<I%ss'%length, length, val2))
00877 _x = self
00878 buff.write(_struct_3I.pack(_x.planning_scene_diff.collision_map.header.seq, _x.planning_scene_diff.collision_map.header.stamp.secs, _x.planning_scene_diff.collision_map.header.stamp.nsecs))
00879 _x = self.planning_scene_diff.collision_map.header.frame_id
00880 length = len(_x)
00881 if python3 or type(_x) == unicode:
00882 _x = _x.encode('utf-8')
00883 length = len(_x)
00884 buff.write(struct.pack('<I%ss'%length, length, _x))
00885 length = len(self.planning_scene_diff.collision_map.boxes)
00886 buff.write(_struct_I.pack(length))
00887 for val1 in self.planning_scene_diff.collision_map.boxes:
00888 _v26 = val1.center
00889 _x = _v26
00890 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00891 _v27 = val1.extents
00892 _x = _v27
00893 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00894 _v28 = val1.axis
00895 _x = _v28
00896 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00897 buff.write(_struct_f.pack(val1.angle))
00898 buff.write(_struct_b.pack(self.motion_plan_request.workspace_parameters.workspace_region_shape.type))
00899 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions)
00900 buff.write(_struct_I.pack(length))
00901 pattern = '<%sd'%length
00902 buff.write(struct.pack(pattern, *self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions))
00903 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles)
00904 buff.write(_struct_I.pack(length))
00905 pattern = '<%si'%length
00906 buff.write(struct.pack(pattern, *self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles))
00907 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices)
00908 buff.write(_struct_I.pack(length))
00909 for val1 in self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices:
00910 _x = val1
00911 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00912 _x = self
00913 buff.write(_struct_3I.pack(_x.motion_plan_request.workspace_parameters.workspace_region_pose.header.seq, _x.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.secs, _x.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.nsecs))
00914 _x = self.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id
00915 length = len(_x)
00916 if python3 or type(_x) == unicode:
00917 _x = _x.encode('utf-8')
00918 length = len(_x)
00919 buff.write(struct.pack('<I%ss'%length, length, _x))
00920 _x = self
00921 buff.write(_struct_7d3I.pack(_x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.x, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.y, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.z, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.x, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.y, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.z, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.w, _x.motion_plan_request.start_state.joint_state.header.seq, _x.motion_plan_request.start_state.joint_state.header.stamp.secs, _x.motion_plan_request.start_state.joint_state.header.stamp.nsecs))
00922 _x = self.motion_plan_request.start_state.joint_state.header.frame_id
00923 length = len(_x)
00924 if python3 or type(_x) == unicode:
00925 _x = _x.encode('utf-8')
00926 length = len(_x)
00927 buff.write(struct.pack('<I%ss'%length, length, _x))
00928 length = len(self.motion_plan_request.start_state.joint_state.name)
00929 buff.write(_struct_I.pack(length))
00930 for val1 in self.motion_plan_request.start_state.joint_state.name:
00931 length = len(val1)
00932 if python3 or type(val1) == unicode:
00933 val1 = val1.encode('utf-8')
00934 length = len(val1)
00935 buff.write(struct.pack('<I%ss'%length, length, val1))
00936 length = len(self.motion_plan_request.start_state.joint_state.position)
00937 buff.write(_struct_I.pack(length))
00938 pattern = '<%sd'%length
00939 buff.write(struct.pack(pattern, *self.motion_plan_request.start_state.joint_state.position))
00940 length = len(self.motion_plan_request.start_state.joint_state.velocity)
00941 buff.write(_struct_I.pack(length))
00942 pattern = '<%sd'%length
00943 buff.write(struct.pack(pattern, *self.motion_plan_request.start_state.joint_state.velocity))
00944 length = len(self.motion_plan_request.start_state.joint_state.effort)
00945 buff.write(_struct_I.pack(length))
00946 pattern = '<%sd'%length
00947 buff.write(struct.pack(pattern, *self.motion_plan_request.start_state.joint_state.effort))
00948 _x = self
00949 buff.write(_struct_2I.pack(_x.motion_plan_request.start_state.multi_dof_joint_state.stamp.secs, _x.motion_plan_request.start_state.multi_dof_joint_state.stamp.nsecs))
00950 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.joint_names)
00951 buff.write(_struct_I.pack(length))
00952 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.joint_names:
00953 length = len(val1)
00954 if python3 or type(val1) == unicode:
00955 val1 = val1.encode('utf-8')
00956 length = len(val1)
00957 buff.write(struct.pack('<I%ss'%length, length, val1))
00958 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids)
00959 buff.write(_struct_I.pack(length))
00960 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids:
00961 length = len(val1)
00962 if python3 or type(val1) == unicode:
00963 val1 = val1.encode('utf-8')
00964 length = len(val1)
00965 buff.write(struct.pack('<I%ss'%length, length, val1))
00966 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids)
00967 buff.write(_struct_I.pack(length))
00968 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids:
00969 length = len(val1)
00970 if python3 or type(val1) == unicode:
00971 val1 = val1.encode('utf-8')
00972 length = len(val1)
00973 buff.write(struct.pack('<I%ss'%length, length, val1))
00974 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.poses)
00975 buff.write(_struct_I.pack(length))
00976 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.poses:
00977 _v29 = val1.position
00978 _x = _v29
00979 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00980 _v30 = val1.orientation
00981 _x = _v30
00982 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00983 length = len(self.motion_plan_request.goal_constraints.joint_constraints)
00984 buff.write(_struct_I.pack(length))
00985 for val1 in self.motion_plan_request.goal_constraints.joint_constraints:
00986 _x = val1.joint_name
00987 length = len(_x)
00988 if python3 or type(_x) == unicode:
00989 _x = _x.encode('utf-8')
00990 length = len(_x)
00991 buff.write(struct.pack('<I%ss'%length, length, _x))
00992 _x = val1
00993 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
00994 length = len(self.motion_plan_request.goal_constraints.position_constraints)
00995 buff.write(_struct_I.pack(length))
00996 for val1 in self.motion_plan_request.goal_constraints.position_constraints:
00997 _v31 = val1.header
00998 buff.write(_struct_I.pack(_v31.seq))
00999 _v32 = _v31.stamp
01000 _x = _v32
01001 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01002 _x = _v31.frame_id
01003 length = len(_x)
01004 if python3 or type(_x) == unicode:
01005 _x = _x.encode('utf-8')
01006 length = len(_x)
01007 buff.write(struct.pack('<I%ss'%length, length, _x))
01008 _x = val1.link_name
01009 length = len(_x)
01010 if python3 or type(_x) == unicode:
01011 _x = _x.encode('utf-8')
01012 length = len(_x)
01013 buff.write(struct.pack('<I%ss'%length, length, _x))
01014 _v33 = val1.target_point_offset
01015 _x = _v33
01016 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01017 _v34 = val1.position
01018 _x = _v34
01019 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01020 _v35 = val1.constraint_region_shape
01021 buff.write(_struct_b.pack(_v35.type))
01022 length = len(_v35.dimensions)
01023 buff.write(_struct_I.pack(length))
01024 pattern = '<%sd'%length
01025 buff.write(struct.pack(pattern, *_v35.dimensions))
01026 length = len(_v35.triangles)
01027 buff.write(_struct_I.pack(length))
01028 pattern = '<%si'%length
01029 buff.write(struct.pack(pattern, *_v35.triangles))
01030 length = len(_v35.vertices)
01031 buff.write(_struct_I.pack(length))
01032 for val3 in _v35.vertices:
01033 _x = val3
01034 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01035 _v36 = val1.constraint_region_orientation
01036 _x = _v36
01037 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01038 buff.write(_struct_d.pack(val1.weight))
01039 length = len(self.motion_plan_request.goal_constraints.orientation_constraints)
01040 buff.write(_struct_I.pack(length))
01041 for val1 in self.motion_plan_request.goal_constraints.orientation_constraints:
01042 _v37 = val1.header
01043 buff.write(_struct_I.pack(_v37.seq))
01044 _v38 = _v37.stamp
01045 _x = _v38
01046 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01047 _x = _v37.frame_id
01048 length = len(_x)
01049 if python3 or type(_x) == unicode:
01050 _x = _x.encode('utf-8')
01051 length = len(_x)
01052 buff.write(struct.pack('<I%ss'%length, length, _x))
01053 _x = val1.link_name
01054 length = len(_x)
01055 if python3 or type(_x) == unicode:
01056 _x = _x.encode('utf-8')
01057 length = len(_x)
01058 buff.write(struct.pack('<I%ss'%length, length, _x))
01059 buff.write(_struct_i.pack(val1.type))
01060 _v39 = val1.orientation
01061 _x = _v39
01062 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01063 _x = val1
01064 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
01065 length = len(self.motion_plan_request.goal_constraints.visibility_constraints)
01066 buff.write(_struct_I.pack(length))
01067 for val1 in self.motion_plan_request.goal_constraints.visibility_constraints:
01068 _v40 = val1.header
01069 buff.write(_struct_I.pack(_v40.seq))
01070 _v41 = _v40.stamp
01071 _x = _v41
01072 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01073 _x = _v40.frame_id
01074 length = len(_x)
01075 if python3 or type(_x) == unicode:
01076 _x = _x.encode('utf-8')
01077 length = len(_x)
01078 buff.write(struct.pack('<I%ss'%length, length, _x))
01079 _v42 = val1.target
01080 _v43 = _v42.header
01081 buff.write(_struct_I.pack(_v43.seq))
01082 _v44 = _v43.stamp
01083 _x = _v44
01084 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01085 _x = _v43.frame_id
01086 length = len(_x)
01087 if python3 or type(_x) == unicode:
01088 _x = _x.encode('utf-8')
01089 length = len(_x)
01090 buff.write(struct.pack('<I%ss'%length, length, _x))
01091 _v45 = _v42.point
01092 _x = _v45
01093 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01094 _v46 = val1.sensor_pose
01095 _v47 = _v46.header
01096 buff.write(_struct_I.pack(_v47.seq))
01097 _v48 = _v47.stamp
01098 _x = _v48
01099 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01100 _x = _v47.frame_id
01101 length = len(_x)
01102 if python3 or type(_x) == unicode:
01103 _x = _x.encode('utf-8')
01104 length = len(_x)
01105 buff.write(struct.pack('<I%ss'%length, length, _x))
01106 _v49 = _v46.pose
01107 _v50 = _v49.position
01108 _x = _v50
01109 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01110 _v51 = _v49.orientation
01111 _x = _v51
01112 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01113 buff.write(_struct_d.pack(val1.absolute_tolerance))
01114 length = len(self.motion_plan_request.path_constraints.joint_constraints)
01115 buff.write(_struct_I.pack(length))
01116 for val1 in self.motion_plan_request.path_constraints.joint_constraints:
01117 _x = val1.joint_name
01118 length = len(_x)
01119 if python3 or type(_x) == unicode:
01120 _x = _x.encode('utf-8')
01121 length = len(_x)
01122 buff.write(struct.pack('<I%ss'%length, length, _x))
01123 _x = val1
01124 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
01125 length = len(self.motion_plan_request.path_constraints.position_constraints)
01126 buff.write(_struct_I.pack(length))
01127 for val1 in self.motion_plan_request.path_constraints.position_constraints:
01128 _v52 = val1.header
01129 buff.write(_struct_I.pack(_v52.seq))
01130 _v53 = _v52.stamp
01131 _x = _v53
01132 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01133 _x = _v52.frame_id
01134 length = len(_x)
01135 if python3 or type(_x) == unicode:
01136 _x = _x.encode('utf-8')
01137 length = len(_x)
01138 buff.write(struct.pack('<I%ss'%length, length, _x))
01139 _x = val1.link_name
01140 length = len(_x)
01141 if python3 or type(_x) == unicode:
01142 _x = _x.encode('utf-8')
01143 length = len(_x)
01144 buff.write(struct.pack('<I%ss'%length, length, _x))
01145 _v54 = val1.target_point_offset
01146 _x = _v54
01147 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01148 _v55 = val1.position
01149 _x = _v55
01150 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01151 _v56 = val1.constraint_region_shape
01152 buff.write(_struct_b.pack(_v56.type))
01153 length = len(_v56.dimensions)
01154 buff.write(_struct_I.pack(length))
01155 pattern = '<%sd'%length
01156 buff.write(struct.pack(pattern, *_v56.dimensions))
01157 length = len(_v56.triangles)
01158 buff.write(_struct_I.pack(length))
01159 pattern = '<%si'%length
01160 buff.write(struct.pack(pattern, *_v56.triangles))
01161 length = len(_v56.vertices)
01162 buff.write(_struct_I.pack(length))
01163 for val3 in _v56.vertices:
01164 _x = val3
01165 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01166 _v57 = val1.constraint_region_orientation
01167 _x = _v57
01168 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01169 buff.write(_struct_d.pack(val1.weight))
01170 length = len(self.motion_plan_request.path_constraints.orientation_constraints)
01171 buff.write(_struct_I.pack(length))
01172 for val1 in self.motion_plan_request.path_constraints.orientation_constraints:
01173 _v58 = val1.header
01174 buff.write(_struct_I.pack(_v58.seq))
01175 _v59 = _v58.stamp
01176 _x = _v59
01177 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01178 _x = _v58.frame_id
01179 length = len(_x)
01180 if python3 or type(_x) == unicode:
01181 _x = _x.encode('utf-8')
01182 length = len(_x)
01183 buff.write(struct.pack('<I%ss'%length, length, _x))
01184 _x = val1.link_name
01185 length = len(_x)
01186 if python3 or type(_x) == unicode:
01187 _x = _x.encode('utf-8')
01188 length = len(_x)
01189 buff.write(struct.pack('<I%ss'%length, length, _x))
01190 buff.write(_struct_i.pack(val1.type))
01191 _v60 = val1.orientation
01192 _x = _v60
01193 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01194 _x = val1
01195 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
01196 length = len(self.motion_plan_request.path_constraints.visibility_constraints)
01197 buff.write(_struct_I.pack(length))
01198 for val1 in self.motion_plan_request.path_constraints.visibility_constraints:
01199 _v61 = val1.header
01200 buff.write(_struct_I.pack(_v61.seq))
01201 _v62 = _v61.stamp
01202 _x = _v62
01203 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01204 _x = _v61.frame_id
01205 length = len(_x)
01206 if python3 or type(_x) == unicode:
01207 _x = _x.encode('utf-8')
01208 length = len(_x)
01209 buff.write(struct.pack('<I%ss'%length, length, _x))
01210 _v63 = val1.target
01211 _v64 = _v63.header
01212 buff.write(_struct_I.pack(_v64.seq))
01213 _v65 = _v64.stamp
01214 _x = _v65
01215 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01216 _x = _v64.frame_id
01217 length = len(_x)
01218 if python3 or type(_x) == unicode:
01219 _x = _x.encode('utf-8')
01220 length = len(_x)
01221 buff.write(struct.pack('<I%ss'%length, length, _x))
01222 _v66 = _v63.point
01223 _x = _v66
01224 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01225 _v67 = val1.sensor_pose
01226 _v68 = _v67.header
01227 buff.write(_struct_I.pack(_v68.seq))
01228 _v69 = _v68.stamp
01229 _x = _v69
01230 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01231 _x = _v68.frame_id
01232 length = len(_x)
01233 if python3 or type(_x) == unicode:
01234 _x = _x.encode('utf-8')
01235 length = len(_x)
01236 buff.write(struct.pack('<I%ss'%length, length, _x))
01237 _v70 = _v67.pose
01238 _v71 = _v70.position
01239 _x = _v71
01240 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01241 _v72 = _v70.orientation
01242 _x = _v72
01243 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01244 buff.write(_struct_d.pack(val1.absolute_tolerance))
01245 _x = self.motion_plan_request.planner_id
01246 length = len(_x)
01247 if python3 or type(_x) == unicode:
01248 _x = _x.encode('utf-8')
01249 length = len(_x)
01250 buff.write(struct.pack('<I%ss'%length, length, _x))
01251 _x = self.motion_plan_request.group_name
01252 length = len(_x)
01253 if python3 or type(_x) == unicode:
01254 _x = _x.encode('utf-8')
01255 length = len(_x)
01256 buff.write(struct.pack('<I%ss'%length, length, _x))
01257 _x = self
01258 buff.write(_struct_7i.pack(_x.motion_plan_request.num_planning_attempts, _x.motion_plan_request.allowed_planning_time.secs, _x.motion_plan_request.allowed_planning_time.nsecs, _x.motion_plan_request.expected_path_duration.secs, _x.motion_plan_request.expected_path_duration.nsecs, _x.motion_plan_request.expected_path_dt.secs, _x.motion_plan_request.expected_path_dt.nsecs))
01259 length = len(self.operations.collision_operations)
01260 buff.write(_struct_I.pack(length))
01261 for val1 in self.operations.collision_operations:
01262 _x = val1.object1
01263 length = len(_x)
01264 if python3 or type(_x) == unicode:
01265 _x = _x.encode('utf-8')
01266 length = len(_x)
01267 buff.write(struct.pack('<I%ss'%length, length, _x))
01268 _x = val1.object2
01269 length = len(_x)
01270 if python3 or type(_x) == unicode:
01271 _x = _x.encode('utf-8')
01272 length = len(_x)
01273 buff.write(struct.pack('<I%ss'%length, length, _x))
01274 _x = val1
01275 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
01276 _x = self
01277 buff.write(_struct_4B.pack(_x.accept_partial_plans, _x.accept_invalid_goals, _x.disable_ik, _x.disable_collision_monitoring))
01278 except struct.error as se: self._check_types(se)
01279 except TypeError as te: self._check_types(te)
01280
01281 def deserialize(self, str):
01282 """
01283 unpack serialized message in str into this message instance
01284 :param str: byte array of serialized message, ``str``
01285 """
01286 try:
01287 if self.planning_scene_diff is None:
01288 self.planning_scene_diff = arm_navigation_msgs.msg.PlanningScene()
01289 if self.motion_plan_request is None:
01290 self.motion_plan_request = arm_navigation_msgs.msg.MotionPlanRequest()
01291 if self.operations is None:
01292 self.operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
01293 end = 0
01294 start = end
01295 end += 4
01296 (length,) = _struct_I.unpack(str[start:end])
01297 start = end
01298 end += length
01299 if python3:
01300 self.planner_service_name = str[start:end].decode('utf-8')
01301 else:
01302 self.planner_service_name = str[start:end]
01303 _x = self
01304 start = end
01305 end += 12
01306 (_x.planning_scene_diff.robot_state.joint_state.header.seq, _x.planning_scene_diff.robot_state.joint_state.header.stamp.secs, _x.planning_scene_diff.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01307 start = end
01308 end += 4
01309 (length,) = _struct_I.unpack(str[start:end])
01310 start = end
01311 end += length
01312 if python3:
01313 self.planning_scene_diff.robot_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
01314 else:
01315 self.planning_scene_diff.robot_state.joint_state.header.frame_id = str[start:end]
01316 start = end
01317 end += 4
01318 (length,) = _struct_I.unpack(str[start:end])
01319 self.planning_scene_diff.robot_state.joint_state.name = []
01320 for i in range(0, length):
01321 start = end
01322 end += 4
01323 (length,) = _struct_I.unpack(str[start:end])
01324 start = end
01325 end += length
01326 if python3:
01327 val1 = str[start:end].decode('utf-8')
01328 else:
01329 val1 = str[start:end]
01330 self.planning_scene_diff.robot_state.joint_state.name.append(val1)
01331 start = end
01332 end += 4
01333 (length,) = _struct_I.unpack(str[start:end])
01334 pattern = '<%sd'%length
01335 start = end
01336 end += struct.calcsize(pattern)
01337 self.planning_scene_diff.robot_state.joint_state.position = struct.unpack(pattern, str[start:end])
01338 start = end
01339 end += 4
01340 (length,) = _struct_I.unpack(str[start:end])
01341 pattern = '<%sd'%length
01342 start = end
01343 end += struct.calcsize(pattern)
01344 self.planning_scene_diff.robot_state.joint_state.velocity = struct.unpack(pattern, str[start:end])
01345 start = end
01346 end += 4
01347 (length,) = _struct_I.unpack(str[start:end])
01348 pattern = '<%sd'%length
01349 start = end
01350 end += struct.calcsize(pattern)
01351 self.planning_scene_diff.robot_state.joint_state.effort = struct.unpack(pattern, str[start:end])
01352 _x = self
01353 start = end
01354 end += 8
01355 (_x.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.secs, _x.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01356 start = end
01357 end += 4
01358 (length,) = _struct_I.unpack(str[start:end])
01359 self.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names = []
01360 for i in range(0, length):
01361 start = end
01362 end += 4
01363 (length,) = _struct_I.unpack(str[start:end])
01364 start = end
01365 end += length
01366 if python3:
01367 val1 = str[start:end].decode('utf-8')
01368 else:
01369 val1 = str[start:end]
01370 self.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names.append(val1)
01371 start = end
01372 end += 4
01373 (length,) = _struct_I.unpack(str[start:end])
01374 self.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids = []
01375 for i in range(0, length):
01376 start = end
01377 end += 4
01378 (length,) = _struct_I.unpack(str[start:end])
01379 start = end
01380 end += length
01381 if python3:
01382 val1 = str[start:end].decode('utf-8')
01383 else:
01384 val1 = str[start:end]
01385 self.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids.append(val1)
01386 start = end
01387 end += 4
01388 (length,) = _struct_I.unpack(str[start:end])
01389 self.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids = []
01390 for i in range(0, length):
01391 start = end
01392 end += 4
01393 (length,) = _struct_I.unpack(str[start:end])
01394 start = end
01395 end += length
01396 if python3:
01397 val1 = str[start:end].decode('utf-8')
01398 else:
01399 val1 = str[start:end]
01400 self.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids.append(val1)
01401 start = end
01402 end += 4
01403 (length,) = _struct_I.unpack(str[start:end])
01404 self.planning_scene_diff.robot_state.multi_dof_joint_state.poses = []
01405 for i in range(0, length):
01406 val1 = geometry_msgs.msg.Pose()
01407 _v73 = val1.position
01408 _x = _v73
01409 start = end
01410 end += 24
01411 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01412 _v74 = val1.orientation
01413 _x = _v74
01414 start = end
01415 end += 32
01416 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01417 self.planning_scene_diff.robot_state.multi_dof_joint_state.poses.append(val1)
01418 start = end
01419 end += 4
01420 (length,) = _struct_I.unpack(str[start:end])
01421 self.planning_scene_diff.fixed_frame_transforms = []
01422 for i in range(0, length):
01423 val1 = geometry_msgs.msg.TransformStamped()
01424 _v75 = val1.header
01425 start = end
01426 end += 4
01427 (_v75.seq,) = _struct_I.unpack(str[start:end])
01428 _v76 = _v75.stamp
01429 _x = _v76
01430 start = end
01431 end += 8
01432 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01433 start = end
01434 end += 4
01435 (length,) = _struct_I.unpack(str[start:end])
01436 start = end
01437 end += length
01438 if python3:
01439 _v75.frame_id = str[start:end].decode('utf-8')
01440 else:
01441 _v75.frame_id = str[start:end]
01442 start = end
01443 end += 4
01444 (length,) = _struct_I.unpack(str[start:end])
01445 start = end
01446 end += length
01447 if python3:
01448 val1.child_frame_id = str[start:end].decode('utf-8')
01449 else:
01450 val1.child_frame_id = str[start:end]
01451 _v77 = val1.transform
01452 _v78 = _v77.translation
01453 _x = _v78
01454 start = end
01455 end += 24
01456 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01457 _v79 = _v77.rotation
01458 _x = _v79
01459 start = end
01460 end += 32
01461 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01462 self.planning_scene_diff.fixed_frame_transforms.append(val1)
01463 start = end
01464 end += 4
01465 (length,) = _struct_I.unpack(str[start:end])
01466 self.planning_scene_diff.allowed_collision_matrix.link_names = []
01467 for i in range(0, length):
01468 start = end
01469 end += 4
01470 (length,) = _struct_I.unpack(str[start:end])
01471 start = end
01472 end += length
01473 if python3:
01474 val1 = str[start:end].decode('utf-8')
01475 else:
01476 val1 = str[start:end]
01477 self.planning_scene_diff.allowed_collision_matrix.link_names.append(val1)
01478 start = end
01479 end += 4
01480 (length,) = _struct_I.unpack(str[start:end])
01481 self.planning_scene_diff.allowed_collision_matrix.entries = []
01482 for i in range(0, length):
01483 val1 = arm_navigation_msgs.msg.AllowedCollisionEntry()
01484 start = end
01485 end += 4
01486 (length,) = _struct_I.unpack(str[start:end])
01487 pattern = '<%sB'%length
01488 start = end
01489 end += struct.calcsize(pattern)
01490 val1.enabled = struct.unpack(pattern, str[start:end])
01491 val1.enabled = map(bool, val1.enabled)
01492 self.planning_scene_diff.allowed_collision_matrix.entries.append(val1)
01493 start = end
01494 end += 4
01495 (length,) = _struct_I.unpack(str[start:end])
01496 self.planning_scene_diff.allowed_contacts = []
01497 for i in range(0, length):
01498 val1 = arm_navigation_msgs.msg.AllowedContactSpecification()
01499 start = end
01500 end += 4
01501 (length,) = _struct_I.unpack(str[start:end])
01502 start = end
01503 end += length
01504 if python3:
01505 val1.name = str[start:end].decode('utf-8')
01506 else:
01507 val1.name = str[start:end]
01508 _v80 = val1.shape
01509 start = end
01510 end += 1
01511 (_v80.type,) = _struct_b.unpack(str[start:end])
01512 start = end
01513 end += 4
01514 (length,) = _struct_I.unpack(str[start:end])
01515 pattern = '<%sd'%length
01516 start = end
01517 end += struct.calcsize(pattern)
01518 _v80.dimensions = struct.unpack(pattern, str[start:end])
01519 start = end
01520 end += 4
01521 (length,) = _struct_I.unpack(str[start:end])
01522 pattern = '<%si'%length
01523 start = end
01524 end += struct.calcsize(pattern)
01525 _v80.triangles = struct.unpack(pattern, str[start:end])
01526 start = end
01527 end += 4
01528 (length,) = _struct_I.unpack(str[start:end])
01529 _v80.vertices = []
01530 for i in range(0, length):
01531 val3 = geometry_msgs.msg.Point()
01532 _x = val3
01533 start = end
01534 end += 24
01535 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01536 _v80.vertices.append(val3)
01537 _v81 = val1.pose_stamped
01538 _v82 = _v81.header
01539 start = end
01540 end += 4
01541 (_v82.seq,) = _struct_I.unpack(str[start:end])
01542 _v83 = _v82.stamp
01543 _x = _v83
01544 start = end
01545 end += 8
01546 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01547 start = end
01548 end += 4
01549 (length,) = _struct_I.unpack(str[start:end])
01550 start = end
01551 end += length
01552 if python3:
01553 _v82.frame_id = str[start:end].decode('utf-8')
01554 else:
01555 _v82.frame_id = str[start:end]
01556 _v84 = _v81.pose
01557 _v85 = _v84.position
01558 _x = _v85
01559 start = end
01560 end += 24
01561 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01562 _v86 = _v84.orientation
01563 _x = _v86
01564 start = end
01565 end += 32
01566 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01567 start = end
01568 end += 4
01569 (length,) = _struct_I.unpack(str[start:end])
01570 val1.link_names = []
01571 for i in range(0, length):
01572 start = end
01573 end += 4
01574 (length,) = _struct_I.unpack(str[start:end])
01575 start = end
01576 end += length
01577 if python3:
01578 val2 = str[start:end].decode('utf-8')
01579 else:
01580 val2 = str[start:end]
01581 val1.link_names.append(val2)
01582 start = end
01583 end += 8
01584 (val1.penetration_depth,) = _struct_d.unpack(str[start:end])
01585 self.planning_scene_diff.allowed_contacts.append(val1)
01586 start = end
01587 end += 4
01588 (length,) = _struct_I.unpack(str[start:end])
01589 self.planning_scene_diff.link_padding = []
01590 for i in range(0, length):
01591 val1 = arm_navigation_msgs.msg.LinkPadding()
01592 start = end
01593 end += 4
01594 (length,) = _struct_I.unpack(str[start:end])
01595 start = end
01596 end += length
01597 if python3:
01598 val1.link_name = str[start:end].decode('utf-8')
01599 else:
01600 val1.link_name = str[start:end]
01601 start = end
01602 end += 8
01603 (val1.padding,) = _struct_d.unpack(str[start:end])
01604 self.planning_scene_diff.link_padding.append(val1)
01605 start = end
01606 end += 4
01607 (length,) = _struct_I.unpack(str[start:end])
01608 self.planning_scene_diff.collision_objects = []
01609 for i in range(0, length):
01610 val1 = arm_navigation_msgs.msg.CollisionObject()
01611 _v87 = val1.header
01612 start = end
01613 end += 4
01614 (_v87.seq,) = _struct_I.unpack(str[start:end])
01615 _v88 = _v87.stamp
01616 _x = _v88
01617 start = end
01618 end += 8
01619 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01620 start = end
01621 end += 4
01622 (length,) = _struct_I.unpack(str[start:end])
01623 start = end
01624 end += length
01625 if python3:
01626 _v87.frame_id = str[start:end].decode('utf-8')
01627 else:
01628 _v87.frame_id = str[start:end]
01629 start = end
01630 end += 4
01631 (length,) = _struct_I.unpack(str[start:end])
01632 start = end
01633 end += length
01634 if python3:
01635 val1.id = str[start:end].decode('utf-8')
01636 else:
01637 val1.id = str[start:end]
01638 start = end
01639 end += 4
01640 (val1.padding,) = _struct_f.unpack(str[start:end])
01641 _v89 = val1.operation
01642 start = end
01643 end += 1
01644 (_v89.operation,) = _struct_b.unpack(str[start:end])
01645 start = end
01646 end += 4
01647 (length,) = _struct_I.unpack(str[start:end])
01648 val1.shapes = []
01649 for i in range(0, length):
01650 val2 = arm_navigation_msgs.msg.Shape()
01651 start = end
01652 end += 1
01653 (val2.type,) = _struct_b.unpack(str[start:end])
01654 start = end
01655 end += 4
01656 (length,) = _struct_I.unpack(str[start:end])
01657 pattern = '<%sd'%length
01658 start = end
01659 end += struct.calcsize(pattern)
01660 val2.dimensions = struct.unpack(pattern, str[start:end])
01661 start = end
01662 end += 4
01663 (length,) = _struct_I.unpack(str[start:end])
01664 pattern = '<%si'%length
01665 start = end
01666 end += struct.calcsize(pattern)
01667 val2.triangles = struct.unpack(pattern, str[start:end])
01668 start = end
01669 end += 4
01670 (length,) = _struct_I.unpack(str[start:end])
01671 val2.vertices = []
01672 for i in range(0, length):
01673 val3 = geometry_msgs.msg.Point()
01674 _x = val3
01675 start = end
01676 end += 24
01677 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01678 val2.vertices.append(val3)
01679 val1.shapes.append(val2)
01680 start = end
01681 end += 4
01682 (length,) = _struct_I.unpack(str[start:end])
01683 val1.poses = []
01684 for i in range(0, length):
01685 val2 = geometry_msgs.msg.Pose()
01686 _v90 = val2.position
01687 _x = _v90
01688 start = end
01689 end += 24
01690 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01691 _v91 = val2.orientation
01692 _x = _v91
01693 start = end
01694 end += 32
01695 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01696 val1.poses.append(val2)
01697 self.planning_scene_diff.collision_objects.append(val1)
01698 start = end
01699 end += 4
01700 (length,) = _struct_I.unpack(str[start:end])
01701 self.planning_scene_diff.attached_collision_objects = []
01702 for i in range(0, length):
01703 val1 = arm_navigation_msgs.msg.AttachedCollisionObject()
01704 start = end
01705 end += 4
01706 (length,) = _struct_I.unpack(str[start:end])
01707 start = end
01708 end += length
01709 if python3:
01710 val1.link_name = str[start:end].decode('utf-8')
01711 else:
01712 val1.link_name = str[start:end]
01713 _v92 = val1.object
01714 _v93 = _v92.header
01715 start = end
01716 end += 4
01717 (_v93.seq,) = _struct_I.unpack(str[start:end])
01718 _v94 = _v93.stamp
01719 _x = _v94
01720 start = end
01721 end += 8
01722 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01723 start = end
01724 end += 4
01725 (length,) = _struct_I.unpack(str[start:end])
01726 start = end
01727 end += length
01728 if python3:
01729 _v93.frame_id = str[start:end].decode('utf-8')
01730 else:
01731 _v93.frame_id = str[start:end]
01732 start = end
01733 end += 4
01734 (length,) = _struct_I.unpack(str[start:end])
01735 start = end
01736 end += length
01737 if python3:
01738 _v92.id = str[start:end].decode('utf-8')
01739 else:
01740 _v92.id = str[start:end]
01741 start = end
01742 end += 4
01743 (_v92.padding,) = _struct_f.unpack(str[start:end])
01744 _v95 = _v92.operation
01745 start = end
01746 end += 1
01747 (_v95.operation,) = _struct_b.unpack(str[start:end])
01748 start = end
01749 end += 4
01750 (length,) = _struct_I.unpack(str[start:end])
01751 _v92.shapes = []
01752 for i in range(0, length):
01753 val3 = arm_navigation_msgs.msg.Shape()
01754 start = end
01755 end += 1
01756 (val3.type,) = _struct_b.unpack(str[start:end])
01757 start = end
01758 end += 4
01759 (length,) = _struct_I.unpack(str[start:end])
01760 pattern = '<%sd'%length
01761 start = end
01762 end += struct.calcsize(pattern)
01763 val3.dimensions = struct.unpack(pattern, str[start:end])
01764 start = end
01765 end += 4
01766 (length,) = _struct_I.unpack(str[start:end])
01767 pattern = '<%si'%length
01768 start = end
01769 end += struct.calcsize(pattern)
01770 val3.triangles = struct.unpack(pattern, str[start:end])
01771 start = end
01772 end += 4
01773 (length,) = _struct_I.unpack(str[start:end])
01774 val3.vertices = []
01775 for i in range(0, length):
01776 val4 = geometry_msgs.msg.Point()
01777 _x = val4
01778 start = end
01779 end += 24
01780 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01781 val3.vertices.append(val4)
01782 _v92.shapes.append(val3)
01783 start = end
01784 end += 4
01785 (length,) = _struct_I.unpack(str[start:end])
01786 _v92.poses = []
01787 for i in range(0, length):
01788 val3 = geometry_msgs.msg.Pose()
01789 _v96 = val3.position
01790 _x = _v96
01791 start = end
01792 end += 24
01793 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01794 _v97 = val3.orientation
01795 _x = _v97
01796 start = end
01797 end += 32
01798 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01799 _v92.poses.append(val3)
01800 start = end
01801 end += 4
01802 (length,) = _struct_I.unpack(str[start:end])
01803 val1.touch_links = []
01804 for i in range(0, length):
01805 start = end
01806 end += 4
01807 (length,) = _struct_I.unpack(str[start:end])
01808 start = end
01809 end += length
01810 if python3:
01811 val2 = str[start:end].decode('utf-8')
01812 else:
01813 val2 = str[start:end]
01814 val1.touch_links.append(val2)
01815 self.planning_scene_diff.attached_collision_objects.append(val1)
01816 _x = self
01817 start = end
01818 end += 12
01819 (_x.planning_scene_diff.collision_map.header.seq, _x.planning_scene_diff.collision_map.header.stamp.secs, _x.planning_scene_diff.collision_map.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01820 start = end
01821 end += 4
01822 (length,) = _struct_I.unpack(str[start:end])
01823 start = end
01824 end += length
01825 if python3:
01826 self.planning_scene_diff.collision_map.header.frame_id = str[start:end].decode('utf-8')
01827 else:
01828 self.planning_scene_diff.collision_map.header.frame_id = str[start:end]
01829 start = end
01830 end += 4
01831 (length,) = _struct_I.unpack(str[start:end])
01832 self.planning_scene_diff.collision_map.boxes = []
01833 for i in range(0, length):
01834 val1 = arm_navigation_msgs.msg.OrientedBoundingBox()
01835 _v98 = val1.center
01836 _x = _v98
01837 start = end
01838 end += 12
01839 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01840 _v99 = val1.extents
01841 _x = _v99
01842 start = end
01843 end += 12
01844 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01845 _v100 = val1.axis
01846 _x = _v100
01847 start = end
01848 end += 12
01849 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01850 start = end
01851 end += 4
01852 (val1.angle,) = _struct_f.unpack(str[start:end])
01853 self.planning_scene_diff.collision_map.boxes.append(val1)
01854 start = end
01855 end += 1
01856 (self.motion_plan_request.workspace_parameters.workspace_region_shape.type,) = _struct_b.unpack(str[start:end])
01857 start = end
01858 end += 4
01859 (length,) = _struct_I.unpack(str[start:end])
01860 pattern = '<%sd'%length
01861 start = end
01862 end += struct.calcsize(pattern)
01863 self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions = struct.unpack(pattern, str[start:end])
01864 start = end
01865 end += 4
01866 (length,) = _struct_I.unpack(str[start:end])
01867 pattern = '<%si'%length
01868 start = end
01869 end += struct.calcsize(pattern)
01870 self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles = struct.unpack(pattern, str[start:end])
01871 start = end
01872 end += 4
01873 (length,) = _struct_I.unpack(str[start:end])
01874 self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices = []
01875 for i in range(0, length):
01876 val1 = geometry_msgs.msg.Point()
01877 _x = val1
01878 start = end
01879 end += 24
01880 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01881 self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices.append(val1)
01882 _x = self
01883 start = end
01884 end += 12
01885 (_x.motion_plan_request.workspace_parameters.workspace_region_pose.header.seq, _x.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.secs, _x.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01886 start = end
01887 end += 4
01888 (length,) = _struct_I.unpack(str[start:end])
01889 start = end
01890 end += length
01891 if python3:
01892 self.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id = str[start:end].decode('utf-8')
01893 else:
01894 self.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id = str[start:end]
01895 _x = self
01896 start = end
01897 end += 68
01898 (_x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.x, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.y, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.z, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.x, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.y, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.z, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.w, _x.motion_plan_request.start_state.joint_state.header.seq, _x.motion_plan_request.start_state.joint_state.header.stamp.secs, _x.motion_plan_request.start_state.joint_state.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
01899 start = end
01900 end += 4
01901 (length,) = _struct_I.unpack(str[start:end])
01902 start = end
01903 end += length
01904 if python3:
01905 self.motion_plan_request.start_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
01906 else:
01907 self.motion_plan_request.start_state.joint_state.header.frame_id = str[start:end]
01908 start = end
01909 end += 4
01910 (length,) = _struct_I.unpack(str[start:end])
01911 self.motion_plan_request.start_state.joint_state.name = []
01912 for i in range(0, length):
01913 start = end
01914 end += 4
01915 (length,) = _struct_I.unpack(str[start:end])
01916 start = end
01917 end += length
01918 if python3:
01919 val1 = str[start:end].decode('utf-8')
01920 else:
01921 val1 = str[start:end]
01922 self.motion_plan_request.start_state.joint_state.name.append(val1)
01923 start = end
01924 end += 4
01925 (length,) = _struct_I.unpack(str[start:end])
01926 pattern = '<%sd'%length
01927 start = end
01928 end += struct.calcsize(pattern)
01929 self.motion_plan_request.start_state.joint_state.position = struct.unpack(pattern, str[start:end])
01930 start = end
01931 end += 4
01932 (length,) = _struct_I.unpack(str[start:end])
01933 pattern = '<%sd'%length
01934 start = end
01935 end += struct.calcsize(pattern)
01936 self.motion_plan_request.start_state.joint_state.velocity = struct.unpack(pattern, str[start:end])
01937 start = end
01938 end += 4
01939 (length,) = _struct_I.unpack(str[start:end])
01940 pattern = '<%sd'%length
01941 start = end
01942 end += struct.calcsize(pattern)
01943 self.motion_plan_request.start_state.joint_state.effort = struct.unpack(pattern, str[start:end])
01944 _x = self
01945 start = end
01946 end += 8
01947 (_x.motion_plan_request.start_state.multi_dof_joint_state.stamp.secs, _x.motion_plan_request.start_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01948 start = end
01949 end += 4
01950 (length,) = _struct_I.unpack(str[start:end])
01951 self.motion_plan_request.start_state.multi_dof_joint_state.joint_names = []
01952 for i in range(0, length):
01953 start = end
01954 end += 4
01955 (length,) = _struct_I.unpack(str[start:end])
01956 start = end
01957 end += length
01958 if python3:
01959 val1 = str[start:end].decode('utf-8')
01960 else:
01961 val1 = str[start:end]
01962 self.motion_plan_request.start_state.multi_dof_joint_state.joint_names.append(val1)
01963 start = end
01964 end += 4
01965 (length,) = _struct_I.unpack(str[start:end])
01966 self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = []
01967 for i in range(0, length):
01968 start = end
01969 end += 4
01970 (length,) = _struct_I.unpack(str[start:end])
01971 start = end
01972 end += length
01973 if python3:
01974 val1 = str[start:end].decode('utf-8')
01975 else:
01976 val1 = str[start:end]
01977 self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append(val1)
01978 start = end
01979 end += 4
01980 (length,) = _struct_I.unpack(str[start:end])
01981 self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = []
01982 for i in range(0, length):
01983 start = end
01984 end += 4
01985 (length,) = _struct_I.unpack(str[start:end])
01986 start = end
01987 end += length
01988 if python3:
01989 val1 = str[start:end].decode('utf-8')
01990 else:
01991 val1 = str[start:end]
01992 self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append(val1)
01993 start = end
01994 end += 4
01995 (length,) = _struct_I.unpack(str[start:end])
01996 self.motion_plan_request.start_state.multi_dof_joint_state.poses = []
01997 for i in range(0, length):
01998 val1 = geometry_msgs.msg.Pose()
01999 _v101 = val1.position
02000 _x = _v101
02001 start = end
02002 end += 24
02003 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02004 _v102 = val1.orientation
02005 _x = _v102
02006 start = end
02007 end += 32
02008 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02009 self.motion_plan_request.start_state.multi_dof_joint_state.poses.append(val1)
02010 start = end
02011 end += 4
02012 (length,) = _struct_I.unpack(str[start:end])
02013 self.motion_plan_request.goal_constraints.joint_constraints = []
02014 for i in range(0, length):
02015 val1 = arm_navigation_msgs.msg.JointConstraint()
02016 start = end
02017 end += 4
02018 (length,) = _struct_I.unpack(str[start:end])
02019 start = end
02020 end += length
02021 if python3:
02022 val1.joint_name = str[start:end].decode('utf-8')
02023 else:
02024 val1.joint_name = str[start:end]
02025 _x = val1
02026 start = end
02027 end += 32
02028 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
02029 self.motion_plan_request.goal_constraints.joint_constraints.append(val1)
02030 start = end
02031 end += 4
02032 (length,) = _struct_I.unpack(str[start:end])
02033 self.motion_plan_request.goal_constraints.position_constraints = []
02034 for i in range(0, length):
02035 val1 = arm_navigation_msgs.msg.PositionConstraint()
02036 _v103 = val1.header
02037 start = end
02038 end += 4
02039 (_v103.seq,) = _struct_I.unpack(str[start:end])
02040 _v104 = _v103.stamp
02041 _x = _v104
02042 start = end
02043 end += 8
02044 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02045 start = end
02046 end += 4
02047 (length,) = _struct_I.unpack(str[start:end])
02048 start = end
02049 end += length
02050 if python3:
02051 _v103.frame_id = str[start:end].decode('utf-8')
02052 else:
02053 _v103.frame_id = str[start:end]
02054 start = end
02055 end += 4
02056 (length,) = _struct_I.unpack(str[start:end])
02057 start = end
02058 end += length
02059 if python3:
02060 val1.link_name = str[start:end].decode('utf-8')
02061 else:
02062 val1.link_name = str[start:end]
02063 _v105 = val1.target_point_offset
02064 _x = _v105
02065 start = end
02066 end += 24
02067 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02068 _v106 = val1.position
02069 _x = _v106
02070 start = end
02071 end += 24
02072 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02073 _v107 = val1.constraint_region_shape
02074 start = end
02075 end += 1
02076 (_v107.type,) = _struct_b.unpack(str[start:end])
02077 start = end
02078 end += 4
02079 (length,) = _struct_I.unpack(str[start:end])
02080 pattern = '<%sd'%length
02081 start = end
02082 end += struct.calcsize(pattern)
02083 _v107.dimensions = struct.unpack(pattern, str[start:end])
02084 start = end
02085 end += 4
02086 (length,) = _struct_I.unpack(str[start:end])
02087 pattern = '<%si'%length
02088 start = end
02089 end += struct.calcsize(pattern)
02090 _v107.triangles = struct.unpack(pattern, str[start:end])
02091 start = end
02092 end += 4
02093 (length,) = _struct_I.unpack(str[start:end])
02094 _v107.vertices = []
02095 for i in range(0, length):
02096 val3 = geometry_msgs.msg.Point()
02097 _x = val3
02098 start = end
02099 end += 24
02100 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02101 _v107.vertices.append(val3)
02102 _v108 = val1.constraint_region_orientation
02103 _x = _v108
02104 start = end
02105 end += 32
02106 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02107 start = end
02108 end += 8
02109 (val1.weight,) = _struct_d.unpack(str[start:end])
02110 self.motion_plan_request.goal_constraints.position_constraints.append(val1)
02111 start = end
02112 end += 4
02113 (length,) = _struct_I.unpack(str[start:end])
02114 self.motion_plan_request.goal_constraints.orientation_constraints = []
02115 for i in range(0, length):
02116 val1 = arm_navigation_msgs.msg.OrientationConstraint()
02117 _v109 = val1.header
02118 start = end
02119 end += 4
02120 (_v109.seq,) = _struct_I.unpack(str[start:end])
02121 _v110 = _v109.stamp
02122 _x = _v110
02123 start = end
02124 end += 8
02125 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02126 start = end
02127 end += 4
02128 (length,) = _struct_I.unpack(str[start:end])
02129 start = end
02130 end += length
02131 if python3:
02132 _v109.frame_id = str[start:end].decode('utf-8')
02133 else:
02134 _v109.frame_id = str[start:end]
02135 start = end
02136 end += 4
02137 (length,) = _struct_I.unpack(str[start:end])
02138 start = end
02139 end += length
02140 if python3:
02141 val1.link_name = str[start:end].decode('utf-8')
02142 else:
02143 val1.link_name = str[start:end]
02144 start = end
02145 end += 4
02146 (val1.type,) = _struct_i.unpack(str[start:end])
02147 _v111 = val1.orientation
02148 _x = _v111
02149 start = end
02150 end += 32
02151 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02152 _x = val1
02153 start = end
02154 end += 32
02155 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
02156 self.motion_plan_request.goal_constraints.orientation_constraints.append(val1)
02157 start = end
02158 end += 4
02159 (length,) = _struct_I.unpack(str[start:end])
02160 self.motion_plan_request.goal_constraints.visibility_constraints = []
02161 for i in range(0, length):
02162 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
02163 _v112 = val1.header
02164 start = end
02165 end += 4
02166 (_v112.seq,) = _struct_I.unpack(str[start:end])
02167 _v113 = _v112.stamp
02168 _x = _v113
02169 start = end
02170 end += 8
02171 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02172 start = end
02173 end += 4
02174 (length,) = _struct_I.unpack(str[start:end])
02175 start = end
02176 end += length
02177 if python3:
02178 _v112.frame_id = str[start:end].decode('utf-8')
02179 else:
02180 _v112.frame_id = str[start:end]
02181 _v114 = val1.target
02182 _v115 = _v114.header
02183 start = end
02184 end += 4
02185 (_v115.seq,) = _struct_I.unpack(str[start:end])
02186 _v116 = _v115.stamp
02187 _x = _v116
02188 start = end
02189 end += 8
02190 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02191 start = end
02192 end += 4
02193 (length,) = _struct_I.unpack(str[start:end])
02194 start = end
02195 end += length
02196 if python3:
02197 _v115.frame_id = str[start:end].decode('utf-8')
02198 else:
02199 _v115.frame_id = str[start:end]
02200 _v117 = _v114.point
02201 _x = _v117
02202 start = end
02203 end += 24
02204 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02205 _v118 = val1.sensor_pose
02206 _v119 = _v118.header
02207 start = end
02208 end += 4
02209 (_v119.seq,) = _struct_I.unpack(str[start:end])
02210 _v120 = _v119.stamp
02211 _x = _v120
02212 start = end
02213 end += 8
02214 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02215 start = end
02216 end += 4
02217 (length,) = _struct_I.unpack(str[start:end])
02218 start = end
02219 end += length
02220 if python3:
02221 _v119.frame_id = str[start:end].decode('utf-8')
02222 else:
02223 _v119.frame_id = str[start:end]
02224 _v121 = _v118.pose
02225 _v122 = _v121.position
02226 _x = _v122
02227 start = end
02228 end += 24
02229 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02230 _v123 = _v121.orientation
02231 _x = _v123
02232 start = end
02233 end += 32
02234 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02235 start = end
02236 end += 8
02237 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
02238 self.motion_plan_request.goal_constraints.visibility_constraints.append(val1)
02239 start = end
02240 end += 4
02241 (length,) = _struct_I.unpack(str[start:end])
02242 self.motion_plan_request.path_constraints.joint_constraints = []
02243 for i in range(0, length):
02244 val1 = arm_navigation_msgs.msg.JointConstraint()
02245 start = end
02246 end += 4
02247 (length,) = _struct_I.unpack(str[start:end])
02248 start = end
02249 end += length
02250 if python3:
02251 val1.joint_name = str[start:end].decode('utf-8')
02252 else:
02253 val1.joint_name = str[start:end]
02254 _x = val1
02255 start = end
02256 end += 32
02257 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
02258 self.motion_plan_request.path_constraints.joint_constraints.append(val1)
02259 start = end
02260 end += 4
02261 (length,) = _struct_I.unpack(str[start:end])
02262 self.motion_plan_request.path_constraints.position_constraints = []
02263 for i in range(0, length):
02264 val1 = arm_navigation_msgs.msg.PositionConstraint()
02265 _v124 = val1.header
02266 start = end
02267 end += 4
02268 (_v124.seq,) = _struct_I.unpack(str[start:end])
02269 _v125 = _v124.stamp
02270 _x = _v125
02271 start = end
02272 end += 8
02273 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02274 start = end
02275 end += 4
02276 (length,) = _struct_I.unpack(str[start:end])
02277 start = end
02278 end += length
02279 if python3:
02280 _v124.frame_id = str[start:end].decode('utf-8')
02281 else:
02282 _v124.frame_id = str[start:end]
02283 start = end
02284 end += 4
02285 (length,) = _struct_I.unpack(str[start:end])
02286 start = end
02287 end += length
02288 if python3:
02289 val1.link_name = str[start:end].decode('utf-8')
02290 else:
02291 val1.link_name = str[start:end]
02292 _v126 = val1.target_point_offset
02293 _x = _v126
02294 start = end
02295 end += 24
02296 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02297 _v127 = val1.position
02298 _x = _v127
02299 start = end
02300 end += 24
02301 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02302 _v128 = val1.constraint_region_shape
02303 start = end
02304 end += 1
02305 (_v128.type,) = _struct_b.unpack(str[start:end])
02306 start = end
02307 end += 4
02308 (length,) = _struct_I.unpack(str[start:end])
02309 pattern = '<%sd'%length
02310 start = end
02311 end += struct.calcsize(pattern)
02312 _v128.dimensions = struct.unpack(pattern, str[start:end])
02313 start = end
02314 end += 4
02315 (length,) = _struct_I.unpack(str[start:end])
02316 pattern = '<%si'%length
02317 start = end
02318 end += struct.calcsize(pattern)
02319 _v128.triangles = struct.unpack(pattern, str[start:end])
02320 start = end
02321 end += 4
02322 (length,) = _struct_I.unpack(str[start:end])
02323 _v128.vertices = []
02324 for i in range(0, length):
02325 val3 = geometry_msgs.msg.Point()
02326 _x = val3
02327 start = end
02328 end += 24
02329 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02330 _v128.vertices.append(val3)
02331 _v129 = val1.constraint_region_orientation
02332 _x = _v129
02333 start = end
02334 end += 32
02335 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02336 start = end
02337 end += 8
02338 (val1.weight,) = _struct_d.unpack(str[start:end])
02339 self.motion_plan_request.path_constraints.position_constraints.append(val1)
02340 start = end
02341 end += 4
02342 (length,) = _struct_I.unpack(str[start:end])
02343 self.motion_plan_request.path_constraints.orientation_constraints = []
02344 for i in range(0, length):
02345 val1 = arm_navigation_msgs.msg.OrientationConstraint()
02346 _v130 = val1.header
02347 start = end
02348 end += 4
02349 (_v130.seq,) = _struct_I.unpack(str[start:end])
02350 _v131 = _v130.stamp
02351 _x = _v131
02352 start = end
02353 end += 8
02354 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02355 start = end
02356 end += 4
02357 (length,) = _struct_I.unpack(str[start:end])
02358 start = end
02359 end += length
02360 if python3:
02361 _v130.frame_id = str[start:end].decode('utf-8')
02362 else:
02363 _v130.frame_id = str[start:end]
02364 start = end
02365 end += 4
02366 (length,) = _struct_I.unpack(str[start:end])
02367 start = end
02368 end += length
02369 if python3:
02370 val1.link_name = str[start:end].decode('utf-8')
02371 else:
02372 val1.link_name = str[start:end]
02373 start = end
02374 end += 4
02375 (val1.type,) = _struct_i.unpack(str[start:end])
02376 _v132 = val1.orientation
02377 _x = _v132
02378 start = end
02379 end += 32
02380 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02381 _x = val1
02382 start = end
02383 end += 32
02384 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
02385 self.motion_plan_request.path_constraints.orientation_constraints.append(val1)
02386 start = end
02387 end += 4
02388 (length,) = _struct_I.unpack(str[start:end])
02389 self.motion_plan_request.path_constraints.visibility_constraints = []
02390 for i in range(0, length):
02391 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
02392 _v133 = val1.header
02393 start = end
02394 end += 4
02395 (_v133.seq,) = _struct_I.unpack(str[start:end])
02396 _v134 = _v133.stamp
02397 _x = _v134
02398 start = end
02399 end += 8
02400 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02401 start = end
02402 end += 4
02403 (length,) = _struct_I.unpack(str[start:end])
02404 start = end
02405 end += length
02406 if python3:
02407 _v133.frame_id = str[start:end].decode('utf-8')
02408 else:
02409 _v133.frame_id = str[start:end]
02410 _v135 = val1.target
02411 _v136 = _v135.header
02412 start = end
02413 end += 4
02414 (_v136.seq,) = _struct_I.unpack(str[start:end])
02415 _v137 = _v136.stamp
02416 _x = _v137
02417 start = end
02418 end += 8
02419 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02420 start = end
02421 end += 4
02422 (length,) = _struct_I.unpack(str[start:end])
02423 start = end
02424 end += length
02425 if python3:
02426 _v136.frame_id = str[start:end].decode('utf-8')
02427 else:
02428 _v136.frame_id = str[start:end]
02429 _v138 = _v135.point
02430 _x = _v138
02431 start = end
02432 end += 24
02433 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02434 _v139 = val1.sensor_pose
02435 _v140 = _v139.header
02436 start = end
02437 end += 4
02438 (_v140.seq,) = _struct_I.unpack(str[start:end])
02439 _v141 = _v140.stamp
02440 _x = _v141
02441 start = end
02442 end += 8
02443 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02444 start = end
02445 end += 4
02446 (length,) = _struct_I.unpack(str[start:end])
02447 start = end
02448 end += length
02449 if python3:
02450 _v140.frame_id = str[start:end].decode('utf-8')
02451 else:
02452 _v140.frame_id = str[start:end]
02453 _v142 = _v139.pose
02454 _v143 = _v142.position
02455 _x = _v143
02456 start = end
02457 end += 24
02458 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02459 _v144 = _v142.orientation
02460 _x = _v144
02461 start = end
02462 end += 32
02463 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02464 start = end
02465 end += 8
02466 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
02467 self.motion_plan_request.path_constraints.visibility_constraints.append(val1)
02468 start = end
02469 end += 4
02470 (length,) = _struct_I.unpack(str[start:end])
02471 start = end
02472 end += length
02473 if python3:
02474 self.motion_plan_request.planner_id = str[start:end].decode('utf-8')
02475 else:
02476 self.motion_plan_request.planner_id = str[start:end]
02477 start = end
02478 end += 4
02479 (length,) = _struct_I.unpack(str[start:end])
02480 start = end
02481 end += length
02482 if python3:
02483 self.motion_plan_request.group_name = str[start:end].decode('utf-8')
02484 else:
02485 self.motion_plan_request.group_name = str[start:end]
02486 _x = self
02487 start = end
02488 end += 28
02489 (_x.motion_plan_request.num_planning_attempts, _x.motion_plan_request.allowed_planning_time.secs, _x.motion_plan_request.allowed_planning_time.nsecs, _x.motion_plan_request.expected_path_duration.secs, _x.motion_plan_request.expected_path_duration.nsecs, _x.motion_plan_request.expected_path_dt.secs, _x.motion_plan_request.expected_path_dt.nsecs,) = _struct_7i.unpack(str[start:end])
02490 start = end
02491 end += 4
02492 (length,) = _struct_I.unpack(str[start:end])
02493 self.operations.collision_operations = []
02494 for i in range(0, length):
02495 val1 = arm_navigation_msgs.msg.CollisionOperation()
02496 start = end
02497 end += 4
02498 (length,) = _struct_I.unpack(str[start:end])
02499 start = end
02500 end += length
02501 if python3:
02502 val1.object1 = str[start:end].decode('utf-8')
02503 else:
02504 val1.object1 = str[start:end]
02505 start = end
02506 end += 4
02507 (length,) = _struct_I.unpack(str[start:end])
02508 start = end
02509 end += length
02510 if python3:
02511 val1.object2 = str[start:end].decode('utf-8')
02512 else:
02513 val1.object2 = str[start:end]
02514 _x = val1
02515 start = end
02516 end += 12
02517 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
02518 self.operations.collision_operations.append(val1)
02519 _x = self
02520 start = end
02521 end += 4
02522 (_x.accept_partial_plans, _x.accept_invalid_goals, _x.disable_ik, _x.disable_collision_monitoring,) = _struct_4B.unpack(str[start:end])
02523 self.accept_partial_plans = bool(self.accept_partial_plans)
02524 self.accept_invalid_goals = bool(self.accept_invalid_goals)
02525 self.disable_ik = bool(self.disable_ik)
02526 self.disable_collision_monitoring = bool(self.disable_collision_monitoring)
02527 return self
02528 except struct.error as e:
02529 raise genpy.DeserializationError(e)
02530
02531
02532 def serialize_numpy(self, buff, numpy):
02533 """
02534 serialize message with numpy array types into buffer
02535 :param buff: buffer, ``StringIO``
02536 :param numpy: numpy python module
02537 """
02538 try:
02539 _x = self.planner_service_name
02540 length = len(_x)
02541 if python3 or type(_x) == unicode:
02542 _x = _x.encode('utf-8')
02543 length = len(_x)
02544 buff.write(struct.pack('<I%ss'%length, length, _x))
02545 _x = self
02546 buff.write(_struct_3I.pack(_x.planning_scene_diff.robot_state.joint_state.header.seq, _x.planning_scene_diff.robot_state.joint_state.header.stamp.secs, _x.planning_scene_diff.robot_state.joint_state.header.stamp.nsecs))
02547 _x = self.planning_scene_diff.robot_state.joint_state.header.frame_id
02548 length = len(_x)
02549 if python3 or type(_x) == unicode:
02550 _x = _x.encode('utf-8')
02551 length = len(_x)
02552 buff.write(struct.pack('<I%ss'%length, length, _x))
02553 length = len(self.planning_scene_diff.robot_state.joint_state.name)
02554 buff.write(_struct_I.pack(length))
02555 for val1 in self.planning_scene_diff.robot_state.joint_state.name:
02556 length = len(val1)
02557 if python3 or type(val1) == unicode:
02558 val1 = val1.encode('utf-8')
02559 length = len(val1)
02560 buff.write(struct.pack('<I%ss'%length, length, val1))
02561 length = len(self.planning_scene_diff.robot_state.joint_state.position)
02562 buff.write(_struct_I.pack(length))
02563 pattern = '<%sd'%length
02564 buff.write(self.planning_scene_diff.robot_state.joint_state.position.tostring())
02565 length = len(self.planning_scene_diff.robot_state.joint_state.velocity)
02566 buff.write(_struct_I.pack(length))
02567 pattern = '<%sd'%length
02568 buff.write(self.planning_scene_diff.robot_state.joint_state.velocity.tostring())
02569 length = len(self.planning_scene_diff.robot_state.joint_state.effort)
02570 buff.write(_struct_I.pack(length))
02571 pattern = '<%sd'%length
02572 buff.write(self.planning_scene_diff.robot_state.joint_state.effort.tostring())
02573 _x = self
02574 buff.write(_struct_2I.pack(_x.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.secs, _x.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.nsecs))
02575 length = len(self.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names)
02576 buff.write(_struct_I.pack(length))
02577 for val1 in self.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names:
02578 length = len(val1)
02579 if python3 or type(val1) == unicode:
02580 val1 = val1.encode('utf-8')
02581 length = len(val1)
02582 buff.write(struct.pack('<I%ss'%length, length, val1))
02583 length = len(self.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids)
02584 buff.write(_struct_I.pack(length))
02585 for val1 in self.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids:
02586 length = len(val1)
02587 if python3 or type(val1) == unicode:
02588 val1 = val1.encode('utf-8')
02589 length = len(val1)
02590 buff.write(struct.pack('<I%ss'%length, length, val1))
02591 length = len(self.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids)
02592 buff.write(_struct_I.pack(length))
02593 for val1 in self.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids:
02594 length = len(val1)
02595 if python3 or type(val1) == unicode:
02596 val1 = val1.encode('utf-8')
02597 length = len(val1)
02598 buff.write(struct.pack('<I%ss'%length, length, val1))
02599 length = len(self.planning_scene_diff.robot_state.multi_dof_joint_state.poses)
02600 buff.write(_struct_I.pack(length))
02601 for val1 in self.planning_scene_diff.robot_state.multi_dof_joint_state.poses:
02602 _v145 = val1.position
02603 _x = _v145
02604 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02605 _v146 = val1.orientation
02606 _x = _v146
02607 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02608 length = len(self.planning_scene_diff.fixed_frame_transforms)
02609 buff.write(_struct_I.pack(length))
02610 for val1 in self.planning_scene_diff.fixed_frame_transforms:
02611 _v147 = val1.header
02612 buff.write(_struct_I.pack(_v147.seq))
02613 _v148 = _v147.stamp
02614 _x = _v148
02615 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02616 _x = _v147.frame_id
02617 length = len(_x)
02618 if python3 or type(_x) == unicode:
02619 _x = _x.encode('utf-8')
02620 length = len(_x)
02621 buff.write(struct.pack('<I%ss'%length, length, _x))
02622 _x = val1.child_frame_id
02623 length = len(_x)
02624 if python3 or type(_x) == unicode:
02625 _x = _x.encode('utf-8')
02626 length = len(_x)
02627 buff.write(struct.pack('<I%ss'%length, length, _x))
02628 _v149 = val1.transform
02629 _v150 = _v149.translation
02630 _x = _v150
02631 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02632 _v151 = _v149.rotation
02633 _x = _v151
02634 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02635 length = len(self.planning_scene_diff.allowed_collision_matrix.link_names)
02636 buff.write(_struct_I.pack(length))
02637 for val1 in self.planning_scene_diff.allowed_collision_matrix.link_names:
02638 length = len(val1)
02639 if python3 or type(val1) == unicode:
02640 val1 = val1.encode('utf-8')
02641 length = len(val1)
02642 buff.write(struct.pack('<I%ss'%length, length, val1))
02643 length = len(self.planning_scene_diff.allowed_collision_matrix.entries)
02644 buff.write(_struct_I.pack(length))
02645 for val1 in self.planning_scene_diff.allowed_collision_matrix.entries:
02646 length = len(val1.enabled)
02647 buff.write(_struct_I.pack(length))
02648 pattern = '<%sB'%length
02649 buff.write(val1.enabled.tostring())
02650 length = len(self.planning_scene_diff.allowed_contacts)
02651 buff.write(_struct_I.pack(length))
02652 for val1 in self.planning_scene_diff.allowed_contacts:
02653 _x = val1.name
02654 length = len(_x)
02655 if python3 or type(_x) == unicode:
02656 _x = _x.encode('utf-8')
02657 length = len(_x)
02658 buff.write(struct.pack('<I%ss'%length, length, _x))
02659 _v152 = val1.shape
02660 buff.write(_struct_b.pack(_v152.type))
02661 length = len(_v152.dimensions)
02662 buff.write(_struct_I.pack(length))
02663 pattern = '<%sd'%length
02664 buff.write(_v152.dimensions.tostring())
02665 length = len(_v152.triangles)
02666 buff.write(_struct_I.pack(length))
02667 pattern = '<%si'%length
02668 buff.write(_v152.triangles.tostring())
02669 length = len(_v152.vertices)
02670 buff.write(_struct_I.pack(length))
02671 for val3 in _v152.vertices:
02672 _x = val3
02673 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02674 _v153 = val1.pose_stamped
02675 _v154 = _v153.header
02676 buff.write(_struct_I.pack(_v154.seq))
02677 _v155 = _v154.stamp
02678 _x = _v155
02679 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02680 _x = _v154.frame_id
02681 length = len(_x)
02682 if python3 or type(_x) == unicode:
02683 _x = _x.encode('utf-8')
02684 length = len(_x)
02685 buff.write(struct.pack('<I%ss'%length, length, _x))
02686 _v156 = _v153.pose
02687 _v157 = _v156.position
02688 _x = _v157
02689 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02690 _v158 = _v156.orientation
02691 _x = _v158
02692 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02693 length = len(val1.link_names)
02694 buff.write(_struct_I.pack(length))
02695 for val2 in val1.link_names:
02696 length = len(val2)
02697 if python3 or type(val2) == unicode:
02698 val2 = val2.encode('utf-8')
02699 length = len(val2)
02700 buff.write(struct.pack('<I%ss'%length, length, val2))
02701 buff.write(_struct_d.pack(val1.penetration_depth))
02702 length = len(self.planning_scene_diff.link_padding)
02703 buff.write(_struct_I.pack(length))
02704 for val1 in self.planning_scene_diff.link_padding:
02705 _x = val1.link_name
02706 length = len(_x)
02707 if python3 or type(_x) == unicode:
02708 _x = _x.encode('utf-8')
02709 length = len(_x)
02710 buff.write(struct.pack('<I%ss'%length, length, _x))
02711 buff.write(_struct_d.pack(val1.padding))
02712 length = len(self.planning_scene_diff.collision_objects)
02713 buff.write(_struct_I.pack(length))
02714 for val1 in self.planning_scene_diff.collision_objects:
02715 _v159 = val1.header
02716 buff.write(_struct_I.pack(_v159.seq))
02717 _v160 = _v159.stamp
02718 _x = _v160
02719 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02720 _x = _v159.frame_id
02721 length = len(_x)
02722 if python3 or type(_x) == unicode:
02723 _x = _x.encode('utf-8')
02724 length = len(_x)
02725 buff.write(struct.pack('<I%ss'%length, length, _x))
02726 _x = val1.id
02727 length = len(_x)
02728 if python3 or type(_x) == unicode:
02729 _x = _x.encode('utf-8')
02730 length = len(_x)
02731 buff.write(struct.pack('<I%ss'%length, length, _x))
02732 buff.write(_struct_f.pack(val1.padding))
02733 _v161 = val1.operation
02734 buff.write(_struct_b.pack(_v161.operation))
02735 length = len(val1.shapes)
02736 buff.write(_struct_I.pack(length))
02737 for val2 in val1.shapes:
02738 buff.write(_struct_b.pack(val2.type))
02739 length = len(val2.dimensions)
02740 buff.write(_struct_I.pack(length))
02741 pattern = '<%sd'%length
02742 buff.write(val2.dimensions.tostring())
02743 length = len(val2.triangles)
02744 buff.write(_struct_I.pack(length))
02745 pattern = '<%si'%length
02746 buff.write(val2.triangles.tostring())
02747 length = len(val2.vertices)
02748 buff.write(_struct_I.pack(length))
02749 for val3 in val2.vertices:
02750 _x = val3
02751 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02752 length = len(val1.poses)
02753 buff.write(_struct_I.pack(length))
02754 for val2 in val1.poses:
02755 _v162 = val2.position
02756 _x = _v162
02757 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02758 _v163 = val2.orientation
02759 _x = _v163
02760 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02761 length = len(self.planning_scene_diff.attached_collision_objects)
02762 buff.write(_struct_I.pack(length))
02763 for val1 in self.planning_scene_diff.attached_collision_objects:
02764 _x = val1.link_name
02765 length = len(_x)
02766 if python3 or type(_x) == unicode:
02767 _x = _x.encode('utf-8')
02768 length = len(_x)
02769 buff.write(struct.pack('<I%ss'%length, length, _x))
02770 _v164 = val1.object
02771 _v165 = _v164.header
02772 buff.write(_struct_I.pack(_v165.seq))
02773 _v166 = _v165.stamp
02774 _x = _v166
02775 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02776 _x = _v165.frame_id
02777 length = len(_x)
02778 if python3 or type(_x) == unicode:
02779 _x = _x.encode('utf-8')
02780 length = len(_x)
02781 buff.write(struct.pack('<I%ss'%length, length, _x))
02782 _x = _v164.id
02783 length = len(_x)
02784 if python3 or type(_x) == unicode:
02785 _x = _x.encode('utf-8')
02786 length = len(_x)
02787 buff.write(struct.pack('<I%ss'%length, length, _x))
02788 buff.write(_struct_f.pack(_v164.padding))
02789 _v167 = _v164.operation
02790 buff.write(_struct_b.pack(_v167.operation))
02791 length = len(_v164.shapes)
02792 buff.write(_struct_I.pack(length))
02793 for val3 in _v164.shapes:
02794 buff.write(_struct_b.pack(val3.type))
02795 length = len(val3.dimensions)
02796 buff.write(_struct_I.pack(length))
02797 pattern = '<%sd'%length
02798 buff.write(val3.dimensions.tostring())
02799 length = len(val3.triangles)
02800 buff.write(_struct_I.pack(length))
02801 pattern = '<%si'%length
02802 buff.write(val3.triangles.tostring())
02803 length = len(val3.vertices)
02804 buff.write(_struct_I.pack(length))
02805 for val4 in val3.vertices:
02806 _x = val4
02807 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02808 length = len(_v164.poses)
02809 buff.write(_struct_I.pack(length))
02810 for val3 in _v164.poses:
02811 _v168 = val3.position
02812 _x = _v168
02813 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02814 _v169 = val3.orientation
02815 _x = _v169
02816 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02817 length = len(val1.touch_links)
02818 buff.write(_struct_I.pack(length))
02819 for val2 in val1.touch_links:
02820 length = len(val2)
02821 if python3 or type(val2) == unicode:
02822 val2 = val2.encode('utf-8')
02823 length = len(val2)
02824 buff.write(struct.pack('<I%ss'%length, length, val2))
02825 _x = self
02826 buff.write(_struct_3I.pack(_x.planning_scene_diff.collision_map.header.seq, _x.planning_scene_diff.collision_map.header.stamp.secs, _x.planning_scene_diff.collision_map.header.stamp.nsecs))
02827 _x = self.planning_scene_diff.collision_map.header.frame_id
02828 length = len(_x)
02829 if python3 or type(_x) == unicode:
02830 _x = _x.encode('utf-8')
02831 length = len(_x)
02832 buff.write(struct.pack('<I%ss'%length, length, _x))
02833 length = len(self.planning_scene_diff.collision_map.boxes)
02834 buff.write(_struct_I.pack(length))
02835 for val1 in self.planning_scene_diff.collision_map.boxes:
02836 _v170 = val1.center
02837 _x = _v170
02838 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
02839 _v171 = val1.extents
02840 _x = _v171
02841 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
02842 _v172 = val1.axis
02843 _x = _v172
02844 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
02845 buff.write(_struct_f.pack(val1.angle))
02846 buff.write(_struct_b.pack(self.motion_plan_request.workspace_parameters.workspace_region_shape.type))
02847 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions)
02848 buff.write(_struct_I.pack(length))
02849 pattern = '<%sd'%length
02850 buff.write(self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions.tostring())
02851 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles)
02852 buff.write(_struct_I.pack(length))
02853 pattern = '<%si'%length
02854 buff.write(self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles.tostring())
02855 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices)
02856 buff.write(_struct_I.pack(length))
02857 for val1 in self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices:
02858 _x = val1
02859 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02860 _x = self
02861 buff.write(_struct_3I.pack(_x.motion_plan_request.workspace_parameters.workspace_region_pose.header.seq, _x.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.secs, _x.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.nsecs))
02862 _x = self.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id
02863 length = len(_x)
02864 if python3 or type(_x) == unicode:
02865 _x = _x.encode('utf-8')
02866 length = len(_x)
02867 buff.write(struct.pack('<I%ss'%length, length, _x))
02868 _x = self
02869 buff.write(_struct_7d3I.pack(_x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.x, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.y, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.z, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.x, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.y, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.z, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.w, _x.motion_plan_request.start_state.joint_state.header.seq, _x.motion_plan_request.start_state.joint_state.header.stamp.secs, _x.motion_plan_request.start_state.joint_state.header.stamp.nsecs))
02870 _x = self.motion_plan_request.start_state.joint_state.header.frame_id
02871 length = len(_x)
02872 if python3 or type(_x) == unicode:
02873 _x = _x.encode('utf-8')
02874 length = len(_x)
02875 buff.write(struct.pack('<I%ss'%length, length, _x))
02876 length = len(self.motion_plan_request.start_state.joint_state.name)
02877 buff.write(_struct_I.pack(length))
02878 for val1 in self.motion_plan_request.start_state.joint_state.name:
02879 length = len(val1)
02880 if python3 or type(val1) == unicode:
02881 val1 = val1.encode('utf-8')
02882 length = len(val1)
02883 buff.write(struct.pack('<I%ss'%length, length, val1))
02884 length = len(self.motion_plan_request.start_state.joint_state.position)
02885 buff.write(_struct_I.pack(length))
02886 pattern = '<%sd'%length
02887 buff.write(self.motion_plan_request.start_state.joint_state.position.tostring())
02888 length = len(self.motion_plan_request.start_state.joint_state.velocity)
02889 buff.write(_struct_I.pack(length))
02890 pattern = '<%sd'%length
02891 buff.write(self.motion_plan_request.start_state.joint_state.velocity.tostring())
02892 length = len(self.motion_plan_request.start_state.joint_state.effort)
02893 buff.write(_struct_I.pack(length))
02894 pattern = '<%sd'%length
02895 buff.write(self.motion_plan_request.start_state.joint_state.effort.tostring())
02896 _x = self
02897 buff.write(_struct_2I.pack(_x.motion_plan_request.start_state.multi_dof_joint_state.stamp.secs, _x.motion_plan_request.start_state.multi_dof_joint_state.stamp.nsecs))
02898 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.joint_names)
02899 buff.write(_struct_I.pack(length))
02900 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.joint_names:
02901 length = len(val1)
02902 if python3 or type(val1) == unicode:
02903 val1 = val1.encode('utf-8')
02904 length = len(val1)
02905 buff.write(struct.pack('<I%ss'%length, length, val1))
02906 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids)
02907 buff.write(_struct_I.pack(length))
02908 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids:
02909 length = len(val1)
02910 if python3 or type(val1) == unicode:
02911 val1 = val1.encode('utf-8')
02912 length = len(val1)
02913 buff.write(struct.pack('<I%ss'%length, length, val1))
02914 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids)
02915 buff.write(_struct_I.pack(length))
02916 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids:
02917 length = len(val1)
02918 if python3 or type(val1) == unicode:
02919 val1 = val1.encode('utf-8')
02920 length = len(val1)
02921 buff.write(struct.pack('<I%ss'%length, length, val1))
02922 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.poses)
02923 buff.write(_struct_I.pack(length))
02924 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.poses:
02925 _v173 = val1.position
02926 _x = _v173
02927 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02928 _v174 = val1.orientation
02929 _x = _v174
02930 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02931 length = len(self.motion_plan_request.goal_constraints.joint_constraints)
02932 buff.write(_struct_I.pack(length))
02933 for val1 in self.motion_plan_request.goal_constraints.joint_constraints:
02934 _x = val1.joint_name
02935 length = len(_x)
02936 if python3 or type(_x) == unicode:
02937 _x = _x.encode('utf-8')
02938 length = len(_x)
02939 buff.write(struct.pack('<I%ss'%length, length, _x))
02940 _x = val1
02941 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
02942 length = len(self.motion_plan_request.goal_constraints.position_constraints)
02943 buff.write(_struct_I.pack(length))
02944 for val1 in self.motion_plan_request.goal_constraints.position_constraints:
02945 _v175 = val1.header
02946 buff.write(_struct_I.pack(_v175.seq))
02947 _v176 = _v175.stamp
02948 _x = _v176
02949 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02950 _x = _v175.frame_id
02951 length = len(_x)
02952 if python3 or type(_x) == unicode:
02953 _x = _x.encode('utf-8')
02954 length = len(_x)
02955 buff.write(struct.pack('<I%ss'%length, length, _x))
02956 _x = val1.link_name
02957 length = len(_x)
02958 if python3 or type(_x) == unicode:
02959 _x = _x.encode('utf-8')
02960 length = len(_x)
02961 buff.write(struct.pack('<I%ss'%length, length, _x))
02962 _v177 = val1.target_point_offset
02963 _x = _v177
02964 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02965 _v178 = val1.position
02966 _x = _v178
02967 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02968 _v179 = val1.constraint_region_shape
02969 buff.write(_struct_b.pack(_v179.type))
02970 length = len(_v179.dimensions)
02971 buff.write(_struct_I.pack(length))
02972 pattern = '<%sd'%length
02973 buff.write(_v179.dimensions.tostring())
02974 length = len(_v179.triangles)
02975 buff.write(_struct_I.pack(length))
02976 pattern = '<%si'%length
02977 buff.write(_v179.triangles.tostring())
02978 length = len(_v179.vertices)
02979 buff.write(_struct_I.pack(length))
02980 for val3 in _v179.vertices:
02981 _x = val3
02982 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02983 _v180 = val1.constraint_region_orientation
02984 _x = _v180
02985 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02986 buff.write(_struct_d.pack(val1.weight))
02987 length = len(self.motion_plan_request.goal_constraints.orientation_constraints)
02988 buff.write(_struct_I.pack(length))
02989 for val1 in self.motion_plan_request.goal_constraints.orientation_constraints:
02990 _v181 = val1.header
02991 buff.write(_struct_I.pack(_v181.seq))
02992 _v182 = _v181.stamp
02993 _x = _v182
02994 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02995 _x = _v181.frame_id
02996 length = len(_x)
02997 if python3 or type(_x) == unicode:
02998 _x = _x.encode('utf-8')
02999 length = len(_x)
03000 buff.write(struct.pack('<I%ss'%length, length, _x))
03001 _x = val1.link_name
03002 length = len(_x)
03003 if python3 or type(_x) == unicode:
03004 _x = _x.encode('utf-8')
03005 length = len(_x)
03006 buff.write(struct.pack('<I%ss'%length, length, _x))
03007 buff.write(_struct_i.pack(val1.type))
03008 _v183 = val1.orientation
03009 _x = _v183
03010 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03011 _x = val1
03012 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
03013 length = len(self.motion_plan_request.goal_constraints.visibility_constraints)
03014 buff.write(_struct_I.pack(length))
03015 for val1 in self.motion_plan_request.goal_constraints.visibility_constraints:
03016 _v184 = val1.header
03017 buff.write(_struct_I.pack(_v184.seq))
03018 _v185 = _v184.stamp
03019 _x = _v185
03020 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03021 _x = _v184.frame_id
03022 length = len(_x)
03023 if python3 or type(_x) == unicode:
03024 _x = _x.encode('utf-8')
03025 length = len(_x)
03026 buff.write(struct.pack('<I%ss'%length, length, _x))
03027 _v186 = val1.target
03028 _v187 = _v186.header
03029 buff.write(_struct_I.pack(_v187.seq))
03030 _v188 = _v187.stamp
03031 _x = _v188
03032 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03033 _x = _v187.frame_id
03034 length = len(_x)
03035 if python3 or type(_x) == unicode:
03036 _x = _x.encode('utf-8')
03037 length = len(_x)
03038 buff.write(struct.pack('<I%ss'%length, length, _x))
03039 _v189 = _v186.point
03040 _x = _v189
03041 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03042 _v190 = val1.sensor_pose
03043 _v191 = _v190.header
03044 buff.write(_struct_I.pack(_v191.seq))
03045 _v192 = _v191.stamp
03046 _x = _v192
03047 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03048 _x = _v191.frame_id
03049 length = len(_x)
03050 if python3 or type(_x) == unicode:
03051 _x = _x.encode('utf-8')
03052 length = len(_x)
03053 buff.write(struct.pack('<I%ss'%length, length, _x))
03054 _v193 = _v190.pose
03055 _v194 = _v193.position
03056 _x = _v194
03057 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03058 _v195 = _v193.orientation
03059 _x = _v195
03060 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03061 buff.write(_struct_d.pack(val1.absolute_tolerance))
03062 length = len(self.motion_plan_request.path_constraints.joint_constraints)
03063 buff.write(_struct_I.pack(length))
03064 for val1 in self.motion_plan_request.path_constraints.joint_constraints:
03065 _x = val1.joint_name
03066 length = len(_x)
03067 if python3 or type(_x) == unicode:
03068 _x = _x.encode('utf-8')
03069 length = len(_x)
03070 buff.write(struct.pack('<I%ss'%length, length, _x))
03071 _x = val1
03072 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
03073 length = len(self.motion_plan_request.path_constraints.position_constraints)
03074 buff.write(_struct_I.pack(length))
03075 for val1 in self.motion_plan_request.path_constraints.position_constraints:
03076 _v196 = val1.header
03077 buff.write(_struct_I.pack(_v196.seq))
03078 _v197 = _v196.stamp
03079 _x = _v197
03080 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03081 _x = _v196.frame_id
03082 length = len(_x)
03083 if python3 or type(_x) == unicode:
03084 _x = _x.encode('utf-8')
03085 length = len(_x)
03086 buff.write(struct.pack('<I%ss'%length, length, _x))
03087 _x = val1.link_name
03088 length = len(_x)
03089 if python3 or type(_x) == unicode:
03090 _x = _x.encode('utf-8')
03091 length = len(_x)
03092 buff.write(struct.pack('<I%ss'%length, length, _x))
03093 _v198 = val1.target_point_offset
03094 _x = _v198
03095 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03096 _v199 = val1.position
03097 _x = _v199
03098 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03099 _v200 = val1.constraint_region_shape
03100 buff.write(_struct_b.pack(_v200.type))
03101 length = len(_v200.dimensions)
03102 buff.write(_struct_I.pack(length))
03103 pattern = '<%sd'%length
03104 buff.write(_v200.dimensions.tostring())
03105 length = len(_v200.triangles)
03106 buff.write(_struct_I.pack(length))
03107 pattern = '<%si'%length
03108 buff.write(_v200.triangles.tostring())
03109 length = len(_v200.vertices)
03110 buff.write(_struct_I.pack(length))
03111 for val3 in _v200.vertices:
03112 _x = val3
03113 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03114 _v201 = val1.constraint_region_orientation
03115 _x = _v201
03116 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03117 buff.write(_struct_d.pack(val1.weight))
03118 length = len(self.motion_plan_request.path_constraints.orientation_constraints)
03119 buff.write(_struct_I.pack(length))
03120 for val1 in self.motion_plan_request.path_constraints.orientation_constraints:
03121 _v202 = val1.header
03122 buff.write(_struct_I.pack(_v202.seq))
03123 _v203 = _v202.stamp
03124 _x = _v203
03125 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03126 _x = _v202.frame_id
03127 length = len(_x)
03128 if python3 or type(_x) == unicode:
03129 _x = _x.encode('utf-8')
03130 length = len(_x)
03131 buff.write(struct.pack('<I%ss'%length, length, _x))
03132 _x = val1.link_name
03133 length = len(_x)
03134 if python3 or type(_x) == unicode:
03135 _x = _x.encode('utf-8')
03136 length = len(_x)
03137 buff.write(struct.pack('<I%ss'%length, length, _x))
03138 buff.write(_struct_i.pack(val1.type))
03139 _v204 = val1.orientation
03140 _x = _v204
03141 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03142 _x = val1
03143 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
03144 length = len(self.motion_plan_request.path_constraints.visibility_constraints)
03145 buff.write(_struct_I.pack(length))
03146 for val1 in self.motion_plan_request.path_constraints.visibility_constraints:
03147 _v205 = val1.header
03148 buff.write(_struct_I.pack(_v205.seq))
03149 _v206 = _v205.stamp
03150 _x = _v206
03151 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03152 _x = _v205.frame_id
03153 length = len(_x)
03154 if python3 or type(_x) == unicode:
03155 _x = _x.encode('utf-8')
03156 length = len(_x)
03157 buff.write(struct.pack('<I%ss'%length, length, _x))
03158 _v207 = val1.target
03159 _v208 = _v207.header
03160 buff.write(_struct_I.pack(_v208.seq))
03161 _v209 = _v208.stamp
03162 _x = _v209
03163 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03164 _x = _v208.frame_id
03165 length = len(_x)
03166 if python3 or type(_x) == unicode:
03167 _x = _x.encode('utf-8')
03168 length = len(_x)
03169 buff.write(struct.pack('<I%ss'%length, length, _x))
03170 _v210 = _v207.point
03171 _x = _v210
03172 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03173 _v211 = val1.sensor_pose
03174 _v212 = _v211.header
03175 buff.write(_struct_I.pack(_v212.seq))
03176 _v213 = _v212.stamp
03177 _x = _v213
03178 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03179 _x = _v212.frame_id
03180 length = len(_x)
03181 if python3 or type(_x) == unicode:
03182 _x = _x.encode('utf-8')
03183 length = len(_x)
03184 buff.write(struct.pack('<I%ss'%length, length, _x))
03185 _v214 = _v211.pose
03186 _v215 = _v214.position
03187 _x = _v215
03188 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03189 _v216 = _v214.orientation
03190 _x = _v216
03191 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03192 buff.write(_struct_d.pack(val1.absolute_tolerance))
03193 _x = self.motion_plan_request.planner_id
03194 length = len(_x)
03195 if python3 or type(_x) == unicode:
03196 _x = _x.encode('utf-8')
03197 length = len(_x)
03198 buff.write(struct.pack('<I%ss'%length, length, _x))
03199 _x = self.motion_plan_request.group_name
03200 length = len(_x)
03201 if python3 or type(_x) == unicode:
03202 _x = _x.encode('utf-8')
03203 length = len(_x)
03204 buff.write(struct.pack('<I%ss'%length, length, _x))
03205 _x = self
03206 buff.write(_struct_7i.pack(_x.motion_plan_request.num_planning_attempts, _x.motion_plan_request.allowed_planning_time.secs, _x.motion_plan_request.allowed_planning_time.nsecs, _x.motion_plan_request.expected_path_duration.secs, _x.motion_plan_request.expected_path_duration.nsecs, _x.motion_plan_request.expected_path_dt.secs, _x.motion_plan_request.expected_path_dt.nsecs))
03207 length = len(self.operations.collision_operations)
03208 buff.write(_struct_I.pack(length))
03209 for val1 in self.operations.collision_operations:
03210 _x = val1.object1
03211 length = len(_x)
03212 if python3 or type(_x) == unicode:
03213 _x = _x.encode('utf-8')
03214 length = len(_x)
03215 buff.write(struct.pack('<I%ss'%length, length, _x))
03216 _x = val1.object2
03217 length = len(_x)
03218 if python3 or type(_x) == unicode:
03219 _x = _x.encode('utf-8')
03220 length = len(_x)
03221 buff.write(struct.pack('<I%ss'%length, length, _x))
03222 _x = val1
03223 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
03224 _x = self
03225 buff.write(_struct_4B.pack(_x.accept_partial_plans, _x.accept_invalid_goals, _x.disable_ik, _x.disable_collision_monitoring))
03226 except struct.error as se: self._check_types(se)
03227 except TypeError as te: self._check_types(te)
03228
03229 def deserialize_numpy(self, str, numpy):
03230 """
03231 unpack serialized message in str into this message instance using numpy for array types
03232 :param str: byte array of serialized message, ``str``
03233 :param numpy: numpy python module
03234 """
03235 try:
03236 if self.planning_scene_diff is None:
03237 self.planning_scene_diff = arm_navigation_msgs.msg.PlanningScene()
03238 if self.motion_plan_request is None:
03239 self.motion_plan_request = arm_navigation_msgs.msg.MotionPlanRequest()
03240 if self.operations is None:
03241 self.operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
03242 end = 0
03243 start = end
03244 end += 4
03245 (length,) = _struct_I.unpack(str[start:end])
03246 start = end
03247 end += length
03248 if python3:
03249 self.planner_service_name = str[start:end].decode('utf-8')
03250 else:
03251 self.planner_service_name = str[start:end]
03252 _x = self
03253 start = end
03254 end += 12
03255 (_x.planning_scene_diff.robot_state.joint_state.header.seq, _x.planning_scene_diff.robot_state.joint_state.header.stamp.secs, _x.planning_scene_diff.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
03256 start = end
03257 end += 4
03258 (length,) = _struct_I.unpack(str[start:end])
03259 start = end
03260 end += length
03261 if python3:
03262 self.planning_scene_diff.robot_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
03263 else:
03264 self.planning_scene_diff.robot_state.joint_state.header.frame_id = str[start:end]
03265 start = end
03266 end += 4
03267 (length,) = _struct_I.unpack(str[start:end])
03268 self.planning_scene_diff.robot_state.joint_state.name = []
03269 for i in range(0, length):
03270 start = end
03271 end += 4
03272 (length,) = _struct_I.unpack(str[start:end])
03273 start = end
03274 end += length
03275 if python3:
03276 val1 = str[start:end].decode('utf-8')
03277 else:
03278 val1 = str[start:end]
03279 self.planning_scene_diff.robot_state.joint_state.name.append(val1)
03280 start = end
03281 end += 4
03282 (length,) = _struct_I.unpack(str[start:end])
03283 pattern = '<%sd'%length
03284 start = end
03285 end += struct.calcsize(pattern)
03286 self.planning_scene_diff.robot_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03287 start = end
03288 end += 4
03289 (length,) = _struct_I.unpack(str[start:end])
03290 pattern = '<%sd'%length
03291 start = end
03292 end += struct.calcsize(pattern)
03293 self.planning_scene_diff.robot_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03294 start = end
03295 end += 4
03296 (length,) = _struct_I.unpack(str[start:end])
03297 pattern = '<%sd'%length
03298 start = end
03299 end += struct.calcsize(pattern)
03300 self.planning_scene_diff.robot_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03301 _x = self
03302 start = end
03303 end += 8
03304 (_x.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.secs, _x.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
03305 start = end
03306 end += 4
03307 (length,) = _struct_I.unpack(str[start:end])
03308 self.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names = []
03309 for i in range(0, length):
03310 start = end
03311 end += 4
03312 (length,) = _struct_I.unpack(str[start:end])
03313 start = end
03314 end += length
03315 if python3:
03316 val1 = str[start:end].decode('utf-8')
03317 else:
03318 val1 = str[start:end]
03319 self.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names.append(val1)
03320 start = end
03321 end += 4
03322 (length,) = _struct_I.unpack(str[start:end])
03323 self.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids = []
03324 for i in range(0, length):
03325 start = end
03326 end += 4
03327 (length,) = _struct_I.unpack(str[start:end])
03328 start = end
03329 end += length
03330 if python3:
03331 val1 = str[start:end].decode('utf-8')
03332 else:
03333 val1 = str[start:end]
03334 self.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids.append(val1)
03335 start = end
03336 end += 4
03337 (length,) = _struct_I.unpack(str[start:end])
03338 self.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids = []
03339 for i in range(0, length):
03340 start = end
03341 end += 4
03342 (length,) = _struct_I.unpack(str[start:end])
03343 start = end
03344 end += length
03345 if python3:
03346 val1 = str[start:end].decode('utf-8')
03347 else:
03348 val1 = str[start:end]
03349 self.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids.append(val1)
03350 start = end
03351 end += 4
03352 (length,) = _struct_I.unpack(str[start:end])
03353 self.planning_scene_diff.robot_state.multi_dof_joint_state.poses = []
03354 for i in range(0, length):
03355 val1 = geometry_msgs.msg.Pose()
03356 _v217 = val1.position
03357 _x = _v217
03358 start = end
03359 end += 24
03360 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03361 _v218 = val1.orientation
03362 _x = _v218
03363 start = end
03364 end += 32
03365 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03366 self.planning_scene_diff.robot_state.multi_dof_joint_state.poses.append(val1)
03367 start = end
03368 end += 4
03369 (length,) = _struct_I.unpack(str[start:end])
03370 self.planning_scene_diff.fixed_frame_transforms = []
03371 for i in range(0, length):
03372 val1 = geometry_msgs.msg.TransformStamped()
03373 _v219 = val1.header
03374 start = end
03375 end += 4
03376 (_v219.seq,) = _struct_I.unpack(str[start:end])
03377 _v220 = _v219.stamp
03378 _x = _v220
03379 start = end
03380 end += 8
03381 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03382 start = end
03383 end += 4
03384 (length,) = _struct_I.unpack(str[start:end])
03385 start = end
03386 end += length
03387 if python3:
03388 _v219.frame_id = str[start:end].decode('utf-8')
03389 else:
03390 _v219.frame_id = str[start:end]
03391 start = end
03392 end += 4
03393 (length,) = _struct_I.unpack(str[start:end])
03394 start = end
03395 end += length
03396 if python3:
03397 val1.child_frame_id = str[start:end].decode('utf-8')
03398 else:
03399 val1.child_frame_id = str[start:end]
03400 _v221 = val1.transform
03401 _v222 = _v221.translation
03402 _x = _v222
03403 start = end
03404 end += 24
03405 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03406 _v223 = _v221.rotation
03407 _x = _v223
03408 start = end
03409 end += 32
03410 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03411 self.planning_scene_diff.fixed_frame_transforms.append(val1)
03412 start = end
03413 end += 4
03414 (length,) = _struct_I.unpack(str[start:end])
03415 self.planning_scene_diff.allowed_collision_matrix.link_names = []
03416 for i in range(0, length):
03417 start = end
03418 end += 4
03419 (length,) = _struct_I.unpack(str[start:end])
03420 start = end
03421 end += length
03422 if python3:
03423 val1 = str[start:end].decode('utf-8')
03424 else:
03425 val1 = str[start:end]
03426 self.planning_scene_diff.allowed_collision_matrix.link_names.append(val1)
03427 start = end
03428 end += 4
03429 (length,) = _struct_I.unpack(str[start:end])
03430 self.planning_scene_diff.allowed_collision_matrix.entries = []
03431 for i in range(0, length):
03432 val1 = arm_navigation_msgs.msg.AllowedCollisionEntry()
03433 start = end
03434 end += 4
03435 (length,) = _struct_I.unpack(str[start:end])
03436 pattern = '<%sB'%length
03437 start = end
03438 end += struct.calcsize(pattern)
03439 val1.enabled = numpy.frombuffer(str[start:end], dtype=numpy.bool, count=length)
03440 val1.enabled = map(bool, val1.enabled)
03441 self.planning_scene_diff.allowed_collision_matrix.entries.append(val1)
03442 start = end
03443 end += 4
03444 (length,) = _struct_I.unpack(str[start:end])
03445 self.planning_scene_diff.allowed_contacts = []
03446 for i in range(0, length):
03447 val1 = arm_navigation_msgs.msg.AllowedContactSpecification()
03448 start = end
03449 end += 4
03450 (length,) = _struct_I.unpack(str[start:end])
03451 start = end
03452 end += length
03453 if python3:
03454 val1.name = str[start:end].decode('utf-8')
03455 else:
03456 val1.name = str[start:end]
03457 _v224 = val1.shape
03458 start = end
03459 end += 1
03460 (_v224.type,) = _struct_b.unpack(str[start:end])
03461 start = end
03462 end += 4
03463 (length,) = _struct_I.unpack(str[start:end])
03464 pattern = '<%sd'%length
03465 start = end
03466 end += struct.calcsize(pattern)
03467 _v224.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03468 start = end
03469 end += 4
03470 (length,) = _struct_I.unpack(str[start:end])
03471 pattern = '<%si'%length
03472 start = end
03473 end += struct.calcsize(pattern)
03474 _v224.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
03475 start = end
03476 end += 4
03477 (length,) = _struct_I.unpack(str[start:end])
03478 _v224.vertices = []
03479 for i in range(0, length):
03480 val3 = geometry_msgs.msg.Point()
03481 _x = val3
03482 start = end
03483 end += 24
03484 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03485 _v224.vertices.append(val3)
03486 _v225 = val1.pose_stamped
03487 _v226 = _v225.header
03488 start = end
03489 end += 4
03490 (_v226.seq,) = _struct_I.unpack(str[start:end])
03491 _v227 = _v226.stamp
03492 _x = _v227
03493 start = end
03494 end += 8
03495 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03496 start = end
03497 end += 4
03498 (length,) = _struct_I.unpack(str[start:end])
03499 start = end
03500 end += length
03501 if python3:
03502 _v226.frame_id = str[start:end].decode('utf-8')
03503 else:
03504 _v226.frame_id = str[start:end]
03505 _v228 = _v225.pose
03506 _v229 = _v228.position
03507 _x = _v229
03508 start = end
03509 end += 24
03510 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03511 _v230 = _v228.orientation
03512 _x = _v230
03513 start = end
03514 end += 32
03515 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03516 start = end
03517 end += 4
03518 (length,) = _struct_I.unpack(str[start:end])
03519 val1.link_names = []
03520 for i in range(0, length):
03521 start = end
03522 end += 4
03523 (length,) = _struct_I.unpack(str[start:end])
03524 start = end
03525 end += length
03526 if python3:
03527 val2 = str[start:end].decode('utf-8')
03528 else:
03529 val2 = str[start:end]
03530 val1.link_names.append(val2)
03531 start = end
03532 end += 8
03533 (val1.penetration_depth,) = _struct_d.unpack(str[start:end])
03534 self.planning_scene_diff.allowed_contacts.append(val1)
03535 start = end
03536 end += 4
03537 (length,) = _struct_I.unpack(str[start:end])
03538 self.planning_scene_diff.link_padding = []
03539 for i in range(0, length):
03540 val1 = arm_navigation_msgs.msg.LinkPadding()
03541 start = end
03542 end += 4
03543 (length,) = _struct_I.unpack(str[start:end])
03544 start = end
03545 end += length
03546 if python3:
03547 val1.link_name = str[start:end].decode('utf-8')
03548 else:
03549 val1.link_name = str[start:end]
03550 start = end
03551 end += 8
03552 (val1.padding,) = _struct_d.unpack(str[start:end])
03553 self.planning_scene_diff.link_padding.append(val1)
03554 start = end
03555 end += 4
03556 (length,) = _struct_I.unpack(str[start:end])
03557 self.planning_scene_diff.collision_objects = []
03558 for i in range(0, length):
03559 val1 = arm_navigation_msgs.msg.CollisionObject()
03560 _v231 = val1.header
03561 start = end
03562 end += 4
03563 (_v231.seq,) = _struct_I.unpack(str[start:end])
03564 _v232 = _v231.stamp
03565 _x = _v232
03566 start = end
03567 end += 8
03568 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03569 start = end
03570 end += 4
03571 (length,) = _struct_I.unpack(str[start:end])
03572 start = end
03573 end += length
03574 if python3:
03575 _v231.frame_id = str[start:end].decode('utf-8')
03576 else:
03577 _v231.frame_id = str[start:end]
03578 start = end
03579 end += 4
03580 (length,) = _struct_I.unpack(str[start:end])
03581 start = end
03582 end += length
03583 if python3:
03584 val1.id = str[start:end].decode('utf-8')
03585 else:
03586 val1.id = str[start:end]
03587 start = end
03588 end += 4
03589 (val1.padding,) = _struct_f.unpack(str[start:end])
03590 _v233 = val1.operation
03591 start = end
03592 end += 1
03593 (_v233.operation,) = _struct_b.unpack(str[start:end])
03594 start = end
03595 end += 4
03596 (length,) = _struct_I.unpack(str[start:end])
03597 val1.shapes = []
03598 for i in range(0, length):
03599 val2 = arm_navigation_msgs.msg.Shape()
03600 start = end
03601 end += 1
03602 (val2.type,) = _struct_b.unpack(str[start:end])
03603 start = end
03604 end += 4
03605 (length,) = _struct_I.unpack(str[start:end])
03606 pattern = '<%sd'%length
03607 start = end
03608 end += struct.calcsize(pattern)
03609 val2.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03610 start = end
03611 end += 4
03612 (length,) = _struct_I.unpack(str[start:end])
03613 pattern = '<%si'%length
03614 start = end
03615 end += struct.calcsize(pattern)
03616 val2.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
03617 start = end
03618 end += 4
03619 (length,) = _struct_I.unpack(str[start:end])
03620 val2.vertices = []
03621 for i in range(0, length):
03622 val3 = geometry_msgs.msg.Point()
03623 _x = val3
03624 start = end
03625 end += 24
03626 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03627 val2.vertices.append(val3)
03628 val1.shapes.append(val2)
03629 start = end
03630 end += 4
03631 (length,) = _struct_I.unpack(str[start:end])
03632 val1.poses = []
03633 for i in range(0, length):
03634 val2 = geometry_msgs.msg.Pose()
03635 _v234 = val2.position
03636 _x = _v234
03637 start = end
03638 end += 24
03639 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03640 _v235 = val2.orientation
03641 _x = _v235
03642 start = end
03643 end += 32
03644 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03645 val1.poses.append(val2)
03646 self.planning_scene_diff.collision_objects.append(val1)
03647 start = end
03648 end += 4
03649 (length,) = _struct_I.unpack(str[start:end])
03650 self.planning_scene_diff.attached_collision_objects = []
03651 for i in range(0, length):
03652 val1 = arm_navigation_msgs.msg.AttachedCollisionObject()
03653 start = end
03654 end += 4
03655 (length,) = _struct_I.unpack(str[start:end])
03656 start = end
03657 end += length
03658 if python3:
03659 val1.link_name = str[start:end].decode('utf-8')
03660 else:
03661 val1.link_name = str[start:end]
03662 _v236 = val1.object
03663 _v237 = _v236.header
03664 start = end
03665 end += 4
03666 (_v237.seq,) = _struct_I.unpack(str[start:end])
03667 _v238 = _v237.stamp
03668 _x = _v238
03669 start = end
03670 end += 8
03671 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03672 start = end
03673 end += 4
03674 (length,) = _struct_I.unpack(str[start:end])
03675 start = end
03676 end += length
03677 if python3:
03678 _v237.frame_id = str[start:end].decode('utf-8')
03679 else:
03680 _v237.frame_id = str[start:end]
03681 start = end
03682 end += 4
03683 (length,) = _struct_I.unpack(str[start:end])
03684 start = end
03685 end += length
03686 if python3:
03687 _v236.id = str[start:end].decode('utf-8')
03688 else:
03689 _v236.id = str[start:end]
03690 start = end
03691 end += 4
03692 (_v236.padding,) = _struct_f.unpack(str[start:end])
03693 _v239 = _v236.operation
03694 start = end
03695 end += 1
03696 (_v239.operation,) = _struct_b.unpack(str[start:end])
03697 start = end
03698 end += 4
03699 (length,) = _struct_I.unpack(str[start:end])
03700 _v236.shapes = []
03701 for i in range(0, length):
03702 val3 = arm_navigation_msgs.msg.Shape()
03703 start = end
03704 end += 1
03705 (val3.type,) = _struct_b.unpack(str[start:end])
03706 start = end
03707 end += 4
03708 (length,) = _struct_I.unpack(str[start:end])
03709 pattern = '<%sd'%length
03710 start = end
03711 end += struct.calcsize(pattern)
03712 val3.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03713 start = end
03714 end += 4
03715 (length,) = _struct_I.unpack(str[start:end])
03716 pattern = '<%si'%length
03717 start = end
03718 end += struct.calcsize(pattern)
03719 val3.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
03720 start = end
03721 end += 4
03722 (length,) = _struct_I.unpack(str[start:end])
03723 val3.vertices = []
03724 for i in range(0, length):
03725 val4 = geometry_msgs.msg.Point()
03726 _x = val4
03727 start = end
03728 end += 24
03729 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03730 val3.vertices.append(val4)
03731 _v236.shapes.append(val3)
03732 start = end
03733 end += 4
03734 (length,) = _struct_I.unpack(str[start:end])
03735 _v236.poses = []
03736 for i in range(0, length):
03737 val3 = geometry_msgs.msg.Pose()
03738 _v240 = val3.position
03739 _x = _v240
03740 start = end
03741 end += 24
03742 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03743 _v241 = val3.orientation
03744 _x = _v241
03745 start = end
03746 end += 32
03747 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03748 _v236.poses.append(val3)
03749 start = end
03750 end += 4
03751 (length,) = _struct_I.unpack(str[start:end])
03752 val1.touch_links = []
03753 for i in range(0, length):
03754 start = end
03755 end += 4
03756 (length,) = _struct_I.unpack(str[start:end])
03757 start = end
03758 end += length
03759 if python3:
03760 val2 = str[start:end].decode('utf-8')
03761 else:
03762 val2 = str[start:end]
03763 val1.touch_links.append(val2)
03764 self.planning_scene_diff.attached_collision_objects.append(val1)
03765 _x = self
03766 start = end
03767 end += 12
03768 (_x.planning_scene_diff.collision_map.header.seq, _x.planning_scene_diff.collision_map.header.stamp.secs, _x.planning_scene_diff.collision_map.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
03769 start = end
03770 end += 4
03771 (length,) = _struct_I.unpack(str[start:end])
03772 start = end
03773 end += length
03774 if python3:
03775 self.planning_scene_diff.collision_map.header.frame_id = str[start:end].decode('utf-8')
03776 else:
03777 self.planning_scene_diff.collision_map.header.frame_id = str[start:end]
03778 start = end
03779 end += 4
03780 (length,) = _struct_I.unpack(str[start:end])
03781 self.planning_scene_diff.collision_map.boxes = []
03782 for i in range(0, length):
03783 val1 = arm_navigation_msgs.msg.OrientedBoundingBox()
03784 _v242 = val1.center
03785 _x = _v242
03786 start = end
03787 end += 12
03788 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
03789 _v243 = val1.extents
03790 _x = _v243
03791 start = end
03792 end += 12
03793 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
03794 _v244 = val1.axis
03795 _x = _v244
03796 start = end
03797 end += 12
03798 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
03799 start = end
03800 end += 4
03801 (val1.angle,) = _struct_f.unpack(str[start:end])
03802 self.planning_scene_diff.collision_map.boxes.append(val1)
03803 start = end
03804 end += 1
03805 (self.motion_plan_request.workspace_parameters.workspace_region_shape.type,) = _struct_b.unpack(str[start:end])
03806 start = end
03807 end += 4
03808 (length,) = _struct_I.unpack(str[start:end])
03809 pattern = '<%sd'%length
03810 start = end
03811 end += struct.calcsize(pattern)
03812 self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03813 start = end
03814 end += 4
03815 (length,) = _struct_I.unpack(str[start:end])
03816 pattern = '<%si'%length
03817 start = end
03818 end += struct.calcsize(pattern)
03819 self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
03820 start = end
03821 end += 4
03822 (length,) = _struct_I.unpack(str[start:end])
03823 self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices = []
03824 for i in range(0, length):
03825 val1 = geometry_msgs.msg.Point()
03826 _x = val1
03827 start = end
03828 end += 24
03829 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03830 self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices.append(val1)
03831 _x = self
03832 start = end
03833 end += 12
03834 (_x.motion_plan_request.workspace_parameters.workspace_region_pose.header.seq, _x.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.secs, _x.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
03835 start = end
03836 end += 4
03837 (length,) = _struct_I.unpack(str[start:end])
03838 start = end
03839 end += length
03840 if python3:
03841 self.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id = str[start:end].decode('utf-8')
03842 else:
03843 self.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id = str[start:end]
03844 _x = self
03845 start = end
03846 end += 68
03847 (_x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.x, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.y, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.z, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.x, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.y, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.z, _x.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.w, _x.motion_plan_request.start_state.joint_state.header.seq, _x.motion_plan_request.start_state.joint_state.header.stamp.secs, _x.motion_plan_request.start_state.joint_state.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
03848 start = end
03849 end += 4
03850 (length,) = _struct_I.unpack(str[start:end])
03851 start = end
03852 end += length
03853 if python3:
03854 self.motion_plan_request.start_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
03855 else:
03856 self.motion_plan_request.start_state.joint_state.header.frame_id = str[start:end]
03857 start = end
03858 end += 4
03859 (length,) = _struct_I.unpack(str[start:end])
03860 self.motion_plan_request.start_state.joint_state.name = []
03861 for i in range(0, length):
03862 start = end
03863 end += 4
03864 (length,) = _struct_I.unpack(str[start:end])
03865 start = end
03866 end += length
03867 if python3:
03868 val1 = str[start:end].decode('utf-8')
03869 else:
03870 val1 = str[start:end]
03871 self.motion_plan_request.start_state.joint_state.name.append(val1)
03872 start = end
03873 end += 4
03874 (length,) = _struct_I.unpack(str[start:end])
03875 pattern = '<%sd'%length
03876 start = end
03877 end += struct.calcsize(pattern)
03878 self.motion_plan_request.start_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03879 start = end
03880 end += 4
03881 (length,) = _struct_I.unpack(str[start:end])
03882 pattern = '<%sd'%length
03883 start = end
03884 end += struct.calcsize(pattern)
03885 self.motion_plan_request.start_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03886 start = end
03887 end += 4
03888 (length,) = _struct_I.unpack(str[start:end])
03889 pattern = '<%sd'%length
03890 start = end
03891 end += struct.calcsize(pattern)
03892 self.motion_plan_request.start_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03893 _x = self
03894 start = end
03895 end += 8
03896 (_x.motion_plan_request.start_state.multi_dof_joint_state.stamp.secs, _x.motion_plan_request.start_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
03897 start = end
03898 end += 4
03899 (length,) = _struct_I.unpack(str[start:end])
03900 self.motion_plan_request.start_state.multi_dof_joint_state.joint_names = []
03901 for i in range(0, length):
03902 start = end
03903 end += 4
03904 (length,) = _struct_I.unpack(str[start:end])
03905 start = end
03906 end += length
03907 if python3:
03908 val1 = str[start:end].decode('utf-8')
03909 else:
03910 val1 = str[start:end]
03911 self.motion_plan_request.start_state.multi_dof_joint_state.joint_names.append(val1)
03912 start = end
03913 end += 4
03914 (length,) = _struct_I.unpack(str[start:end])
03915 self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = []
03916 for i in range(0, length):
03917 start = end
03918 end += 4
03919 (length,) = _struct_I.unpack(str[start:end])
03920 start = end
03921 end += length
03922 if python3:
03923 val1 = str[start:end].decode('utf-8')
03924 else:
03925 val1 = str[start:end]
03926 self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append(val1)
03927 start = end
03928 end += 4
03929 (length,) = _struct_I.unpack(str[start:end])
03930 self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = []
03931 for i in range(0, length):
03932 start = end
03933 end += 4
03934 (length,) = _struct_I.unpack(str[start:end])
03935 start = end
03936 end += length
03937 if python3:
03938 val1 = str[start:end].decode('utf-8')
03939 else:
03940 val1 = str[start:end]
03941 self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append(val1)
03942 start = end
03943 end += 4
03944 (length,) = _struct_I.unpack(str[start:end])
03945 self.motion_plan_request.start_state.multi_dof_joint_state.poses = []
03946 for i in range(0, length):
03947 val1 = geometry_msgs.msg.Pose()
03948 _v245 = val1.position
03949 _x = _v245
03950 start = end
03951 end += 24
03952 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03953 _v246 = val1.orientation
03954 _x = _v246
03955 start = end
03956 end += 32
03957 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03958 self.motion_plan_request.start_state.multi_dof_joint_state.poses.append(val1)
03959 start = end
03960 end += 4
03961 (length,) = _struct_I.unpack(str[start:end])
03962 self.motion_plan_request.goal_constraints.joint_constraints = []
03963 for i in range(0, length):
03964 val1 = arm_navigation_msgs.msg.JointConstraint()
03965 start = end
03966 end += 4
03967 (length,) = _struct_I.unpack(str[start:end])
03968 start = end
03969 end += length
03970 if python3:
03971 val1.joint_name = str[start:end].decode('utf-8')
03972 else:
03973 val1.joint_name = str[start:end]
03974 _x = val1
03975 start = end
03976 end += 32
03977 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
03978 self.motion_plan_request.goal_constraints.joint_constraints.append(val1)
03979 start = end
03980 end += 4
03981 (length,) = _struct_I.unpack(str[start:end])
03982 self.motion_plan_request.goal_constraints.position_constraints = []
03983 for i in range(0, length):
03984 val1 = arm_navigation_msgs.msg.PositionConstraint()
03985 _v247 = val1.header
03986 start = end
03987 end += 4
03988 (_v247.seq,) = _struct_I.unpack(str[start:end])
03989 _v248 = _v247.stamp
03990 _x = _v248
03991 start = end
03992 end += 8
03993 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03994 start = end
03995 end += 4
03996 (length,) = _struct_I.unpack(str[start:end])
03997 start = end
03998 end += length
03999 if python3:
04000 _v247.frame_id = str[start:end].decode('utf-8')
04001 else:
04002 _v247.frame_id = str[start:end]
04003 start = end
04004 end += 4
04005 (length,) = _struct_I.unpack(str[start:end])
04006 start = end
04007 end += length
04008 if python3:
04009 val1.link_name = str[start:end].decode('utf-8')
04010 else:
04011 val1.link_name = str[start:end]
04012 _v249 = val1.target_point_offset
04013 _x = _v249
04014 start = end
04015 end += 24
04016 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04017 _v250 = val1.position
04018 _x = _v250
04019 start = end
04020 end += 24
04021 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04022 _v251 = val1.constraint_region_shape
04023 start = end
04024 end += 1
04025 (_v251.type,) = _struct_b.unpack(str[start:end])
04026 start = end
04027 end += 4
04028 (length,) = _struct_I.unpack(str[start:end])
04029 pattern = '<%sd'%length
04030 start = end
04031 end += struct.calcsize(pattern)
04032 _v251.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
04033 start = end
04034 end += 4
04035 (length,) = _struct_I.unpack(str[start:end])
04036 pattern = '<%si'%length
04037 start = end
04038 end += struct.calcsize(pattern)
04039 _v251.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
04040 start = end
04041 end += 4
04042 (length,) = _struct_I.unpack(str[start:end])
04043 _v251.vertices = []
04044 for i in range(0, length):
04045 val3 = geometry_msgs.msg.Point()
04046 _x = val3
04047 start = end
04048 end += 24
04049 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04050 _v251.vertices.append(val3)
04051 _v252 = val1.constraint_region_orientation
04052 _x = _v252
04053 start = end
04054 end += 32
04055 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04056 start = end
04057 end += 8
04058 (val1.weight,) = _struct_d.unpack(str[start:end])
04059 self.motion_plan_request.goal_constraints.position_constraints.append(val1)
04060 start = end
04061 end += 4
04062 (length,) = _struct_I.unpack(str[start:end])
04063 self.motion_plan_request.goal_constraints.orientation_constraints = []
04064 for i in range(0, length):
04065 val1 = arm_navigation_msgs.msg.OrientationConstraint()
04066 _v253 = val1.header
04067 start = end
04068 end += 4
04069 (_v253.seq,) = _struct_I.unpack(str[start:end])
04070 _v254 = _v253.stamp
04071 _x = _v254
04072 start = end
04073 end += 8
04074 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04075 start = end
04076 end += 4
04077 (length,) = _struct_I.unpack(str[start:end])
04078 start = end
04079 end += length
04080 if python3:
04081 _v253.frame_id = str[start:end].decode('utf-8')
04082 else:
04083 _v253.frame_id = str[start:end]
04084 start = end
04085 end += 4
04086 (length,) = _struct_I.unpack(str[start:end])
04087 start = end
04088 end += length
04089 if python3:
04090 val1.link_name = str[start:end].decode('utf-8')
04091 else:
04092 val1.link_name = str[start:end]
04093 start = end
04094 end += 4
04095 (val1.type,) = _struct_i.unpack(str[start:end])
04096 _v255 = val1.orientation
04097 _x = _v255
04098 start = end
04099 end += 32
04100 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04101 _x = val1
04102 start = end
04103 end += 32
04104 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
04105 self.motion_plan_request.goal_constraints.orientation_constraints.append(val1)
04106 start = end
04107 end += 4
04108 (length,) = _struct_I.unpack(str[start:end])
04109 self.motion_plan_request.goal_constraints.visibility_constraints = []
04110 for i in range(0, length):
04111 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
04112 _v256 = val1.header
04113 start = end
04114 end += 4
04115 (_v256.seq,) = _struct_I.unpack(str[start:end])
04116 _v257 = _v256.stamp
04117 _x = _v257
04118 start = end
04119 end += 8
04120 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04121 start = end
04122 end += 4
04123 (length,) = _struct_I.unpack(str[start:end])
04124 start = end
04125 end += length
04126 if python3:
04127 _v256.frame_id = str[start:end].decode('utf-8')
04128 else:
04129 _v256.frame_id = str[start:end]
04130 _v258 = val1.target
04131 _v259 = _v258.header
04132 start = end
04133 end += 4
04134 (_v259.seq,) = _struct_I.unpack(str[start:end])
04135 _v260 = _v259.stamp
04136 _x = _v260
04137 start = end
04138 end += 8
04139 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04140 start = end
04141 end += 4
04142 (length,) = _struct_I.unpack(str[start:end])
04143 start = end
04144 end += length
04145 if python3:
04146 _v259.frame_id = str[start:end].decode('utf-8')
04147 else:
04148 _v259.frame_id = str[start:end]
04149 _v261 = _v258.point
04150 _x = _v261
04151 start = end
04152 end += 24
04153 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04154 _v262 = val1.sensor_pose
04155 _v263 = _v262.header
04156 start = end
04157 end += 4
04158 (_v263.seq,) = _struct_I.unpack(str[start:end])
04159 _v264 = _v263.stamp
04160 _x = _v264
04161 start = end
04162 end += 8
04163 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04164 start = end
04165 end += 4
04166 (length,) = _struct_I.unpack(str[start:end])
04167 start = end
04168 end += length
04169 if python3:
04170 _v263.frame_id = str[start:end].decode('utf-8')
04171 else:
04172 _v263.frame_id = str[start:end]
04173 _v265 = _v262.pose
04174 _v266 = _v265.position
04175 _x = _v266
04176 start = end
04177 end += 24
04178 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04179 _v267 = _v265.orientation
04180 _x = _v267
04181 start = end
04182 end += 32
04183 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04184 start = end
04185 end += 8
04186 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
04187 self.motion_plan_request.goal_constraints.visibility_constraints.append(val1)
04188 start = end
04189 end += 4
04190 (length,) = _struct_I.unpack(str[start:end])
04191 self.motion_plan_request.path_constraints.joint_constraints = []
04192 for i in range(0, length):
04193 val1 = arm_navigation_msgs.msg.JointConstraint()
04194 start = end
04195 end += 4
04196 (length,) = _struct_I.unpack(str[start:end])
04197 start = end
04198 end += length
04199 if python3:
04200 val1.joint_name = str[start:end].decode('utf-8')
04201 else:
04202 val1.joint_name = str[start:end]
04203 _x = val1
04204 start = end
04205 end += 32
04206 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
04207 self.motion_plan_request.path_constraints.joint_constraints.append(val1)
04208 start = end
04209 end += 4
04210 (length,) = _struct_I.unpack(str[start:end])
04211 self.motion_plan_request.path_constraints.position_constraints = []
04212 for i in range(0, length):
04213 val1 = arm_navigation_msgs.msg.PositionConstraint()
04214 _v268 = val1.header
04215 start = end
04216 end += 4
04217 (_v268.seq,) = _struct_I.unpack(str[start:end])
04218 _v269 = _v268.stamp
04219 _x = _v269
04220 start = end
04221 end += 8
04222 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04223 start = end
04224 end += 4
04225 (length,) = _struct_I.unpack(str[start:end])
04226 start = end
04227 end += length
04228 if python3:
04229 _v268.frame_id = str[start:end].decode('utf-8')
04230 else:
04231 _v268.frame_id = str[start:end]
04232 start = end
04233 end += 4
04234 (length,) = _struct_I.unpack(str[start:end])
04235 start = end
04236 end += length
04237 if python3:
04238 val1.link_name = str[start:end].decode('utf-8')
04239 else:
04240 val1.link_name = str[start:end]
04241 _v270 = val1.target_point_offset
04242 _x = _v270
04243 start = end
04244 end += 24
04245 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04246 _v271 = val1.position
04247 _x = _v271
04248 start = end
04249 end += 24
04250 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04251 _v272 = val1.constraint_region_shape
04252 start = end
04253 end += 1
04254 (_v272.type,) = _struct_b.unpack(str[start:end])
04255 start = end
04256 end += 4
04257 (length,) = _struct_I.unpack(str[start:end])
04258 pattern = '<%sd'%length
04259 start = end
04260 end += struct.calcsize(pattern)
04261 _v272.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
04262 start = end
04263 end += 4
04264 (length,) = _struct_I.unpack(str[start:end])
04265 pattern = '<%si'%length
04266 start = end
04267 end += struct.calcsize(pattern)
04268 _v272.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
04269 start = end
04270 end += 4
04271 (length,) = _struct_I.unpack(str[start:end])
04272 _v272.vertices = []
04273 for i in range(0, length):
04274 val3 = geometry_msgs.msg.Point()
04275 _x = val3
04276 start = end
04277 end += 24
04278 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04279 _v272.vertices.append(val3)
04280 _v273 = val1.constraint_region_orientation
04281 _x = _v273
04282 start = end
04283 end += 32
04284 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04285 start = end
04286 end += 8
04287 (val1.weight,) = _struct_d.unpack(str[start:end])
04288 self.motion_plan_request.path_constraints.position_constraints.append(val1)
04289 start = end
04290 end += 4
04291 (length,) = _struct_I.unpack(str[start:end])
04292 self.motion_plan_request.path_constraints.orientation_constraints = []
04293 for i in range(0, length):
04294 val1 = arm_navigation_msgs.msg.OrientationConstraint()
04295 _v274 = val1.header
04296 start = end
04297 end += 4
04298 (_v274.seq,) = _struct_I.unpack(str[start:end])
04299 _v275 = _v274.stamp
04300 _x = _v275
04301 start = end
04302 end += 8
04303 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04304 start = end
04305 end += 4
04306 (length,) = _struct_I.unpack(str[start:end])
04307 start = end
04308 end += length
04309 if python3:
04310 _v274.frame_id = str[start:end].decode('utf-8')
04311 else:
04312 _v274.frame_id = str[start:end]
04313 start = end
04314 end += 4
04315 (length,) = _struct_I.unpack(str[start:end])
04316 start = end
04317 end += length
04318 if python3:
04319 val1.link_name = str[start:end].decode('utf-8')
04320 else:
04321 val1.link_name = str[start:end]
04322 start = end
04323 end += 4
04324 (val1.type,) = _struct_i.unpack(str[start:end])
04325 _v276 = val1.orientation
04326 _x = _v276
04327 start = end
04328 end += 32
04329 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04330 _x = val1
04331 start = end
04332 end += 32
04333 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
04334 self.motion_plan_request.path_constraints.orientation_constraints.append(val1)
04335 start = end
04336 end += 4
04337 (length,) = _struct_I.unpack(str[start:end])
04338 self.motion_plan_request.path_constraints.visibility_constraints = []
04339 for i in range(0, length):
04340 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
04341 _v277 = val1.header
04342 start = end
04343 end += 4
04344 (_v277.seq,) = _struct_I.unpack(str[start:end])
04345 _v278 = _v277.stamp
04346 _x = _v278
04347 start = end
04348 end += 8
04349 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04350 start = end
04351 end += 4
04352 (length,) = _struct_I.unpack(str[start:end])
04353 start = end
04354 end += length
04355 if python3:
04356 _v277.frame_id = str[start:end].decode('utf-8')
04357 else:
04358 _v277.frame_id = str[start:end]
04359 _v279 = val1.target
04360 _v280 = _v279.header
04361 start = end
04362 end += 4
04363 (_v280.seq,) = _struct_I.unpack(str[start:end])
04364 _v281 = _v280.stamp
04365 _x = _v281
04366 start = end
04367 end += 8
04368 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04369 start = end
04370 end += 4
04371 (length,) = _struct_I.unpack(str[start:end])
04372 start = end
04373 end += length
04374 if python3:
04375 _v280.frame_id = str[start:end].decode('utf-8')
04376 else:
04377 _v280.frame_id = str[start:end]
04378 _v282 = _v279.point
04379 _x = _v282
04380 start = end
04381 end += 24
04382 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04383 _v283 = val1.sensor_pose
04384 _v284 = _v283.header
04385 start = end
04386 end += 4
04387 (_v284.seq,) = _struct_I.unpack(str[start:end])
04388 _v285 = _v284.stamp
04389 _x = _v285
04390 start = end
04391 end += 8
04392 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04393 start = end
04394 end += 4
04395 (length,) = _struct_I.unpack(str[start:end])
04396 start = end
04397 end += length
04398 if python3:
04399 _v284.frame_id = str[start:end].decode('utf-8')
04400 else:
04401 _v284.frame_id = str[start:end]
04402 _v286 = _v283.pose
04403 _v287 = _v286.position
04404 _x = _v287
04405 start = end
04406 end += 24
04407 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04408 _v288 = _v286.orientation
04409 _x = _v288
04410 start = end
04411 end += 32
04412 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04413 start = end
04414 end += 8
04415 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
04416 self.motion_plan_request.path_constraints.visibility_constraints.append(val1)
04417 start = end
04418 end += 4
04419 (length,) = _struct_I.unpack(str[start:end])
04420 start = end
04421 end += length
04422 if python3:
04423 self.motion_plan_request.planner_id = str[start:end].decode('utf-8')
04424 else:
04425 self.motion_plan_request.planner_id = str[start:end]
04426 start = end
04427 end += 4
04428 (length,) = _struct_I.unpack(str[start:end])
04429 start = end
04430 end += length
04431 if python3:
04432 self.motion_plan_request.group_name = str[start:end].decode('utf-8')
04433 else:
04434 self.motion_plan_request.group_name = str[start:end]
04435 _x = self
04436 start = end
04437 end += 28
04438 (_x.motion_plan_request.num_planning_attempts, _x.motion_plan_request.allowed_planning_time.secs, _x.motion_plan_request.allowed_planning_time.nsecs, _x.motion_plan_request.expected_path_duration.secs, _x.motion_plan_request.expected_path_duration.nsecs, _x.motion_plan_request.expected_path_dt.secs, _x.motion_plan_request.expected_path_dt.nsecs,) = _struct_7i.unpack(str[start:end])
04439 start = end
04440 end += 4
04441 (length,) = _struct_I.unpack(str[start:end])
04442 self.operations.collision_operations = []
04443 for i in range(0, length):
04444 val1 = arm_navigation_msgs.msg.CollisionOperation()
04445 start = end
04446 end += 4
04447 (length,) = _struct_I.unpack(str[start:end])
04448 start = end
04449 end += length
04450 if python3:
04451 val1.object1 = str[start:end].decode('utf-8')
04452 else:
04453 val1.object1 = str[start:end]
04454 start = end
04455 end += 4
04456 (length,) = _struct_I.unpack(str[start:end])
04457 start = end
04458 end += length
04459 if python3:
04460 val1.object2 = str[start:end].decode('utf-8')
04461 else:
04462 val1.object2 = str[start:end]
04463 _x = val1
04464 start = end
04465 end += 12
04466 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
04467 self.operations.collision_operations.append(val1)
04468 _x = self
04469 start = end
04470 end += 4
04471 (_x.accept_partial_plans, _x.accept_invalid_goals, _x.disable_ik, _x.disable_collision_monitoring,) = _struct_4B.unpack(str[start:end])
04472 self.accept_partial_plans = bool(self.accept_partial_plans)
04473 self.accept_invalid_goals = bool(self.accept_invalid_goals)
04474 self.disable_ik = bool(self.disable_ik)
04475 self.disable_collision_monitoring = bool(self.disable_collision_monitoring)
04476 return self
04477 except struct.error as e:
04478 raise genpy.DeserializationError(e)
04479
04480 _struct_I = genpy.struct_I
04481 _struct_7d3I = struct.Struct("<7d3I")
04482 _struct_b = struct.Struct("<b")
04483 _struct_d = struct.Struct("<d")
04484 _struct_f = struct.Struct("<f")
04485 _struct_di = struct.Struct("<di")
04486 _struct_7i = struct.Struct("<7i")
04487 _struct_3f = struct.Struct("<3f")
04488 _struct_i = struct.Struct("<i")
04489 _struct_3I = struct.Struct("<3I")
04490 _struct_4d = struct.Struct("<4d")
04491 _struct_4B = struct.Struct("<4B")
04492 _struct_2I = struct.Struct("<2I")
04493 _struct_3d = struct.Struct("<3d")