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