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