Commit b8ed8ad2 authored by Federico Ciuffardi's avatar Federico Ciuffardi
Browse files

integrated with ros (1 bug fixed)

parent 1bcdfbfb
......@@ -121,3 +121,307 @@
55.264061 0.000000
55.264061 0.016667
55.264061 0.016666
55.264061 -0.033333
55.264061 -0.000000
55.264061 0.000000
55.264061 0.033334
55.264061 0.000000
55.264061 -0.016667
55.264061 0.016666
55.264061 0.000000
55.264061 -0.016667
55.264061 0.066667
55.264061 -0.033333
55.264061 0.050000
2.400549 0.150000
3.515089 -0.083333
4.389575 0.033333
5.675583 0.000000
6.635802 0.050000
7.750343 -0.066667
8.796296 0.050000
10.099451 -0.050000
11.402606 0.000000
12.500000 0.033333
13.563100 -0.033333
15.020576 0.083333
15.963648 0.033333
16.769548 -0.083333
17.644033 0.000000
18.912895 0.033333
19.701647 -0.083333
20.353224 0.016667
2.400549 0.100000
3.515089 -0.016667
4.389575 -0.016667
5.572702 0.050000
6.652949 -0.016667
7.475995 0.000000
8.624828 0.066667
9.927983 -0.066667
10.853909 -0.016667
12.242799 0.050000
13.305898 -0.033333
14.094650 0.050000
15.072017 -0.016667
15.963648 0.016667
16.598080 -0.083333
17.952675 0.066667
18.810015 -0.016667
2.400549 0.116667
3.532236 -0.050000
4.406722 0.000000
5.658436 0.016667
6.618656 0.016666
7.750343 0.033334
8.796296 -0.050000
10.099451 0.033333
2.400549 0.116667
2.400549 0.116667
3.532236 -0.050000
4.389575 0.033333
5.555556 0.000000
6.652949 -0.033333
7.475995 -0.000000
8.624828 0.050000
9.893690 -0.000000
10.768176 -0.050000
12.277092 0.083333
13.288752 -0.033333
14.351852 0.066667
15.209190 -0.050000
15.843621 -0.016667
16.220850 -0.083333
16.666668 0.083333
17.421125 -0.016667
18.569958 0.050000
19.547325 -0.050000
20.610424 0.016666
21.965021 0.083333
23.148148 -0.066667
24.039782 0.033334
24.931414 -0.000000
25.840193 0.033333
2.400549 0.150000
2.400549 0.133333
3.532236 -0.066667
4.389575 0.016667
5.555556 0.016667
6.652949 0.016667
7.458848 -0.016667
8.624828 0.016667
9.893690 -0.016667
10.733882 -0.033333
12.242799 0.033333
13.374485 0.033334
13.974624 -0.050000
14.317558 -0.016667
14.523320 0.016667
14.831962 0.000000
15.432098 -0.016667
2.400549 0.133333
3.532236 -0.066667
4.406722 -0.016667
5.641289 0.033333
6.635802 -0.016667
7.767489 0.016667
8.830590 0.016667
10.082304 0.050000
11.351166 -0.066667
12.448560 0.033334
13.528806 -0.016667
14.986282 0.066667
15.997942 0.033333
17.438272 -0.033333
19.050070 0.033333
19.118656 -0.166667
20.010288 0.083333
20.713306 -0.016667
21.227709 -0.016667
21.724966 0.000000
22.393690 0.033333
22.976681 -0.016667
23.679697 0.050000
24.399862 -0.033333
25.205761 0.000000
25.222910 -0.050000
25.823044 0.033333
26.080248 0.033334
26.371742 -0.033334
26.457478 -0.016667
26.954731 0.100000
27.194786 -0.033334
27.554871 -0.050000
28.017834 0.066667
28.155008 -0.066667
28.515089 0.000000
28.772291 0.033333
28.960905 -0.016666
29.200958 0.000000
29.200958 -0.066667
29.200958 0.016667
29.766804 0.066667
30.281206 0.033333
30.366941 -0.083333
31.121401 0.066667
31.532923 -0.016667
31.995884 -0.033334
32.475994 0.083333
32.733196 -0.066667
32.921810 -0.016666
33.333336 0.033333
34.156380 0.100000
34.567902 -0.100000
35.288067 0.050000
35.956791 0.033333
36.076817 -0.100000
36.608368 0.033333
36.779835 -0.050000
36.779835 -0.016667
36.779835 0.000000
2.400549 0.116667
3.515089 -0.050000
4.406722 0.033333
5.658436 -0.033334
6.687243 0.050000
7.887517 -0.033333
9.070644 -0.016666
10.065158 0.016667
11.111112 -0.016667
11.882716 -0.016667
12.860082 0.033333
13.477365 0.033334
13.974624 -0.066667
14.729081 0.066667
15.226337 -0.033333
15.294925 -0.016667
15.346365 -0.033333
15.637859 0.033333
16.049381 0.083333
16.117970 -0.100000
16.580933 0.066667
16.580933 -0.066667
16.820988 0.050000
17.438272 0.033333
17.952675 -0.050000
18.655691 0.083333
19.238684 -0.016667
19.993141 -0.000000
20.730452 0.033333
20.730452 -0.150000
20.730452 0.000000
20.730452 0.016667
20.730452 -0.016667
20.730452 0.000000
2.400549 0.116667
3.515089 -0.050000
4.372428 -0.016667
5.572702 0.050000
6.652949 -0.016666
7.475995 -0.033334
8.710563 0.050000
9.910837 0.016667
11.128258 -0.050000
12.277092 0.016667
13.563100 0.033334
14.797668 -0.000000
15.294925 0.000000
16.083675 -0.016667
16.649521 -0.033333
17.575445 0.016666
18.775721 0.066667
20.061728 -0.050000
21.159121 0.016667
22.273663 0.033334
23.010973 -0.050000
24.176956 0.016667
25.102880 0.033333
25.925926 -0.016667
26.457478 0.083333
26.526064 -0.166667
26.594650 0.166667
26.800411 -0.016667
27.074760 -0.083333
27.572018 0.033334
27.674898 -0.033334
27.897804 0.016667
28.240740 0.000000
28.652264 -0.033333
29.561043 0.100000
30.435530 0.016667
31.104252 -0.050000
31.584362 -0.066667
32.887520 0.166667
33.676270 -0.083333
34.104939 -0.083333
34.567902 -0.000000
35.442387 0.133333
36.471195 -0.016667
37.345676 0.033333
38.477367 0.016667
39.248970 -0.050000
40.243484 -0.016667
41.529491 0.050000
42.729767 0.100000
43.655693 -0.133333
44.907406 0.200000
45.730453 -0.133333
46.159122 -0.033334
46.724968 -0.016667
47.376545 -0.000000
48.113857 0.033334
48.696846 0.000000
49.348423 0.000000
50.034298 -0.000000
50.685871 -0.016667
51.457478 0.033333
51.851852 -0.050000
52.794926 0.216667
53.686558 -0.116667
54.269547 -0.066667
54.852539 0.066667
54.972565 -0.083333
55.298351 0.183333
55.521263 -0.133333
55.761318 -0.016667
56.584366 0.066667
57.407410 0.050000
58.076130 -0.133333
58.727711 0.266667
58.984913 -0.200000
59.482170 -0.050000
59.842247 0.050000
2.400549 0.100000
3.532236 -0.066667
4.406722 0.033333
5.658436 0.033334
6.635802 -0.000000
7.750343 0.000000
8.813443 0.016667
10.099451 -0.016667
11.385460 0.000000
12.465707 0.016667
13.545953 -0.033334
15.003429 0.050000
15.963648 0.066667
16.803841 -0.100000
17.626886 0.016666
18.775721 0.000000
19.581619 -0.000000
19.770233 -0.050000
20.164608 0.050000
20.850479 -0.033334
21.502058 0.033333
22.050755 0.050000
22.205076 -0.066667
22.222223 -0.050000
22.222223 0.000000
22.222223 -0.016667
22.222223 0.000000
22.222223 0.000000
22.222223 -0.000000
22.222223 0.000000
22.222223 -0.000000
22.222223 0.000000
22.222223 -0.016667
22.222223 0.033333
......@@ -121,3 +121,307 @@
55.264061 0.166667
55.264061 0.183333
55.264061 0.200000
55.264061 0.166667
55.264061 0.166667
55.264061 0.166667
55.264061 0.200000
55.264061 0.200000
55.264061 0.183333
55.264061 0.200000
55.264061 0.200000
55.264061 0.183333
55.264061 0.250000
55.264061 0.216667
55.264061 0.266667
2.400549 0.150000
3.515089 0.066667
4.389575 0.100000
5.675583 0.100000
6.635802 0.150000
7.750343 0.083333
8.796296 0.133333
10.099451 0.083333
11.402606 0.083333
12.500000 0.116667
13.563100 0.083333
15.020576 0.166667
15.963648 0.200000
16.769548 0.116667
17.644033 0.116667
18.912895 0.150000
19.701647 0.066667
20.353224 0.083333
2.400549 0.100000
3.515089 0.083333
4.389575 0.066667
5.572702 0.116667
6.652949 0.100000
7.475995 0.100000
8.624828 0.166667
9.927983 0.100000
10.853909 0.083333
12.242799 0.133333
13.305898 0.100000
14.094650 0.150000
15.072017 0.133333
15.963648 0.150000
16.598080 0.066667
17.952675 0.133333
18.810015 0.116667
2.400549 0.116667
3.532236 0.066667
4.406722 0.066667
5.658436 0.083333
6.618656 0.100000
7.750343 0.133333
8.796296 0.083333
10.099451 0.116667
2.400549 0.116667
2.400549 0.116667
3.532236 0.066667
4.389575 0.100000
5.555556 0.100000
6.652949 0.066667
7.475995 0.066667
8.624828 0.116667
9.893690 0.116667
10.768176 0.066667
12.277092 0.150000
13.288752 0.116667
14.351852 0.183333
15.209190 0.133333
15.843621 0.116667
16.220850 0.033333
16.666668 0.116667
17.421125 0.100000
18.569958 0.150000
19.547325 0.100000
20.610424 0.116667
21.965021 0.200000
23.148148 0.133333
24.039782 0.166667
24.931414 0.166667
25.840193 0.200000
2.400549 0.150000
2.400549 0.133333
3.532236 0.066667
4.389575 0.083333
5.555556 0.100000
6.652949 0.116667
7.458848 0.100000
8.624828 0.116667
9.893690 0.100000
10.733882 0.066667
12.242799 0.100000
13.374485 0.133333
13.974624 0.083333
14.317558 0.066667
14.523320 0.083333
14.831962 0.083333
15.432098 0.066667
2.400549 0.133333
3.532236 0.066667
4.406722 0.050000
5.641289 0.083333
6.635802 0.066667
7.767489 0.083333
8.830590 0.100000
10.082304 0.150000
11.351166 0.083333
12.448560 0.116667
13.528806 0.100000
14.986282 0.166667
15.997942 0.200000
17.438272 0.166667
19.050070 0.200000
19.118656 0.033333
20.010288 0.116667
20.713306 0.100000
21.227709 0.083333
21.724966 0.083333
22.393690 0.116667
22.976681 0.100000
23.679697 0.150000
24.399862 0.116667
25.205761 0.116667
25.222910 0.066667
25.823044 0.100000
26.080248 0.133333
26.371742 0.100000
26.457478 0.083333
26.954731 0.183333
27.194786 0.150000
27.554871 0.100000
28.017834 0.166667
28.155008 0.100000
28.515089 0.100000
28.772291 0.133333
28.960905 0.116667
29.200958 0.116667
29.200958 0.050000
29.200958 0.066667
29.766804 0.133333
30.281206 0.166667
30.366941 0.083333
31.121401 0.150000
31.532923 0.133333
31.995884 0.100000
32.475994 0.183333
32.733196 0.116667
32.921810 0.100000
33.333336 0.133333
34.156380 0.233333
34.567902 0.133333
35.288067 0.183333
35.956791 0.216667
36.076817 0.116667
36.608368 0.150000
36.779835 0.100000
36.779835 0.083333
36.779835 0.083333
2.400549 0.116667
3.515089 0.066667
4.406722 0.100000
5.658436 0.066667
6.687243 0.116667
7.887517 0.083333
9.070644 0.066667
10.065158 0.083333
11.111112 0.066667
11.882716 0.050000
12.860082 0.083333
13.477365 0.116667
13.974624 0.050000
14.729081 0.116667
15.226337 0.083333
15.294925 0.066667
15.346365 0.033333
15.637859 0.066667
16.049381 0.150000
16.117970 0.050000
16.580933 0.116667
16.580933 0.050000
16.820988 0.100000
17.438272 0.133333
17.952675 0.083333
18.655691 0.166667
19.238684 0.150000
19.993141 0.150000
20.730452 0.183333
20.730452 0.033333
20.730452 0.033333
20.730452 0.050000
20.730452 0.033333
20.730452 0.033333
2.400549 0.116667
3.515089 0.066667
4.372428 0.050000
5.572702 0.100000
6.652949 0.083333
7.475995 0.050000
8.710563 0.100000
9.910837 0.116667
11.128258 0.066667
12.277092 0.083333
13.563100 0.116667
14.797668 0.116667
15.294925 0.116667
16.083675 0.100000
16.649521 0.066667
17.575445 0.083333
18.775721 0.150000
20.061728 0.100000
21.159121 0.116667
22.273663 0.150000
23.010973 0.100000
24.176956 0.116667
25.102880 0.150000
25.925926 0.133333
26.457478 0.216667
26.526064 0.050000
26.594650 0.216667
26.800411 0.200000
27.074760 0.116667
27.572018 0.150000
27.674898 0.116667
27.897804 0.133333
28.240740 0.133333
28.652264 0.100000
29.561043 0.200000
30.435530 0.216667
31.104252 0.166667
31.584362 0.100000
32.887520 0.266667
33.676270 0.183333
34.104939 0.100000
34.567902 0.100000
35.442387 0.233333
36.471195 0.216667
37.345676 0.250000
38.477367 0.266667
39.248970 0.216667
40.243484 0.200000
41.529491 0.250000
42.729767 0.350000
43.655693 0.216667
44.907406 0.416667
45.730453 0.283334
46.159122 0.250000
46.724968 0.233333
47.376545 0.233333
48.113857 0.266667
48.696846 0.266667
49.348423 0.266667
50.034298 0.266667
50.685871 0.250000
51.457478 0.283333
51.851852 0.233333
52.794926 0.450000
53.686558 0.333333
54.269547 0.266667
54.852539 0.333333
54.972565 0.250000
55.298351 0.433333
55.521263 0.300000
55.761318 0.283333
56.584366 0.350000
57.407410 0.400000
58.076130 0.266667
58.727711 0.533333
58.984913 0.333333
59.482170 0.283333
59.842247 0.333333
2.400549 0.100000
3.532236 0.033333
4.406722 0.066667
5.658436 0.100000
6.635802 0.100000
7.750343 0.100000
8.813443 0.116667
10.099451 0.100000
11.385460 0.100000
12.465707 0.116667
13.545953 0.083333
15.003429 0.133333
15.963648 0.200000
16.803841 0.100000
17.626886 0.116667
18.775721 0.116667
19.581619 0.116667
19.770233 0.066667
20.164608 0.116667
20.850479 0.083333
21.502058 0.116667
22.050755 0.166667
22.205076 0.100000
22.222223 0.050000
22.222223 0.050000
22.222223 0.033333
22.222223 0.033333
22.222223 0.033333
22.222223 0.033333
22.222223 0.033334
22.222223 0.033333
22.222223 0.033333
22.222223 0.016667
22.222223 0.050000
......@@ -742,10 +742,10 @@ void GVDIC::update_possible_crit_distance_map(){
// update frontiers
for(auto it = free_frontier_to_false.begin(); it != free_frontier_to_false.end(); it++){
pos frontier = (*it);
// cout<<"free frontier to false "<<frontier<<endl;
//cout<<"free frontier to false "<<frontier<<endl;
pos critical_to_update = disMapIC->cell_data[frontier].first_source;
if(critical_to_update != NULL_POS){
// cout<<"criticals to update "<<critical_to_update<<" ";
//cout<<"criticals to update "<<critical_to_update<<" ";
erase_frontier(critical_to_update, frontier);
}
// cout<<endl;
......@@ -784,7 +784,7 @@ void GVDIC::update_criticals_mind_f(){
void GVDIC::erase_frontier(pos critical, pos frontier){
criticals[critical].erase_frontier(frontier, disMapIC->cell_data[frontier].dist);
criticals[critical].erase_frontier(frontier, disMapIC->cell_data[frontier].dist);//may set to_update to true
if(criticals[critical].frontiers.size() == 0){
criticals.erase(critical);
......@@ -798,8 +798,6 @@ void GVDIC::erase_frontier(pos critical, pos frontier){
void GVDIC::add_frontier(pos critical, pos frontier){
if(!is_elem(criticals, critical)){
criticals[critical] = critical_info2();
criticals[critical].mind_f = std::pair<pos, float>(pos(),MAXFLOAT);
criticals[critical].to_update = false;
}