00001 """autogenerated by genmsg_py from Observers.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import geometry_msgs.msg
00006 import art_msgs.msg
00007 import nav_msgs.msg
00008 import std_msgs.msg
00009
00010 class Observers(roslib.message.Message):
00011 _md5sum = "0a372cf6f0b7d6ece7528e8f1e5e6eab"
00012 _type = "art_msgs/Observers"
00013 _has_header = True
00014 _full_text = """# Observations from a set of observers
00015 # TODO: move to a separate art_observers package
00016 # $Id: Observers.msg 644 2010-09-28 03:19:07Z jack.oquin $
00017
00018 # list of observer ID values
00019 int32 Nearest_forward = 0 # current or closest lane
00020 int32 Nearest_backward = 1
00021 int32 Adjacent_left = 2 # adjacent lane to left
00022 int32 Adjacent_right = 3 # adjacent lane to right
00023 int32 All_left = 4 # all lanes to left
00024 int32 All_right = 5 # all lanes to right
00025 int32 Change_lanes = 6 # unused
00026 int32 Merge_into_nearest = 7 # merge into nearest lane on diff seg
00027 int32 Merge_across_all = 8 # merge across all lanes on diff seg
00028 int32 Intersection = 9 # intersection precedence
00029 int32 N_Observers = 10
00030
00031 Header header
00032 nav_msgs/Odometry odom # position when observations made
00033 Observation[] obs # vector of observations
00034
00035 ================================================================================
00036 MSG: std_msgs/Header
00037 # Standard metadata for higher-level stamped data types.
00038 # This is generally used to communicate timestamped data
00039 # in a particular coordinate frame.
00040 #
00041 # sequence ID: consecutively increasing ID
00042 uint32 seq
00043 #Two-integer timestamp that is expressed as:
00044 # * stamp.secs: seconds (stamp_secs) since epoch
00045 # * stamp.nsecs: nanoseconds since stamp_secs
00046 # time-handling sugar is provided by the client library
00047 time stamp
00048 #Frame this data is associated with
00049 # 0: no frame
00050 # 1: global frame
00051 string frame_id
00052
00053 ================================================================================
00054 MSG: nav_msgs/Odometry
00055 # This represents an estimate of a position and velocity in free space.
00056 # The pose in this message should be specified in the coordinate frame given by header.frame_id.
00057 # The twist in this message should be specified in the coordinate frame given by the child_frame_id
00058 Header header
00059 string child_frame_id
00060 geometry_msgs/PoseWithCovariance pose
00061 geometry_msgs/TwistWithCovariance twist
00062
00063 ================================================================================
00064 MSG: geometry_msgs/PoseWithCovariance
00065 # This represents a pose in free space with uncertainty.
00066
00067 Pose pose
00068
00069 # Row-major representation of the 6x6 covariance matrix
00070 # The orientation parameters use a fixed-axis representation.
00071 # In order, the parameters are:
00072 # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
00073 float64[36] covariance
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/TwistWithCovariance
00099 # This expresses velocity in free space with uncertianty.
00100
00101 Twist twist
00102
00103 # Row-major representation of the 6x6 covariance matrix
00104 # The orientation parameters use a fixed-axis representation.
00105 # In order, the parameters are:
00106 # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
00107 float64[36] covariance
00108
00109 ================================================================================
00110 MSG: geometry_msgs/Twist
00111 # This expresses velocity in free space broken into it's linear and angular parts.
00112 Vector3 linear
00113 Vector3 angular
00114
00115 ================================================================================
00116 MSG: geometry_msgs/Vector3
00117 # This represents a vector in free space.
00118
00119 float64 x
00120 float64 y
00121 float64 z
00122 ================================================================================
00123 MSG: art_msgs/Observation
00124 # result returned from a single observer
00125 # TODO: move to a separate art_observers package
00126 # $Id: Observation.msg 644 2010-09-28 03:19:07Z jack.oquin $
00127
00128 int32 oid # observer ID
00129 string name # observer name
00130
00131 bool applicable # true if obseravation is applicable
00132 bool clear # true if clear to go
00133
00134 # optional time and distance to nearest obstacle
00135 # (only reported by some observers)
00136 float32 time
00137 float32 distance
00138 float32 velocity
00139 int32 nobjects # number of objects
00140
00141 """
00142
00143 Nearest_forward = 0
00144 Nearest_backward = 1
00145 Adjacent_left = 2
00146 Adjacent_right = 3
00147 All_left = 4
00148 All_right = 5
00149 Change_lanes = 6
00150 Merge_into_nearest = 7
00151 Merge_across_all = 8
00152 Intersection = 9
00153 N_Observers = 10
00154
00155 __slots__ = ['header','odom','obs']
00156 _slot_types = ['Header','nav_msgs/Odometry','art_msgs/Observation[]']
00157
00158 def __init__(self, *args, **kwds):
00159 """
00160 Constructor. Any message fields that are implicitly/explicitly
00161 set to None will be assigned a default value. The recommend
00162 use is keyword arguments as this is more robust to future message
00163 changes. You cannot mix in-order arguments and keyword arguments.
00164
00165 The available fields are:
00166 header,odom,obs
00167
00168 @param args: complete set of field values, in .msg order
00169 @param kwds: use keyword arguments corresponding to message field names
00170 to set specific fields.
00171 """
00172 if args or kwds:
00173 super(Observers, self).__init__(*args, **kwds)
00174
00175 if self.header is None:
00176 self.header = std_msgs.msg._Header.Header()
00177 if self.odom is None:
00178 self.odom = nav_msgs.msg.Odometry()
00179 if self.obs is None:
00180 self.obs = []
00181 else:
00182 self.header = std_msgs.msg._Header.Header()
00183 self.odom = nav_msgs.msg.Odometry()
00184 self.obs = []
00185
00186 def _get_types(self):
00187 """
00188 internal API method
00189 """
00190 return self._slot_types
00191
00192 def serialize(self, buff):
00193 """
00194 serialize message into buffer
00195 @param buff: buffer
00196 @type buff: StringIO
00197 """
00198 try:
00199 _x = self
00200 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00201 _x = self.header.frame_id
00202 length = len(_x)
00203 buff.write(struct.pack('<I%ss'%length, length, _x))
00204 _x = self
00205 buff.write(_struct_3I.pack(_x.odom.header.seq, _x.odom.header.stamp.secs, _x.odom.header.stamp.nsecs))
00206 _x = self.odom.header.frame_id
00207 length = len(_x)
00208 buff.write(struct.pack('<I%ss'%length, length, _x))
00209 _x = self.odom.child_frame_id
00210 length = len(_x)
00211 buff.write(struct.pack('<I%ss'%length, length, _x))
00212 _x = self
00213 buff.write(_struct_7d.pack(_x.odom.pose.pose.position.x, _x.odom.pose.pose.position.y, _x.odom.pose.pose.position.z, _x.odom.pose.pose.orientation.x, _x.odom.pose.pose.orientation.y, _x.odom.pose.pose.orientation.z, _x.odom.pose.pose.orientation.w))
00214 buff.write(_struct_36d.pack(*self.odom.pose.covariance))
00215 _x = self
00216 buff.write(_struct_6d.pack(_x.odom.twist.twist.linear.x, _x.odom.twist.twist.linear.y, _x.odom.twist.twist.linear.z, _x.odom.twist.twist.angular.x, _x.odom.twist.twist.angular.y, _x.odom.twist.twist.angular.z))
00217 buff.write(_struct_36d.pack(*self.odom.twist.covariance))
00218 length = len(self.obs)
00219 buff.write(_struct_I.pack(length))
00220 for val1 in self.obs:
00221 buff.write(_struct_i.pack(val1.oid))
00222 _x = val1.name
00223 length = len(_x)
00224 buff.write(struct.pack('<I%ss'%length, length, _x))
00225 _x = val1
00226 buff.write(_struct_2B3fi.pack(_x.applicable, _x.clear, _x.time, _x.distance, _x.velocity, _x.nobjects))
00227 except struct.error, se: self._check_types(se)
00228 except TypeError, te: self._check_types(te)
00229
00230 def deserialize(self, str):
00231 """
00232 unpack serialized message in str into this message instance
00233 @param str: byte array of serialized message
00234 @type str: str
00235 """
00236 try:
00237 if self.header is None:
00238 self.header = std_msgs.msg._Header.Header()
00239 if self.odom is None:
00240 self.odom = nav_msgs.msg.Odometry()
00241 end = 0
00242 _x = self
00243 start = end
00244 end += 12
00245 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00246 start = end
00247 end += 4
00248 (length,) = _struct_I.unpack(str[start:end])
00249 start = end
00250 end += length
00251 self.header.frame_id = str[start:end]
00252 _x = self
00253 start = end
00254 end += 12
00255 (_x.odom.header.seq, _x.odom.header.stamp.secs, _x.odom.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00256 start = end
00257 end += 4
00258 (length,) = _struct_I.unpack(str[start:end])
00259 start = end
00260 end += length
00261 self.odom.header.frame_id = str[start:end]
00262 start = end
00263 end += 4
00264 (length,) = _struct_I.unpack(str[start:end])
00265 start = end
00266 end += length
00267 self.odom.child_frame_id = str[start:end]
00268 _x = self
00269 start = end
00270 end += 56
00271 (_x.odom.pose.pose.position.x, _x.odom.pose.pose.position.y, _x.odom.pose.pose.position.z, _x.odom.pose.pose.orientation.x, _x.odom.pose.pose.orientation.y, _x.odom.pose.pose.orientation.z, _x.odom.pose.pose.orientation.w,) = _struct_7d.unpack(str[start:end])
00272 start = end
00273 end += 288
00274 self.odom.pose.covariance = _struct_36d.unpack(str[start:end])
00275 _x = self
00276 start = end
00277 end += 48
00278 (_x.odom.twist.twist.linear.x, _x.odom.twist.twist.linear.y, _x.odom.twist.twist.linear.z, _x.odom.twist.twist.angular.x, _x.odom.twist.twist.angular.y, _x.odom.twist.twist.angular.z,) = _struct_6d.unpack(str[start:end])
00279 start = end
00280 end += 288
00281 self.odom.twist.covariance = _struct_36d.unpack(str[start:end])
00282 start = end
00283 end += 4
00284 (length,) = _struct_I.unpack(str[start:end])
00285 self.obs = []
00286 for i in xrange(0, length):
00287 val1 = art_msgs.msg.Observation()
00288 start = end
00289 end += 4
00290 (val1.oid,) = _struct_i.unpack(str[start:end])
00291 start = end
00292 end += 4
00293 (length,) = _struct_I.unpack(str[start:end])
00294 start = end
00295 end += length
00296 val1.name = str[start:end]
00297 _x = val1
00298 start = end
00299 end += 18
00300 (_x.applicable, _x.clear, _x.time, _x.distance, _x.velocity, _x.nobjects,) = _struct_2B3fi.unpack(str[start:end])
00301 val1.applicable = bool(val1.applicable)
00302 val1.clear = bool(val1.clear)
00303 self.obs.append(val1)
00304 return self
00305 except struct.error, e:
00306 raise roslib.message.DeserializationError(e)
00307
00308
00309 def serialize_numpy(self, buff, numpy):
00310 """
00311 serialize message with numpy array types into buffer
00312 @param buff: buffer
00313 @type buff: StringIO
00314 @param numpy: numpy python module
00315 @type numpy module
00316 """
00317 try:
00318 _x = self
00319 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00320 _x = self.header.frame_id
00321 length = len(_x)
00322 buff.write(struct.pack('<I%ss'%length, length, _x))
00323 _x = self
00324 buff.write(_struct_3I.pack(_x.odom.header.seq, _x.odom.header.stamp.secs, _x.odom.header.stamp.nsecs))
00325 _x = self.odom.header.frame_id
00326 length = len(_x)
00327 buff.write(struct.pack('<I%ss'%length, length, _x))
00328 _x = self.odom.child_frame_id
00329 length = len(_x)
00330 buff.write(struct.pack('<I%ss'%length, length, _x))
00331 _x = self
00332 buff.write(_struct_7d.pack(_x.odom.pose.pose.position.x, _x.odom.pose.pose.position.y, _x.odom.pose.pose.position.z, _x.odom.pose.pose.orientation.x, _x.odom.pose.pose.orientation.y, _x.odom.pose.pose.orientation.z, _x.odom.pose.pose.orientation.w))
00333 buff.write(self.odom.pose.covariance.tostring())
00334 _x = self
00335 buff.write(_struct_6d.pack(_x.odom.twist.twist.linear.x, _x.odom.twist.twist.linear.y, _x.odom.twist.twist.linear.z, _x.odom.twist.twist.angular.x, _x.odom.twist.twist.angular.y, _x.odom.twist.twist.angular.z))
00336 buff.write(self.odom.twist.covariance.tostring())
00337 length = len(self.obs)
00338 buff.write(_struct_I.pack(length))
00339 for val1 in self.obs:
00340 buff.write(_struct_i.pack(val1.oid))
00341 _x = val1.name
00342 length = len(_x)
00343 buff.write(struct.pack('<I%ss'%length, length, _x))
00344 _x = val1
00345 buff.write(_struct_2B3fi.pack(_x.applicable, _x.clear, _x.time, _x.distance, _x.velocity, _x.nobjects))
00346 except struct.error, se: self._check_types(se)
00347 except TypeError, te: self._check_types(te)
00348
00349 def deserialize_numpy(self, str, numpy):
00350 """
00351 unpack serialized message in str into this message instance using numpy for array types
00352 @param str: byte array of serialized message
00353 @type str: str
00354 @param numpy: numpy python module
00355 @type numpy: module
00356 """
00357 try:
00358 if self.header is None:
00359 self.header = std_msgs.msg._Header.Header()
00360 if self.odom is None:
00361 self.odom = nav_msgs.msg.Odometry()
00362 end = 0
00363 _x = self
00364 start = end
00365 end += 12
00366 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00367 start = end
00368 end += 4
00369 (length,) = _struct_I.unpack(str[start:end])
00370 start = end
00371 end += length
00372 self.header.frame_id = str[start:end]
00373 _x = self
00374 start = end
00375 end += 12
00376 (_x.odom.header.seq, _x.odom.header.stamp.secs, _x.odom.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00377 start = end
00378 end += 4
00379 (length,) = _struct_I.unpack(str[start:end])
00380 start = end
00381 end += length
00382 self.odom.header.frame_id = str[start:end]
00383 start = end
00384 end += 4
00385 (length,) = _struct_I.unpack(str[start:end])
00386 start = end
00387 end += length
00388 self.odom.child_frame_id = str[start:end]
00389 _x = self
00390 start = end
00391 end += 56
00392 (_x.odom.pose.pose.position.x, _x.odom.pose.pose.position.y, _x.odom.pose.pose.position.z, _x.odom.pose.pose.orientation.x, _x.odom.pose.pose.orientation.y, _x.odom.pose.pose.orientation.z, _x.odom.pose.pose.orientation.w,) = _struct_7d.unpack(str[start:end])
00393 start = end
00394 end += 288
00395 self.odom.pose.covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=36)
00396 _x = self
00397 start = end
00398 end += 48
00399 (_x.odom.twist.twist.linear.x, _x.odom.twist.twist.linear.y, _x.odom.twist.twist.linear.z, _x.odom.twist.twist.angular.x, _x.odom.twist.twist.angular.y, _x.odom.twist.twist.angular.z,) = _struct_6d.unpack(str[start:end])
00400 start = end
00401 end += 288
00402 self.odom.twist.covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=36)
00403 start = end
00404 end += 4
00405 (length,) = _struct_I.unpack(str[start:end])
00406 self.obs = []
00407 for i in xrange(0, length):
00408 val1 = art_msgs.msg.Observation()
00409 start = end
00410 end += 4
00411 (val1.oid,) = _struct_i.unpack(str[start:end])
00412 start = end
00413 end += 4
00414 (length,) = _struct_I.unpack(str[start:end])
00415 start = end
00416 end += length
00417 val1.name = str[start:end]
00418 _x = val1
00419 start = end
00420 end += 18
00421 (_x.applicable, _x.clear, _x.time, _x.distance, _x.velocity, _x.nobjects,) = _struct_2B3fi.unpack(str[start:end])
00422 val1.applicable = bool(val1.applicable)
00423 val1.clear = bool(val1.clear)
00424 self.obs.append(val1)
00425 return self
00426 except struct.error, e:
00427 raise roslib.message.DeserializationError(e)
00428
00429 _struct_I = roslib.message.struct_I
00430 _struct_7d = struct.Struct("<7d")
00431 _struct_6d = struct.Struct("<6d")
00432 _struct_i = struct.Struct("<i")
00433 _struct_36d = struct.Struct("<36d")
00434 _struct_2B3fi = struct.Struct("<2B3fi")
00435 _struct_3I = struct.Struct("<3I")