$search
00001 """autogenerated by genmsg_py from WorldState.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import geometry_msgs.msg 00006 import std_msgs.msg 00007 00008 class WorldState(roslib.message.Message): 00009 _md5sum = "de1a9de3ab7ba97ac0e9ec01a4eb481e" 00010 _type = "gazebo/WorldState" 00011 _has_header = True #flag to mark the presence of a Header object 00012 _full_text = """#This message is deprecated. Please use the version in gazebo_msgs instead. 00013 00014 # This is a message that holds data necessary to reconstruct a snapshot of the world 00015 # 00016 # = Approach to Message Passing = 00017 # The state of the world is defined by either 00018 # 1. Inertial Model pose, twist 00019 # * kinematic data - connectivity graph from Model to each Link 00020 # * joint angles 00021 # * joint velocities 00022 # * Applied forces - Body wrench 00023 # * relative transform from Body to each collision Geom 00024 # Or 00025 # 2. Inertial (absolute) Body pose, twist, wrench 00026 # * relative transform from Body to each collision Geom - constant, so not sent over wire 00027 # * back compute from canonical body info to get Model pose and twist. 00028 # 00029 # Chooing (2.) because it matches most physics engines out there 00030 # and is simpler. 00031 # 00032 # = Future = 00033 # Consider impacts on using reduced coordinates / graph (parent/child links) approach 00034 # constraint and physics solvers. 00035 # 00036 # = Application = 00037 # This message is used to do the following: 00038 # * reconstruct the world and objects for sensor generation 00039 # * stop / start simulation - need pose, twist, wrench of each body 00040 # * collision detection - need pose of each collision geometry. velocity/acceleration if 00041 # 00042 # = Assumptions = 00043 # Assuming that each (physics) processor node locally already has 00044 # * collision information - Trimesh for Geoms, etc 00045 # * relative transforms from Body to Geom - this is assumed to be fixed, do not send oved wire 00046 # * inertial information - does not vary in time 00047 # * visual information - does not vary in time 00048 # 00049 00050 Header header 00051 00052 string[] name 00053 geometry_msgs/Pose[] pose 00054 geometry_msgs/Twist[] twist 00055 geometry_msgs/Wrench[] wrench 00056 00057 ================================================================================ 00058 MSG: std_msgs/Header 00059 # Standard metadata for higher-level stamped data types. 00060 # This is generally used to communicate timestamped data 00061 # in a particular coordinate frame. 00062 # 00063 # sequence ID: consecutively increasing ID 00064 uint32 seq 00065 #Two-integer timestamp that is expressed as: 00066 # * stamp.secs: seconds (stamp_secs) since epoch 00067 # * stamp.nsecs: nanoseconds since stamp_secs 00068 # time-handling sugar is provided by the client library 00069 time stamp 00070 #Frame this data is associated with 00071 # 0: no frame 00072 # 1: global frame 00073 string frame_id 00074 00075 ================================================================================ 00076 MSG: geometry_msgs/Pose 00077 # A representation of pose in free space, composed of postion and orientation. 00078 Point position 00079 Quaternion orientation 00080 00081 ================================================================================ 00082 MSG: geometry_msgs/Point 00083 # This contains the position of a point in free space 00084 float64 x 00085 float64 y 00086 float64 z 00087 00088 ================================================================================ 00089 MSG: geometry_msgs/Quaternion 00090 # This represents an orientation in free space in quaternion form. 00091 00092 float64 x 00093 float64 y 00094 float64 z 00095 float64 w 00096 00097 ================================================================================ 00098 MSG: geometry_msgs/Twist 00099 # This expresses velocity in free space broken into it's linear and angular parts. 00100 Vector3 linear 00101 Vector3 angular 00102 00103 ================================================================================ 00104 MSG: geometry_msgs/Vector3 00105 # This represents a vector in free space. 00106 00107 float64 x 00108 float64 y 00109 float64 z 00110 ================================================================================ 00111 MSG: geometry_msgs/Wrench 00112 # This represents force in free space, seperated into 00113 # it's linear and angular parts. 00114 Vector3 force 00115 Vector3 torque 00116 00117 """ 00118 __slots__ = ['header','name','pose','twist','wrench'] 00119 _slot_types = ['Header','string[]','geometry_msgs/Pose[]','geometry_msgs/Twist[]','geometry_msgs/Wrench[]'] 00120 00121 def __init__(self, *args, **kwds): 00122 """ 00123 Constructor. Any message fields that are implicitly/explicitly 00124 set to None will be assigned a default value. The recommend 00125 use is keyword arguments as this is more robust to future message 00126 changes. You cannot mix in-order arguments and keyword arguments. 00127 00128 The available fields are: 00129 header,name,pose,twist,wrench 00130 00131 @param args: complete set of field values, in .msg order 00132 @param kwds: use keyword arguments corresponding to message field names 00133 to set specific fields. 00134 """ 00135 if args or kwds: 00136 super(WorldState, self).__init__(*args, **kwds) 00137 #message fields cannot be None, assign default values for those that are 00138 if self.header is None: 00139 self.header = std_msgs.msg._Header.Header() 00140 if self.name is None: 00141 self.name = [] 00142 if self.pose is None: 00143 self.pose = [] 00144 if self.twist is None: 00145 self.twist = [] 00146 if self.wrench is None: 00147 self.wrench = [] 00148 else: 00149 self.header = std_msgs.msg._Header.Header() 00150 self.name = [] 00151 self.pose = [] 00152 self.twist = [] 00153 self.wrench = [] 00154 00155 def _get_types(self): 00156 """ 00157 internal API method 00158 """ 00159 return self._slot_types 00160 00161 def serialize(self, buff): 00162 """ 00163 serialize message into buffer 00164 @param buff: buffer 00165 @type buff: StringIO 00166 """ 00167 try: 00168 _x = self 00169 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00170 _x = self.header.frame_id 00171 length = len(_x) 00172 buff.write(struct.pack('<I%ss'%length, length, _x)) 00173 length = len(self.name) 00174 buff.write(_struct_I.pack(length)) 00175 for val1 in self.name: 00176 length = len(val1) 00177 buff.write(struct.pack('<I%ss'%length, length, val1)) 00178 length = len(self.pose) 00179 buff.write(_struct_I.pack(length)) 00180 for val1 in self.pose: 00181 _v1 = val1.position 00182 _x = _v1 00183 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00184 _v2 = val1.orientation 00185 _x = _v2 00186 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00187 length = len(self.twist) 00188 buff.write(_struct_I.pack(length)) 00189 for val1 in self.twist: 00190 _v3 = val1.linear 00191 _x = _v3 00192 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00193 _v4 = val1.angular 00194 _x = _v4 00195 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00196 length = len(self.wrench) 00197 buff.write(_struct_I.pack(length)) 00198 for val1 in self.wrench: 00199 _v5 = val1.force 00200 _x = _v5 00201 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00202 _v6 = val1.torque 00203 _x = _v6 00204 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00205 except struct.error as se: self._check_types(se) 00206 except TypeError as te: self._check_types(te) 00207 00208 def deserialize(self, str): 00209 """ 00210 unpack serialized message in str into this message instance 00211 @param str: byte array of serialized message 00212 @type str: str 00213 """ 00214 try: 00215 if self.header is None: 00216 self.header = std_msgs.msg._Header.Header() 00217 end = 0 00218 _x = self 00219 start = end 00220 end += 12 00221 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00222 start = end 00223 end += 4 00224 (length,) = _struct_I.unpack(str[start:end]) 00225 start = end 00226 end += length 00227 self.header.frame_id = str[start:end] 00228 start = end 00229 end += 4 00230 (length,) = _struct_I.unpack(str[start:end]) 00231 self.name = [] 00232 for i in range(0, length): 00233 start = end 00234 end += 4 00235 (length,) = _struct_I.unpack(str[start:end]) 00236 start = end 00237 end += length 00238 val1 = str[start:end] 00239 self.name.append(val1) 00240 start = end 00241 end += 4 00242 (length,) = _struct_I.unpack(str[start:end]) 00243 self.pose = [] 00244 for i in range(0, length): 00245 val1 = geometry_msgs.msg.Pose() 00246 _v7 = val1.position 00247 _x = _v7 00248 start = end 00249 end += 24 00250 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00251 _v8 = val1.orientation 00252 _x = _v8 00253 start = end 00254 end += 32 00255 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00256 self.pose.append(val1) 00257 start = end 00258 end += 4 00259 (length,) = _struct_I.unpack(str[start:end]) 00260 self.twist = [] 00261 for i in range(0, length): 00262 val1 = geometry_msgs.msg.Twist() 00263 _v9 = val1.linear 00264 _x = _v9 00265 start = end 00266 end += 24 00267 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00268 _v10 = val1.angular 00269 _x = _v10 00270 start = end 00271 end += 24 00272 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00273 self.twist.append(val1) 00274 start = end 00275 end += 4 00276 (length,) = _struct_I.unpack(str[start:end]) 00277 self.wrench = [] 00278 for i in range(0, length): 00279 val1 = geometry_msgs.msg.Wrench() 00280 _v11 = val1.force 00281 _x = _v11 00282 start = end 00283 end += 24 00284 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00285 _v12 = val1.torque 00286 _x = _v12 00287 start = end 00288 end += 24 00289 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00290 self.wrench.append(val1) 00291 return self 00292 except struct.error as e: 00293 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00294 00295 00296 def serialize_numpy(self, buff, numpy): 00297 """ 00298 serialize message with numpy array types into buffer 00299 @param buff: buffer 00300 @type buff: StringIO 00301 @param numpy: numpy python module 00302 @type numpy module 00303 """ 00304 try: 00305 _x = self 00306 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00307 _x = self.header.frame_id 00308 length = len(_x) 00309 buff.write(struct.pack('<I%ss'%length, length, _x)) 00310 length = len(self.name) 00311 buff.write(_struct_I.pack(length)) 00312 for val1 in self.name: 00313 length = len(val1) 00314 buff.write(struct.pack('<I%ss'%length, length, val1)) 00315 length = len(self.pose) 00316 buff.write(_struct_I.pack(length)) 00317 for val1 in self.pose: 00318 _v13 = val1.position 00319 _x = _v13 00320 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00321 _v14 = val1.orientation 00322 _x = _v14 00323 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00324 length = len(self.twist) 00325 buff.write(_struct_I.pack(length)) 00326 for val1 in self.twist: 00327 _v15 = val1.linear 00328 _x = _v15 00329 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00330 _v16 = val1.angular 00331 _x = _v16 00332 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00333 length = len(self.wrench) 00334 buff.write(_struct_I.pack(length)) 00335 for val1 in self.wrench: 00336 _v17 = val1.force 00337 _x = _v17 00338 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00339 _v18 = val1.torque 00340 _x = _v18 00341 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00342 except struct.error as se: self._check_types(se) 00343 except TypeError as te: self._check_types(te) 00344 00345 def deserialize_numpy(self, str, numpy): 00346 """ 00347 unpack serialized message in str into this message instance using numpy for array types 00348 @param str: byte array of serialized message 00349 @type str: str 00350 @param numpy: numpy python module 00351 @type numpy: module 00352 """ 00353 try: 00354 if self.header is None: 00355 self.header = std_msgs.msg._Header.Header() 00356 end = 0 00357 _x = self 00358 start = end 00359 end += 12 00360 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00361 start = end 00362 end += 4 00363 (length,) = _struct_I.unpack(str[start:end]) 00364 start = end 00365 end += length 00366 self.header.frame_id = str[start:end] 00367 start = end 00368 end += 4 00369 (length,) = _struct_I.unpack(str[start:end]) 00370 self.name = [] 00371 for i in range(0, length): 00372 start = end 00373 end += 4 00374 (length,) = _struct_I.unpack(str[start:end]) 00375 start = end 00376 end += length 00377 val1 = str[start:end] 00378 self.name.append(val1) 00379 start = end 00380 end += 4 00381 (length,) = _struct_I.unpack(str[start:end]) 00382 self.pose = [] 00383 for i in range(0, length): 00384 val1 = geometry_msgs.msg.Pose() 00385 _v19 = val1.position 00386 _x = _v19 00387 start = end 00388 end += 24 00389 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00390 _v20 = val1.orientation 00391 _x = _v20 00392 start = end 00393 end += 32 00394 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00395 self.pose.append(val1) 00396 start = end 00397 end += 4 00398 (length,) = _struct_I.unpack(str[start:end]) 00399 self.twist = [] 00400 for i in range(0, length): 00401 val1 = geometry_msgs.msg.Twist() 00402 _v21 = val1.linear 00403 _x = _v21 00404 start = end 00405 end += 24 00406 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00407 _v22 = val1.angular 00408 _x = _v22 00409 start = end 00410 end += 24 00411 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00412 self.twist.append(val1) 00413 start = end 00414 end += 4 00415 (length,) = _struct_I.unpack(str[start:end]) 00416 self.wrench = [] 00417 for i in range(0, length): 00418 val1 = geometry_msgs.msg.Wrench() 00419 _v23 = val1.force 00420 _x = _v23 00421 start = end 00422 end += 24 00423 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00424 _v24 = val1.torque 00425 _x = _v24 00426 start = end 00427 end += 24 00428 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00429 self.wrench.append(val1) 00430 return self 00431 except struct.error as e: 00432 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00433 00434 _struct_I = roslib.message.struct_I 00435 _struct_3I = struct.Struct("<3I") 00436 _struct_4d = struct.Struct("<4d") 00437 _struct_3d = struct.Struct("<3d")