def compute_errors(self): self._bag = Recording.get_instance() assert self._bag is not None, 'Recording has not been created' assert self._bag.is_init, 'Topics have not been sorted from the rosbag' t_start = self._bag.start_time t_end = self._bag.end_time self._errors = list() for p_act in self._bag.actual.points: if t_start <= p_act.t and p_act.t <= t_end: if len(self._errors): if p_act.t <= self._errors[-1].t: continue p_des = self._bag.desired.interpolate(p_act.t) self._errors.append(TrajectoryError(p_des, p_act))
def compute_errors(self): self._bag = Recording.get_instance() assert self._bag is not None, 'Recording has not been created' # assert self._bag.is_init, 'Topics have not been sorted from the rosbag' if self._bag.parsers['error'].error is None: t_start = self._bag.parsers['trajectory'].start_time t_end = self._bag.parsers['trajectory'].end_time self._errors = list() for p_act in self._bag.parsers['trajectory'].odometry.points: if t_start <= p_act.t and p_act.t <= t_end: if len(self._errors): if p_act.t <= self._errors[-1].t: continue p_des = self._bag.parsers[ 'trajectory'].reference.interpolate(p_act.t) self._errors.append(TrajectoryError(p_des, p_act))