class OpenLoopRollout():
def __init__(self, particles, state_lock):
#print("Initalized")
self.particle_list= np.array(particles)
self.particle_list_kin = np.array(particles)
self.true_pose_list = None
self.motionModel = OdometryMotionModel(particles, state_lock)
#self.kinematic = KinematicMotionModel(particles, state_lock)
def apply_motion_model(self, msg):
#print(msg)
self.motionModel.motion_cb(msg)
#print("Motion model particle: ",self.motionModel.inner.particles)
self.particle_list = np.concatenate((self.particle_list, self.motionModel.inner.particles))
def apply_kin_model(self, msg, topic):
if topic == "/vesc/sensors/core":
self.kinematic.motion_cb(msg)
self.particle_list_kin = np.concatenate((self.particle_list_kin, self.kinematic.inner.particles))
elif topic == "/vesc/sensors/servo_position_command":
self.kinematic.servo_cb(msg)
def add_true_pose(self, particle):
if self.true_pose_list is None:
self.true_pose_list = np.array(particle)
else:
self.true_pose_list = np.concatenate((self.true_pose_list, particle))
def plot_particles(self):
#print("First Particle: " ,self.particle_list)
x = self.particle_list[:,0]
y = self.particle_list[:,1]
x_kin = self.particle_list_kin[:,0]
y_kin = self.particle_list_kin[:,1]
x_true = self.true_pose_list[:,0]
y_true = self.true_pose_list[:,1]
#print("X: ", x)
#print("Y: ", y)
plt.plot(x,y)
#plt.plot(x_kin, y_kin)
plt.plot(x_true, y_true)
plt.show()
class NoisePropagation():
def __init__(self, particles, state_lock):
print("Initalized")
self.odom = OdometryMotionModel(particles, state_lock)
self.kine = KinematicMotionModel(particles, state_lock)
self.odomMessage = []
self.kineMessage = []
#self.odomParticles = [particles]
self.odomParticles = np.array([particles])
self.kineParticles = np.array([particles])
def addOdomMessage(self, msg):
#self.odomMessage.append(msg)
self.odom.motion_cb(msg)
#self.odomParticles.append(np.copy(self.odom.inner.particles))
self.odomParticles = np.concatenate(
(self.odomParticles, [np.copy(self.odom.inner.particles)]), axis=0)
def addKineVelMessage(self, msg):
#self.kindVelMessage.append(msg)
self.kine.motion_cb(msg)
#self.kineParticles.append(self.kine.inner.particles)
self.kineParticles = np.concatenate(
(self.kineParticles, [np.copy(self.kine.inner.particles)]), axis=0)
def addKineServMessage(self, msg):
#self.kindServMessage.append(msg)
self.kine.servo_cb(msg)
def plotOdomParticles(self):
x_total = []
y_total = []
c = iter(cm.rainbow(np.linspace(0, 1, len(self.odomParticles) + 1)))
#assert len(self.odomParticles) == 20
for i in range(len(self.odomParticles)):
current_particle = self.odomParticles[i]
#print(current_particle.shape)
x = current_particle[:, 0]
#x = column(current_particle, 0)
if x_total == []:
x_total = x
else:
x_total += x
print("X: ", x)
assert len(x) == 500
y = current_particle[:, 1]
#y = column(current_particle, 1)
if y_total == []:
y_total = y
else:
y_total += y
#print("X", x)
#plt.scatter(x, y)
plt.scatter(x, y, color=next(c))
plt.show()
def plotKineParticles(self):
c = iter(cm.rainbow(np.linspace(0, 1, len(self.kineParticles) + 1)))
#assert len(self.odomParticles) == 20
for i in range(len(self.kineParticles)):
current_particle = self.kineParticles[i]
#print(current_particle.shape)
x = current_particle[:, 0]
#x = column(current_particle, 0)
assert len(x) == 500
y = current_particle[:, 1]
#y = column(current_particle, 1)
#print("X", x)
#plt.scatter(x, y)
plt.scatter(x, y, color=next(c))
plt.show()