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