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