def getPredictAt(self, timestamp_s: float):
timestamp_s = round(timestamp_s, 2)
if timestamp_s not in self._p_pose:
return None, None
# self.predict(dT=param._PREDICT_STEP, pT=param._PREDICT_TIME)
pose = self._p_pose[timestamp_s]
posePoly = pfnc.rectangle(pose, self._length, self._width)
return pose, posePoly
def getPoly(self, timestamp_s: float):
"""
Return the bounding polygon of vehicle
"""
pose = self.getPoseAt(timestamp_s)
if pose is None:
return None
return pfnc.rectangle(pose, self._length, self._width)
def getPoly(self, timestamp_s: float):
"""
Return the bounding polygon of vehicle
"""
if timestamp_s in self._l_pose:
pose = self._l_pose[timestamp_s]
return pfnc.rectangle(pose, self._length, self._width)
else:
return None
def exportPredict(self):
l_p = []
self._p_pose.keys()
for k in self._p_pose:
p_pose = self._p_pose[k]
stdLon = np.sqrt(p_pose.covLatLong[0, 0])
stdLat = np.sqrt(p_pose.covLatLong[1, 1])
exportP = {
'pos': [p_pose.x_m, p_pose.y_m, p_pose.yaw_rad],
'std': np.array([stdLon, stdLat]) + self._lw_std,
'poly': pfnc.rectangle(p_pose, self._length, self._width),
}
l_p.append(exportP)
return l_p
def getCurrentPoly(self):
return pfnc.rectangle(self._currentPose, self._length, self._width)
def getPredictPoly(self, timestamp_s: float):
if timestamp_s in self._p_pose:
pose = self._p_pose[timestamp_s]
return pfnc.rectangle(pose, self._length, self._width)
else:
return None
