enhance barrier, simplify gps-graph and add multi-WP csv load
This commit is contained in:
dave
parent
ccdb80a4cf
commit
f906be2d97
|
|
@ -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,14 +268,14 @@ 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(id,"dis=",dis,"target=",target,"label=",nei_id)
|
||||
#log("x=",cDir.x,"y=",cDir.y)
|
||||
return cDir
|
||||
}
|
||||
|
|
@ -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(){
|
||||
gotoWP(TransitionToJoined)
|
||||
|
||||
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)
|
||||
}
|
||||
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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,20 +47,23 @@ 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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
@ -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){
|
||||
s.leave()
|
||||
gotoWP(picture)
|
||||
if(s!=nil){
|
||||
if(s.in())
|
||||
s.leave()
|
||||
}
|
||||
} 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
|
||||
uav_takepicture()
|
||||
statef=idle
|
||||
if(ptime==PICTURE_WAIT/2)
|
||||
uav_takepicture()
|
||||
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;
|
||||
}
|
||||
|
|
@ -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() {
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -9,8 +9,8 @@
|
|||
#include "ros/ros.h"
|
||||
#include "buzz_utility.h"
|
||||
|
||||
#define EARTH_RADIUS (double) 6371000.0
|
||||
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
|
||||
#define EARTH_RADIUS (double) 6371000.0
|
||||
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
|
||||
|
||||
namespace buzzuav_closures{
|
||||
typedef enum {
|
||||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
/----------------------------------------*/
|
||||
|
|
@ -105,10 +110,10 @@ namespace buzzuav_closures{
|
|||
}
|
||||
|
||||
void rb_from_gps(double nei[], double out[], double cur[]){
|
||||
double d_lon = nei[1] - cur[1];
|
||||
double d_lat = nei[0] - cur[0];
|
||||
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
|
||||
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
|
||||
double d_lon = nei[1] - cur[1];
|
||||
double d_lat = nei[0] - cur[0];
|
||||
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
|
||||
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
|
||||
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
|
||||
out[1] = constrainAngle(atan2(ned_y,ned_x));
|
||||
out[2] = 0.0;
|
||||
|
|
@ -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;
|
||||
rb_from_gps(tmp, rb, cur_pos);
|
||||
ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]);
|
||||
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)", 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;
|
||||
}
|
||||
|
|
@ -249,7 +301,7 @@ namespace buzzuav_closures{
|
|||
|
||||
mavros_msgs::Mavlink get_status(){
|
||||
mavros_msgs::Mavlink payload_out;
|
||||
map< int, buzz_utility::neighbors_status >::iterator it;
|
||||
map< int, buzz_utility::neighbors_status >::iterator it;
|
||||
for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++it){
|
||||
payload_out.payload64.push_back(it->first);
|
||||
payload_out.payload64.push_back(it->second.gps_strenght);
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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],
|
||||
|
|
|
|||
Loading…
Reference in New Issue