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