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