-
Notifications
You must be signed in to change notification settings - Fork 31
/
runBehaviorBasedNoisyMapping.m
47 lines (46 loc) · 1.18 KB
/
runBehaviorBasedNoisyMapping.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
%% a test script for simulator class
clear all;
close all;
% generate map for the simulation
size = 15;
resolution = 10;
numObstacles = 8;
space = 5;
filter_on = true;
%p = zeros(size*resolution);
map_gen = MapGenerate(size,size,space,resolution);
[p,map_gen] = map_gen.addBounds(2);
for i = 1:numObstacles
%p = add_random_circle(p);
[p,map_gen] = map_gen.addRandomObstacle(1.0,0.5);
end
map = binaryOccupancyMap(p,resolution);
%% specify some parameters
numRobots = 3;
numSensors = 20;
sensorRange = 5;
showTraj = false;
initial_poses = 5*(rand(3,numRobots).*[0.5;0.5;0]) + [0.5;0.5;0];
robotInfos = cell(1,numRobots);
for i = 1:numRobots
t = "DiffDrive"; % differential drive dynamics
R = 0.1;
L = 0.5;
s = numSensors;
r = sensorRange;
show = showTraj;
robotInfo = RobotInfo(t,R,L,s,r);
robotInfos{i} = robotInfo;
end
swarmInfo = SwarmInfo(numRobots,robotInfos,initial_poses,false);
%% virtual structure simulation
sim = BehaviorBasedMappingNoisySimulation(map,swarmInfo,filter_on);
for i = 1:800
sim = sim.step();
axis([0 15 0 15])
%pause(0.02);
points = sim.mapper.points;
figure(2);
scatter(points(:,1),points(:,2));
axis([0 15 0 15])
end