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
