merge sim rosbuzz

This commit is contained in:
dave 2018-11-05 04:15:31 -05:00
commit 7e5a3b1624
18 changed files with 391 additions and 248 deletions

View File

@ -13,6 +13,7 @@ include "act/neighborcomm.bzz"
TARGET_ALTITUDE = 15.0 # m. TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps PICTURE_WAIT = 20 # steps
WP_STIG = 8
path_it = 0 path_it = 0
pic_time = 0 pic_time = 0
@ -100,7 +101,7 @@ firsttimeinwp = 1
wpreached = 1 wpreached = 1
function indiWP() { function indiWP() {
if(firsttimeinwp) { if(firsttimeinwp) {
v_wp = stigmergy.create(8) v_wp = stigmergy.create(WP_STIG)
storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude) storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
firsttimeinwp = 0 firsttimeinwp = 0
} }
@ -111,21 +112,25 @@ function indiWP() {
storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude) storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
return return
} else { } else {
v_wp.put(rc_goto.id,{.lat=rc_goto.latitude, .lon=rc_goto.longitude, .pro=0}) var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0)
v_wp.put(rc_goto.id,ls)
reset_rc() reset_rc()
} }
} }
wp = v_wp.get(id) #if(vstig_buzzy == 0) {
if(wp!=nil) { wpi = v_wp.get(id)
#log(wp.pro,wpreached) if(wpi!=nil) {
if(wp.pro == 0) { wp = unpackWP2i(wpi)
wpreached = 0 if(wp.pro == 0) {
storegoal(wp.lat, wp.lon, pose.position.altitude) wpreached = 0
v_wp.put(id,{.lat=wp.lat, .lon=wp.lon, .pro=1}) storegoal(wp.lat, wp.lon, pose.position.altitude)
return var ls = packWP2i(wp.lat, wp.lon, 1)
} v_wp.put(id,ls)
} return
}
}
#}
if(wpreached!=1) if(wpreached!=1)
goto_gps(indiWP_done) goto_gps(indiWP_done)

View File

@ -102,21 +102,6 @@ function is_in(dictionary, x, y){
return 0 return 0
} }
function table_print(t) {
foreach(t, function(key, value) {
log(key, " -> ", value)
})
}
function table_copy(t) {
var t2 = {}
foreach(t, function(key, value) {
t2[key] = value
})
return t2
}
function print_pos(t) { function print_pos(t) {
var ir=1 var ir=1
while(ir <= size(t)) { while(ir <= size(t)) {

View File

@ -6,6 +6,7 @@ include "taskallocate/graphs/shapes_P.bzz"
include "taskallocate/graphs/shapes_O.bzz" include "taskallocate/graphs/shapes_O.bzz"
include "taskallocate/graphs/shapes_L.bzz" include "taskallocate/graphs/shapes_L.bzz"
include "taskallocate/graphs/shapes_Y.bzz" include "taskallocate/graphs/shapes_Y.bzz"
#include "utils/table.bzz"
ROBOT_RADIUS = 50 ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
@ -82,60 +83,6 @@ m_lockstig = 1
# Lennard-Jones parameters, may need change # Lennard-Jones parameters, may need change
EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots
#
#return the number of value in table
#
function count(table,value){
var number=0
var i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
}
i=i+1
}
return number
}
#
#map from int to state
#
function i2s(value){
if(value==1){
return "GRAPH_FREE"
}
else if(value==2){
return "GRAPH_ASKING"
}
else if(value==3){
return "GRAPH_JOINING"
}
else if(value==4){
return "GRAPH_JOINED"
}
else if(value==5){
return "GRAPH_LOCK"
}
}
#
#map from state to int
#
function s2i(value){
if(value=="GRAPH_FREE"){
return 1
}
else if(value=="GRAPH_ASKING"){
return 2
}
else if(value=="GRAPH_JOINING"){
return 3
}
else if(value=="GRAPH_JOINED"){
return 4
}
else if(value=="GRAPH_LOCK"){
return 5
}
}
# #
#map form int to response #map form int to response
# #

View File

@ -1,89 +1,4 @@
0,-73.1531935978886,45.5084960903092,15,15 1,-73.609219,45.510336,1
1,-73.1530989420915,45.5085624255498,15,15 2,-73.608913,45.510723,0
2,-73.1530042862771,45.5086287608025,15,15 3,-73.610035,45.510182,1
3,-73.1529096304626,45.5086950960552,15,15 4,-73.609235,45.510114,0
4,-73.1529458247399,45.5087204611798,15,15
5,-73.1530404805543,45.5086541259271,15,15
6,-73.1531351363515,45.5085877906865,15,15
7,-73.1532297921486,45.508521455446,15,15
8,-73.1533244479457,45.5084551202054,15,15
9,-73.1533606422273,45.508480485333,15,15
10,-73.1532659864302,45.5085468205736,15,15
11,-73.153171330633,45.5086131558142,15,15
12,-73.1530766748359,45.5086794910548,15,15
13,-73.1529820190215,45.5087458263075,15,15
14,-73.1530182132901,45.5087711914261,15,15
15,-73.1531128691045,45.5087048561733,15,15
16,-73.1532075249016,45.5086385209327,15,15
17,-73.1533021806988,45.5085721856922,15,15
18,-73.1533968364959,45.5085058504516,15,15
19,-73.1534330307645,45.5085312155701,15,15
20,-73.1533383749674,45.5085975508107,15,15
21,-73.1532437191702,45.5086638860513,15,15
22,-73.1531490633731,45.5087302212919,15,15
23,-73.1530544075587,45.5087965565446,15,15
24,-73.1530906018273,45.5088219216632,15,15
25,-73.1531852576417,45.5087555864105,15,15
26,-73.1532799134389,45.5086892511699,15,15
27,-73.153374569236,45.5086229159293,15,15
28,-73.1534692250331,45.5085565806887,15,15
29,-73.1535054193017,45.5085819458072,15,15
30,-73.1534107635046,45.5086482810478,15,15
31,-73.1533161077075,45.5087146162884,15,15
32,-73.1532214519103,45.508780951529,15,15
33,-73.1531267960959,45.5088472867817,15,15
34,-73.1531629903645,45.5088726519003,15,15
35,-73.1532576461789,45.5088063166476,15,15
36,-73.1533523019761,45.508739981407,15,15
37,-73.1534469577732,45.5086736461664,15,15
38,-73.1535416135703,45.5086073109258,15,15
39,-73.1535778078389,45.5086326760444,15,15
40,-73.1534831520418,45.5086990112849,15,15
41,-73.1533884962447,45.5087653465255,15,15
42,-73.1532938404476,45.5088316817661,15,15
43,-73.1531991846331,45.5088980170188,15,15
44,-73.1532353789017,45.5089233821374,15,15
45,-73.1533300347162,45.5088570468847,15,15
46,-73.1534246905133,45.5087907116441,15,15
47,-73.1535193463104,45.5087243764035,15,15
48,-73.1536140021075,45.5086580411629,15,15
49,-73.1536501963762,45.5086834062815,15,15
50,-73.153555540579,45.508749741522,15,15
51,-73.1534608847819,45.5088160767626,15,15
52,-73.1533662289848,45.5088824120032,15,15
53,-73.1532715731703,45.508948747256,15,15
54,-73.1533077674389,45.5089741123745,15,15
55,-73.1534024232534,45.5089077771218,15,15
56,-73.1534970790505,45.5088414418812,15,15
57,-73.1535917348476,45.5087751066406,15,15
58,-73.1536863906448,45.5087087714,15,15
59,-73.1537225849134,45.5087341365186,15,15
60,-73.1536279291163,45.5088004717592,15,15
61,-73.1535332733191,45.5088668069997,15,15
62,-73.153438617522,45.5089331422403,15,15
63,-73.1533439617076,45.5089994774931,15,15
64,-73.1533801559762,45.5090248426116,15,15
65,-73.1534748117906,45.5089585073589,15,15
66,-73.1535694675877,45.5088921721183,15,15
67,-73.1536641233849,45.5088258368777,15,15
68,-73.153758779182,45.5087595016371,15,15
69,-73.1537949734506,45.5087848667557,15,15
70,-73.1537003176535,45.5088512019963,15,15
71,-73.1536056618563,45.5089175372369,15,15
72,-73.1535110060592,45.5089838724775,15,15
73,-73.1534163502448,45.5090502077302,15,15
74,-73.1534525445134,45.5090755728487,15,15
75,-73.1535472003278,45.509009237596,15,15
76,-73.1536418561249,45.5089429023554,15,15
77,-73.1537365119221,45.5088765671148,15,15
78,-73.1538311677192,45.5088102318742,15,15
79,-73.1538673619878,45.5088355969928,15,15
80,-73.1537727061907,45.5089019322334,15,15
81,-73.1536780503936,45.508968267474,15,15
82,-73.1535833945964,45.5090346027146,15,15
83,-73.153488738782,45.5091009379673,15,15
84,-73.1535249330852,45.5091263031101,15,15
85,-73.1536195888996,45.5090599678574,15,15
86,-73.1537142446968,45.5089936326168,15,15
87,-73.1538089004939,45.5089272973762,15,15
88,-73.153903556291,45.5088609621356,15,15
1 0 1 -73.1531935978886 -73.609219 45.5084960903092 45.510336 15 1 15
2 1 2 -73.1530989420915 -73.608913 45.5085624255498 45.510723 15 0 15
3 2 3 -73.1530042862771 -73.610035 45.5086287608025 45.510182 15 1 15
4 3 4 -73.1529096304626 -73.609235 45.5086950960552 45.510114 15 0 15
4 -73.1529458247399 45.5087204611798 15 15
5 -73.1530404805543 45.5086541259271 15 15
6 -73.1531351363515 45.5085877906865 15 15
7 -73.1532297921486 45.508521455446 15 15
8 -73.1533244479457 45.5084551202054 15 15
9 -73.1533606422273 45.508480485333 15 15
10 -73.1532659864302 45.5085468205736 15 15
11 -73.153171330633 45.5086131558142 15 15
12 -73.1530766748359 45.5086794910548 15 15
13 -73.1529820190215 45.5087458263075 15 15
14 -73.1530182132901 45.5087711914261 15 15
15 -73.1531128691045 45.5087048561733 15 15
16 -73.1532075249016 45.5086385209327 15 15
17 -73.1533021806988 45.5085721856922 15 15
18 -73.1533968364959 45.5085058504516 15 15
19 -73.1534330307645 45.5085312155701 15 15
20 -73.1533383749674 45.5085975508107 15 15
21 -73.1532437191702 45.5086638860513 15 15
22 -73.1531490633731 45.5087302212919 15 15
23 -73.1530544075587 45.5087965565446 15 15
24 -73.1530906018273 45.5088219216632 15 15
25 -73.1531852576417 45.5087555864105 15 15
26 -73.1532799134389 45.5086892511699 15 15
27 -73.153374569236 45.5086229159293 15 15
28 -73.1534692250331 45.5085565806887 15 15
29 -73.1535054193017 45.5085819458072 15 15
30 -73.1534107635046 45.5086482810478 15 15
31 -73.1533161077075 45.5087146162884 15 15
32 -73.1532214519103 45.508780951529 15 15
33 -73.1531267960959 45.5088472867817 15 15
34 -73.1531629903645 45.5088726519003 15 15
35 -73.1532576461789 45.5088063166476 15 15
36 -73.1533523019761 45.508739981407 15 15
37 -73.1534469577732 45.5086736461664 15 15
38 -73.1535416135703 45.5086073109258 15 15
39 -73.1535778078389 45.5086326760444 15 15
40 -73.1534831520418 45.5086990112849 15 15
41 -73.1533884962447 45.5087653465255 15 15
42 -73.1532938404476 45.5088316817661 15 15
43 -73.1531991846331 45.5088980170188 15 15
44 -73.1532353789017 45.5089233821374 15 15
45 -73.1533300347162 45.5088570468847 15 15
46 -73.1534246905133 45.5087907116441 15 15
47 -73.1535193463104 45.5087243764035 15 15
48 -73.1536140021075 45.5086580411629 15 15
49 -73.1536501963762 45.5086834062815 15 15
50 -73.153555540579 45.508749741522 15 15
51 -73.1534608847819 45.5088160767626 15 15
52 -73.1533662289848 45.5088824120032 15 15
53 -73.1532715731703 45.508948747256 15 15
54 -73.1533077674389 45.5089741123745 15 15
55 -73.1534024232534 45.5089077771218 15 15
56 -73.1534970790505 45.5088414418812 15 15
57 -73.1535917348476 45.5087751066406 15 15
58 -73.1536863906448 45.5087087714 15 15
59 -73.1537225849134 45.5087341365186 15 15
60 -73.1536279291163 45.5088004717592 15 15
61 -73.1535332733191 45.5088668069997 15 15
62 -73.153438617522 45.5089331422403 15 15
63 -73.1533439617076 45.5089994774931 15 15
64 -73.1533801559762 45.5090248426116 15 15
65 -73.1534748117906 45.5089585073589 15 15
66 -73.1535694675877 45.5088921721183 15 15
67 -73.1536641233849 45.5088258368777 15 15
68 -73.153758779182 45.5087595016371 15 15
69 -73.1537949734506 45.5087848667557 15 15
70 -73.1537003176535 45.5088512019963 15 15
71 -73.1536056618563 45.5089175372369 15 15
72 -73.1535110060592 45.5089838724775 15 15
73 -73.1534163502448 45.5090502077302 15 15
74 -73.1534525445134 45.5090755728487 15 15
75 -73.1535472003278 45.509009237596 15 15
76 -73.1536418561249 45.5089429023554 15 15
77 -73.1537365119221 45.5088765671148 15 15
78 -73.1538311677192 45.5088102318742 15 15
79 -73.1538673619878 45.5088355969928 15 15
80 -73.1537727061907 45.5089019322334 15 15
81 -73.1536780503936 45.508968267474 15 15
82 -73.1535833945964 45.5090346027146 15 15
83 -73.153488738782 45.5091009379673 15 15
84 -73.1535249330852 45.5091263031101 15 15
85 -73.1536195888996 45.5090599678574 15 15
86 -73.1537142446968 45.5089936326168 15 15
87 -73.1538089004939 45.5089272973762 15 15
88 -73.153903556291 45.5088609621356 15 15

View File

@ -0,0 +1,89 @@
0,-73.1531935978886,45.5084960903092,15,15
1,-73.1530989420915,45.5085624255498,15,15
2,-73.1530042862771,45.5086287608025,15,15
3,-73.1529096304626,45.5086950960552,15,15
4,-73.1529458247399,45.5087204611798,15,15
5,-73.1530404805543,45.5086541259271,15,15
6,-73.1531351363515,45.5085877906865,15,15
7,-73.1532297921486,45.508521455446,15,15
8,-73.1533244479457,45.5084551202054,15,15
9,-73.1533606422273,45.508480485333,15,15
10,-73.1532659864302,45.5085468205736,15,15
11,-73.153171330633,45.5086131558142,15,15
12,-73.1530766748359,45.5086794910548,15,15
13,-73.1529820190215,45.5087458263075,15,15
14,-73.1530182132901,45.5087711914261,15,15
15,-73.1531128691045,45.5087048561733,15,15
16,-73.1532075249016,45.5086385209327,15,15
17,-73.1533021806988,45.5085721856922,15,15
18,-73.1533968364959,45.5085058504516,15,15
19,-73.1534330307645,45.5085312155701,15,15
20,-73.1533383749674,45.5085975508107,15,15
21,-73.1532437191702,45.5086638860513,15,15
22,-73.1531490633731,45.5087302212919,15,15
23,-73.1530544075587,45.5087965565446,15,15
24,-73.1530906018273,45.5088219216632,15,15
25,-73.1531852576417,45.5087555864105,15,15
26,-73.1532799134389,45.5086892511699,15,15
27,-73.153374569236,45.5086229159293,15,15
28,-73.1534692250331,45.5085565806887,15,15
29,-73.1535054193017,45.5085819458072,15,15
30,-73.1534107635046,45.5086482810478,15,15
31,-73.1533161077075,45.5087146162884,15,15
32,-73.1532214519103,45.508780951529,15,15
33,-73.1531267960959,45.5088472867817,15,15
34,-73.1531629903645,45.5088726519003,15,15
35,-73.1532576461789,45.5088063166476,15,15
36,-73.1533523019761,45.508739981407,15,15
37,-73.1534469577732,45.5086736461664,15,15
38,-73.1535416135703,45.5086073109258,15,15
39,-73.1535778078389,45.5086326760444,15,15
40,-73.1534831520418,45.5086990112849,15,15
41,-73.1533884962447,45.5087653465255,15,15
42,-73.1532938404476,45.5088316817661,15,15
43,-73.1531991846331,45.5088980170188,15,15
44,-73.1532353789017,45.5089233821374,15,15
45,-73.1533300347162,45.5088570468847,15,15
46,-73.1534246905133,45.5087907116441,15,15
47,-73.1535193463104,45.5087243764035,15,15
48,-73.1536140021075,45.5086580411629,15,15
49,-73.1536501963762,45.5086834062815,15,15
50,-73.153555540579,45.508749741522,15,15
51,-73.1534608847819,45.5088160767626,15,15
52,-73.1533662289848,45.5088824120032,15,15
53,-73.1532715731703,45.508948747256,15,15
54,-73.1533077674389,45.5089741123745,15,15
55,-73.1534024232534,45.5089077771218,15,15
56,-73.1534970790505,45.5088414418812,15,15
57,-73.1535917348476,45.5087751066406,15,15
58,-73.1536863906448,45.5087087714,15,15
59,-73.1537225849134,45.5087341365186,15,15
60,-73.1536279291163,45.5088004717592,15,15
61,-73.1535332733191,45.5088668069997,15,15
62,-73.153438617522,45.5089331422403,15,15
63,-73.1533439617076,45.5089994774931,15,15
64,-73.1533801559762,45.5090248426116,15,15
65,-73.1534748117906,45.5089585073589,15,15
66,-73.1535694675877,45.5088921721183,15,15
67,-73.1536641233849,45.5088258368777,15,15
68,-73.153758779182,45.5087595016371,15,15
69,-73.1537949734506,45.5087848667557,15,15
70,-73.1537003176535,45.5088512019963,15,15
71,-73.1536056618563,45.5089175372369,15,15
72,-73.1535110060592,45.5089838724775,15,15
73,-73.1534163502448,45.5090502077302,15,15
74,-73.1534525445134,45.5090755728487,15,15
75,-73.1535472003278,45.509009237596,15,15
76,-73.1536418561249,45.5089429023554,15,15
77,-73.1537365119221,45.5088765671148,15,15
78,-73.1538311677192,45.5088102318742,15,15
79,-73.1538673619878,45.5088355969928,15,15
80,-73.1537727061907,45.5089019322334,15,15
81,-73.1536780503936,45.508968267474,15,15
82,-73.1535833945964,45.5090346027146,15,15
83,-73.153488738782,45.5091009379673,15,15
84,-73.1535249330852,45.5091263031101,15,15
85,-73.1536195888996,45.5090599678574,15,15
86,-73.1537142446968,45.5089936326168,15,15
87,-73.1538089004939,45.5089272973762,15,15
88,-73.153903556291,45.5088609621356,15,15
1 0 -73.1531935978886 45.5084960903092 15 15
2 1 -73.1530989420915 45.5085624255498 15 15
3 2 -73.1530042862771 45.5086287608025 15 15
4 3 -73.1529096304626 45.5086950960552 15 15
5 4 -73.1529458247399 45.5087204611798 15 15
6 5 -73.1530404805543 45.5086541259271 15 15
7 6 -73.1531351363515 45.5085877906865 15 15
8 7 -73.1532297921486 45.508521455446 15 15
9 8 -73.1533244479457 45.5084551202054 15 15
10 9 -73.1533606422273 45.508480485333 15 15
11 10 -73.1532659864302 45.5085468205736 15 15
12 11 -73.153171330633 45.5086131558142 15 15
13 12 -73.1530766748359 45.5086794910548 15 15
14 13 -73.1529820190215 45.5087458263075 15 15
15 14 -73.1530182132901 45.5087711914261 15 15
16 15 -73.1531128691045 45.5087048561733 15 15
17 16 -73.1532075249016 45.5086385209327 15 15
18 17 -73.1533021806988 45.5085721856922 15 15
19 18 -73.1533968364959 45.5085058504516 15 15
20 19 -73.1534330307645 45.5085312155701 15 15
21 20 -73.1533383749674 45.5085975508107 15 15
22 21 -73.1532437191702 45.5086638860513 15 15
23 22 -73.1531490633731 45.5087302212919 15 15
24 23 -73.1530544075587 45.5087965565446 15 15
25 24 -73.1530906018273 45.5088219216632 15 15
26 25 -73.1531852576417 45.5087555864105 15 15
27 26 -73.1532799134389 45.5086892511699 15 15
28 27 -73.153374569236 45.5086229159293 15 15
29 28 -73.1534692250331 45.5085565806887 15 15
30 29 -73.1535054193017 45.5085819458072 15 15
31 30 -73.1534107635046 45.5086482810478 15 15
32 31 -73.1533161077075 45.5087146162884 15 15
33 32 -73.1532214519103 45.508780951529 15 15
34 33 -73.1531267960959 45.5088472867817 15 15
35 34 -73.1531629903645 45.5088726519003 15 15
36 35 -73.1532576461789 45.5088063166476 15 15
37 36 -73.1533523019761 45.508739981407 15 15
38 37 -73.1534469577732 45.5086736461664 15 15
39 38 -73.1535416135703 45.5086073109258 15 15
40 39 -73.1535778078389 45.5086326760444 15 15
41 40 -73.1534831520418 45.5086990112849 15 15
42 41 -73.1533884962447 45.5087653465255 15 15
43 42 -73.1532938404476 45.5088316817661 15 15
44 43 -73.1531991846331 45.5088980170188 15 15
45 44 -73.1532353789017 45.5089233821374 15 15
46 45 -73.1533300347162 45.5088570468847 15 15
47 46 -73.1534246905133 45.5087907116441 15 15
48 47 -73.1535193463104 45.5087243764035 15 15
49 48 -73.1536140021075 45.5086580411629 15 15
50 49 -73.1536501963762 45.5086834062815 15 15
51 50 -73.153555540579 45.508749741522 15 15
52 51 -73.1534608847819 45.5088160767626 15 15
53 52 -73.1533662289848 45.5088824120032 15 15
54 53 -73.1532715731703 45.508948747256 15 15
55 54 -73.1533077674389 45.5089741123745 15 15
56 55 -73.1534024232534 45.5089077771218 15 15
57 56 -73.1534970790505 45.5088414418812 15 15
58 57 -73.1535917348476 45.5087751066406 15 15
59 58 -73.1536863906448 45.5087087714 15 15
60 59 -73.1537225849134 45.5087341365186 15 15
61 60 -73.1536279291163 45.5088004717592 15 15
62 61 -73.1535332733191 45.5088668069997 15 15
63 62 -73.153438617522 45.5089331422403 15 15
64 63 -73.1533439617076 45.5089994774931 15 15
65 64 -73.1533801559762 45.5090248426116 15 15
66 65 -73.1534748117906 45.5089585073589 15 15
67 66 -73.1535694675877 45.5088921721183 15 15
68 67 -73.1536641233849 45.5088258368777 15 15
69 68 -73.153758779182 45.5087595016371 15 15
70 69 -73.1537949734506 45.5087848667557 15 15
71 70 -73.1537003176535 45.5088512019963 15 15
72 71 -73.1536056618563 45.5089175372369 15 15
73 72 -73.1535110060592 45.5089838724775 15 15
74 73 -73.1534163502448 45.5090502077302 15 15
75 74 -73.1534525445134 45.5090755728487 15 15
76 75 -73.1535472003278 45.509009237596 15 15
77 76 -73.1536418561249 45.5089429023554 15 15
78 77 -73.1537365119221 45.5088765671148 15 15
79 78 -73.1538311677192 45.5088102318742 15 15
80 79 -73.1538673619878 45.5088355969928 15 15
81 80 -73.1537727061907 45.5089019322334 15 15
82 81 -73.1536780503936 45.508968267474 15 15
83 82 -73.1535833945964 45.5090346027146 15 15
84 83 -73.153488738782 45.5091009379673 15 15
85 84 -73.1535249330852 45.5091263031101 15 15
86 85 -73.1536195888996 45.5090599678574 15 15
87 86 -73.1537142446968 45.5089936326168 15 15
88 87 -73.1538089004939 45.5089272973762 15 15
89 88 -73.153903556291 45.5088609621356 15 15

View File

@ -61,3 +61,17 @@ function gps_from_vec(vec) {
return Lgoal return Lgoal
} }
GPSoffset = {.lat=45.50, .lon=-73.61}
function packWP2i(in_lat, in_long, processed) {
var dlat = math.round((in_lat - GPSoffset.lat)*1000000)
var dlon = math.round((in_long - GPSoffset.lon)*1000000)
return {.dla=dlat, .dlo=dlon*10+processed}
}
function unpackWP2i(wp_int){
var dlon = (wp_int.dlo-wp_int.dlo%10)/10.0
var pro = wp_int.dlo-dlon*10
return {.lat=wp_int.dla/1000000.0+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=pro}
}

View File

@ -0,0 +1,97 @@
function table_print(t) {
foreach(t, function(key, value) {
log(key, " -> ", value)
})
}
function table_copy(t) {
var t2 = {}
foreach(t, function(key, value) {
t2[key] = value
})
return t2
}
#
#return the number of value in table
#
function count(table,value){
var number=0
var i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
}
i=i+1
}
return number
}
#
#map from int to state
#
function i2s(value){
if(value==1){
return "GRAPH_ASKING"
}
else if(value==2){
return "GRAPH_JOINING"
}
else if(value==3){
return "GRAPH_JOINED"
}
else if(value==4){
return "TURNEDOFF"
}
else if(value==5){
return "BARRIERWAIT"
}
else if(value==6){
return "INDIWP"
}
else if(value==7){
return "TASK_ALLOCATE"
}
else if(value==8){
return "LAUNCH"
}
else if(value==9){
return "STOP"
}
else {
return "UNDETERMINED"
}
}
#
#map from state to int
#
function s2i(value){
if(value=="GRAPH_ASKING"){
return 1
}
else if(value=="GRAPH_JOINING"){
return 2
}
else if(value=="GRAPH_JOINED"){
return 3
}
else if(value=="TURNEDOFF"){
return 4
}
else if(value=="BARRIERWAIT"){
return 5
}
else if(value=="INDIWP"){
return 6
}
else if(value=="TASK_ALLOCATE"){
return 7
}
else if(value=="LAUNCH"){
return 8
}
else if(value=="STOP"){
return 9
}
else
return 0
}

View File

@ -1,8 +1,10 @@
takeoff_heights ={ takeoff_heights ={
.0 = 30.0,
.1 = 21.0, .1 = 21.0,
.2 = 18.0, .2 = 18.0,
.3 = 12.0, .3 = 12.0,
.5 = 6.0, .4 = 28.0,
.5 = 12.0,
.6 = 3.0, .6 = 3.0,
.9 = 15.0, .9 = 15.0,
.11 = 6.0, .11 = 6.0,

View File

@ -8,15 +8,14 @@
# #
STATUS_VSTIG = 20 STATUS_VSTIG = 20
GROUND_VSTIG = 21 GROUND_VSTIG = 21
HIGHEST_ROBOTID = 14 WAIT4STEP = 5
WAIT4STEP = 10 vstig_buzzy = 0
# #
# Init var # Init var
# #
var v_status = {} var v_status = {}
var v_ground = {} var v_ground = {}
b_updating = 0
vstig_counter = WAIT4STEP vstig_counter = WAIT4STEP
@ -27,36 +26,36 @@ function init_swarm() {
function init_stig() { function init_stig() {
v_status = stigmergy.create(STATUS_VSTIG) v_status = stigmergy.create(STATUS_VSTIG)
v_ground = stigmergy.create(GROUND_VSTIG) #v_ground = stigmergy.create(GROUND_VSTIG)
} }
function uav_updatestig() { function uav_updatestig() {
# TODO: Push values on update only. # TODO: Push values on update only?
if(vstig_counter<=0) { if(vstig_counter<=0) {
b_updating=1 vstig_buzzy = 1
#var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status} var ls = battery.capacity*10 + s2i(BVMSTATE)
ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status #log("Pushing ", ls, "on vstig with id:", id)
log("Pushing ", ls, "on vstig with id:", id)
v_status.put(id, ls) v_status.put(id, ls)
vstig_counter=WAIT4STEP vstig_counter=WAIT4STEP
} else if(vstig_counter==WAIT4STEP-1){ # ensure comm. delay between fetch and update stig
vstig_buzzy = 1
vstig_counter=vstig_counter-1
stattab_send()
} else { } else {
b_updating=0 vstig_buzzy = 0
vstig_counter=vstig_counter-1 vstig_counter=vstig_counter-1
} }
} }
function unpackstatus(recv_value){ function unpackstatus(recv_value,state_struct){
gps=(recv_value-recv_value%1000000)/1000000 #state_struct.gp=(recv_value-recv_value%1000000)/1000000
recv_value=recv_value-gps*1000000 #recv_value=recv_value-state_struct.gp*1000000
batt=(recv_value-recv_value%1000)/1000 state_struct.ba=(recv_value-recv_value%10)/10
recv_value=recv_value-batt*1000 recv_value=recv_value-state_struct.ba*10
xbee=(recv_value-recv_value%10)/10 #state_struct.xb=(recv_value-recv_value%10)/10
recv_value=recv_value-xbee*10 #recv_value=recv_value-state_struct.xb*10
fc=recv_value state_struct.st=recv_value
log("- GPS ", gps) return state_struct
log("- Battery ", batt)
log("- Xbee ", xbee)
log("- Status ", fc)
} }
function checkusers() { function checkusers() {
@ -95,37 +94,29 @@ function usertab_print(t) {
function stattab_print() { function stattab_print() {
if(v_status.size()>0) { if(v_status.size()>0) {
if(b_updating==0) { var state_struct = {.id = -1, .gp = 0, .ba = 0, .xb = 0, .st = 0}
u=0 neighbors.foreach(function(rid, data) {
while(u<HIGHEST_ROBOTID){ var nei_state = v_status.get(rid)
tab = v_status.get(u) if(nei_state!=nil){
if(tab!=nil) state_struct.id=u
unpackstatus(tab) state_struct = unpackstatus(nei_state, state_struct)
table_print(state_struct)
}
u=u+1 u=u+1
} } )
}
} }
} }
function stattab_send() { function stattab_send() {
if(v_status.size()>0) { if(v_status.size()>0) {
if(b_updating==0) { var state_struct = {.id = -1, .gp = -1, .ba = -1, .xb = -1, .st = 0}
u=0 neighbors.foreach(function(rid, data) {
while(u<HIGHEST_ROBOTID){ var nei_state = v_status.get(rid)
tab = v_status.get(u) if(nei_state!=nil){
if(tab!=nil){ state_struct.id=rid
recv_value=tab state_struct = unpackstatus(nei_state,state_struct)
gps=(recv_value-recv_value%1000000)/1000000 add_neighborStatus(state_struct)
recv_value=recv_value-gps*1000000
batt=(recv_value-recv_value%1000)/1000
recv_value=recv_value-batt*1000
xbee=(recv_value-recv_value%10)/10
recv_value=recv_value-xbee*10
fc=recv_value
add_neighborStatus(u,gps,batt,xbee,fc)
} }
u=u+1 } )
}
}
} }
} }

View File

@ -2,6 +2,7 @@ include "update.bzz"
# don't use a stigmergy id=11 with this header, for barrier # don't use a stigmergy id=11 with this header, for barrier
# it requires an 'action' function to be defined here. # it requires an 'action' function to be defined here.
include "act/states.bzz" include "act/states.bzz"
include "utils/table.bzz"
include "plan/rrtstar.bzz" include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz" include "taskallocate/graphformGPS.bzz"
include "taskallocate/bidding.bzz" include "taskallocate/bidding.bzz"
@ -27,9 +28,6 @@ LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barr
# 3 -> indoor wheeled vehicle # 3 -> indoor wheeled vehicle
V_TYPE = 0 V_TYPE = 0
goal_list = {
.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
}
# Executed once at init time. # Executed once at init time.
function init() { function init() {
@ -53,7 +51,7 @@ function step() {
rc_cmd_listen() rc_cmd_listen()
# update the vstig (status/net/batt/...) # update the vstig (status/net/batt/...)
# uav_updatestig() uav_updatestig()
# #
# State machine # State machine

View File

@ -20,7 +20,8 @@ namespace buzzuav_closures
* The command to use in Buzz is buzzros_print takes any available datatype in Buzz * The command to use in Buzz is buzzros_print takes any available datatype in Buzz
*/ */
int buzzros_print(buzzvm_t vm); int buzzros_print(buzzvm_t vm);
void setWPlist(std::string path); void setWPlist(std::string file);
void check_targets_sim(double lat, double lon, double *res);
/* /*
* closure to move following a vector * closure to move following a vector

View File

@ -115,6 +115,7 @@ private:
uint64_t payload; uint64_t payload;
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map<int, buzz_utility::Pos_struct> targets_found;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
std::map<int, buzz_utility::neighbor_time> neighbours_time_map; std::map<int, buzz_utility::neighbor_time> neighbours_time_map;
int timer_step = 0; int timer_step = 0;
@ -143,7 +144,7 @@ private:
bool debug = false; bool debug = false;
bool setmode = false; bool setmode = false;
bool BClpose = false; bool BClpose = false;
std::string bzzfile_name; std::string bzzfile_name, WPfile;
std::string bcfname, dbgfname; std::string bcfname, dbgfname;
std::string stand_by; std::string stand_by;
std::string capture_srv_name; std::string capture_srv_name;
@ -155,6 +156,7 @@ private:
ros::ServiceClient stream_client; ros::ServiceClient stream_client;
ros::Publisher payload_pub; ros::Publisher payload_pub;
ros::Publisher MPpayload_pub; ros::Publisher MPpayload_pub;
ros::Publisher targetf_pub;
ros::Publisher neigh_pos_pub; ros::Publisher neigh_pos_pub;
ros::Publisher bvmstate_pub; ros::Publisher bvmstate_pub;
ros::Publisher grid_pub; ros::Publisher grid_pub;

View File

@ -13,6 +13,7 @@ topics:
bstate: bvmstate bstate: bvmstate
npose: neighbours_pos npose: neighbours_pos
fstatus: fleet_status fstatus: fleet_status
targetf: targets_found
services: services:
fcclient: cmd/command fcclient: cmd/command
armclient: cmd/arming armclient: cmd/arming

View File

@ -6,6 +6,7 @@
<arg name="name" default="robot0"/> <arg name="name" default="robot0"/>
<arg name="xbee_plugged" default="true"/> <arg name="xbee_plugged" default="true"/>
<arg name="script" default="testalone"/> <arg name="script" default="testalone"/>
<arg name="wpfile" default="waypointlist"/>
<arg name="launch_config" default="topics"/> <arg name="launch_config" default="topics"/>
<arg name="debug" default="false" /> <arg name="debug" default="false" />
<arg name="setmode" default="false" /> <arg name="setmode" default="false" />
@ -13,6 +14,7 @@
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" > <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/> <rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" /> <param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="WPfile" value="$(find rosbuzz)/buzz_scripts/include/taskallocate/$(arg wpfile).csv" />
<param name="debug" value="$(arg debug)" /> <param name="debug" value="$(arg debug)" />
<param name="xbee_plugged" value="$(arg xbee_plugged)"/> <param name="xbee_plugged" value="$(arg xbee_plugged)"/>
<param name="name" value="$(arg name)"/> <param name="name" value="$(arg name)"/>

View File

@ -6,6 +6,7 @@
<arg name="name" default="robot0"/> <arg name="name" default="robot0"/>
<arg name="xbee_plugged" default="true"/> <arg name="xbee_plugged" default="true"/>
<arg name="script" default="testalone"/> <arg name="script" default="testalone"/>
<arg name="wpfile" default="waypointlist"/>
<arg name="launch_config" default="topics"/> <arg name="launch_config" default="topics"/>
<arg name="debug" default="true" /> <arg name="debug" default="true" />
<arg name="setmode" default="false" /> <arg name="setmode" default="false" />
@ -13,6 +14,7 @@
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args"> <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/> <rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" /> <param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="WPfile" value="$(find rosbuzz)/buzz_scripts/include/taskallocate/$(arg wpfile).csv" />
<param name="debug" value="$(arg debug)" /> <param name="debug" value="$(arg debug)" />
<param name="xbee_plugged" value="$(arg xbee_plugged)"/> <param name="xbee_plugged" value="$(arg xbee_plugged)"/>
<param name="name" value="$(arg name)"/> <param name="name" value="$(arg name)"/>

View File

@ -17,7 +17,7 @@ static buzzvm_t VM = 0;
static char* BO_FNAME = 0; static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0; static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0; static buzzdebug_t DBG_INFO = 0;
static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets static uint32_t MAX_MSG_SIZE = 210;//250; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0; static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG; static std::vector<uint8_t*> IN_MSG;
std::map<int, Pos_struct> users_map; std::map<int, Pos_struct> users_map;
@ -291,6 +291,12 @@ static int testing_buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "reset_rc", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
return VM->state; return VM->state;
} }

View File

@ -54,7 +54,7 @@ int buzzros_print(buzzvm_t vm)
----------------------------------------------------------- */ ----------------------------------------------------------- */
{ {
std::ostringstream buffer(std::ostringstream::ate); std::ostringstream buffer(std::ostringstream::ate);
buffer << "[" << buzz_utility::get_robotid() << "] "; buffer << std::fixed << std::setprecision(6) << "[" << buzz_utility::get_robotid() << "] ";
for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index) for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
{ {
buzzvm_lload(vm, index); buzzvm_lload(vm, index);
@ -98,12 +98,13 @@ int buzzros_print(buzzvm_t vm)
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
void setWPlist(string path) void setWPlist(string file)
/* /*
/ set the absolute path for a csv list of waypoints / set the absolute path for a csv list of waypoints
----------------------------------------------------------- */ ----------------------------------------------------------- */
{ {
WPlistname = path + "include/taskallocate/waypointlist.csv"; WPlistname = file;//path + "include/taskallocate/waypointlist.csv";
parse_gpslist();
} }
float constrainAngle(float x) float constrainAngle(float x)
@ -181,6 +182,28 @@ void parse_gpslist()
fin.close(); fin.close();
} }
void check_targets_sim(double lat, double lon, double *res)
/*
/ check if a listed target is close
----------------------------------------------------------- */
{
map<int, buzz_utility::RB_struct>::iterator it;
for (it = wplist_map.begin(); it != wplist_map.end(); ++it)
{
double rb[3];
double ref[2]={lat, lon};
double tar[2]={it->second.latitude, it->second.longitude};
rb_from_gps(tar, rb, ref);
if(rb[0]<3.0){
ROS_WARN("FOUND A TARGET!!! [%i]", it->first);
res[0] = it->first;
res[1] = it->second.latitude;
res[2] = it->second.longitude;
res[3] = it->second.altitude;
}
}
}
int buzz_exportmap(buzzvm_t vm) int buzz_exportmap(buzzvm_t vm)
/* /*
/ Buzz closure to export a 2D map / Buzz closure to export a 2D map
@ -288,23 +311,42 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
/ closure to add neighbors status to the BVM / closure to add neighbors status to the BVM
/----------------------------------------*/ /----------------------------------------*/
{ {
buzzvm_lnum_assert(vm, 5); buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); // fc buzzvm_lload(vm, 1); // state table
buzzvm_lload(vm, 2); // xbee buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
buzzvm_lload(vm, 3); // batt buzzobj_t t = buzzvm_stack_at(vm, 1);
buzzvm_lload(vm, 4); // gps if(buzzdict_size(t->t.value) != 5) {
buzzvm_lload(vm, 5); // id ROS_WARN("Wrong neighbor status size.");
buzzvm_type_assert(vm, 5, BUZZTYPE_INT); return vm->state;
buzzvm_type_assert(vm, 4, BUZZTYPE_INT); }
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_INT);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzz_utility::neighbors_status newRS; buzz_utility::neighbors_status newRS;
uint8_t id = buzzvm_stack_at(vm, 5)->i.value; buzzvm_dup(vm);
newRS.gps_strenght = buzzvm_stack_at(vm, 4)->i.value; buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1));
newRS.batt_lvl = buzzvm_stack_at(vm, 3)->i.value; buzzvm_tget(vm);
newRS.xbee = buzzvm_stack_at(vm, 2)->i.value; uint8_t id = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "ba", 1));
buzzvm_tget(vm);
newRS.batt_lvl = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "gp", 1));
buzzvm_tget(vm);
newRS.gps_strenght = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "xb", 1));
buzzvm_tget(vm);
newRS.xbee = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "st", 1));
buzzvm_tget(vm);
newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value; newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
map<int, buzz_utility::neighbors_status>::iterator it = neighbors_status_map.find(id); map<int, buzz_utility::neighbors_status>::iterator it = neighbors_status_map.find(id);
if (it != neighbors_status_map.end()) if (it != neighbors_status_map.end())
neighbors_status_map.erase(it); neighbors_status_map.erase(it);
@ -417,8 +459,10 @@ void set_gpsgoal(double goal[3])
rb_from_gps(goal, rb, cur_pos); rb_from_gps(goal, rb, cur_pos);
if (fabs(rb[0]) < 150.0) { if (fabs(rb[0]) < 150.0) {
goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2]; goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2];
ROS_INFO("Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
} } else
ROS_WARN("[%i] GPS GOAL TOO FAR !!-- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
} }
int buzzuav_arm(buzzvm_t vm) int buzzuav_arm(buzzvm_t vm)

View File

@ -28,7 +28,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
bcfname = fname + ".bo"; bcfname = fname + ".bo";
dbgfname = fname + ".bdb"; dbgfname = fname + ".bdb";
buzz_update::set_bzz_file(bzzfile_name.c_str(),debug); buzz_update::set_bzz_file(bzzfile_name.c_str(),debug);
buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); buzzuav_closures::setWPlist(WPfile);
// Initialize variables // Initialize variables
if(setmode) if(setmode)
SetMode("LOITER", 0); SetMode("LOITER", 0);
@ -259,6 +259,13 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
ROS_ERROR("Provide a .bzz file to run in Launch file"); ROS_ERROR("Provide a .bzz file to run in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
if (n_c.getParam("WPfile", WPfile))
;
else
{
ROS_ERROR("Provide a .csv file to with target WP list");
system("rosnode kill rosbuzz_node");
}
// Obtain debug mode from launch file parameter // Obtain debug mode from launch file parameter
if (n_c.getParam("debug", debug)) if (n_c.getParam("debug", debug))
; ;
@ -417,8 +424,15 @@ void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handl
ROS_ERROR("Provide a fleet status out topic name in YAML file"); ROS_ERROR("Provide a fleet status out topic name in YAML file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
if (node_handle.getParam("topics/targetf", topic))
targetf_pub = n_c.advertise<rosbuzz::neigh_pos>(topic, 5);
else
{
ROS_ERROR("Provide a fleet status out topic name in YAML file");
system("rosnode kill rosbuzz_node");
}
if (node_handle.getParam("topics/npose", topic)) if (node_handle.getParam("topics/npose", topic))
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>(topic, MAX_NUMBER_OF_ROBOTS); neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>(topic, 5);
else else
{ {
ROS_ERROR("Provide a Neighbor pose out topic name in YAML file"); ROS_ERROR("Provide a Neighbor pose out topic name in YAML file");
@ -529,11 +543,14 @@ std::string roscontroller::Compile_bzz(std::string bzzfile_name)
void roscontroller::neighbours_pos_publisher() void roscontroller::neighbours_pos_publisher()
/* /*
/ Publish neighbours pos and id in neighbours pos topic / Publish neighbours pos and id in neighbours pos topic AND TARGET ACQUIRED (SIMULATED)
/----------------------------------------------------*/ /----------------------------------------------------*/
{ {
auto current_time = ros::Time::now(); auto current_time = ros::Time::now();
map<int, buzz_utility::Pos_struct>::iterator it; map<int, buzz_utility::Pos_struct>::iterator it;
rosbuzz::neigh_pos msg_target_out;
msg_target_out.header.frame_id = "/world";
msg_target_out.header.stamp = current_time;
rosbuzz::neigh_pos neigh_pos_array; rosbuzz::neigh_pos neigh_pos_array;
neigh_pos_array.header.frame_id = "/world"; neigh_pos_array.header.frame_id = "/world";
neigh_pos_array.header.stamp = current_time; neigh_pos_array.header.stamp = current_time;
@ -551,7 +568,32 @@ void roscontroller::neighbours_pos_publisher()
// cout<<"iterator it val: "<< it-> first << " After convertion: " // cout<<"iterator it val: "<< it-> first << " After convertion: "
// <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl; // <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;
// std::cout<<"long obt"<<neigh_tmp.longitude<<endl; // std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
double tf[4] = {-1, 0, 0, 0};
buzzuav_closures::check_targets_sim((it->second).x, (it->second).y, tf);
if(tf[0]!=-1){
buzz_utility::Pos_struct pos_tmp;
pos_tmp.x = tf[1];
pos_tmp.y = tf[2];
pos_tmp.z = tf[3];
map<int, buzz_utility::Pos_struct>::iterator it = targets_found.find(round(tf[0]));
if (it != targets_found.end())
targets_found.erase(it);
targets_found.insert(make_pair(round(tf[0]), pos_tmp));
}
} }
for (it = targets_found.begin(); it != targets_found.end(); ++it)
{
sensor_msgs::NavSatFix target_tmp;
target_tmp.header.stamp = current_time;
target_tmp.header.frame_id = "/world";
target_tmp.position_covariance_type = it->first; // custom robot id storage
target_tmp.latitude = (it->second).x;
target_tmp.longitude = (it->second).y;
target_tmp.altitude = (it->second).z;
msg_target_out.pos_neigh.push_back(target_tmp);
}
targetf_pub.publish(msg_target_out);
neigh_pos_pub.publish(neigh_pos_array); neigh_pos_pub.publish(neigh_pos_array);
} }