$search
00001 """autogenerated by genmsg_py from Grasp.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import geometry_msgs.msg 00006 import srs_msgs.msg 00007 import std_msgs.msg 00008 00009 class Grasp(roslib.message.Message): 00010 _md5sum = "b4ba413db436f6f49754dff4bc4a1739" 00011 _type = "srs_msgs/Grasp" 00012 _has_header = False #flag to mark the presence of a Header object 00013 _full_text = """srs_msgs/DBGrasp[] grasp_configuration 00014 00015 ================================================================================ 00016 MSG: srs_msgs/DBGrasp 00017 int32 object_id 00018 string hand_type 00019 float64[] sdh_joint_values 00020 geometry_msgs/PoseStamped pre_grasp 00021 geometry_msgs/PoseStamped grasp 00022 string category 00023 00024 ================================================================================ 00025 MSG: geometry_msgs/PoseStamped 00026 # A Pose with reference coordinate frame and timestamp 00027 Header header 00028 Pose pose 00029 00030 ================================================================================ 00031 MSG: std_msgs/Header 00032 # Standard metadata for higher-level stamped data types. 00033 # This is generally used to communicate timestamped data 00034 # in a particular coordinate frame. 00035 # 00036 # sequence ID: consecutively increasing ID 00037 uint32 seq 00038 #Two-integer timestamp that is expressed as: 00039 # * stamp.secs: seconds (stamp_secs) since epoch 00040 # * stamp.nsecs: nanoseconds since stamp_secs 00041 # time-handling sugar is provided by the client library 00042 time stamp 00043 #Frame this data is associated with 00044 # 0: no frame 00045 # 1: global frame 00046 string frame_id 00047 00048 ================================================================================ 00049 MSG: geometry_msgs/Pose 00050 # A representation of pose in free space, composed of postion and orientation. 00051 Point position 00052 Quaternion orientation 00053 00054 ================================================================================ 00055 MSG: geometry_msgs/Point 00056 # This contains the position of a point in free space 00057 float64 x 00058 float64 y 00059 float64 z 00060 00061 ================================================================================ 00062 MSG: geometry_msgs/Quaternion 00063 # This represents an orientation in free space in quaternion form. 00064 00065 float64 x 00066 float64 y 00067 float64 z 00068 float64 w 00069 00070 """ 00071 __slots__ = ['grasp_configuration'] 00072 _slot_types = ['srs_msgs/DBGrasp[]'] 00073 00074 def __init__(self, *args, **kwds): 00075 """ 00076 Constructor. Any message fields that are implicitly/explicitly 00077 set to None will be assigned a default value. The recommend 00078 use is keyword arguments as this is more robust to future message 00079 changes. You cannot mix in-order arguments and keyword arguments. 00080 00081 The available fields are: 00082 grasp_configuration 00083 00084 @param args: complete set of field values, in .msg order 00085 @param kwds: use keyword arguments corresponding to message field names 00086 to set specific fields. 00087 """ 00088 if args or kwds: 00089 super(Grasp, self).__init__(*args, **kwds) 00090 #message fields cannot be None, assign default values for those that are 00091 if self.grasp_configuration is None: 00092 self.grasp_configuration = [] 00093 else: 00094 self.grasp_configuration = [] 00095 00096 def _get_types(self): 00097 """ 00098 internal API method 00099 """ 00100 return self._slot_types 00101 00102 def serialize(self, buff): 00103 """ 00104 serialize message into buffer 00105 @param buff: buffer 00106 @type buff: StringIO 00107 """ 00108 try: 00109 length = len(self.grasp_configuration) 00110 buff.write(_struct_I.pack(length)) 00111 for val1 in self.grasp_configuration: 00112 buff.write(_struct_i.pack(val1.object_id)) 00113 _x = val1.hand_type 00114 length = len(_x) 00115 buff.write(struct.pack('<I%ss'%length, length, _x)) 00116 length = len(val1.sdh_joint_values) 00117 buff.write(_struct_I.pack(length)) 00118 pattern = '<%sd'%length 00119 buff.write(struct.pack(pattern, *val1.sdh_joint_values)) 00120 _v1 = val1.pre_grasp 00121 _v2 = _v1.header 00122 buff.write(_struct_I.pack(_v2.seq)) 00123 _v3 = _v2.stamp 00124 _x = _v3 00125 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00126 _x = _v2.frame_id 00127 length = len(_x) 00128 buff.write(struct.pack('<I%ss'%length, length, _x)) 00129 _v4 = _v1.pose 00130 _v5 = _v4.position 00131 _x = _v5 00132 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00133 _v6 = _v4.orientation 00134 _x = _v6 00135 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00136 _v7 = val1.grasp 00137 _v8 = _v7.header 00138 buff.write(_struct_I.pack(_v8.seq)) 00139 _v9 = _v8.stamp 00140 _x = _v9 00141 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00142 _x = _v8.frame_id 00143 length = len(_x) 00144 buff.write(struct.pack('<I%ss'%length, length, _x)) 00145 _v10 = _v7.pose 00146 _v11 = _v10.position 00147 _x = _v11 00148 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00149 _v12 = _v10.orientation 00150 _x = _v12 00151 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00152 _x = val1.category 00153 length = len(_x) 00154 buff.write(struct.pack('<I%ss'%length, length, _x)) 00155 except struct.error as se: self._check_types(se) 00156 except TypeError as te: self._check_types(te) 00157 00158 def deserialize(self, str): 00159 """ 00160 unpack serialized message in str into this message instance 00161 @param str: byte array of serialized message 00162 @type str: str 00163 """ 00164 try: 00165 end = 0 00166 start = end 00167 end += 4 00168 (length,) = _struct_I.unpack(str[start:end]) 00169 self.grasp_configuration = [] 00170 for i in range(0, length): 00171 val1 = srs_msgs.msg.DBGrasp() 00172 start = end 00173 end += 4 00174 (val1.object_id,) = _struct_i.unpack(str[start:end]) 00175 start = end 00176 end += 4 00177 (length,) = _struct_I.unpack(str[start:end]) 00178 start = end 00179 end += length 00180 val1.hand_type = str[start:end] 00181 start = end 00182 end += 4 00183 (length,) = _struct_I.unpack(str[start:end]) 00184 pattern = '<%sd'%length 00185 start = end 00186 end += struct.calcsize(pattern) 00187 val1.sdh_joint_values = struct.unpack(pattern, str[start:end]) 00188 _v13 = val1.pre_grasp 00189 _v14 = _v13.header 00190 start = end 00191 end += 4 00192 (_v14.seq,) = _struct_I.unpack(str[start:end]) 00193 _v15 = _v14.stamp 00194 _x = _v15 00195 start = end 00196 end += 8 00197 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00198 start = end 00199 end += 4 00200 (length,) = _struct_I.unpack(str[start:end]) 00201 start = end 00202 end += length 00203 _v14.frame_id = str[start:end] 00204 _v16 = _v13.pose 00205 _v17 = _v16.position 00206 _x = _v17 00207 start = end 00208 end += 24 00209 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00210 _v18 = _v16.orientation 00211 _x = _v18 00212 start = end 00213 end += 32 00214 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00215 _v19 = val1.grasp 00216 _v20 = _v19.header 00217 start = end 00218 end += 4 00219 (_v20.seq,) = _struct_I.unpack(str[start:end]) 00220 _v21 = _v20.stamp 00221 _x = _v21 00222 start = end 00223 end += 8 00224 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00225 start = end 00226 end += 4 00227 (length,) = _struct_I.unpack(str[start:end]) 00228 start = end 00229 end += length 00230 _v20.frame_id = str[start:end] 00231 _v22 = _v19.pose 00232 _v23 = _v22.position 00233 _x = _v23 00234 start = end 00235 end += 24 00236 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00237 _v24 = _v22.orientation 00238 _x = _v24 00239 start = end 00240 end += 32 00241 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00242 start = end 00243 end += 4 00244 (length,) = _struct_I.unpack(str[start:end]) 00245 start = end 00246 end += length 00247 val1.category = str[start:end] 00248 self.grasp_configuration.append(val1) 00249 return self 00250 except struct.error as e: 00251 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00252 00253 00254 def serialize_numpy(self, buff, numpy): 00255 """ 00256 serialize message with numpy array types into buffer 00257 @param buff: buffer 00258 @type buff: StringIO 00259 @param numpy: numpy python module 00260 @type numpy module 00261 """ 00262 try: 00263 length = len(self.grasp_configuration) 00264 buff.write(_struct_I.pack(length)) 00265 for val1 in self.grasp_configuration: 00266 buff.write(_struct_i.pack(val1.object_id)) 00267 _x = val1.hand_type 00268 length = len(_x) 00269 buff.write(struct.pack('<I%ss'%length, length, _x)) 00270 length = len(val1.sdh_joint_values) 00271 buff.write(_struct_I.pack(length)) 00272 pattern = '<%sd'%length 00273 buff.write(val1.sdh_joint_values.tostring()) 00274 _v25 = val1.pre_grasp 00275 _v26 = _v25.header 00276 buff.write(_struct_I.pack(_v26.seq)) 00277 _v27 = _v26.stamp 00278 _x = _v27 00279 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00280 _x = _v26.frame_id 00281 length = len(_x) 00282 buff.write(struct.pack('<I%ss'%length, length, _x)) 00283 _v28 = _v25.pose 00284 _v29 = _v28.position 00285 _x = _v29 00286 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00287 _v30 = _v28.orientation 00288 _x = _v30 00289 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00290 _v31 = val1.grasp 00291 _v32 = _v31.header 00292 buff.write(_struct_I.pack(_v32.seq)) 00293 _v33 = _v32.stamp 00294 _x = _v33 00295 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00296 _x = _v32.frame_id 00297 length = len(_x) 00298 buff.write(struct.pack('<I%ss'%length, length, _x)) 00299 _v34 = _v31.pose 00300 _v35 = _v34.position 00301 _x = _v35 00302 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00303 _v36 = _v34.orientation 00304 _x = _v36 00305 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00306 _x = val1.category 00307 length = len(_x) 00308 buff.write(struct.pack('<I%ss'%length, length, _x)) 00309 except struct.error as se: self._check_types(se) 00310 except TypeError as te: self._check_types(te) 00311 00312 def deserialize_numpy(self, str, numpy): 00313 """ 00314 unpack serialized message in str into this message instance using numpy for array types 00315 @param str: byte array of serialized message 00316 @type str: str 00317 @param numpy: numpy python module 00318 @type numpy: module 00319 """ 00320 try: 00321 end = 0 00322 start = end 00323 end += 4 00324 (length,) = _struct_I.unpack(str[start:end]) 00325 self.grasp_configuration = [] 00326 for i in range(0, length): 00327 val1 = srs_msgs.msg.DBGrasp() 00328 start = end 00329 end += 4 00330 (val1.object_id,) = _struct_i.unpack(str[start:end]) 00331 start = end 00332 end += 4 00333 (length,) = _struct_I.unpack(str[start:end]) 00334 start = end 00335 end += length 00336 val1.hand_type = str[start:end] 00337 start = end 00338 end += 4 00339 (length,) = _struct_I.unpack(str[start:end]) 00340 pattern = '<%sd'%length 00341 start = end 00342 end += struct.calcsize(pattern) 00343 val1.sdh_joint_values = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00344 _v37 = val1.pre_grasp 00345 _v38 = _v37.header 00346 start = end 00347 end += 4 00348 (_v38.seq,) = _struct_I.unpack(str[start:end]) 00349 _v39 = _v38.stamp 00350 _x = _v39 00351 start = end 00352 end += 8 00353 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00354 start = end 00355 end += 4 00356 (length,) = _struct_I.unpack(str[start:end]) 00357 start = end 00358 end += length 00359 _v38.frame_id = str[start:end] 00360 _v40 = _v37.pose 00361 _v41 = _v40.position 00362 _x = _v41 00363 start = end 00364 end += 24 00365 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00366 _v42 = _v40.orientation 00367 _x = _v42 00368 start = end 00369 end += 32 00370 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00371 _v43 = val1.grasp 00372 _v44 = _v43.header 00373 start = end 00374 end += 4 00375 (_v44.seq,) = _struct_I.unpack(str[start:end]) 00376 _v45 = _v44.stamp 00377 _x = _v45 00378 start = end 00379 end += 8 00380 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00381 start = end 00382 end += 4 00383 (length,) = _struct_I.unpack(str[start:end]) 00384 start = end 00385 end += length 00386 _v44.frame_id = str[start:end] 00387 _v46 = _v43.pose 00388 _v47 = _v46.position 00389 _x = _v47 00390 start = end 00391 end += 24 00392 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00393 _v48 = _v46.orientation 00394 _x = _v48 00395 start = end 00396 end += 32 00397 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00398 start = end 00399 end += 4 00400 (length,) = _struct_I.unpack(str[start:end]) 00401 start = end 00402 end += length 00403 val1.category = str[start:end] 00404 self.grasp_configuration.append(val1) 00405 return self 00406 except struct.error as e: 00407 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00408 00409 _struct_I = roslib.message.struct_I 00410 _struct_i = struct.Struct("<i") 00411 _struct_4d = struct.Struct("<4d") 00412 _struct_2I = struct.Struct("<2I") 00413 _struct_3d = struct.Struct("<3d")