class Visualizer:
def __init__(self):
# Dimensions
self.screen_width = 1366
self.screen_height = 768
pygame.init()
pygame.display.set_caption('Git Boom Visualizer')
# Screen size
self.screen = pygame.display.set_mode((self.screen_width, self.screen_height), pygame.DOUBLEBUF)
self.particle_manager = ParticleManager(750, self.screen_width, self.screen_height)
# Fps
self.clock = pygame.time.Clock()
self.fps = 60
self.lock = Lock()
self.boost_left = 0
self.font = pygame.font.SysFont("Source Sans Pro", 24)
def post(self):
with self.lock:
self.boost_left = 5000
def run(self):
while True:
event = pygame.event.poll()
if event.type == pygame.QUIT:
break
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_SPACE:
with self.lock:
self.boost_left = 5000
if self.lock.acquire(False):
if self.boost_left > 0:
self.boost_left -= 10
print self.boost_left
self.lock.release()
ms_elapsed = self.clock.tick(self.fps)
self.particle_manager.update(ms_elapsed, self.boost_left)
label = self.font.render(str(self.clock.get_fps()), 1, (255, 255, 255))
self.screen.fill((0, 0, 0))
self.screen.blit(label, (10, 10))
self.particle_manager.draw(self.screen)
pygame.display.flip()
def __init__(self):
rospy.init_node('pf')
self.particle_publisher = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.particle_manager = ParticleManager()
self.sensor_manager = SensorManager()
self.particle_manager.init_particles(self.occupancy_field)
self.scanDistance = 0.2
self.scanAngle = 0.5
self.moved = (0, 0)
class World:
particle_texture = pygame.image.load(
"../assets/textures/particle_white.png")
def __init__(self):
self.particle_mng = ParticleManager()
self.gravity = Vector2()
def set_gravity(self, gravity):
self.gravity = gravity
def refresh_states(self):
self.particle_mng.apply_global_force(self.gravity, True)
def add_particle(self, particle):
self.particle_mng.subscribe_particle(particle)
particle.set_static_forces(self.gravity, True)
def update(self, dt):
self.particle_mng.update(dt)
def integrate(self, dt):
self.particle_mng.integrate(dt)
def render(self, display):
for p in self.particle_mng.particles:
display.blit(self.particle_texture, p.position.get())
def print_particles(self):
for p in self.particle_mng.particles:
print(p)
def __init__(self):
# Dimensions
self.screen_width = 1366
self.screen_height = 768
pygame.init()
pygame.display.set_caption('Git Boom Visualizer')
# Screen size
self.screen = pygame.display.set_mode((self.screen_width, self.screen_height), pygame.DOUBLEBUF)
self.particle_manager = ParticleManager(750, self.screen_width, self.screen_height)
# Fps
self.clock = pygame.time.Clock()
self.fps = 60
self.lock = Lock()
self.boost_left = 0
self.font = pygame.font.SysFont("Source Sans Pro", 24)
def __init__(self):
self.particle_mng = ParticleManager()
self.gravity = Vector2()
class ParticleFilter(object):
""" The class that represents a Particle Filter ROS Node
"""
def __init__(self):
rospy.init_node('pf')
self.particle_publisher = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.particle_manager = ParticleManager()
self.sensor_manager = SensorManager()
self.particle_manager.init_particles(self.occupancy_field)
self.scanDistance = 0.2
self.scanAngle = 0.5
self.moved = (0, 0)
def run(self):
r = rospy.Rate(5)
while not (rospy.is_shutdown()):
#Create a pose array with all of the current particles for easy viewing in Rviz
poseArray = PoseArray(header=Header(
seq=10, stamp=rospy.get_rostime(), frame_id='map'))
for particle in self.particle_manager.particles:
poseArray.poses.append(
self.transform_helper.convert_xy_and_theta_to_pose(
particle.x, particle.y, particle.theta))
#Publish the pose array of the current particles
self.particle_publisher.publish(poseArray)
#If we are not already looking for a new laser scan but have moved a moderate distance, set the flag to look for a new laser scan
if not self.sensor_manager.newLaserScan and (
(getDistance(self.sensor_manager.lastScan[0],
self.sensor_manager.lastScan[1],
self.sensor_manager.pose[0],
self.sensor_manager.pose[1]) > self.scanDistance)
or self.transform_helper.angle_diff(
math.radians(self.sensor_manager.pose[2]),
math.radians(self.sensor_manager.lastScan[2])) >
self.scanAngle):
#Set the flag to look for a new laser scan and mark where we last took laser scan data so we know how far we can move before taking another
self.sensor_manager.newLaserScan = True
self.moved = (getDistance(self.sensor_manager.lastScan[0],
self.sensor_manager.lastScan[1],
self.sensor_manager.pose[0],
self.sensor_manager.pose[1]),
math.degrees(
self.transform_helper.angle_diff(
self.sensor_manager.pose[2],
self.sensor_manager.lastScan[2])))
#Here, we are currently looking for a new laser scan and haven't moved sufficient distance to warrant a new one
elif self.sensor_manager.newLaserScan:
#While we are looking for a new laser scan, wait for the data to come in so that we don't process the particles on old data
while self.sensor_manager.newLaserScan:
continue
#Tranform all particles to match the transform the robot has done since the last laser scan and particle trimming
self.particle_manager.transform_particles(
self.moved[0], self.moved[1])
#Update the particle probabilities so we know how likely each particle is based on current laser scan data
self.particle_manager.update_probabilities(
self.sensor_manager.minRange, self.occupancy_field,
self.sensor_manager.closestAngles)
#Do a random weighted selection of all of the current particles to choose particles to be passed on to the next round of laser scan data
self.particle_manager.trim_particles()
#Generate new particles around the kept particles with some added noise
self.particle_manager.generate_particles()
#Create and publish the pose array with the new particles for viewing in Rviz
poseArray = PoseArray(header=Header(
seq=10, stamp=rospy.get_rostime(), frame_id='map'))
for particle in self.particle_manager.particles:
poseArray.poses.append(
self.transform_helper.convert_xy_and_theta_to_pose(
particle.x, particle.y, particle.theta))
self.particle_publisher.publish(poseArray)
self.transform_helper.send_last_map_to_odom_transform()