def __add__(self, other): if isinstance(other, Pose): x = self.x + other.x y = self.y + other.y theta = wrap_angle(self.theta + other.theta) elif other.shape == (1, 3): x = self.x + other[0][0] y = self.y + other[0][1] theta = wrap_angle(self.theta + other[0][2]) else: raise NotImplementedError, ERR.CANT_ADD_POSE(a) return Pose(x, y, theta)
def __iadd__(self, other): if isinstance(other, Pose): self.x += other.x self.y += other.y self.theta += other.theta elif isinstance(other, np.ndarray) and other.shape == (1, 3): self.x += other[0, 0] self.y += other[0, 1] self.theta += other[0, 2] else: raise NotImplementedError, ERR.CANT_ADD_POSE(other) self.theta = wrap_angle(self.theta) return self