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