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 __init__(self, n_particles, n_viz_particles,
motor_state_topic, servo_state_topic, scan_topic, laser_ray_step,
exclude_max_range_rays, max_range_meters, resample_type,
speed_to_erpm_offset, speed_to_erpm_gain, steering_angle_to_servo_offset,
steering_angle_to_servo_gain, car_length):
self.N_PARTICLES = n_particles # The number of particles
# In this implementation, the total number of
# particles is constant
self.N_VIZ_PARTICLES = n_viz_particles # The number of particles to visualize
self.particle_indices = np.arange(self.N_PARTICLES) # Cached list of particle indices
self.particles = np.zeros((self.N_PARTICLES,3)) # Numpy matrix of dimension N_PARTICLES x 3
self.weights = np.ones(self.N_PARTICLES) / float(self.N_PARTICLES) # Numpy matrix containig weight for each particle
self.state_lock = Lock() # A lock used to prevent concurrency issues. You do not need to worry about this
self.tfl = tf.TransformListener() # Transforms points between coordinate frames
# Get the map
print("Getting map from service: ", MAP_TOPIC)
rospy.wait_for_service(MAP_TOPIC)
map_msg = rospy.ServiceProxy(MAP_TOPIC, GetMap)().map # The map, will get passed to init of sensor model
self.map_info = map_msg.info # Save info about map for later use
# Create numpy array representing map for later use
array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width))
self.permissible_region = np.zeros_like(array_255, dtype=bool)
self.permissible_region[array_255==0] = 1 # Numpy array of dimension (map_msg.info.height, map_msg.info.width),
# With values 0: not permissible, 1: permissible
# Globally initialize the particles
self.initialize_global()
# Publish particle filter state
self.pub_tf = tf.TransformBroadcaster() # Used to create a tf between the map and the laser for visualization
self.pose_pub = rospy.Publisher(PUBLISH_PREFIX + "/inferred_pose", PoseStamped, queue_size = 1) # Publishes the expected pose
self.particle_pub = rospy.Publisher(PUBLISH_PREFIX + "/particles", PoseArray, queue_size = 1) # Publishes a subsample of the particles
self.pub_laser = rospy.Publisher(PUBLISH_PREFIX + "/scan", LaserScan, queue_size = 1) # Publishes the most recent laser scan
self.pub_odom = rospy.Publisher(PUBLISH_PREFIX + "/odom", Odometry, queue_size = 1) # Publishes the path of the car
self.RESAMPLE_TYPE = resample_type # Whether to use naiive or low variance sampling
self.resampler = ReSampler(self.particles, self.weights, self.state_lock) # An object used for resampling
# An object used for applying sensor model
self.sensor_model = SensorModel(scan_topic, laser_ray_step, exclude_max_range_rays,
max_range_meters, map_msg, self.particles, self.weights,
self.state_lock)
# An object used for applying kinematic motion model
self.motion_model = KinematicMotionModel(motor_state_topic, servo_state_topic,
speed_to_erpm_offset, speed_to_erpm_gain,
steering_angle_to_servo_offset, steering_angle_to_servo_gain,
car_length, self.particles, self.state_lock)
# Subscribe to the '/initialpose' topic. Publised by RVIZ. See clicked_pose_cb function in this file for more info
self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose_cb, queue_size=1)
print('Initialization complete')
def __init__(self):
self.MAX_PARTICLES = int(rospy.get_param("~max_particles")) # The maximum number of particles
self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) # The maximum number of particles to visualize
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.particles = np.zeros((self.MAX_PARTICLES,3)) # Numpy matrix of dimension MAX_PARTICLES x 3
self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # Numpy matrix containig weight for each particle
self.state_lock = Lock() # A lock used to prevent concurrency issues. You do not need to worry about this
self.tfl = tf.TransformListener()
# Use the 'static_map' service (launched by MapServer.launch) to get the map
map_service_name = rospy.get_param("~static_map", "static_map")
print("Getting map from service: ", map_service_name)
rospy.wait_for_service(map_service_name)
map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map # The map, will get passed to init of sensor model
self.map_info = map_msg.info # Save info about map for later use
# Create numpy array representing map for later use
array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width))
self.permissible_region = np.zeros_like(array_255, dtype=bool)
self.permissible_region[array_255==0] = 1 # Numpy array of dimension (map_msg.info.height, map_msg.info.width),
# With values 0: not permissible, 1: permissible
# Globally initialize the particles
self.initialize_global()
# Publish particle filter state
self.pose_pub = rospy.Publisher("/pf/ta/viz/inferred_pose", PoseStamped, queue_size = 1) # Publishes the expected pose
self.particle_pub = rospy.Publisher("/pf/ta/viz/particles", PoseArray, queue_size = 1) # Publishes a subsample of the particles
self.pub_tf = tf.TransformBroadcaster() # Used to create a tf between the map and the laser for visualization
self.pub_laser = rospy.Publisher("/pf/ta/viz/scan", LaserScan, queue_size = 1) # Publishes the most recent laser scan
self.pub_odom = rospy.Publisher("/pf/ta/viz/odom", Odometry, queue_size = 1) # Publishes the path of the car
''' HACK VIEW INITIAL DISTRIBUTION'''
'''
self.MAX_VIZ_PARTICLES = 1000
while not rospy.is_shutdown():
self.visualize()
rospy.sleep(0.2)
'''
self.RESAMPLE_TYPE = rospy.get_param("~resample_type", "naiive") # Whether to use naiive or low variance sampling
self.resampler = ReSampler(self.particles, self.weights, self.state_lock) # An object used for resampling
self.sensor_model = SensorModel(map_msg, self.particles, self.weights, self.state_lock) # An object used for applying sensor model
# Subscribe to laser scans. For the callback, use self.sensor_model's lidar_cb function
self.laser_sub = rospy.Subscriber(rospy.get_param("~scan_topic", "/scan"), LaserScan, self.sensor_model.lidar_cb, queue_size=1)
self.MOTION_MODEL_TYPE = rospy.get_param("~motion_model", "kinematic") # Whether to use the odometry or kinematics based motion model
if self.MOTION_MODEL_TYPE == "kinematic":
self.motion_model = KinematicMotionModel(self.particles, self.state_lock) # An object used for applying kinematic motion model
# Subscribe to the state of the vesc (topic: /vesc/sensors/core). For the callback, use self.motion_model's motion_cb function
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/sensors/core"), VescStateStamped, self.motion_model.motion_cb, queue_size=1)
elif self.MOTION_MODEL_TYPE == "odometry":
self.motion_model = OdometryMotionModel(self.particles, self.state_lock)# An object used for applying odometry motion model
# Subscribe to the vesc odometry (topic: /vesc/odom). For the callback, use self.motion_model's motion_cb function
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/odom"), Odometry, self.motion_model.motion_cb, queue_size=1)
else:
print "Unrecognized motion model: "+ self.MOTION_MODEL_TYPE
assert(False)
# Subscribe to the '/initialpose' topic. Publised by RVIZ. See clicked_pose_cb function in this file for more info
self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose_cb, queue_size=1)
#self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose_cb, queue_size=1)
print('Initialization complete')
return "PASS"
if __name__ == '__main__':
num_particles = 100
counts = []
counts1 = []
counts2 = []
#99 tests. Hint: set me to 100 to have insight on the question about resampling
for i in range(0, 99):
start_particles = np.zeros((num_particles, 3)) # Numpy matrix of dimension N_PARTICLES x 3
state_lock = Lock() # A lock used to prevent concurrency issues. You do not need to worry about this
#Instatiate Your Motion Model
motion_model = KinematicMotionModel("/fake_topic1", "/fake_topic2",
0.0, 4350,
0.5, -1.2135,
0.33, start_particles, state_lock)
#Apply 3 different controls
if i % 3 == 0:
counts = test_mm_1(motion_model, counts)
if i % 3 == 1:
counts1 = test_mm_2(motion_model, counts1)
if i % 3 == 2:
counts2 = test_mm_3(motion_model, counts2)
print "Average Number of Particles within " + str(radius) + "cm of the Ground Truth:"
print "Test 1:", np.mean(counts), pass_fail(1, np.mean(counts))
print "Test 2:", np.mean(counts1), pass_fail(2, np.mean(counts1))
print "Test 3:", np.mean(counts2), pass_fail(3, np.mean(counts2))
def __init__(self):
self.MAX_PARTICLES = int(rospy.get_param("~max_particles")) # The maximum number of particles
self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) # The maximum number of particles to visualize
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.particles = np.zeros((self.MAX_PARTICLES,3)) # Numpy matrix of dimension MAX_PARTICLES x 3
self.weights = np.ones(self.MAX_PARTICLES, dtype=np.float64) / float(self.MAX_PARTICLES) # Numpy matrix containig weight for each particle
self.state_lock = Lock() # A lock used to prevent concurrency issues. You do not need to worry about this
# Use the 'static_map' service (launched by MapServer.launch) to get the map
# Will be used to initialize particles and SensorModel
# Store map in variable called 'map_msg'
# YOUR CODE HERE
rospy.wait_for_service('/static_map')
get_map = rospy.ServiceProxy('/static_map', GetMap)
map_msg = get_map().map
# Use dir to show msg fields
# print(dir(map_msg))
# print(dir(map_msg))
# Globally initialize the particles
self.map_free_space = []
self.initialize_global(map_msg)
print("Z!")
# Publish particle filter state
self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose", PoseStamped, queue_size = 1) # Publishes the expected pose
self.particle_pub = rospy.Publisher("/pf/viz/particles", PoseArray, queue_size = 1) # Publishes a subsample of the particles
self.pub_tf = tf.TransformBroadcaster() # Used to create a tf between the map and the laser for visualization
self.pub_laser = rospy.Publisher("/pf/viz/scan", LaserScan, queue_size = 1) # Publishes the most recent laser scan
print("Y!")
self.RESAMPLE_TYPE = rospy.get_param("~resample_type", "naiive") # Whether to use naiive or low variance sampling
self.resampler = ReSampler(self.particles, self.weights, self.state_lock, self.map_free_space) # An object used for resampling
print("X!")
self.sensor_model = SensorModel(map_msg, self.particles, self.weights, self.state_lock) # An object used for applying sensor model
self.laser_sub = rospy.Subscriber(rospy.get_param("~scan_topic", "/scan"), LaserScan, self.sensor_model.lidar_cb, queue_size=1)
print("XZ!")
self.MOTION_MODEL_TYPE = rospy.get_param("~motion_model", "kinematic") # Whether to use the odometry or kinematics based motion model
print("!~!@#@! MOTION MODEL TYPE", self.MOTION_MODEL_TYPE)
if self.MOTION_MODEL_TYPE == "kinematic":
self.motion_model = KinematicMotionModel(self.particles, self.state_lock) # An object used for applying kinematic motion model
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/sensors/core"), VescStateStamped, self.motion_model.motion_cb, queue_size=1)
elif self.MOTION_MODEL_TYPE == "odometry":
self.motion_model = OdometryMotionModel(self.particles, self.state_lock)# An object used for applying odometry motion model
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/odom"), Odometry, self.motion_model.motion_cb, queue_size=1)
elif self.MOTION_MODEL_TYPE == "learned":
from TorchInclude import torch
learned_motion_model = InternalLearnedMotionModel(self.particles, torch.load("../../lab3/src/tanh50k.pt"))
self.motion_model = KinematicLikeMotionModel(self.particles, self.state_lock, learned_motion_model)
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/sensors/core"), VescStateStamped, self.motion_model.motion_cb, queue_size=1)
elif self.MOTION_MODEL_TYPE == "learned_kinematic":
from TorchInclude import torch
kinematic_motion_model = InternalKinematicMotionModel(particles)
learned_kinematic_motion_model = InternalLearnedKinematicMotionModel(self.particles, kinematic_motion_model, torch.load("../../lab1/src/KinematicDropout0.13Itr5k.pt"))
self.motion_model = KinematicLikeMotionModel(self.particles, self.state_lock, learned_kinematic_motion_model)
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/sensors/core"), VescStateStamped, self.motion_model.motion_cb, queue_size=1)
else:
print "Unrecognized motion model: "+ self.MOTION_MODEL_TYPE
assert(False)
print("A!")
# Use to initialize through rviz. Check clicked_pose_cb for more info
self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose_cb, queue_size=1)
def __init__(self):
self.MAX_PARTICLES = int(rospy.get_param(
"~max_particles")) # The maximum number of particles
self.MAX_VIZ_PARTICLES = int(
rospy.get_param("~max_viz_particles"
)) # The maximum number of particles to visualize
self.noise = bool(rospy.get_param("~noise"))
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.particles = np.zeros(
(self.MAX_PARTICLES,
3)) # Numpy matrix of dimension MAX_PARTICLES x 3
self.weights = np.ones(self.MAX_PARTICLES) / float(
self.MAX_PARTICLES
) # Numpy matrix containig weight for each particle
self.viz_seq = 0
self.INIT_NOISE_POS_SIGMA = 0.2
self.INIT_NOISE_THETA_SIGMA = np.pi / 12
self.state_lock = Lock(
) # A lock used to prevent concurrency issues. You do not need to worry about this
# Use the 'static_map' service (launched by MapServer.launch) to get the map
# Will be used to initialize particles and SensorModel
# Store map in variable called 'map_msg'
# YOUR CODE HERE
rospy.wait_for_service('static_map')
static_map = rospy.ServiceProxy('/static_map', GetMap)
map_msg = static_map().map
# Globally initialize the particles
self.initialize_global(map_msg)
# Publish particle filter state
self.pose_pub = rospy.Publisher(
"/pf/viz/inferred_pose", PoseStamped,
queue_size=1) # Publishes the expected pose
self.particle_pub = rospy.Publisher(
"/pf/viz/particles", PoseArray,
queue_size=1) # Publishes a subsample of the particles
self.pub_tf = tf.TransformBroadcaster(
) # Used to create a tf between the map and the laser for visualization
self.pub_laser = rospy.Publisher(
"/pf/viz/scan", LaserScan,
queue_size=1) # Publishes the most recent laser scan
self.RESAMPLE_TYPE = rospy.get_param(
"~resample_type",
"naiive") # Whether to use naiive or low variance sampling
self.resampler = ReSampler(
self.particles, self.weights,
self.state_lock) # An object used for resampling
self.sensor_model = SensorModel(
map_msg, self.particles, self.weights,
self.state_lock) # An object used for applying sensor model
self.laser_sub = rospy.Subscriber(rospy.get_param(
"~scan_topic", "/scan"),
LaserScan,
self.sensor_model.lidar_cb,
queue_size=1)
self.MOTION_MODEL_TYPE = rospy.get_param(
"~motion_model", "kinematic"
) # Whether to use the odometry or kinematics based motion model
if self.MOTION_MODEL_TYPE == "kinematic":
self.motion_model = KinematicMotionModel(
self.particles, self.state_lock, noise=self.noise
) # An object used for applying kinematic motion model
self.motion_sub = rospy.Subscriber(rospy.get_param(
"~motion_topic", "/vesc/sensors/core"),
VescStateStamped,
self.motion_model.motion_cb,
queue_size=1)
elif self.MOTION_MODEL_TYPE == "odometry":
self.motion_model = OdometryMotionModel(
self.particles, self.state_lock, noise=self.noise
) # An object used for applying odometry motion model
self.motion_sub = rospy.Subscriber(rospy.get_param(
"~motion_topic", "/vesc/odom"),
Odometry,
self.motion_model.motion_cb,
queue_size=1)
else:
print "Unrecognized motion model: " + self.MOTION_MODEL_TYPE
assert (False)
# Use to initialize through rviz. Check clicked_pose_cb for more info
self.pose_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose_cb,
queue_size=1)
print("odom")
"""
if topic == '/initialpose':
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
theta = Utils.quaternion_to_angle(
msg.pose.pose.orientation)
particles = np.repeat(np.array([[x, y, theta]],
dtype=np.float64),
numParticles,
axis=0)
weights = np.ones(numParticles) / numParticles
#print("Particles: ", particles)
sensorModel = SensorModel(map_msg, particles, weights,
state_lock)
kinematicModel = KinematicMotionModel(
particles, state_lock)
resampler = ReSampler(
particles, weights, state_lock,
None) # An object used for resampling
st = t
#print("initalpose")
elif topic == '/pf/ta/viz/inferred_pose':
x = msg.pose.position.x
y = msg.pose.position.y
expected_pose = [x, y]
elif topic == "/vesc/sensors/core" and kinematicModel is not None:
kinematicModel.motion_cb(msg)
elif topic == "/vesc/sensors/servo_position_command" and kinematicModel is not None:
kinematicModel.servo_cb(msg)
elif topic == "/scan" and sensorModel is not None:
def __init__(self, n_particles, n_viz_particles, odometry_topic,
motor_state_topic, servo_state_topic, scan_topic,
laser_ray_step, exclude_max_range_rays, max_range_meters,
speed_to_erpm_offset, speed_to_erpm_gain,
steering_angle_to_servo_offset, steering_angle_to_servo_gain,
car_length):
self.N_PARTICLES = n_particles # The number of particles
# In this implementation, the total number of
# particles is constant
self.N_VIZ_PARTICLES = n_viz_particles # The number of particles to visualize
self.particle_indices = np.arange(
self.N_PARTICLES) # Cached list of particle indices
self.particles = np.zeros(
(self.N_PARTICLES, 3)) # Numpy matrix of dimension N_PARTICLES x 3
self.weights = np.ones(self.N_PARTICLES) / float(
self.N_PARTICLES
) # Numpy matrix containing weight for each particle
self.state_lock = Lock(
) # A lock used to prevent concurrency issues. You do not need to worry about this
self.tfl = tf.TransformListener(
) # Transforms points between coordinate frames
# Get the map
map_msg = rospy.wait_for_message(MAP_TOPIC, OccupancyGrid)
self.map_info = map_msg.info # Save info about map for later use
# Create numpy array representing map for later use
array_255 = np.array(map_msg.data).reshape(
(map_msg.info.height, map_msg.info.width))
self.permissible_region = np.zeros_like(array_255, dtype=bool)
self.permissible_region[
array_255 ==
0] = 1 # Numpy array of dimension (map_msg.info.height, map_msg.info.width),
# With values 0: not permissible, 1: permissible
# Globally initialize the particles
# Publish particle filter state
self.pub_tf = tf.TransformBroadcaster(
) # Used to create a tf between the map and the laser for visualization
self.pose_pub = rospy.Publisher(
PUBLISH_PREFIX + "/inferred_pose", PoseStamped,
queue_size=1) # Publishes the expected pose
self.particle_pub = rospy.Publisher(
PUBLISH_PREFIX + "/particles", PoseArray,
queue_size=1) # Publishes a subsample of the particles
self.pub_laser = rospy.Publisher(
PUBLISH_PREFIX + "/scan", LaserScan,
queue_size=1) # Publishes the most recent laser scan
self.pub_odom = rospy.Publisher(
PUBLISH_PREFIX + "/odom", Odometry,
queue_size=1) # Publishes the path of the car
rospy.sleep(1.0)
self.initialize_global()
self.resampler = ReSampler(
self.particles, self.weights,
self.state_lock) # An object used for resampling
# An object used for applying sensor model
self.sensor_model = SensorModel(scan_topic, laser_ray_step,
exclude_max_range_rays,
max_range_meters, map_msg,
self.particles, self.weights,
car_length, self.state_lock)
# An object used for applying kinematic motion model
self.motion_model = KinematicMotionModel(
motor_state_topic, servo_state_topic, speed_to_erpm_offset,
speed_to_erpm_gain, steering_angle_to_servo_offset,
steering_angle_to_servo_gain, car_length, self.particles,
self.state_lock)
self.permissible_x, self.permissible_y = np.where(
self.permissible_region == 1)
# Parameters/flags/vars for global localization
self.global_localize = True
self.global_suspend = False
self.ents = None
self.ents_sum = 0.0
self.noisy_cnt = 0
self.NUM_REGIONS = 5 # number of regions to partition. Simulation: 25, Real: 5.
self.REGIONAL_ROUNDS = 3 # number of updates for regional localization. Simulation 5, Real: 3.
self.regions = []
self.click_mode = True
self.debug_mode = False
if self.debug_mode:
self.global_localize = True # True when doing global localization
# precompute regions. Each region is represented by arrays of x, y indices on the map
region_size = len(self.permissible_x) / self.NUM_REGIONS
# for i in xrange(self.NUM_REGIONS): # row-major
# self.regions.append((self.permissible_x[i*region_size:(i+1)*region_size],
# self.permissible_y[i*region_size:(i+1)*region_size]))
idx = np.argsort(self.permissible_y) # column-major
_px, _py = self.permissible_x[idx], self.permissible_y[idx]
for i in xrange(self.NUM_REGIONS):
self.regions.append((_px[i * region_size:(i + 1) * region_size],
_py[i * region_size:(i + 1) * region_size]))
# Subscribe to the '/initialpose' topic. Publised by RVIZ. See clicked_pose_cb function in this file for more info
# three different reactions to click on rviz
#self.click_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose_cb, queue_size=1)
self.init_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.reinit_cb,
queue_size=1)
rospy.wait_for_message(scan_topic, LaserScan)
print('Initialization complete')
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()