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))
