00001 """autogenerated by genmsg_py from DynamicObstacle.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import dynamic_obs_msgs.msg
00006 import geometry_msgs.msg
00007 import std_msgs.msg
00008
00009 class DynamicObstacle(roslib.message.Message):
00010 _md5sum = "e54c01ba99bdd49728083a910750888c"
00011 _type = "dynamic_obs_msgs/DynamicObstacle"
00012 _has_header = False
00013 _full_text = """#the radius of the dynamic obstacle
00014 float64 radius
00015
00016 #a list of possible trajectories it may follow (with probabilities summing to 1)
00017 DynObsTrajectory[] trajectories
00018
00019 ================================================================================
00020 MSG: dynamic_obs_msgs/DynObsTrajectory
00021 #a dynamic obstacle trajectory
00022
00023 #the probability of the obstacle following this trajectory
00024 float64 probability
00025
00026 #whether this obstacle should be treated like a static obstacle at the end of its trajectory (true)
00027 #or the obstacle is ignored after the end of its trajectory (false)
00028 bool exists_after
00029
00030 #the time parameterized path
00031 geometry_msgs/PoseWithCovarianceStamped[] points
00032
00033 ================================================================================
00034 MSG: geometry_msgs/PoseWithCovarianceStamped
00035 # This expresses an estimated pose with a reference coordinate frame and timestamp
00036
00037 Header header
00038 PoseWithCovariance pose
00039
00040 ================================================================================
00041 MSG: std_msgs/Header
00042 # Standard metadata for higher-level stamped data types.
00043 # This is generally used to communicate timestamped data
00044 # in a particular coordinate frame.
00045 #
00046 # sequence ID: consecutively increasing ID
00047 uint32 seq
00048 #Two-integer timestamp that is expressed as:
00049 # * stamp.secs: seconds (stamp_secs) since epoch
00050 # * stamp.nsecs: nanoseconds since stamp_secs
00051 # time-handling sugar is provided by the client library
00052 time stamp
00053 #Frame this data is associated with
00054 # 0: no frame
00055 # 1: global frame
00056 string frame_id
00057
00058 ================================================================================
00059 MSG: geometry_msgs/PoseWithCovariance
00060 # This represents a pose in free space with uncertainty.
00061
00062 Pose pose
00063
00064 # Row-major representation of the 6x6 covariance matrix
00065 # The orientation parameters use a fixed-axis representation.
00066 # In order, the parameters are:
00067 # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
00068 float64[36] covariance
00069
00070 ================================================================================
00071 MSG: geometry_msgs/Pose
00072 # A representation of pose in free space, composed of postion and orientation.
00073 Point position
00074 Quaternion orientation
00075
00076 ================================================================================
00077 MSG: geometry_msgs/Point
00078 # This contains the position of a point in free space
00079 float64 x
00080 float64 y
00081 float64 z
00082
00083 ================================================================================
00084 MSG: geometry_msgs/Quaternion
00085 # This represents an orientation in free space in quaternion form.
00086
00087 float64 x
00088 float64 y
00089 float64 z
00090 float64 w
00091
00092 """
00093 __slots__ = ['radius','trajectories']
00094 _slot_types = ['float64','dynamic_obs_msgs/DynObsTrajectory[]']
00095
00096 def __init__(self, *args, **kwds):
00097 """
00098 Constructor. Any message fields that are implicitly/explicitly
00099 set to None will be assigned a default value. The recommend
00100 use is keyword arguments as this is more robust to future message
00101 changes. You cannot mix in-order arguments and keyword arguments.
00102
00103 The available fields are:
00104 radius,trajectories
00105
00106 @param args: complete set of field values, in .msg order
00107 @param kwds: use keyword arguments corresponding to message field names
00108 to set specific fields.
00109 """
00110 if args or kwds:
00111 super(DynamicObstacle, self).__init__(*args, **kwds)
00112
00113 if self.radius is None:
00114 self.radius = 0.
00115 if self.trajectories is None:
00116 self.trajectories = []
00117 else:
00118 self.radius = 0.
00119 self.trajectories = []
00120
00121 def _get_types(self):
00122 """
00123 internal API method
00124 """
00125 return self._slot_types
00126
00127 def serialize(self, buff):
00128 """
00129 serialize message into buffer
00130 @param buff: buffer
00131 @type buff: StringIO
00132 """
00133 try:
00134 buff.write(_struct_d.pack(self.radius))
00135 length = len(self.trajectories)
00136 buff.write(_struct_I.pack(length))
00137 for val1 in self.trajectories:
00138 _x = val1
00139 buff.write(_struct_dB.pack(_x.probability, _x.exists_after))
00140 length = len(val1.points)
00141 buff.write(_struct_I.pack(length))
00142 for val2 in val1.points:
00143 _v1 = val2.header
00144 buff.write(_struct_I.pack(_v1.seq))
00145 _v2 = _v1.stamp
00146 _x = _v2
00147 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00148 _x = _v1.frame_id
00149 length = len(_x)
00150 buff.write(struct.pack('<I%ss'%length, length, _x))
00151 _v3 = val2.pose
00152 _v4 = _v3.pose
00153 _v5 = _v4.position
00154 _x = _v5
00155 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00156 _v6 = _v4.orientation
00157 _x = _v6
00158 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00159 buff.write(_struct_36d.pack(*_v3.covariance))
00160 except struct.error, se: self._check_types(se)
00161 except TypeError, te: self._check_types(te)
00162
00163 def deserialize(self, str):
00164 """
00165 unpack serialized message in str into this message instance
00166 @param str: byte array of serialized message
00167 @type str: str
00168 """
00169 try:
00170 end = 0
00171 start = end
00172 end += 8
00173 (self.radius,) = _struct_d.unpack(str[start:end])
00174 start = end
00175 end += 4
00176 (length,) = _struct_I.unpack(str[start:end])
00177 self.trajectories = []
00178 for i in xrange(0, length):
00179 val1 = dynamic_obs_msgs.msg.DynObsTrajectory()
00180 _x = val1
00181 start = end
00182 end += 9
00183 (_x.probability, _x.exists_after,) = _struct_dB.unpack(str[start:end])
00184 val1.exists_after = bool(val1.exists_after)
00185 start = end
00186 end += 4
00187 (length,) = _struct_I.unpack(str[start:end])
00188 val1.points = []
00189 for i in xrange(0, length):
00190 val2 = geometry_msgs.msg.PoseWithCovarianceStamped()
00191 _v7 = val2.header
00192 start = end
00193 end += 4
00194 (_v7.seq,) = _struct_I.unpack(str[start:end])
00195 _v8 = _v7.stamp
00196 _x = _v8
00197 start = end
00198 end += 8
00199 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00200 start = end
00201 end += 4
00202 (length,) = _struct_I.unpack(str[start:end])
00203 start = end
00204 end += length
00205 _v7.frame_id = str[start:end]
00206 _v9 = val2.pose
00207 _v10 = _v9.pose
00208 _v11 = _v10.position
00209 _x = _v11
00210 start = end
00211 end += 24
00212 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00213 _v12 = _v10.orientation
00214 _x = _v12
00215 start = end
00216 end += 32
00217 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00218 start = end
00219 end += 288
00220 _v9.covariance = _struct_36d.unpack(str[start:end])
00221 val1.points.append(val2)
00222 self.trajectories.append(val1)
00223 return self
00224 except struct.error, e:
00225 raise roslib.message.DeserializationError(e)
00226
00227
00228 def serialize_numpy(self, buff, numpy):
00229 """
00230 serialize message with numpy array types into buffer
00231 @param buff: buffer
00232 @type buff: StringIO
00233 @param numpy: numpy python module
00234 @type numpy module
00235 """
00236 try:
00237 buff.write(_struct_d.pack(self.radius))
00238 length = len(self.trajectories)
00239 buff.write(_struct_I.pack(length))
00240 for val1 in self.trajectories:
00241 _x = val1
00242 buff.write(_struct_dB.pack(_x.probability, _x.exists_after))
00243 length = len(val1.points)
00244 buff.write(_struct_I.pack(length))
00245 for val2 in val1.points:
00246 _v13 = val2.header
00247 buff.write(_struct_I.pack(_v13.seq))
00248 _v14 = _v13.stamp
00249 _x = _v14
00250 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00251 _x = _v13.frame_id
00252 length = len(_x)
00253 buff.write(struct.pack('<I%ss'%length, length, _x))
00254 _v15 = val2.pose
00255 _v16 = _v15.pose
00256 _v17 = _v16.position
00257 _x = _v17
00258 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00259 _v18 = _v16.orientation
00260 _x = _v18
00261 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00262 buff.write(_v15.covariance.tostring())
00263 except struct.error, se: self._check_types(se)
00264 except TypeError, te: self._check_types(te)
00265
00266 def deserialize_numpy(self, str, numpy):
00267 """
00268 unpack serialized message in str into this message instance using numpy for array types
00269 @param str: byte array of serialized message
00270 @type str: str
00271 @param numpy: numpy python module
00272 @type numpy: module
00273 """
00274 try:
00275 end = 0
00276 start = end
00277 end += 8
00278 (self.radius,) = _struct_d.unpack(str[start:end])
00279 start = end
00280 end += 4
00281 (length,) = _struct_I.unpack(str[start:end])
00282 self.trajectories = []
00283 for i in xrange(0, length):
00284 val1 = dynamic_obs_msgs.msg.DynObsTrajectory()
00285 _x = val1
00286 start = end
00287 end += 9
00288 (_x.probability, _x.exists_after,) = _struct_dB.unpack(str[start:end])
00289 val1.exists_after = bool(val1.exists_after)
00290 start = end
00291 end += 4
00292 (length,) = _struct_I.unpack(str[start:end])
00293 val1.points = []
00294 for i in xrange(0, length):
00295 val2 = geometry_msgs.msg.PoseWithCovarianceStamped()
00296 _v19 = val2.header
00297 start = end
00298 end += 4
00299 (_v19.seq,) = _struct_I.unpack(str[start:end])
00300 _v20 = _v19.stamp
00301 _x = _v20
00302 start = end
00303 end += 8
00304 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00305 start = end
00306 end += 4
00307 (length,) = _struct_I.unpack(str[start:end])
00308 start = end
00309 end += length
00310 _v19.frame_id = str[start:end]
00311 _v21 = val2.pose
00312 _v22 = _v21.pose
00313 _v23 = _v22.position
00314 _x = _v23
00315 start = end
00316 end += 24
00317 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00318 _v24 = _v22.orientation
00319 _x = _v24
00320 start = end
00321 end += 32
00322 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00323 start = end
00324 end += 288
00325 _v21.covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=36)
00326 val1.points.append(val2)
00327 self.trajectories.append(val1)
00328 return self
00329 except struct.error, e:
00330 raise roslib.message.DeserializationError(e)
00331
00332 _struct_I = roslib.message.struct_I
00333 _struct_d = struct.Struct("<d")
00334 _struct_36d = struct.Struct("<36d")
00335 _struct_dB = struct.Struct("<dB")
00336 _struct_4d = struct.Struct("<4d")
00337 _struct_2I = struct.Struct("<2I")
00338 _struct_3d = struct.Struct("<3d")