Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

enhance barrier, simplify gps-graph and add multi-WP csv load

This commit is contained in:
dave 2017-09-05 23:48:50 -04:00
parent ccdb80a4cf
commit f906be2d97
9 changed files with 334 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
buzz_scripts/graphformGPS.bzz
View File

@ -6,7 +6,7 @@ include "vec2.bzz"
include "update.bzz"
include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
include "barrier.bzz" # reserve stigmergy id=11 for this header.
include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "graphs/shapes_Y.bzz"
@ -268,12 +268,12 @@ function target4label(nei_id){
#calculate LJ vector for neibhor stored in i
function LJ_vec(i){
var dis=m_MessageRange[i]
var bearing=m_MessageBearing[i]
var ljbearing=m_MessageBearing[i]
var nei_id=m_messageID[i]
var target=target4label(nei_id)
var cDir={.x=0.0,.y=0.0}
if(target!="miss"){
cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),bearing)
cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),ljbearing)
}
#log(id,"dis=",dis,"target=",target,"label=",nei_id)
#log("x=",cDir.x,"y=",cDir.y)
@ -383,65 +383,11 @@ function TransitionToAsking(un_label){
#
#Transistion to state joining
#
function set_rc_goto() {
#get information of pred
var i=0
var IDofPred=-1
while(i<m_neighbourCount and IDofPred==-1){
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
IDofPred=i
i=i+1
}
#found pred
if(IDofPred!=-1){
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
#attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate
var S2PGlobalBearing=0
m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing)
if(m_cMeToPred.GlobalBearing>math.pi)
S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi
else
S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi
var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing)
#the vector from self to target in global coordinate
var S2Target=math.vec2.add(S2Pred,P2Target)
#change the vector to local coordinate of self
var S2Target_bearing=math.atan(S2Target.y, S2Target.x)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias
gps_from_rb(math.vec2.length(S2Target)/100.0, S2Target_bearing)
uav_storegoal(goal.latitude, goal.longitude, position.altitude)
}
}
function TransitionToJoining(){
UAVSTATE="STATE_JOINING"
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
set_rc_goto()
neighbors.listen("r",
function(vid,value,rid){
var recv_table={.Label=0,.Bearing=0.0}
recv_table=unpack_guide_msg(value)
#store the received message
if(recv_table.Label==m_nLabel){
m_cMeToPred.GlobalBearing=recv_table.Bearing
}
})
}
#
@ -452,11 +398,10 @@ function TransitionToJoined(){
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("r")
#write statues
#v_tag.put(m_nLabel, m_lockstig)
barrier_create(m_lockstig)
barrier_create()
barrier_ready()
m_navigation.x=0.0
@ -591,13 +536,47 @@ function DoAsking(){
#
#Do joining
#
m_gotjoinedparent = 0
function DoJoining(){
if(m_gotjoinedparent!=1)
set_rc_goto()
else
gotoWP(TransitionToJoined)
#pack the communication package
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel
}
function set_rc_goto() {
#get information of pred
var i=0
var IDofPred=-1
while(i<m_neighbourCount and IDofPred==-1){
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
IDofPred=i
i=i+1
}
#found pred
if(IDofPred!=-1){
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
var S2Target=math.vec2.add(S2Pred,P2Target)
gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
uav_storegoal(goal.latitude, goal.longitude, position.altitude)
m_gotjoinedparent = 1
}
}
#
#Do joined
#
@ -630,7 +609,6 @@ function DoJoined(){
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
##joining wrt this dot,send the global bearing
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
neighbors.broadcast("r",pack_guide_msg(m_messageForJoining))
}
}
#if it is the pred
@ -679,7 +657,7 @@ function DoJoined(){
#if(v_tag.size()==ROBOTS){
# TransitionToLock()
#}
barrier_wait(ROBOTS, TransitionToLock, DoJoined)
barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
}
#

30
buzz_scripts/include/barrier.bzz
View File

@ -8,31 +8,38 @@
# Constants
#
BARRIER_TIMEOUT = 200 # in steps
BARRIER_VSTIG = 80
timeW = 0
barrier = nil
#
# Sets a barrier
#
function barrier_create(vstig_nb) {
function barrier_create() {
# reset
timeW = 0
# create barrier vstig
barrier = stigmergy.create(vstig_nb)
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier!=nil) {
barrier=nil
BARRIER_VSTIG = BARRIER_VSTIG +1
}
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG)
}
function barrier_set(threshold, transf, resumef, vstig_nb) {
function barrier_set(threshold, transf, resumef, bdt) {
statef = function() {
barrier_wait(threshold, transf, resumef);
barrier_wait(threshold, transf, resumef, bdt);
}
UAVSTATE = "BARRIERWAIT"
barrier_create(vstig_nb)
barrier_create()
}
#
# Make yourself ready
#
function barrier_ready() {
log("BARRIER READY -------")
barrier.put(id, 1)
barrier.put("d", 0)
}
@ -40,21 +47,24 @@ function barrier_ready() {
#
# Executes the barrier
#
function barrier_wait(threshold, transf, resumef) {
function barrier_wait(threshold, transf, resumef, bdt) {
barrier.put(id, 1)
barrier.get(id)
if(barrier.size() >= threshold or barrier.get("d")==1) {
#log("----->BS: ", barrier.size())
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
barrier.put("d", 1)
# barrier = nil
timeW = 0
transf()
} else if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!")
timeW = 0
# barrier = nil
resumef()
}
if(bdt!=-1)
neighbors.broadcast("cmd", bdt)
timeW = timeW+1
}

89
buzz_scripts/include/graphs/waypointlist.csv Normal file
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

90
buzz_scripts/include/uavstates.bzz
View File

@ -3,10 +3,13 @@
# FLIGHT-RELATED FUNCTIONS
#
########################################
TARGET_ALTITUDE = 5.0
TARGET_ALTITUDE = 5.0 # m.
UAVSTATE = "TURNEDOFF"
GOTO_MAXVEL = 2
TAKEOFF_VSTIG = 11
PICTURE_WAIT = 40 # steps
GOTO_MAXVEL = 2 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5 # m.
GOTOANG_TOL = 0.1 # rad.
goal = {.range=0, .bearing=0, .latitude=0, .longitude=0}
function uav_initswarm() {
@ -24,18 +27,13 @@ function idle() {
UAVSTATE = "IDLE"
}
firstpassT = 1
function takeoff() {
UAVSTATE = "TAKEOFF"
statef=takeoff
if(firstpassT) {
barrier_create(TAKEOFF_VSTIG)
firstpassT = 0
}
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_wait(ROBOTS, action, land)
barrier_set(ROBOTS, action, land, -1)
barrier_ready()
} else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
@ -43,55 +41,65 @@ function takeoff() {
}
}
firstpassL = 1
function land() {
UAVSTATE = "LAND"
statef=land
if(firstpassL) {
barrier_create(TAKEOFF_VSTIG+1)
firstpassL = 0
}
neighbors.broadcast("cmd", 21)
uav_land()
if(flight.status != 2 and flight.status != 3){
barrier_wait(ROBOTS,turnedoff,land)
barrier_set(ROBOTS,turnedoff,land, 21)
barrier_ready()
}
}
function goto() {
function set_goto(transf) {
UAVSTATE = "GOTO"
statef=goto
statef=function() {
gotoWP(transf);
}
if(rc_goto.id==id){
if(s!=nil){
if(s.in())
s.leave()
gotoWP(picture)
}
} else {
neighbors.broadcast("cmd", 16)
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
}
}
ptime=0
function picture() {
statef=picture
UAVSTATE="PICTURE"
#TODO: change gimbal orientation
if(ptime==PICTURE_WAIT/2)
uav_takepicture()
statef=idle
else if(ptime>=PICTURE_WAIT) {
statef=action
ptime=0
}
ptime=ptime+1
}
function gotoWP(transf) {
# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
rb_from_gps(rc_goto.latitude, rc_goto.longitude)
print(id, " has to move ", goal.range)
print(" has to move ", goal.range)
m_navigation = math.vec2.newp(goal.range, goal.bearing)
if(math.vec2.length(m_navigation)>GOTO_MAXVEL) {
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far.")
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
} else if(math.vec2.length(m_navigation)>0.25)
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
else
} else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination
transf()
else
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
}
function follow() {
@ -145,16 +153,18 @@ function uav_rccmd() {
function uav_neicmd() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and UAVSTATE!="TAKEOFF") {
print("Got (", vid, ",", value, ") from robot #", rid, "(", UAVSTATE, ")")
if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") {
statef=takeoff
} else if(value==21) {
UAVSTATE = "TAKEOFF"
} else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") {
statef=land
} else if(value==400 and UAVSTATE=="IDLE") {
UAVSTATE = "LAND"
} else if(value==400 and UAVSTATE=="TURNEDOFF") {
uav_arm()
} else if(value==401 and UAVSTATE=="IDLE"){
} else if(value==401 and UAVSTATE=="TURNEDOFF"){
uav_disarm()
} else if(value==16){
} else if(value==16 and UAVSTATE=="IDLE"){
# neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid)
# # if(gt.id == id) statef=goto
@ -173,7 +183,7 @@ function LimitAngle(angle){
}
function rb_from_gps(lat, lon) {
print("gps2rb: ",lat,lon,position.latitude,position.longitude)
# print("gps2rb: ",lat,lon,position.latitude,position.longitude)
d_lon = lon - position.longitude
d_lat = lat - position.latitude
ned_x = d_lat/180*math.pi * 6371000.0;
@ -182,12 +192,18 @@ function rb_from_gps(lat, lon) {
goal.bearing = LimitAngle(math.atan(ned_y,ned_x));
}
function gps_from_rb(range, bearing) {
print("rb2gps: ",range,bearing,position.latitude,position.longitude)
function gps_from_vec(vec) {
Vrange = math.vec2.length(vec)
Vbearing = LimitAngle(math.atan(vec.y, vec.x))
# print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude)
latR = position.latitude*math.pi/180.0;
lonR = position.longitude*math.pi/180.0;
target_lat = math.asin(math.sin(latR) * math.cos(range/6371000.0) + math.cos(latR) * math.sin(range/6371000.0) * math.cos(bearing));
target_lon = lonR + math.atan(math.sin(bearing) * math.sin(range/6371000.0) * math.cos(latR), math.cos(range/6371000.0) - math.sin(latR) * math.sin(target_lat));
target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
goal.latitude = target_lat*180.0/math.pi;
goal.longitude = target_lon*180.0/math.pi;
#d_lat = (vec.x / 6371000.0)*180.0/math.pi;
#goal.latitude = d_lat + position.latitude;
#d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
#goal.longitude = d_lon + position.longitude;
}

35
buzz_scripts/testaloneWP.bzz Normal file
View File

@ -0,0 +1,35 @@
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
function action() {
uav_storegoal(-1.0,-1.0,-1.0)
set_goto(picture)
}
# Executed once at init time.
function init() {
statef=turnedoff
UAVSTATE = "TURNEDOFF"
uav_initstig()
TARGET_ALTITUDE = 15.0
}
# Executed at each time step.
function step() {
uav_rccmd()
statef()
log("Current state: ", UAVSTATE)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

4
include/buzz_utility.h
View File

@ -23,8 +23,8 @@ struct pos_struct
typedef struct pos_struct Pos_struct;
struct rb_struct
{
double r,b,la,lo;
rb_struct(double la, double lo, double r,double b):la(la),lo(lo),r(r),b(b){};
double r,b,latitude,longitude,altitude;
rb_struct(double la, double lo, double al, double r,double b):latitude(la),longitude(lo),altitude(al),r(r),b(b){};
rb_struct(){}
};
typedef struct rb_struct RB_struct;

3
include/buzzuav_closures.h
View File

@ -31,13 +31,14 @@ namespace buzzuav_closures{
* The command to use in Buzz is buzzros_print takes any available datatype in Buzz
*/
int buzzros_print(buzzvm_t vm);
void setWPlist(std::string path);
/*
* buzzuav_goto(latitude,longitude,altitude) function in Buzz
* commands the UAV to go to a position supplied
*/
int buzzuav_moveto(buzzvm_t vm);
int buzzuav_storegoal(buzzvm_t vm);
void parse_gpslist();
int buzzuav_takepicture(buzzvm_t vm);
/* Returns the current command from local variable*/
int getcmd();

87
src/buzzuav_closures.cpp
View File

@ -32,6 +32,7 @@ namespace buzzuav_closures{
static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
string WPlistname = "";
std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::Pos_struct> neighbors_map;
@ -83,6 +84,10 @@ namespace buzzuav_closures{
return buzzvm_ret0(vm);
}
void setWPlist(string path){
WPlistname = path + "include/graphs/waypointlist.csv";
}
/*----------------------------------------/
/ Compute GPS destination from current position and desired Range and Bearing
/----------------------------------------*/
@ -119,6 +124,52 @@ namespace buzzuav_closures{
double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
void parse_gpslist()
{
// Open file:
ROS_INFO("WP list file: %s", WPlistname.c_str());
std::ifstream fin(WPlistname.c_str()); // Open in text-mode.
// Opening may fail, always check.
if (!fin) {
ROS_ERROR("GPS list parser, could not open file.");
return;
}
// Prepare a C-string buffer to be used when reading lines.
const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need.
char buffer[MAX_LINE_LENGTH] = {};
const char * DELIMS = "\t ,"; // Tab, space or comma.
// Read the file and load the data:
double lat, lon;
int alt, tilt, tid;
buzz_utility::RB_struct RB_arr;
// Read one line at a time.
while ( fin.getline(buffer, MAX_LINE_LENGTH) ) {
// Extract the tokens:
tid = atoi(strtok( buffer, DELIMS ));
lon = atof(strtok( NULL, DELIMS ));
lat = atof(strtok( NULL, DELIMS ));
alt = atoi(strtok( NULL, DELIMS ));
tilt = atoi(strtok( NULL, DELIMS ));
//ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
RB_arr.latitude=lat;
RB_arr.longitude=lon;
RB_arr.altitude=alt;
// Insert elements.
map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(tid);
if(it!=targets_map.end())
targets_map.erase(it);
targets_map.insert(make_pair(tid, RB_arr));
}
ROS_INFO("----->Saved %i waypoints.", targets_map.size());
// Close the file:
fin.close();
}
/*----------------------------------------/
/ Buzz closure to move following a 2D vector
/----------------------------------------*/
@ -158,9 +209,9 @@ namespace buzzuav_closures{
double rb[3], tmp[3];
map< int, buzz_utility::RB_struct >::iterator it;
for (it=targets_map.begin(); it!=targets_map.end(); ++it){
tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height;
tmp[0]=(it->second).latitude; tmp[1]=(it->second).longitude; tmp[2]=height;
rb_from_gps(tmp, rb, cur_pos);
ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]);
//ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
buzzvm_push(vm, targettbl);
/* When we get here, the "targets" table is on top of the stack */
//ROS_INFO("Buzz_utility will save user %i.", it->first);
@ -206,8 +257,9 @@ namespace buzzuav_closures{
if(fabs(rb[0])<100.0) {
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
buzz_utility::RB_struct RB_arr;
RB_arr.la=tmp[0];
RB_arr.lo=tmp[1];
RB_arr.latitude=tmp[0];
RB_arr.longitude=tmp[1];
RB_arr.altitude=tmp[2];
RB_arr.r=rb[0];
RB_arr.b=rb[1];
map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(uid);
@ -217,7 +269,7 @@ namespace buzzuav_closures{
//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
return vm->state;
} else
printf(" ---------- Target too far %f\n",rb[0]);
ROS_WARN(" ---------- Target too far %f",rb[0]);
return 0;
}
@ -284,11 +336,26 @@ namespace buzzuav_closures{
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float lat = buzzvm_stack_at(vm, 3)->f.value;
float lon = buzzvm_stack_at(vm, 2)->f.value;
float alt = buzzvm_stack_at(vm, 1)->f.value;
ROS_ERROR("DEBUG ---- %f %f %f",lat,lon,alt);
rc_set_goto((int)buzz_utility::get_robotid(), lat, lon, alt);
double goal[3];
goal[0] = buzzvm_stack_at(vm, 3)->f.value;
goal[1] = buzzvm_stack_at(vm, 2)->f.value;
goal[2] = buzzvm_stack_at(vm, 1)->f.value;
if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){
if(targets_map.size()<=0)
parse_gpslist();
goal[0] = targets_map.begin()->second.latitude;
goal[1] = targets_map.begin()->second.longitude;
goal[2] = targets_map.begin()->second.altitude;
targets_map.erase(targets_map.begin()->first);
}
double rb[3];
rb_from_gps(goal, rb, cur_pos);
if(fabs(rb[0])<150.0)
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
ROS_ERROR("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]);
return buzzvm_ret0(vm);
}

7
src/roscontroller.cpp
View File

@ -17,6 +17,7 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv)
bcfname = fname + ".bo";
dbgfname = fname + ".bdb";
set_bzz_file(bzzfile_name.c_str());
buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/");
/*Initialize variables*/
// Solo things
SetMode("LOITER", 0);
@ -751,8 +752,8 @@ void roscontroller::flight_controller_service_call()
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
} else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
ROS_INFO("TAKING A PICTURE HERE!! --------------")
;
ROS_INFO("TAKING A PICTURE HERE!! --------------");
ros::Duration(0.2).sleep();
}
}
/*----------------------------------------------
@ -1092,7 +1093,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
gps_rb(nei_pos, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/
uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3));
ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
// ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
/*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],
cvt_neighbours_pos_payload[1],