Esempio n. 1
0
    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
Esempio n. 2
0
    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
Esempio n. 3
0
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)
Esempio n. 4
0
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)
Esempio n. 5
0
    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