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