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