def __init__(self, n_particles, copy=False):
self.map = OccupancyGridMap(copy)
if not copy:
x_co = random.gauss(0, 0.2)
y_co = random.gauss(0, 0.2)
theta = random.gauss(0, 0.05)
self.pose = Pose(x_co, y_co, theta)
self.weight = 1.0 / n_particles
def __init__(self, n_particles, copy=False):
self.map = OccupancyGridMap(copy)
if not copy:
x_co = random.gauss(0, 0.2)
y_co = random.gauss(0, 0.2)
theta = random.gauss(0, 0.05)
self.pose = Pose(x_co, y_co, theta)
self.weight = 1.0 / n_particles
class Particle:
def __init__(self, n_particles, copy=False):
self.map = OccupancyGridMap(copy)
if not copy:
x_co = random.gauss(0, 0.2)
y_co = random.gauss(0, 0.2)
theta = random.gauss(0, 0.05)
self.pose = Pose(x_co, y_co, theta)
self.weight = 1.0 / n_particles
def update_motion(self, motion, timedelta):
self.pose = motion_model.calculate_pose(self.pose, motion, timedelta, self.map)
self.map.add_pose(self.pose)
def update_sensor(self, update):
self.weight += sensor_model.calc_weight(update, self.pose, self.map)
sensor_model.update_map(update, self.pose, self.map)
class Particle:
def __init__(self, n_particles, copy=False):
self.map = OccupancyGridMap(copy)
if not copy:
x_co = random.gauss(0, 0.2)
y_co = random.gauss(0, 0.2)
theta = random.gauss(0, 0.05)
self.pose = Pose(x_co, y_co, theta)
self.weight = 1.0 / n_particles
def update_motion(self, motion, timedelta):
self.pose = motion_model.calculate_pose(self.pose, motion, timedelta,
self.map)
self.map.add_pose(self.pose)
def update_sensor(self, update):
self.weight += sensor_model.calc_weight(update, self.pose, self.map)
sensor_model.update_map(update, self.pose, self.map)
env_config = utils.load_config('../config/environment.yaml')
agent_config = utils.load_config('../config/agent.yaml')
grid_map_config = utils.load_config('../config/grid_map.yaml')
# Initialize 2D Environment
env = utils.load_env_from_img(env_config['img_src'])
env = cv2.resize(env, (round(env_config['scale'] * env.shape[1]),
round(env_config['scale'] * env.shape[0])),
interpolation=cv2.INTER_LINEAR)
# Initialize Agent
robot_pos = np.array([150.0, 100.0, 0.0])
robot = Robot(robot_pos, agent_config['robot'], agent_config['sensor'])
# Initialize Grid Map
m = OccupancyGridMap(grid_map_config, grid_size=1.0)
sensor_data = robot.measure(env)
SensorMapping(m, robot.pose, robot.sensor.config, sensor_data)
# Initialize Particle
pf = ParticleFilter(robot_pos.copy(), agent_config['robot'],
agent_config['sensor'], copy.deepcopy(m), 10)
img = Draw(env, 1, robot.pose, sensor_data, robot.sensor.config)
mimg = AdaptiveGetMap(m)
cv2.imshow('view', img)
cv2.imshow('map', mimg)
# Main Loop
while (1):
# Input Control