$search
00001 """autogenerated by genmsg_py from PreplanHeadScanGoal.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 PreplanHeadScanGoal(roslib.message.Message): 00012 _md5sum = "565652fd52e32966cc5c599108653bf9" 00013 _type = "head_monitor_msgs/PreplanHeadScanGoal" 00014 _has_header = False #flag to mark the presence of a Header object 00015 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00016 # The motion plan request 00017 string group_name 00018 string head_monitor_link 00019 arm_navigation_msgs/MotionPlanRequest motion_plan_request 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__ = ['group_name','head_monitor_link','motion_plan_request'] 00290 _slot_types = ['string','string','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 group_name,head_monitor_link,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(PreplanHeadScanGoal, self).__init__(*args, **kwds) 00308 #message fields cannot be None, assign default values for those that are 00309 if self.group_name is None: 00310 self.group_name = '' 00311 if self.head_monitor_link is None: 00312 self.head_monitor_link = '' 00313 if self.motion_plan_request is None: 00314 self.motion_plan_request = arm_navigation_msgs.msg.MotionPlanRequest() 00315 else: 00316 self.group_name = '' 00317 self.head_monitor_link = '' 00318 self.motion_plan_request = arm_navigation_msgs.msg.MotionPlanRequest() 00319 00320 def _get_types(self): 00321 """ 00322 internal API method 00323 """ 00324 return self._slot_types 00325 00326 def serialize(self, buff): 00327 """ 00328 serialize message into buffer 00329 @param buff: buffer 00330 @type buff: StringIO 00331 """ 00332 try: 00333 _x = self.group_name 00334 length = len(_x) 00335 buff.write(struct.pack('<I%ss'%length, length, _x)) 00336 _x = self.head_monitor_link 00337 length = len(_x) 00338 buff.write(struct.pack('<I%ss'%length, length, _x)) 00339 buff.write(_struct_b.pack(self.motion_plan_request.workspace_parameters.workspace_region_shape.type)) 00340 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions) 00341 buff.write(_struct_I.pack(length)) 00342 pattern = '<%sd'%length 00343 buff.write(struct.pack(pattern, *self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions)) 00344 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles) 00345 buff.write(_struct_I.pack(length)) 00346 pattern = '<%si'%length 00347 buff.write(struct.pack(pattern, *self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles)) 00348 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices) 00349 buff.write(_struct_I.pack(length)) 00350 for val1 in self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices: 00351 _x = val1 00352 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00353 _x = self 00354 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)) 00355 _x = self.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id 00356 length = len(_x) 00357 buff.write(struct.pack('<I%ss'%length, length, _x)) 00358 _x = self 00359 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)) 00360 _x = self.motion_plan_request.start_state.joint_state.header.frame_id 00361 length = len(_x) 00362 buff.write(struct.pack('<I%ss'%length, length, _x)) 00363 length = len(self.motion_plan_request.start_state.joint_state.name) 00364 buff.write(_struct_I.pack(length)) 00365 for val1 in self.motion_plan_request.start_state.joint_state.name: 00366 length = len(val1) 00367 buff.write(struct.pack('<I%ss'%length, length, val1)) 00368 length = len(self.motion_plan_request.start_state.joint_state.position) 00369 buff.write(_struct_I.pack(length)) 00370 pattern = '<%sd'%length 00371 buff.write(struct.pack(pattern, *self.motion_plan_request.start_state.joint_state.position)) 00372 length = len(self.motion_plan_request.start_state.joint_state.velocity) 00373 buff.write(_struct_I.pack(length)) 00374 pattern = '<%sd'%length 00375 buff.write(struct.pack(pattern, *self.motion_plan_request.start_state.joint_state.velocity)) 00376 length = len(self.motion_plan_request.start_state.joint_state.effort) 00377 buff.write(_struct_I.pack(length)) 00378 pattern = '<%sd'%length 00379 buff.write(struct.pack(pattern, *self.motion_plan_request.start_state.joint_state.effort)) 00380 _x = self 00381 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)) 00382 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.joint_names) 00383 buff.write(_struct_I.pack(length)) 00384 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.joint_names: 00385 length = len(val1) 00386 buff.write(struct.pack('<I%ss'%length, length, val1)) 00387 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids) 00388 buff.write(_struct_I.pack(length)) 00389 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids: 00390 length = len(val1) 00391 buff.write(struct.pack('<I%ss'%length, length, val1)) 00392 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids) 00393 buff.write(_struct_I.pack(length)) 00394 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids: 00395 length = len(val1) 00396 buff.write(struct.pack('<I%ss'%length, length, val1)) 00397 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.poses) 00398 buff.write(_struct_I.pack(length)) 00399 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.poses: 00400 _v1 = val1.position 00401 _x = _v1 00402 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00403 _v2 = val1.orientation 00404 _x = _v2 00405 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00406 length = len(self.motion_plan_request.goal_constraints.joint_constraints) 00407 buff.write(_struct_I.pack(length)) 00408 for val1 in self.motion_plan_request.goal_constraints.joint_constraints: 00409 _x = val1.joint_name 00410 length = len(_x) 00411 buff.write(struct.pack('<I%ss'%length, length, _x)) 00412 _x = val1 00413 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 00414 length = len(self.motion_plan_request.goal_constraints.position_constraints) 00415 buff.write(_struct_I.pack(length)) 00416 for val1 in self.motion_plan_request.goal_constraints.position_constraints: 00417 _v3 = val1.header 00418 buff.write(_struct_I.pack(_v3.seq)) 00419 _v4 = _v3.stamp 00420 _x = _v4 00421 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00422 _x = _v3.frame_id 00423 length = len(_x) 00424 buff.write(struct.pack('<I%ss'%length, length, _x)) 00425 _x = val1.link_name 00426 length = len(_x) 00427 buff.write(struct.pack('<I%ss'%length, length, _x)) 00428 _v5 = val1.target_point_offset 00429 _x = _v5 00430 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00431 _v6 = val1.position 00432 _x = _v6 00433 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00434 _v7 = val1.constraint_region_shape 00435 buff.write(_struct_b.pack(_v7.type)) 00436 length = len(_v7.dimensions) 00437 buff.write(_struct_I.pack(length)) 00438 pattern = '<%sd'%length 00439 buff.write(struct.pack(pattern, *_v7.dimensions)) 00440 length = len(_v7.triangles) 00441 buff.write(_struct_I.pack(length)) 00442 pattern = '<%si'%length 00443 buff.write(struct.pack(pattern, *_v7.triangles)) 00444 length = len(_v7.vertices) 00445 buff.write(_struct_I.pack(length)) 00446 for val3 in _v7.vertices: 00447 _x = val3 00448 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00449 _v8 = val1.constraint_region_orientation 00450 _x = _v8 00451 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00452 buff.write(_struct_d.pack(val1.weight)) 00453 length = len(self.motion_plan_request.goal_constraints.orientation_constraints) 00454 buff.write(_struct_I.pack(length)) 00455 for val1 in self.motion_plan_request.goal_constraints.orientation_constraints: 00456 _v9 = val1.header 00457 buff.write(_struct_I.pack(_v9.seq)) 00458 _v10 = _v9.stamp 00459 _x = _v10 00460 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00461 _x = _v9.frame_id 00462 length = len(_x) 00463 buff.write(struct.pack('<I%ss'%length, length, _x)) 00464 _x = val1.link_name 00465 length = len(_x) 00466 buff.write(struct.pack('<I%ss'%length, length, _x)) 00467 buff.write(_struct_i.pack(val1.type)) 00468 _v11 = val1.orientation 00469 _x = _v11 00470 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00471 _x = val1 00472 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 00473 length = len(self.motion_plan_request.goal_constraints.visibility_constraints) 00474 buff.write(_struct_I.pack(length)) 00475 for val1 in self.motion_plan_request.goal_constraints.visibility_constraints: 00476 _v12 = val1.header 00477 buff.write(_struct_I.pack(_v12.seq)) 00478 _v13 = _v12.stamp 00479 _x = _v13 00480 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00481 _x = _v12.frame_id 00482 length = len(_x) 00483 buff.write(struct.pack('<I%ss'%length, length, _x)) 00484 _v14 = val1.target 00485 _v15 = _v14.header 00486 buff.write(_struct_I.pack(_v15.seq)) 00487 _v16 = _v15.stamp 00488 _x = _v16 00489 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00490 _x = _v15.frame_id 00491 length = len(_x) 00492 buff.write(struct.pack('<I%ss'%length, length, _x)) 00493 _v17 = _v14.point 00494 _x = _v17 00495 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00496 _v18 = val1.sensor_pose 00497 _v19 = _v18.header 00498 buff.write(_struct_I.pack(_v19.seq)) 00499 _v20 = _v19.stamp 00500 _x = _v20 00501 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00502 _x = _v19.frame_id 00503 length = len(_x) 00504 buff.write(struct.pack('<I%ss'%length, length, _x)) 00505 _v21 = _v18.pose 00506 _v22 = _v21.position 00507 _x = _v22 00508 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00509 _v23 = _v21.orientation 00510 _x = _v23 00511 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00512 buff.write(_struct_d.pack(val1.absolute_tolerance)) 00513 length = len(self.motion_plan_request.path_constraints.joint_constraints) 00514 buff.write(_struct_I.pack(length)) 00515 for val1 in self.motion_plan_request.path_constraints.joint_constraints: 00516 _x = val1.joint_name 00517 length = len(_x) 00518 buff.write(struct.pack('<I%ss'%length, length, _x)) 00519 _x = val1 00520 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 00521 length = len(self.motion_plan_request.path_constraints.position_constraints) 00522 buff.write(_struct_I.pack(length)) 00523 for val1 in self.motion_plan_request.path_constraints.position_constraints: 00524 _v24 = val1.header 00525 buff.write(_struct_I.pack(_v24.seq)) 00526 _v25 = _v24.stamp 00527 _x = _v25 00528 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00529 _x = _v24.frame_id 00530 length = len(_x) 00531 buff.write(struct.pack('<I%ss'%length, length, _x)) 00532 _x = val1.link_name 00533 length = len(_x) 00534 buff.write(struct.pack('<I%ss'%length, length, _x)) 00535 _v26 = val1.target_point_offset 00536 _x = _v26 00537 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00538 _v27 = val1.position 00539 _x = _v27 00540 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00541 _v28 = val1.constraint_region_shape 00542 buff.write(_struct_b.pack(_v28.type)) 00543 length = len(_v28.dimensions) 00544 buff.write(_struct_I.pack(length)) 00545 pattern = '<%sd'%length 00546 buff.write(struct.pack(pattern, *_v28.dimensions)) 00547 length = len(_v28.triangles) 00548 buff.write(_struct_I.pack(length)) 00549 pattern = '<%si'%length 00550 buff.write(struct.pack(pattern, *_v28.triangles)) 00551 length = len(_v28.vertices) 00552 buff.write(_struct_I.pack(length)) 00553 for val3 in _v28.vertices: 00554 _x = val3 00555 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00556 _v29 = val1.constraint_region_orientation 00557 _x = _v29 00558 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00559 buff.write(_struct_d.pack(val1.weight)) 00560 length = len(self.motion_plan_request.path_constraints.orientation_constraints) 00561 buff.write(_struct_I.pack(length)) 00562 for val1 in self.motion_plan_request.path_constraints.orientation_constraints: 00563 _v30 = val1.header 00564 buff.write(_struct_I.pack(_v30.seq)) 00565 _v31 = _v30.stamp 00566 _x = _v31 00567 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00568 _x = _v30.frame_id 00569 length = len(_x) 00570 buff.write(struct.pack('<I%ss'%length, length, _x)) 00571 _x = val1.link_name 00572 length = len(_x) 00573 buff.write(struct.pack('<I%ss'%length, length, _x)) 00574 buff.write(_struct_i.pack(val1.type)) 00575 _v32 = val1.orientation 00576 _x = _v32 00577 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00578 _x = val1 00579 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 00580 length = len(self.motion_plan_request.path_constraints.visibility_constraints) 00581 buff.write(_struct_I.pack(length)) 00582 for val1 in self.motion_plan_request.path_constraints.visibility_constraints: 00583 _v33 = val1.header 00584 buff.write(_struct_I.pack(_v33.seq)) 00585 _v34 = _v33.stamp 00586 _x = _v34 00587 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00588 _x = _v33.frame_id 00589 length = len(_x) 00590 buff.write(struct.pack('<I%ss'%length, length, _x)) 00591 _v35 = val1.target 00592 _v36 = _v35.header 00593 buff.write(_struct_I.pack(_v36.seq)) 00594 _v37 = _v36.stamp 00595 _x = _v37 00596 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00597 _x = _v36.frame_id 00598 length = len(_x) 00599 buff.write(struct.pack('<I%ss'%length, length, _x)) 00600 _v38 = _v35.point 00601 _x = _v38 00602 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00603 _v39 = val1.sensor_pose 00604 _v40 = _v39.header 00605 buff.write(_struct_I.pack(_v40.seq)) 00606 _v41 = _v40.stamp 00607 _x = _v41 00608 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00609 _x = _v40.frame_id 00610 length = len(_x) 00611 buff.write(struct.pack('<I%ss'%length, length, _x)) 00612 _v42 = _v39.pose 00613 _v43 = _v42.position 00614 _x = _v43 00615 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00616 _v44 = _v42.orientation 00617 _x = _v44 00618 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00619 buff.write(_struct_d.pack(val1.absolute_tolerance)) 00620 _x = self.motion_plan_request.planner_id 00621 length = len(_x) 00622 buff.write(struct.pack('<I%ss'%length, length, _x)) 00623 _x = self.motion_plan_request.group_name 00624 length = len(_x) 00625 buff.write(struct.pack('<I%ss'%length, length, _x)) 00626 _x = self 00627 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)) 00628 except struct.error as se: self._check_types(se) 00629 except TypeError as te: self._check_types(te) 00630 00631 def deserialize(self, str): 00632 """ 00633 unpack serialized message in str into this message instance 00634 @param str: byte array of serialized message 00635 @type str: str 00636 """ 00637 try: 00638 if self.motion_plan_request is None: 00639 self.motion_plan_request = arm_navigation_msgs.msg.MotionPlanRequest() 00640 end = 0 00641 start = end 00642 end += 4 00643 (length,) = _struct_I.unpack(str[start:end]) 00644 start = end 00645 end += length 00646 self.group_name = str[start:end] 00647 start = end 00648 end += 4 00649 (length,) = _struct_I.unpack(str[start:end]) 00650 start = end 00651 end += length 00652 self.head_monitor_link = str[start:end] 00653 start = end 00654 end += 1 00655 (self.motion_plan_request.workspace_parameters.workspace_region_shape.type,) = _struct_b.unpack(str[start:end]) 00656 start = end 00657 end += 4 00658 (length,) = _struct_I.unpack(str[start:end]) 00659 pattern = '<%sd'%length 00660 start = end 00661 end += struct.calcsize(pattern) 00662 self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions = struct.unpack(pattern, str[start:end]) 00663 start = end 00664 end += 4 00665 (length,) = _struct_I.unpack(str[start:end]) 00666 pattern = '<%si'%length 00667 start = end 00668 end += struct.calcsize(pattern) 00669 self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles = struct.unpack(pattern, str[start:end]) 00670 start = end 00671 end += 4 00672 (length,) = _struct_I.unpack(str[start:end]) 00673 self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices = [] 00674 for i in range(0, length): 00675 val1 = geometry_msgs.msg.Point() 00676 _x = val1 00677 start = end 00678 end += 24 00679 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00680 self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices.append(val1) 00681 _x = self 00682 start = end 00683 end += 12 00684 (_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]) 00685 start = end 00686 end += 4 00687 (length,) = _struct_I.unpack(str[start:end]) 00688 start = end 00689 end += length 00690 self.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id = str[start:end] 00691 _x = self 00692 start = end 00693 end += 68 00694 (_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]) 00695 start = end 00696 end += 4 00697 (length,) = _struct_I.unpack(str[start:end]) 00698 start = end 00699 end += length 00700 self.motion_plan_request.start_state.joint_state.header.frame_id = str[start:end] 00701 start = end 00702 end += 4 00703 (length,) = _struct_I.unpack(str[start:end]) 00704 self.motion_plan_request.start_state.joint_state.name = [] 00705 for i in range(0, length): 00706 start = end 00707 end += 4 00708 (length,) = _struct_I.unpack(str[start:end]) 00709 start = end 00710 end += length 00711 val1 = str[start:end] 00712 self.motion_plan_request.start_state.joint_state.name.append(val1) 00713 start = end 00714 end += 4 00715 (length,) = _struct_I.unpack(str[start:end]) 00716 pattern = '<%sd'%length 00717 start = end 00718 end += struct.calcsize(pattern) 00719 self.motion_plan_request.start_state.joint_state.position = struct.unpack(pattern, str[start:end]) 00720 start = end 00721 end += 4 00722 (length,) = _struct_I.unpack(str[start:end]) 00723 pattern = '<%sd'%length 00724 start = end 00725 end += struct.calcsize(pattern) 00726 self.motion_plan_request.start_state.joint_state.velocity = struct.unpack(pattern, str[start:end]) 00727 start = end 00728 end += 4 00729 (length,) = _struct_I.unpack(str[start:end]) 00730 pattern = '<%sd'%length 00731 start = end 00732 end += struct.calcsize(pattern) 00733 self.motion_plan_request.start_state.joint_state.effort = struct.unpack(pattern, str[start:end]) 00734 _x = self 00735 start = end 00736 end += 8 00737 (_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]) 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.joint_names = [] 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.joint_names.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.frame_ids = [] 00754 for i in range(0, length): 00755 start = end 00756 end += 4 00757 (length,) = _struct_I.unpack(str[start:end]) 00758 start = end 00759 end += length 00760 val1 = str[start:end] 00761 self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append(val1) 00762 start = end 00763 end += 4 00764 (length,) = _struct_I.unpack(str[start:end]) 00765 self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [] 00766 for i in range(0, length): 00767 start = end 00768 end += 4 00769 (length,) = _struct_I.unpack(str[start:end]) 00770 start = end 00771 end += length 00772 val1 = str[start:end] 00773 self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append(val1) 00774 start = end 00775 end += 4 00776 (length,) = _struct_I.unpack(str[start:end]) 00777 self.motion_plan_request.start_state.multi_dof_joint_state.poses = [] 00778 for i in range(0, length): 00779 val1 = geometry_msgs.msg.Pose() 00780 _v45 = val1.position 00781 _x = _v45 00782 start = end 00783 end += 24 00784 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00785 _v46 = val1.orientation 00786 _x = _v46 00787 start = end 00788 end += 32 00789 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00790 self.motion_plan_request.start_state.multi_dof_joint_state.poses.append(val1) 00791 start = end 00792 end += 4 00793 (length,) = _struct_I.unpack(str[start:end]) 00794 self.motion_plan_request.goal_constraints.joint_constraints = [] 00795 for i in range(0, length): 00796 val1 = arm_navigation_msgs.msg.JointConstraint() 00797 start = end 00798 end += 4 00799 (length,) = _struct_I.unpack(str[start:end]) 00800 start = end 00801 end += length 00802 val1.joint_name = str[start:end] 00803 _x = val1 00804 start = end 00805 end += 32 00806 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 00807 self.motion_plan_request.goal_constraints.joint_constraints.append(val1) 00808 start = end 00809 end += 4 00810 (length,) = _struct_I.unpack(str[start:end]) 00811 self.motion_plan_request.goal_constraints.position_constraints = [] 00812 for i in range(0, length): 00813 val1 = arm_navigation_msgs.msg.PositionConstraint() 00814 _v47 = val1.header 00815 start = end 00816 end += 4 00817 (_v47.seq,) = _struct_I.unpack(str[start:end]) 00818 _v48 = _v47.stamp 00819 _x = _v48 00820 start = end 00821 end += 8 00822 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00823 start = end 00824 end += 4 00825 (length,) = _struct_I.unpack(str[start:end]) 00826 start = end 00827 end += length 00828 _v47.frame_id = str[start:end] 00829 start = end 00830 end += 4 00831 (length,) = _struct_I.unpack(str[start:end]) 00832 start = end 00833 end += length 00834 val1.link_name = str[start:end] 00835 _v49 = val1.target_point_offset 00836 _x = _v49 00837 start = end 00838 end += 24 00839 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00840 _v50 = val1.position 00841 _x = _v50 00842 start = end 00843 end += 24 00844 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00845 _v51 = val1.constraint_region_shape 00846 start = end 00847 end += 1 00848 (_v51.type,) = _struct_b.unpack(str[start:end]) 00849 start = end 00850 end += 4 00851 (length,) = _struct_I.unpack(str[start:end]) 00852 pattern = '<%sd'%length 00853 start = end 00854 end += struct.calcsize(pattern) 00855 _v51.dimensions = struct.unpack(pattern, str[start:end]) 00856 start = end 00857 end += 4 00858 (length,) = _struct_I.unpack(str[start:end]) 00859 pattern = '<%si'%length 00860 start = end 00861 end += struct.calcsize(pattern) 00862 _v51.triangles = struct.unpack(pattern, str[start:end]) 00863 start = end 00864 end += 4 00865 (length,) = _struct_I.unpack(str[start:end]) 00866 _v51.vertices = [] 00867 for i in range(0, length): 00868 val3 = geometry_msgs.msg.Point() 00869 _x = val3 00870 start = end 00871 end += 24 00872 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00873 _v51.vertices.append(val3) 00874 _v52 = val1.constraint_region_orientation 00875 _x = _v52 00876 start = end 00877 end += 32 00878 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00879 start = end 00880 end += 8 00881 (val1.weight,) = _struct_d.unpack(str[start:end]) 00882 self.motion_plan_request.goal_constraints.position_constraints.append(val1) 00883 start = end 00884 end += 4 00885 (length,) = _struct_I.unpack(str[start:end]) 00886 self.motion_plan_request.goal_constraints.orientation_constraints = [] 00887 for i in range(0, length): 00888 val1 = arm_navigation_msgs.msg.OrientationConstraint() 00889 _v53 = val1.header 00890 start = end 00891 end += 4 00892 (_v53.seq,) = _struct_I.unpack(str[start:end]) 00893 _v54 = _v53.stamp 00894 _x = _v54 00895 start = end 00896 end += 8 00897 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00898 start = end 00899 end += 4 00900 (length,) = _struct_I.unpack(str[start:end]) 00901 start = end 00902 end += length 00903 _v53.frame_id = str[start:end] 00904 start = end 00905 end += 4 00906 (length,) = _struct_I.unpack(str[start:end]) 00907 start = end 00908 end += length 00909 val1.link_name = str[start:end] 00910 start = end 00911 end += 4 00912 (val1.type,) = _struct_i.unpack(str[start:end]) 00913 _v55 = val1.orientation 00914 _x = _v55 00915 start = end 00916 end += 32 00917 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00918 _x = val1 00919 start = end 00920 end += 32 00921 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 00922 self.motion_plan_request.goal_constraints.orientation_constraints.append(val1) 00923 start = end 00924 end += 4 00925 (length,) = _struct_I.unpack(str[start:end]) 00926 self.motion_plan_request.goal_constraints.visibility_constraints = [] 00927 for i in range(0, length): 00928 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 00929 _v56 = val1.header 00930 start = end 00931 end += 4 00932 (_v56.seq,) = _struct_I.unpack(str[start:end]) 00933 _v57 = _v56.stamp 00934 _x = _v57 00935 start = end 00936 end += 8 00937 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00938 start = end 00939 end += 4 00940 (length,) = _struct_I.unpack(str[start:end]) 00941 start = end 00942 end += length 00943 _v56.frame_id = str[start:end] 00944 _v58 = val1.target 00945 _v59 = _v58.header 00946 start = end 00947 end += 4 00948 (_v59.seq,) = _struct_I.unpack(str[start:end]) 00949 _v60 = _v59.stamp 00950 _x = _v60 00951 start = end 00952 end += 8 00953 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00954 start = end 00955 end += 4 00956 (length,) = _struct_I.unpack(str[start:end]) 00957 start = end 00958 end += length 00959 _v59.frame_id = str[start:end] 00960 _v61 = _v58.point 00961 _x = _v61 00962 start = end 00963 end += 24 00964 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00965 _v62 = val1.sensor_pose 00966 _v63 = _v62.header 00967 start = end 00968 end += 4 00969 (_v63.seq,) = _struct_I.unpack(str[start:end]) 00970 _v64 = _v63.stamp 00971 _x = _v64 00972 start = end 00973 end += 8 00974 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00975 start = end 00976 end += 4 00977 (length,) = _struct_I.unpack(str[start:end]) 00978 start = end 00979 end += length 00980 _v63.frame_id = str[start:end] 00981 _v65 = _v62.pose 00982 _v66 = _v65.position 00983 _x = _v66 00984 start = end 00985 end += 24 00986 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00987 _v67 = _v65.orientation 00988 _x = _v67 00989 start = end 00990 end += 32 00991 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00992 start = end 00993 end += 8 00994 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 00995 self.motion_plan_request.goal_constraints.visibility_constraints.append(val1) 00996 start = end 00997 end += 4 00998 (length,) = _struct_I.unpack(str[start:end]) 00999 self.motion_plan_request.path_constraints.joint_constraints = [] 01000 for i in range(0, length): 01001 val1 = arm_navigation_msgs.msg.JointConstraint() 01002 start = end 01003 end += 4 01004 (length,) = _struct_I.unpack(str[start:end]) 01005 start = end 01006 end += length 01007 val1.joint_name = str[start:end] 01008 _x = val1 01009 start = end 01010 end += 32 01011 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 01012 self.motion_plan_request.path_constraints.joint_constraints.append(val1) 01013 start = end 01014 end += 4 01015 (length,) = _struct_I.unpack(str[start:end]) 01016 self.motion_plan_request.path_constraints.position_constraints = [] 01017 for i in range(0, length): 01018 val1 = arm_navigation_msgs.msg.PositionConstraint() 01019 _v68 = val1.header 01020 start = end 01021 end += 4 01022 (_v68.seq,) = _struct_I.unpack(str[start:end]) 01023 _v69 = _v68.stamp 01024 _x = _v69 01025 start = end 01026 end += 8 01027 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01028 start = end 01029 end += 4 01030 (length,) = _struct_I.unpack(str[start:end]) 01031 start = end 01032 end += length 01033 _v68.frame_id = str[start:end] 01034 start = end 01035 end += 4 01036 (length,) = _struct_I.unpack(str[start:end]) 01037 start = end 01038 end += length 01039 val1.link_name = str[start:end] 01040 _v70 = val1.target_point_offset 01041 _x = _v70 01042 start = end 01043 end += 24 01044 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01045 _v71 = val1.position 01046 _x = _v71 01047 start = end 01048 end += 24 01049 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01050 _v72 = val1.constraint_region_shape 01051 start = end 01052 end += 1 01053 (_v72.type,) = _struct_b.unpack(str[start:end]) 01054 start = end 01055 end += 4 01056 (length,) = _struct_I.unpack(str[start:end]) 01057 pattern = '<%sd'%length 01058 start = end 01059 end += struct.calcsize(pattern) 01060 _v72.dimensions = struct.unpack(pattern, str[start:end]) 01061 start = end 01062 end += 4 01063 (length,) = _struct_I.unpack(str[start:end]) 01064 pattern = '<%si'%length 01065 start = end 01066 end += struct.calcsize(pattern) 01067 _v72.triangles = struct.unpack(pattern, str[start:end]) 01068 start = end 01069 end += 4 01070 (length,) = _struct_I.unpack(str[start:end]) 01071 _v72.vertices = [] 01072 for i in range(0, length): 01073 val3 = geometry_msgs.msg.Point() 01074 _x = val3 01075 start = end 01076 end += 24 01077 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01078 _v72.vertices.append(val3) 01079 _v73 = val1.constraint_region_orientation 01080 _x = _v73 01081 start = end 01082 end += 32 01083 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01084 start = end 01085 end += 8 01086 (val1.weight,) = _struct_d.unpack(str[start:end]) 01087 self.motion_plan_request.path_constraints.position_constraints.append(val1) 01088 start = end 01089 end += 4 01090 (length,) = _struct_I.unpack(str[start:end]) 01091 self.motion_plan_request.path_constraints.orientation_constraints = [] 01092 for i in range(0, length): 01093 val1 = arm_navigation_msgs.msg.OrientationConstraint() 01094 _v74 = val1.header 01095 start = end 01096 end += 4 01097 (_v74.seq,) = _struct_I.unpack(str[start:end]) 01098 _v75 = _v74.stamp 01099 _x = _v75 01100 start = end 01101 end += 8 01102 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01103 start = end 01104 end += 4 01105 (length,) = _struct_I.unpack(str[start:end]) 01106 start = end 01107 end += length 01108 _v74.frame_id = str[start:end] 01109 start = end 01110 end += 4 01111 (length,) = _struct_I.unpack(str[start:end]) 01112 start = end 01113 end += length 01114 val1.link_name = str[start:end] 01115 start = end 01116 end += 4 01117 (val1.type,) = _struct_i.unpack(str[start:end]) 01118 _v76 = val1.orientation 01119 _x = _v76 01120 start = end 01121 end += 32 01122 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01123 _x = val1 01124 start = end 01125 end += 32 01126 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 01127 self.motion_plan_request.path_constraints.orientation_constraints.append(val1) 01128 start = end 01129 end += 4 01130 (length,) = _struct_I.unpack(str[start:end]) 01131 self.motion_plan_request.path_constraints.visibility_constraints = [] 01132 for i in range(0, length): 01133 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 01134 _v77 = val1.header 01135 start = end 01136 end += 4 01137 (_v77.seq,) = _struct_I.unpack(str[start:end]) 01138 _v78 = _v77.stamp 01139 _x = _v78 01140 start = end 01141 end += 8 01142 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01143 start = end 01144 end += 4 01145 (length,) = _struct_I.unpack(str[start:end]) 01146 start = end 01147 end += length 01148 _v77.frame_id = str[start:end] 01149 _v79 = val1.target 01150 _v80 = _v79.header 01151 start = end 01152 end += 4 01153 (_v80.seq,) = _struct_I.unpack(str[start:end]) 01154 _v81 = _v80.stamp 01155 _x = _v81 01156 start = end 01157 end += 8 01158 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01159 start = end 01160 end += 4 01161 (length,) = _struct_I.unpack(str[start:end]) 01162 start = end 01163 end += length 01164 _v80.frame_id = str[start:end] 01165 _v82 = _v79.point 01166 _x = _v82 01167 start = end 01168 end += 24 01169 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01170 _v83 = val1.sensor_pose 01171 _v84 = _v83.header 01172 start = end 01173 end += 4 01174 (_v84.seq,) = _struct_I.unpack(str[start:end]) 01175 _v85 = _v84.stamp 01176 _x = _v85 01177 start = end 01178 end += 8 01179 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01180 start = end 01181 end += 4 01182 (length,) = _struct_I.unpack(str[start:end]) 01183 start = end 01184 end += length 01185 _v84.frame_id = str[start:end] 01186 _v86 = _v83.pose 01187 _v87 = _v86.position 01188 _x = _v87 01189 start = end 01190 end += 24 01191 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01192 _v88 = _v86.orientation 01193 _x = _v88 01194 start = end 01195 end += 32 01196 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01197 start = end 01198 end += 8 01199 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 01200 self.motion_plan_request.path_constraints.visibility_constraints.append(val1) 01201 start = end 01202 end += 4 01203 (length,) = _struct_I.unpack(str[start:end]) 01204 start = end 01205 end += length 01206 self.motion_plan_request.planner_id = str[start:end] 01207 start = end 01208 end += 4 01209 (length,) = _struct_I.unpack(str[start:end]) 01210 start = end 01211 end += length 01212 self.motion_plan_request.group_name = str[start:end] 01213 _x = self 01214 start = end 01215 end += 28 01216 (_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]) 01217 return self 01218 except struct.error as e: 01219 raise roslib.message.DeserializationError(e) #most likely buffer underfill 01220 01221 01222 def serialize_numpy(self, buff, numpy): 01223 """ 01224 serialize message with numpy array types into buffer 01225 @param buff: buffer 01226 @type buff: StringIO 01227 @param numpy: numpy python module 01228 @type numpy module 01229 """ 01230 try: 01231 _x = self.group_name 01232 length = len(_x) 01233 buff.write(struct.pack('<I%ss'%length, length, _x)) 01234 _x = self.head_monitor_link 01235 length = len(_x) 01236 buff.write(struct.pack('<I%ss'%length, length, _x)) 01237 buff.write(_struct_b.pack(self.motion_plan_request.workspace_parameters.workspace_region_shape.type)) 01238 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions) 01239 buff.write(_struct_I.pack(length)) 01240 pattern = '<%sd'%length 01241 buff.write(self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions.tostring()) 01242 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles) 01243 buff.write(_struct_I.pack(length)) 01244 pattern = '<%si'%length 01245 buff.write(self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles.tostring()) 01246 length = len(self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices) 01247 buff.write(_struct_I.pack(length)) 01248 for val1 in self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices: 01249 _x = val1 01250 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01251 _x = self 01252 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)) 01253 _x = self.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id 01254 length = len(_x) 01255 buff.write(struct.pack('<I%ss'%length, length, _x)) 01256 _x = self 01257 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)) 01258 _x = self.motion_plan_request.start_state.joint_state.header.frame_id 01259 length = len(_x) 01260 buff.write(struct.pack('<I%ss'%length, length, _x)) 01261 length = len(self.motion_plan_request.start_state.joint_state.name) 01262 buff.write(_struct_I.pack(length)) 01263 for val1 in self.motion_plan_request.start_state.joint_state.name: 01264 length = len(val1) 01265 buff.write(struct.pack('<I%ss'%length, length, val1)) 01266 length = len(self.motion_plan_request.start_state.joint_state.position) 01267 buff.write(_struct_I.pack(length)) 01268 pattern = '<%sd'%length 01269 buff.write(self.motion_plan_request.start_state.joint_state.position.tostring()) 01270 length = len(self.motion_plan_request.start_state.joint_state.velocity) 01271 buff.write(_struct_I.pack(length)) 01272 pattern = '<%sd'%length 01273 buff.write(self.motion_plan_request.start_state.joint_state.velocity.tostring()) 01274 length = len(self.motion_plan_request.start_state.joint_state.effort) 01275 buff.write(_struct_I.pack(length)) 01276 pattern = '<%sd'%length 01277 buff.write(self.motion_plan_request.start_state.joint_state.effort.tostring()) 01278 _x = self 01279 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)) 01280 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.joint_names) 01281 buff.write(_struct_I.pack(length)) 01282 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.joint_names: 01283 length = len(val1) 01284 buff.write(struct.pack('<I%ss'%length, length, val1)) 01285 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids) 01286 buff.write(_struct_I.pack(length)) 01287 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids: 01288 length = len(val1) 01289 buff.write(struct.pack('<I%ss'%length, length, val1)) 01290 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids) 01291 buff.write(_struct_I.pack(length)) 01292 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids: 01293 length = len(val1) 01294 buff.write(struct.pack('<I%ss'%length, length, val1)) 01295 length = len(self.motion_plan_request.start_state.multi_dof_joint_state.poses) 01296 buff.write(_struct_I.pack(length)) 01297 for val1 in self.motion_plan_request.start_state.multi_dof_joint_state.poses: 01298 _v89 = val1.position 01299 _x = _v89 01300 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01301 _v90 = val1.orientation 01302 _x = _v90 01303 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01304 length = len(self.motion_plan_request.goal_constraints.joint_constraints) 01305 buff.write(_struct_I.pack(length)) 01306 for val1 in self.motion_plan_request.goal_constraints.joint_constraints: 01307 _x = val1.joint_name 01308 length = len(_x) 01309 buff.write(struct.pack('<I%ss'%length, length, _x)) 01310 _x = val1 01311 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 01312 length = len(self.motion_plan_request.goal_constraints.position_constraints) 01313 buff.write(_struct_I.pack(length)) 01314 for val1 in self.motion_plan_request.goal_constraints.position_constraints: 01315 _v91 = val1.header 01316 buff.write(_struct_I.pack(_v91.seq)) 01317 _v92 = _v91.stamp 01318 _x = _v92 01319 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01320 _x = _v91.frame_id 01321 length = len(_x) 01322 buff.write(struct.pack('<I%ss'%length, length, _x)) 01323 _x = val1.link_name 01324 length = len(_x) 01325 buff.write(struct.pack('<I%ss'%length, length, _x)) 01326 _v93 = val1.target_point_offset 01327 _x = _v93 01328 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01329 _v94 = val1.position 01330 _x = _v94 01331 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01332 _v95 = val1.constraint_region_shape 01333 buff.write(_struct_b.pack(_v95.type)) 01334 length = len(_v95.dimensions) 01335 buff.write(_struct_I.pack(length)) 01336 pattern = '<%sd'%length 01337 buff.write(_v95.dimensions.tostring()) 01338 length = len(_v95.triangles) 01339 buff.write(_struct_I.pack(length)) 01340 pattern = '<%si'%length 01341 buff.write(_v95.triangles.tostring()) 01342 length = len(_v95.vertices) 01343 buff.write(_struct_I.pack(length)) 01344 for val3 in _v95.vertices: 01345 _x = val3 01346 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01347 _v96 = val1.constraint_region_orientation 01348 _x = _v96 01349 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01350 buff.write(_struct_d.pack(val1.weight)) 01351 length = len(self.motion_plan_request.goal_constraints.orientation_constraints) 01352 buff.write(_struct_I.pack(length)) 01353 for val1 in self.motion_plan_request.goal_constraints.orientation_constraints: 01354 _v97 = val1.header 01355 buff.write(_struct_I.pack(_v97.seq)) 01356 _v98 = _v97.stamp 01357 _x = _v98 01358 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01359 _x = _v97.frame_id 01360 length = len(_x) 01361 buff.write(struct.pack('<I%ss'%length, length, _x)) 01362 _x = val1.link_name 01363 length = len(_x) 01364 buff.write(struct.pack('<I%ss'%length, length, _x)) 01365 buff.write(_struct_i.pack(val1.type)) 01366 _v99 = val1.orientation 01367 _x = _v99 01368 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01369 _x = val1 01370 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 01371 length = len(self.motion_plan_request.goal_constraints.visibility_constraints) 01372 buff.write(_struct_I.pack(length)) 01373 for val1 in self.motion_plan_request.goal_constraints.visibility_constraints: 01374 _v100 = val1.header 01375 buff.write(_struct_I.pack(_v100.seq)) 01376 _v101 = _v100.stamp 01377 _x = _v101 01378 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01379 _x = _v100.frame_id 01380 length = len(_x) 01381 buff.write(struct.pack('<I%ss'%length, length, _x)) 01382 _v102 = val1.target 01383 _v103 = _v102.header 01384 buff.write(_struct_I.pack(_v103.seq)) 01385 _v104 = _v103.stamp 01386 _x = _v104 01387 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01388 _x = _v103.frame_id 01389 length = len(_x) 01390 buff.write(struct.pack('<I%ss'%length, length, _x)) 01391 _v105 = _v102.point 01392 _x = _v105 01393 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01394 _v106 = val1.sensor_pose 01395 _v107 = _v106.header 01396 buff.write(_struct_I.pack(_v107.seq)) 01397 _v108 = _v107.stamp 01398 _x = _v108 01399 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01400 _x = _v107.frame_id 01401 length = len(_x) 01402 buff.write(struct.pack('<I%ss'%length, length, _x)) 01403 _v109 = _v106.pose 01404 _v110 = _v109.position 01405 _x = _v110 01406 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01407 _v111 = _v109.orientation 01408 _x = _v111 01409 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01410 buff.write(_struct_d.pack(val1.absolute_tolerance)) 01411 length = len(self.motion_plan_request.path_constraints.joint_constraints) 01412 buff.write(_struct_I.pack(length)) 01413 for val1 in self.motion_plan_request.path_constraints.joint_constraints: 01414 _x = val1.joint_name 01415 length = len(_x) 01416 buff.write(struct.pack('<I%ss'%length, length, _x)) 01417 _x = val1 01418 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 01419 length = len(self.motion_plan_request.path_constraints.position_constraints) 01420 buff.write(_struct_I.pack(length)) 01421 for val1 in self.motion_plan_request.path_constraints.position_constraints: 01422 _v112 = val1.header 01423 buff.write(_struct_I.pack(_v112.seq)) 01424 _v113 = _v112.stamp 01425 _x = _v113 01426 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01427 _x = _v112.frame_id 01428 length = len(_x) 01429 buff.write(struct.pack('<I%ss'%length, length, _x)) 01430 _x = val1.link_name 01431 length = len(_x) 01432 buff.write(struct.pack('<I%ss'%length, length, _x)) 01433 _v114 = val1.target_point_offset 01434 _x = _v114 01435 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01436 _v115 = val1.position 01437 _x = _v115 01438 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01439 _v116 = val1.constraint_region_shape 01440 buff.write(_struct_b.pack(_v116.type)) 01441 length = len(_v116.dimensions) 01442 buff.write(_struct_I.pack(length)) 01443 pattern = '<%sd'%length 01444 buff.write(_v116.dimensions.tostring()) 01445 length = len(_v116.triangles) 01446 buff.write(_struct_I.pack(length)) 01447 pattern = '<%si'%length 01448 buff.write(_v116.triangles.tostring()) 01449 length = len(_v116.vertices) 01450 buff.write(_struct_I.pack(length)) 01451 for val3 in _v116.vertices: 01452 _x = val3 01453 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01454 _v117 = val1.constraint_region_orientation 01455 _x = _v117 01456 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01457 buff.write(_struct_d.pack(val1.weight)) 01458 length = len(self.motion_plan_request.path_constraints.orientation_constraints) 01459 buff.write(_struct_I.pack(length)) 01460 for val1 in self.motion_plan_request.path_constraints.orientation_constraints: 01461 _v118 = val1.header 01462 buff.write(_struct_I.pack(_v118.seq)) 01463 _v119 = _v118.stamp 01464 _x = _v119 01465 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01466 _x = _v118.frame_id 01467 length = len(_x) 01468 buff.write(struct.pack('<I%ss'%length, length, _x)) 01469 _x = val1.link_name 01470 length = len(_x) 01471 buff.write(struct.pack('<I%ss'%length, length, _x)) 01472 buff.write(_struct_i.pack(val1.type)) 01473 _v120 = val1.orientation 01474 _x = _v120 01475 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01476 _x = val1 01477 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 01478 length = len(self.motion_plan_request.path_constraints.visibility_constraints) 01479 buff.write(_struct_I.pack(length)) 01480 for val1 in self.motion_plan_request.path_constraints.visibility_constraints: 01481 _v121 = val1.header 01482 buff.write(_struct_I.pack(_v121.seq)) 01483 _v122 = _v121.stamp 01484 _x = _v122 01485 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01486 _x = _v121.frame_id 01487 length = len(_x) 01488 buff.write(struct.pack('<I%ss'%length, length, _x)) 01489 _v123 = val1.target 01490 _v124 = _v123.header 01491 buff.write(_struct_I.pack(_v124.seq)) 01492 _v125 = _v124.stamp 01493 _x = _v125 01494 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01495 _x = _v124.frame_id 01496 length = len(_x) 01497 buff.write(struct.pack('<I%ss'%length, length, _x)) 01498 _v126 = _v123.point 01499 _x = _v126 01500 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01501 _v127 = val1.sensor_pose 01502 _v128 = _v127.header 01503 buff.write(_struct_I.pack(_v128.seq)) 01504 _v129 = _v128.stamp 01505 _x = _v129 01506 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01507 _x = _v128.frame_id 01508 length = len(_x) 01509 buff.write(struct.pack('<I%ss'%length, length, _x)) 01510 _v130 = _v127.pose 01511 _v131 = _v130.position 01512 _x = _v131 01513 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01514 _v132 = _v130.orientation 01515 _x = _v132 01516 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01517 buff.write(_struct_d.pack(val1.absolute_tolerance)) 01518 _x = self.motion_plan_request.planner_id 01519 length = len(_x) 01520 buff.write(struct.pack('<I%ss'%length, length, _x)) 01521 _x = self.motion_plan_request.group_name 01522 length = len(_x) 01523 buff.write(struct.pack('<I%ss'%length, length, _x)) 01524 _x = self 01525 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)) 01526 except struct.error as se: self._check_types(se) 01527 except TypeError as te: self._check_types(te) 01528 01529 def deserialize_numpy(self, str, numpy): 01530 """ 01531 unpack serialized message in str into this message instance using numpy for array types 01532 @param str: byte array of serialized message 01533 @type str: str 01534 @param numpy: numpy python module 01535 @type numpy: module 01536 """ 01537 try: 01538 if self.motion_plan_request is None: 01539 self.motion_plan_request = arm_navigation_msgs.msg.MotionPlanRequest() 01540 end = 0 01541 start = end 01542 end += 4 01543 (length,) = _struct_I.unpack(str[start:end]) 01544 start = end 01545 end += length 01546 self.group_name = str[start:end] 01547 start = end 01548 end += 4 01549 (length,) = _struct_I.unpack(str[start:end]) 01550 start = end 01551 end += length 01552 self.head_monitor_link = str[start:end] 01553 start = end 01554 end += 1 01555 (self.motion_plan_request.workspace_parameters.workspace_region_shape.type,) = _struct_b.unpack(str[start:end]) 01556 start = end 01557 end += 4 01558 (length,) = _struct_I.unpack(str[start:end]) 01559 pattern = '<%sd'%length 01560 start = end 01561 end += struct.calcsize(pattern) 01562 self.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01563 start = end 01564 end += 4 01565 (length,) = _struct_I.unpack(str[start:end]) 01566 pattern = '<%si'%length 01567 start = end 01568 end += struct.calcsize(pattern) 01569 self.motion_plan_request.workspace_parameters.workspace_region_shape.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 01570 start = end 01571 end += 4 01572 (length,) = _struct_I.unpack(str[start:end]) 01573 self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices = [] 01574 for i in range(0, length): 01575 val1 = geometry_msgs.msg.Point() 01576 _x = val1 01577 start = end 01578 end += 24 01579 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01580 self.motion_plan_request.workspace_parameters.workspace_region_shape.vertices.append(val1) 01581 _x = self 01582 start = end 01583 end += 12 01584 (_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]) 01585 start = end 01586 end += 4 01587 (length,) = _struct_I.unpack(str[start:end]) 01588 start = end 01589 end += length 01590 self.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id = str[start:end] 01591 _x = self 01592 start = end 01593 end += 68 01594 (_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]) 01595 start = end 01596 end += 4 01597 (length,) = _struct_I.unpack(str[start:end]) 01598 start = end 01599 end += length 01600 self.motion_plan_request.start_state.joint_state.header.frame_id = str[start:end] 01601 start = end 01602 end += 4 01603 (length,) = _struct_I.unpack(str[start:end]) 01604 self.motion_plan_request.start_state.joint_state.name = [] 01605 for i in range(0, length): 01606 start = end 01607 end += 4 01608 (length,) = _struct_I.unpack(str[start:end]) 01609 start = end 01610 end += length 01611 val1 = str[start:end] 01612 self.motion_plan_request.start_state.joint_state.name.append(val1) 01613 start = end 01614 end += 4 01615 (length,) = _struct_I.unpack(str[start:end]) 01616 pattern = '<%sd'%length 01617 start = end 01618 end += struct.calcsize(pattern) 01619 self.motion_plan_request.start_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01620 start = end 01621 end += 4 01622 (length,) = _struct_I.unpack(str[start:end]) 01623 pattern = '<%sd'%length 01624 start = end 01625 end += struct.calcsize(pattern) 01626 self.motion_plan_request.start_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01627 start = end 01628 end += 4 01629 (length,) = _struct_I.unpack(str[start:end]) 01630 pattern = '<%sd'%length 01631 start = end 01632 end += struct.calcsize(pattern) 01633 self.motion_plan_request.start_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01634 _x = self 01635 start = end 01636 end += 8 01637 (_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]) 01638 start = end 01639 end += 4 01640 (length,) = _struct_I.unpack(str[start:end]) 01641 self.motion_plan_request.start_state.multi_dof_joint_state.joint_names = [] 01642 for i in range(0, length): 01643 start = end 01644 end += 4 01645 (length,) = _struct_I.unpack(str[start:end]) 01646 start = end 01647 end += length 01648 val1 = str[start:end] 01649 self.motion_plan_request.start_state.multi_dof_joint_state.joint_names.append(val1) 01650 start = end 01651 end += 4 01652 (length,) = _struct_I.unpack(str[start:end]) 01653 self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [] 01654 for i in range(0, length): 01655 start = end 01656 end += 4 01657 (length,) = _struct_I.unpack(str[start:end]) 01658 start = end 01659 end += length 01660 val1 = str[start:end] 01661 self.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append(val1) 01662 start = end 01663 end += 4 01664 (length,) = _struct_I.unpack(str[start:end]) 01665 self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [] 01666 for i in range(0, length): 01667 start = end 01668 end += 4 01669 (length,) = _struct_I.unpack(str[start:end]) 01670 start = end 01671 end += length 01672 val1 = str[start:end] 01673 self.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append(val1) 01674 start = end 01675 end += 4 01676 (length,) = _struct_I.unpack(str[start:end]) 01677 self.motion_plan_request.start_state.multi_dof_joint_state.poses = [] 01678 for i in range(0, length): 01679 val1 = geometry_msgs.msg.Pose() 01680 _v133 = val1.position 01681 _x = _v133 01682 start = end 01683 end += 24 01684 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01685 _v134 = val1.orientation 01686 _x = _v134 01687 start = end 01688 end += 32 01689 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01690 self.motion_plan_request.start_state.multi_dof_joint_state.poses.append(val1) 01691 start = end 01692 end += 4 01693 (length,) = _struct_I.unpack(str[start:end]) 01694 self.motion_plan_request.goal_constraints.joint_constraints = [] 01695 for i in range(0, length): 01696 val1 = arm_navigation_msgs.msg.JointConstraint() 01697 start = end 01698 end += 4 01699 (length,) = _struct_I.unpack(str[start:end]) 01700 start = end 01701 end += length 01702 val1.joint_name = str[start:end] 01703 _x = val1 01704 start = end 01705 end += 32 01706 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 01707 self.motion_plan_request.goal_constraints.joint_constraints.append(val1) 01708 start = end 01709 end += 4 01710 (length,) = _struct_I.unpack(str[start:end]) 01711 self.motion_plan_request.goal_constraints.position_constraints = [] 01712 for i in range(0, length): 01713 val1 = arm_navigation_msgs.msg.PositionConstraint() 01714 _v135 = val1.header 01715 start = end 01716 end += 4 01717 (_v135.seq,) = _struct_I.unpack(str[start:end]) 01718 _v136 = _v135.stamp 01719 _x = _v136 01720 start = end 01721 end += 8 01722 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01723 start = end 01724 end += 4 01725 (length,) = _struct_I.unpack(str[start:end]) 01726 start = end 01727 end += length 01728 _v135.frame_id = str[start:end] 01729 start = end 01730 end += 4 01731 (length,) = _struct_I.unpack(str[start:end]) 01732 start = end 01733 end += length 01734 val1.link_name = str[start:end] 01735 _v137 = val1.target_point_offset 01736 _x = _v137 01737 start = end 01738 end += 24 01739 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01740 _v138 = val1.position 01741 _x = _v138 01742 start = end 01743 end += 24 01744 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01745 _v139 = val1.constraint_region_shape 01746 start = end 01747 end += 1 01748 (_v139.type,) = _struct_b.unpack(str[start:end]) 01749 start = end 01750 end += 4 01751 (length,) = _struct_I.unpack(str[start:end]) 01752 pattern = '<%sd'%length 01753 start = end 01754 end += struct.calcsize(pattern) 01755 _v139.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01756 start = end 01757 end += 4 01758 (length,) = _struct_I.unpack(str[start:end]) 01759 pattern = '<%si'%length 01760 start = end 01761 end += struct.calcsize(pattern) 01762 _v139.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 01763 start = end 01764 end += 4 01765 (length,) = _struct_I.unpack(str[start:end]) 01766 _v139.vertices = [] 01767 for i in range(0, length): 01768 val3 = geometry_msgs.msg.Point() 01769 _x = val3 01770 start = end 01771 end += 24 01772 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01773 _v139.vertices.append(val3) 01774 _v140 = val1.constraint_region_orientation 01775 _x = _v140 01776 start = end 01777 end += 32 01778 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01779 start = end 01780 end += 8 01781 (val1.weight,) = _struct_d.unpack(str[start:end]) 01782 self.motion_plan_request.goal_constraints.position_constraints.append(val1) 01783 start = end 01784 end += 4 01785 (length,) = _struct_I.unpack(str[start:end]) 01786 self.motion_plan_request.goal_constraints.orientation_constraints = [] 01787 for i in range(0, length): 01788 val1 = arm_navigation_msgs.msg.OrientationConstraint() 01789 _v141 = val1.header 01790 start = end 01791 end += 4 01792 (_v141.seq,) = _struct_I.unpack(str[start:end]) 01793 _v142 = _v141.stamp 01794 _x = _v142 01795 start = end 01796 end += 8 01797 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01798 start = end 01799 end += 4 01800 (length,) = _struct_I.unpack(str[start:end]) 01801 start = end 01802 end += length 01803 _v141.frame_id = str[start:end] 01804 start = end 01805 end += 4 01806 (length,) = _struct_I.unpack(str[start:end]) 01807 start = end 01808 end += length 01809 val1.link_name = str[start:end] 01810 start = end 01811 end += 4 01812 (val1.type,) = _struct_i.unpack(str[start:end]) 01813 _v143 = val1.orientation 01814 _x = _v143 01815 start = end 01816 end += 32 01817 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01818 _x = val1 01819 start = end 01820 end += 32 01821 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 01822 self.motion_plan_request.goal_constraints.orientation_constraints.append(val1) 01823 start = end 01824 end += 4 01825 (length,) = _struct_I.unpack(str[start:end]) 01826 self.motion_plan_request.goal_constraints.visibility_constraints = [] 01827 for i in range(0, length): 01828 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 01829 _v144 = val1.header 01830 start = end 01831 end += 4 01832 (_v144.seq,) = _struct_I.unpack(str[start:end]) 01833 _v145 = _v144.stamp 01834 _x = _v145 01835 start = end 01836 end += 8 01837 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01838 start = end 01839 end += 4 01840 (length,) = _struct_I.unpack(str[start:end]) 01841 start = end 01842 end += length 01843 _v144.frame_id = str[start:end] 01844 _v146 = val1.target 01845 _v147 = _v146.header 01846 start = end 01847 end += 4 01848 (_v147.seq,) = _struct_I.unpack(str[start:end]) 01849 _v148 = _v147.stamp 01850 _x = _v148 01851 start = end 01852 end += 8 01853 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01854 start = end 01855 end += 4 01856 (length,) = _struct_I.unpack(str[start:end]) 01857 start = end 01858 end += length 01859 _v147.frame_id = str[start:end] 01860 _v149 = _v146.point 01861 _x = _v149 01862 start = end 01863 end += 24 01864 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01865 _v150 = val1.sensor_pose 01866 _v151 = _v150.header 01867 start = end 01868 end += 4 01869 (_v151.seq,) = _struct_I.unpack(str[start:end]) 01870 _v152 = _v151.stamp 01871 _x = _v152 01872 start = end 01873 end += 8 01874 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01875 start = end 01876 end += 4 01877 (length,) = _struct_I.unpack(str[start:end]) 01878 start = end 01879 end += length 01880 _v151.frame_id = str[start:end] 01881 _v153 = _v150.pose 01882 _v154 = _v153.position 01883 _x = _v154 01884 start = end 01885 end += 24 01886 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01887 _v155 = _v153.orientation 01888 _x = _v155 01889 start = end 01890 end += 32 01891 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01892 start = end 01893 end += 8 01894 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 01895 self.motion_plan_request.goal_constraints.visibility_constraints.append(val1) 01896 start = end 01897 end += 4 01898 (length,) = _struct_I.unpack(str[start:end]) 01899 self.motion_plan_request.path_constraints.joint_constraints = [] 01900 for i in range(0, length): 01901 val1 = arm_navigation_msgs.msg.JointConstraint() 01902 start = end 01903 end += 4 01904 (length,) = _struct_I.unpack(str[start:end]) 01905 start = end 01906 end += length 01907 val1.joint_name = str[start:end] 01908 _x = val1 01909 start = end 01910 end += 32 01911 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 01912 self.motion_plan_request.path_constraints.joint_constraints.append(val1) 01913 start = end 01914 end += 4 01915 (length,) = _struct_I.unpack(str[start:end]) 01916 self.motion_plan_request.path_constraints.position_constraints = [] 01917 for i in range(0, length): 01918 val1 = arm_navigation_msgs.msg.PositionConstraint() 01919 _v156 = val1.header 01920 start = end 01921 end += 4 01922 (_v156.seq,) = _struct_I.unpack(str[start:end]) 01923 _v157 = _v156.stamp 01924 _x = _v157 01925 start = end 01926 end += 8 01927 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01928 start = end 01929 end += 4 01930 (length,) = _struct_I.unpack(str[start:end]) 01931 start = end 01932 end += length 01933 _v156.frame_id = str[start:end] 01934 start = end 01935 end += 4 01936 (length,) = _struct_I.unpack(str[start:end]) 01937 start = end 01938 end += length 01939 val1.link_name = str[start:end] 01940 _v158 = val1.target_point_offset 01941 _x = _v158 01942 start = end 01943 end += 24 01944 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01945 _v159 = val1.position 01946 _x = _v159 01947 start = end 01948 end += 24 01949 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01950 _v160 = val1.constraint_region_shape 01951 start = end 01952 end += 1 01953 (_v160.type,) = _struct_b.unpack(str[start:end]) 01954 start = end 01955 end += 4 01956 (length,) = _struct_I.unpack(str[start:end]) 01957 pattern = '<%sd'%length 01958 start = end 01959 end += struct.calcsize(pattern) 01960 _v160.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01961 start = end 01962 end += 4 01963 (length,) = _struct_I.unpack(str[start:end]) 01964 pattern = '<%si'%length 01965 start = end 01966 end += struct.calcsize(pattern) 01967 _v160.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 01968 start = end 01969 end += 4 01970 (length,) = _struct_I.unpack(str[start:end]) 01971 _v160.vertices = [] 01972 for i in range(0, length): 01973 val3 = geometry_msgs.msg.Point() 01974 _x = val3 01975 start = end 01976 end += 24 01977 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01978 _v160.vertices.append(val3) 01979 _v161 = val1.constraint_region_orientation 01980 _x = _v161 01981 start = end 01982 end += 32 01983 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01984 start = end 01985 end += 8 01986 (val1.weight,) = _struct_d.unpack(str[start:end]) 01987 self.motion_plan_request.path_constraints.position_constraints.append(val1) 01988 start = end 01989 end += 4 01990 (length,) = _struct_I.unpack(str[start:end]) 01991 self.motion_plan_request.path_constraints.orientation_constraints = [] 01992 for i in range(0, length): 01993 val1 = arm_navigation_msgs.msg.OrientationConstraint() 01994 _v162 = val1.header 01995 start = end 01996 end += 4 01997 (_v162.seq,) = _struct_I.unpack(str[start:end]) 01998 _v163 = _v162.stamp 01999 _x = _v163 02000 start = end 02001 end += 8 02002 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02003 start = end 02004 end += 4 02005 (length,) = _struct_I.unpack(str[start:end]) 02006 start = end 02007 end += length 02008 _v162.frame_id = str[start:end] 02009 start = end 02010 end += 4 02011 (length,) = _struct_I.unpack(str[start:end]) 02012 start = end 02013 end += length 02014 val1.link_name = str[start:end] 02015 start = end 02016 end += 4 02017 (val1.type,) = _struct_i.unpack(str[start:end]) 02018 _v164 = val1.orientation 02019 _x = _v164 02020 start = end 02021 end += 32 02022 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02023 _x = val1 02024 start = end 02025 end += 32 02026 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 02027 self.motion_plan_request.path_constraints.orientation_constraints.append(val1) 02028 start = end 02029 end += 4 02030 (length,) = _struct_I.unpack(str[start:end]) 02031 self.motion_plan_request.path_constraints.visibility_constraints = [] 02032 for i in range(0, length): 02033 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 02034 _v165 = val1.header 02035 start = end 02036 end += 4 02037 (_v165.seq,) = _struct_I.unpack(str[start:end]) 02038 _v166 = _v165.stamp 02039 _x = _v166 02040 start = end 02041 end += 8 02042 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02043 start = end 02044 end += 4 02045 (length,) = _struct_I.unpack(str[start:end]) 02046 start = end 02047 end += length 02048 _v165.frame_id = str[start:end] 02049 _v167 = val1.target 02050 _v168 = _v167.header 02051 start = end 02052 end += 4 02053 (_v168.seq,) = _struct_I.unpack(str[start:end]) 02054 _v169 = _v168.stamp 02055 _x = _v169 02056 start = end 02057 end += 8 02058 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02059 start = end 02060 end += 4 02061 (length,) = _struct_I.unpack(str[start:end]) 02062 start = end 02063 end += length 02064 _v168.frame_id = str[start:end] 02065 _v170 = _v167.point 02066 _x = _v170 02067 start = end 02068 end += 24 02069 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02070 _v171 = val1.sensor_pose 02071 _v172 = _v171.header 02072 start = end 02073 end += 4 02074 (_v172.seq,) = _struct_I.unpack(str[start:end]) 02075 _v173 = _v172.stamp 02076 _x = _v173 02077 start = end 02078 end += 8 02079 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02080 start = end 02081 end += 4 02082 (length,) = _struct_I.unpack(str[start:end]) 02083 start = end 02084 end += length 02085 _v172.frame_id = str[start:end] 02086 _v174 = _v171.pose 02087 _v175 = _v174.position 02088 _x = _v175 02089 start = end 02090 end += 24 02091 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02092 _v176 = _v174.orientation 02093 _x = _v176 02094 start = end 02095 end += 32 02096 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02097 start = end 02098 end += 8 02099 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 02100 self.motion_plan_request.path_constraints.visibility_constraints.append(val1) 02101 start = end 02102 end += 4 02103 (length,) = _struct_I.unpack(str[start:end]) 02104 start = end 02105 end += length 02106 self.motion_plan_request.planner_id = str[start:end] 02107 start = end 02108 end += 4 02109 (length,) = _struct_I.unpack(str[start:end]) 02110 start = end 02111 end += length 02112 self.motion_plan_request.group_name = str[start:end] 02113 _x = self 02114 start = end 02115 end += 28 02116 (_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]) 02117 return self 02118 except struct.error as e: 02119 raise roslib.message.DeserializationError(e) #most likely buffer underfill 02120 02121 _struct_I = roslib.message.struct_I 02122 _struct_b = struct.Struct("<b") 02123 _struct_d = struct.Struct("<d") 02124 _struct_7i = struct.Struct("<7i") 02125 _struct_2I = struct.Struct("<2I") 02126 _struct_i = struct.Struct("<i") 02127 _struct_3I = struct.Struct("<3I") 02128 _struct_4d = struct.Struct("<4d") 02129 _struct_7d3I = struct.Struct("<7d3I") 02130 _struct_3d = struct.Struct("<3d")