From ccdb80a4cfb191532613c2195cfb06d27f314aac Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 5 Sep 2017 00:34:59 -0400 Subject: [PATCH] changes in barrier and new graph from GPS --- buzz_scripts/graphform.bzz | 87 ++- buzz_scripts/graphformGPS.bzz | 829 +++++++++++++++++++++++++++++ buzz_scripts/include/barrier.bzz | 35 +- buzz_scripts/include/uavstates.bzz | 89 ++-- include/buzzuav_closures.h | 2 + src/buzz_utility.cpp | 7 +- src/buzzuav_closures.cpp | 31 +- src/roscontroller.cpp | 6 +- 8 files changed, 980 insertions(+), 106 deletions(-) create mode 100644 buzz_scripts/graphformGPS.bzz diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 339e6ce..95b5946 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -44,7 +44,7 @@ m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Respo m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} #Current robot state -m_eState="STATE_FREE" +# m_eState="STATE_FREE" # replace with default UAVSTATE #navigation vector m_navigation={.x=0,.y=0} @@ -361,19 +361,19 @@ function UpdateNodeInfo(){ #Transistion to state free # function TransitionToFree(){ - m_eState="STATE_FREE" + UAVSTATE="STATE_FREE" m_unWaitCount=m_unLabelSearchWaitTime - m_selfMessage.State=s2i(m_eState) + m_selfMessage.State=s2i(UAVSTATE) } # #Transistion to state asking # function TransitionToAsking(un_label){ - m_eState="STATE_ASKING" + UAVSTATE="STATE_ASKING" m_nLabel=un_label m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different - m_selfMessage.State=s2i(m_eState) + m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqID=m_unRequestId @@ -384,8 +384,8 @@ function TransitionToAsking(un_label){ #Transistion to state joining # function TransitionToJoining(){ - m_eState="STATE_JOINING" - m_selfMessage.State=s2i(m_eState) + UAVSTATE="STATE_JOINING" + m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel m_unWaitCount=m_unJoiningLostPeriod @@ -405,8 +405,8 @@ function TransitionToJoining(){ #Transistion to state joined # function TransitionToJoined(){ - m_eState="STATE_JOINED" - m_selfMessage.State=s2i(m_eState) + UAVSTATE="STATE_JOINED" + m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel m_vecNodes[m_nLabel].State="ASSIGNED" neighbors.ignore("r") @@ -426,8 +426,8 @@ function TransitionToJoined(){ #Transistion to state Lock, lock the current formation # function TransitionToLock(){ - m_eState="STATE_LOCK" - m_selfMessage.State=s2i(m_eState) + UAVSTATE="STATE_LOCK" + m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel m_vecNodes[m_nLabel].State="ASSIGNED" #record neighbor distance @@ -448,7 +448,7 @@ while(i0) @@ -527,7 +527,7 @@ function DoFree() { uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } #set message - m_selfMessage.State=s2i(m_eState) + m_selfMessage.State=s2i(UAVSTATE) } @@ -558,7 +558,7 @@ function DoAsking(){ if(psResponse==-1){ #no response, wait m_unWaitCount=m_unWaitCount-1 - m_selfMessage.State=s2i(m_eState) + m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqID=m_unRequestId if(m_unWaitCount==0){ @@ -576,7 +576,6 @@ function DoAsking(){ if(m_MessageReqID[psResponse]==m_unRequestId){ if(m_MessageResponse[psResponse]=="REQ_GRANTED"){ TransitionToJoining() - #TransitionToJoined() return } else{ @@ -656,7 +655,7 @@ function DoJoining(){ } #pack the communication package - m_selfMessage.State=s2i(m_eState) + m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel } @@ -665,7 +664,7 @@ function DoJoining(){ #Do joined # function DoJoined(){ - m_selfMessage.State=s2i(m_eState) + m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel #collect all requests @@ -754,7 +753,7 @@ function DoJoined(){ #Do Lock # function DoLock(){ -m_selfMessage.State=s2i(m_eState) +m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel m_navigation.x=0.0 m_navigation.y=0.0 @@ -772,7 +771,10 @@ uav_moveto(m_navigation.x, m_navigation.y, 0.0) function action(){ statef=action - UAVSTATE="GRAPH" + UAVSTATE="STATE_FREE" + + # reset the graph + Reset() } # @@ -792,27 +794,24 @@ function init() { # Join Swarm # uav_initswarm() - v_tag = stigmergy.create(m_lockstig) - uav_initstig() + v_tag = stigmergy.create(m_lockstig) + uav_initstig() # go to diff. height since no collision avoidance implemented yet TARGET_ALTITUDE = 2.5 + id * 1.5 statef=turnedoff UAVSTATE = "TURNEDOFF" - - # reset the graph - Reset() } # # Executed every step # -function step(){ +function step() { # listen to potential RC uav_rccmd() # get the swarm commands uav_neicmd() # update the vstig (status/net/batt) - uav_updatestig() + uav_updatestig() #update the graph UpdateNodeInfo() @@ -821,27 +820,23 @@ function step(){ # # graph state machine # - if(UAVSTATE=="GRAPH"){ - if(m_eState=="STATE_FREE") - DoFree() - else if(m_eState=="STATE_ESCAPE") - DoEscape() - else if(m_eState=="STATE_ASKING") - DoAsking() - else if(m_eState=="STATE_JOINING") - DoJoining() - else if(m_eState=="STATE_JOINED") - DoJoined() - else if(m_eState=="STATE_LOCK") - DoLock() - } + if(UAVSTATE=="STATE_FREE") + statef=DoFree + else if(UAVSTATE=="STATE_ESCAPE") + statef=DoEscape + else if(UAVSTATE=="STATE_ASKING") + statef=DoAsking + else if(UAVSTATE=="STATE_JOINING") + statef=DoJoining + else if(UAVSTATE=="STATE_JOINED") + statef=DoJoined + else if(UAVSTATE=="STATE_LOCK") + statef=DoLock # high level UAV state machine statef() - - debug(m_eState,m_nLabel) - log("Current state: ", UAVSTATE) + log("Current state: ", UAVSTATE, " and label: ", m_nLabel) log("Swarm size: ", ROBOTS) #navigation @@ -868,12 +863,8 @@ function step(){ # Executed when reset # function Reset(){ -# m_vecNodes={} -# m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary -# m_vecNodes_fixed={} -# m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") Read_Graph() - m_nLabel=-1 + m_nLabel=-1 #start listening start_listen() diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz new file mode 100644 index 0000000..78caa0d --- /dev/null +++ b/buzz_scripts/graphformGPS.bzz @@ -0,0 +1,829 @@ +# +# Include files +# +include "string.bzz" +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 "uavstates.bzz" # require an 'action' function to be defined here. + +include "graphs/shapes_Y.bzz" + +ROBOT_RADIUS = 50 +ROBOT_DIAMETER = 2.0*ROBOT_RADIUS +ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER +ROOT_ID = 2 + +# max velocity in cm/step +ROBOT_MAXVEL = 150.0 + +# +# Global variables +# + +# +# Save message from all neighours +#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot +m_MessageState={}#store received neighbour message +m_MessageLabel={}#store received neighbour message +m_MessageReqLabel={}#store received neighbour message +m_MessageReqID={}#store received neighbour message +m_MessageResponse={}#store received neighbour message +m_MessageRange={}#store received neighbour message +m_MessageBearing={}#store received neighbour message +m_neighbourCount=0#used to cunt neighbours +#Save message from one neighbour +#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing +m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} + +# +#Save the message to send +#The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) +m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + +#Current robot state +# m_eState="STATE_FREE" # replace with default UAVSTATE + +#navigation vector +m_navigation={.x=0,.y=0} + +#Debug message to be displayed in qt-opengl +#m_ossDebugMsg + +#Debug vector to draw +#CVector2 m_cDebugVector + +#Current label being requested or chosen (-1 when none) +m_nLabel=-1 +m_messageID={} +#neighbor distance to lock the current pattern +lock_neighbor_id={} +lock_neighbor_dis={} + +#Label request id +m_unRequestId=0 + +#Global bias, used to map local coordinate to global coordinate +m_bias=0 + +#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate +m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + +#Counter to wait for something to happen +m_unWaitCount=0 + +#Number of steps to wait before looking for a free label +m_unLabelSearchWaitTime=0 + +#Number of steps to wait for an answer to be received +m_unResponseTimeThreshold=0 + +#Number of steps to wait until giving up joining +m_unJoiningLostPeriod=0 + +#Tolerance distance to a target location +m_fTargetDistanceTolerance=0 + +#step cunt +step_cunt=0 + +# virtual stigmergy for the LOCK barrier. +m_lockstig = 1 + +# Lennard-Jones parameters, may need change +EPSILON = 1800 #13.5 the LJ parameter for other robots + +# Lennard-Jones interaction magnitude + +function FlockInteraction(dist,target,epsilon){ + var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + return mag +} + +# +#return the number of value in table +# +function count(table,value){ + var number=0 + var i=0 + while(i=0){ + pon=0 + } + else{ + pon=1 + } + var b=math.abs(send_table.Bearing) + send_value=r_id*1000+pon*100+b + return send_value +} +# +#unpack message +# +function unpackmessage(recv_value){ + var wan=(recv_value-recv_value%100000)/100000 + recv_value=recv_value-wan*100000 + var qian=(recv_value-recv_value%10000)/10000 + recv_value=recv_value-qian*10000 + var bai=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-bai*1000 + var shi=(recv_value-recv_value%10)/10 + recv_value=recv_value-shi*10 + var ge=recv_value + var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0} + return_table.State=wan + return_table.Label=qian + return_table.ReqLabel=bai + return_table.ReqID=shi + return_table.Response=ge + return return_table +} +# +#unpack guide message +# +function unpack_guide_msg(recv_value){ +log(id,"I pass value=",recv_value) + var qian=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-qian*1000 + var bai=(recv_value-recv_value%100)/100 + recv_value=recv_value-bai*100 + var b=recv_value + var return_table={.Label=0.0,.Bearing=0.0} + return_table.Label=qian + if(bai==1){ + b=b*-1.0 + } + return_table.Bearing=b + return return_table +} + +#get the target distance to neighbr nei_id +function target4label(nei_id){ + var return_val="miss" + var i=0 + while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ + m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 + if(m_vecNodes[i].StateAge==0) + m_vecNodes[i].State="UNASSIGNED" + } + i=i+1 + } +} + +# +#Transistion to state free +# +function TransitionToFree(){ + UAVSTATE="STATE_FREE" + m_unWaitCount=m_unLabelSearchWaitTime + m_selfMessage.State=s2i(UAVSTATE) +} + +# +#Transistion to state asking +# +function TransitionToAsking(un_label){ + UAVSTATE="STATE_ASKING" + m_nLabel=un_label + m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different + m_selfMessage.State=s2i(UAVSTATE) + m_selfMessage.ReqLabel=m_nLabel + m_selfMessage.ReqID=m_unRequestId + + m_unWaitCount=m_unResponseTimeThreshold +} + +# +#Transistion to state joining +# +function set_rc_goto() { + #get information of pred + var i=0 + var IDofPred=-1 + while(imath.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 + } + }) +} + +# +#Transistion to state joined +# +function TransitionToJoined(){ + UAVSTATE="STATE_JOINED" + 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_ready() + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) +} + +# +#Transistion to state Lock, lock the current formation +# +function TransitionToLock(){ + UAVSTATE="STATE_LOCK" + m_selfMessage.State=s2i(UAVSTATE) + m_selfMessage.Label=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + #record neighbor distance + lock_neighbor_id={} + lock_neighbor_dis={} + var i=0 + while(i0) + m_unWaitCount=m_unWaitCount-1 + + #find a set of joined robots + var setJoinedLabels={} + var setJoinedIndexes={} + var i=0 + var j=0 + while(i0){ + TransitionToAsking(unFoundLabel) + return + } + + #set message + m_selfMessage.State=s2i(UAVSTATE) + +} + +# +#Do asking +# +function DoAsking(){ + #look for response from predecessor + var i=0 + var psResponse=-1 + while(im_MessageRange[mapRequests[i]]) + ReqIndex=i + i=i+1 + } + #get the best index, whose ReqLabel and Reqid are + ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]] + var ReqID=m_MessageReqID[mapRequests[ReqIndex]] + m_selfMessage.ReqLabel=ReqLabel + m_selfMessage.ReqID=ReqID + m_selfMessage.Response=r2i("REQ_GRANTED") + m_vecNodes[ReqLabel].State="ASSIGNING" + log("Label=",ReqLabel) + log("ID=",ReqID) + m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod + } + + #lost pred, wait for some time and transit to free + if(seenPred==0){ + m_unWaitCount=m_unWaitCount-1 + if(m_unWaitCount==0){ + TransitionToFree() + return + } + } + + #check if should to transists to lock + #write statues + #v_tag.get(m_nLabel) + #log(v_tag.size(), " of ", ROBOTS, " ready to lock") + #if(v_tag.size()==ROBOTS){ + # TransitionToLock() + #} + barrier_wait(ROBOTS, TransitionToLock, DoJoined) +} + +# +#Do Lock +# +function DoLock(){ + m_selfMessage.State=s2i(UAVSTATE) + m_selfMessage.Label=m_nLabel + m_navigation.x=0.0 + m_navigation.y=0.0 + #calculate motion vection + if(m_nLabel==0){ + m_navigation.x=0.0 #change value so that robot 0 will move + m_navigation.y=0.0 + } + if(m_nLabel!=0){ + m_navigation=motion_vector() + } + #move + uav_moveto(m_navigation.x, m_navigation.y, 0.0) +} + +function action(){ + statef=action + UAVSTATE="STATE_FREE" + + # reset the graph + Reset() +} + +# +# Executed at init +# +function init() { + # + # Global parameters for graph formation + # + m_unResponseTimeThreshold=10 + m_unLabelSearchWaitTime=10 + m_fTargetDistanceTolerance=100 + m_fTargetAngleTolerance=0.1 + m_unJoiningLostPeriod=100 + + # + # Join Swarm + # + uav_initswarm() + #v_tag = stigmergy.create(m_lockstig) + uav_initstig() + # go to diff. height since no collision avoidance implemented yet + TARGET_ALTITUDE = 2.5 + id * 1.5 + statef=turnedoff + UAVSTATE = "TURNEDOFF" +} + +# +# Executed every step +# +function step() { + # listen to potential RC + uav_rccmd() + # get the swarm commands + uav_neicmd() + # update the vstig (status/net/batt) + uav_updatestig() + + #update the graph + UpdateNodeInfo() + #reset message package to be sent + m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + # + # graph state machine + # + if(UAVSTATE=="STATE_FREE") + statef=DoFree + else if(UAVSTATE=="STATE_ESCAPE") + statef=DoEscape + else if(UAVSTATE=="STATE_ASKING") + statef=DoAsking + else if(UAVSTATE=="STATE_JOINING") + statef=DoJoining + else if(UAVSTATE=="STATE_JOINED") + statef=DoJoined + else if(UAVSTATE=="STATE_LOCK") + statef=DoLock + + # high level UAV state machine + statef() + + log("Current state: ", UAVSTATE, " and label: ", m_nLabel) + log("Swarm size: ", ROBOTS) + + #navigation + #broadcast message + neighbors.broadcast("m",packmessage(m_selfMessage)) + + # + #clean message storage + m_MessageState={}#store received neighbour message + m_MessageLabel={}#store received neighbour message + m_MessageReqLabel={}#store received neighbour message + m_MessageReqID={}#store received neighbour message + m_MessageResponse={}#store received neighbour message + m_MessageRange={}#store received neighbour message + m_MessageBearing={}#store received neighbour message + m_neighbourCount=0 + + + #step cunt+1 + step_cunt=step_cunt+1 +} + +# +# Executed when reset +# +function Reset(){ + Read_Graph() + m_nLabel=-1 + + #start listening + start_listen() + # + #set initial state, only one robot choose [A], while the rest choose [B] + # + #[A]The robot used to triger the formation process is defined as joined, + if(id==ROOT_ID){ + m_nLabel=0 + TransitionToJoined() + } + #[B]Other robots are defined as free. + else{ + TransitionToFree() + } +} + +# +# Executed upon destroy +# +function destroy() { + #clear neighbour message + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) + m_vecNodes={} + #stop listening + neighbors.ignore("m") +} diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/barrier.bzz index dbf5978..9fe0f28 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -7,44 +7,53 @@ # # Constants # -BARRIER_VSTIG = 11 BARRIER_TIMEOUT = 200 # in steps +timeW = 0 # # Sets a barrier # -function barrier_set(threshold, transf, resumef, bdt) { +function barrier_create(vstig_nb) { + # reset + timeW = 0 + # create barrier vstig + barrier = stigmergy.create(vstig_nb) +} + +function barrier_set(threshold, transf, resumef, vstig_nb) { statef = function() { - barrier_wait(threshold, transf, resumef, bdt); + barrier_wait(threshold, transf, resumef); } - barrier = stigmergy.create(BARRIER_VSTIG) + UAVSTATE = "BARRIERWAIT" + barrier_create(vstig_nb) } # # Make yourself ready # function barrier_ready() { + log("BARRIER READY -------") barrier.put(id, 1) + barrier.put("d", 0) } # # Executes the barrier # -timeW=0 -function barrier_wait(threshold, transf, resumef, bdt) { +function barrier_wait(threshold, transf, resumef) { barrier.put(id, 1) - UAVSTATE = "BARRIERWAIT" barrier.get(id) - if(barrier.size() >= threshold) { -# getlowest() + if(barrier.size() >= threshold or barrier.get("d")==1) { + barrier.put("d", 1) +# barrier = nil + timeW = 0 transf() } else if(timeW >= BARRIER_TIMEOUT) { - barrier = nil + timeW = 0 +# barrier = nil resumef() - timeW=0 - } else if(bdt!=-1) - neighbors.broadcast("cmd", bdt) + } timeW = timeW+1 } diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 8b5a9dd..5d90034 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -6,7 +6,8 @@ TARGET_ALTITUDE = 5.0 UAVSTATE = "TURNEDOFF" GOTO_MAXVEL = 2 -goal = {.range=0, .bearing=0} +TAKEOFF_VSTIG = 11 +goal = {.range=0, .bearing=0, .latitude=0, .longitude=0} function uav_initswarm() { s = swarm.create(1) @@ -23,60 +24,76 @@ 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_set(ROBOTS,action,land,-1) - barrier_ready() - } - else { - log("Altitude: ", TARGET_ALTITUDE) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } + barrier_wait(ROBOTS, action, land) + } else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } } +firstpassL = 1 function land() { UAVSTATE = "LAND" statef=land - if(flight.status == 2 or flight.status == 3){ - neighbors.broadcast("cmd", 21) - uav_land() - } - else { - barrier_set(ROBOTS,turnedoff,land,21) - barrier_ready() - timeW=0 - #barrier = nil - #statef=idle + 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) } } function goto() { UAVSTATE = "GOTO" statef=goto - # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) + if(rc_goto.id==id){ s.leave() - rb_from_gps(rc_goto.latitude, rc_goto.longitude) - print(id, " has to move ", goal.range) - m_navigation = math.vec2.newp(goal.range, goal.bearing) - if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { - 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 - statef = idle + gotoWP(picture) } else { neighbors.broadcast("cmd", 16) neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) } } +function picture() { + #TODO: change gimbal orientation + uav_takepicture() + statef=idle +} + +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) + m_navigation = math.vec2.newp(goal.range, goal.bearing) + if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { + 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 + transf() +} + function follow() { if(size(targets)>0) { UAVSTATE = "FOLLOW" @@ -156,11 +173,21 @@ function LimitAngle(angle){ } function rb_from_gps(lat, lon) { - print("rb dbg: ",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; ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); goal.bearing = LimitAngle(math.atan(ned_y,ned_x)); +} + +function gps_from_rb(range, bearing) { + print("rb2gps: ",range,bearing,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)); + goal.latitude = target_lat*180.0/math.pi; + goal.longitude = target_lon*180.0/math.pi; } \ No newline at end of file diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 4723269..61c7000 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -22,6 +22,7 @@ namespace buzzuav_closures{ COMMAND_DISARM, COMMAND_GOTO, COMMAND_MOVETO, + COMMAND_PICTURE, } Custom_CommandCode; /* @@ -37,6 +38,7 @@ int buzzros_print(buzzvm_t vm); */ int buzzuav_moveto(buzzvm_t vm); int buzzuav_storegoal(buzzvm_t vm); +int buzzuav_takepicture(buzzvm_t vm); /* Returns the current command from local variable*/ int getcmd(); /*Sets goto position */ diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 01d960a..b191b90 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -304,9 +304,12 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture)); + buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); buzzvm_gstore(VM); @@ -349,7 +352,7 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 95d2656..418b297 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -142,7 +142,7 @@ namespace buzzuav_closures{ cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]); - buzz_cmd= COMMAND_MOVETO; // TO DO what should we use + buzz_cmd = COMMAND_MOVETO; // TO DO what should we use return buzzvm_ret0(vm); } @@ -264,19 +264,32 @@ namespace buzzuav_closures{ message_number++;*/ return payload_out; } + /*----------------------------------------/ + / Buzz closure to take a picture here. + /----------------------------------------*/ + int buzzuav_takepicture(buzzvm_t vm) { + cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; + buzz_cmd = COMMAND_PICTURE; + return buzzvm_ret0(vm); + } + /*----------------------------------------/ / Buzz closure to store locally a GPS destination from the fleet /----------------------------------------*/ int buzzuav_storegoal(buzzvm_t vm) { buzzvm_lnum_assert(vm, 3); - buzzvm_lload(vm, 1); // latitude - buzzvm_lload(vm, 2); // longitude - buzzvm_lload(vm, 3); // altitude - buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - rc_set_goto(buzzvm_stack_at(vm, 1)->f.value, buzzvm_stack_at(vm, 2)->f.value, buzzvm_stack_at(vm, 3)->f.value, (int)buzz_utility::get_robotid()); - return buzzvm_ret0(vm); + buzzvm_lload(vm, 1); // altitude + buzzvm_lload(vm, 2); // longitude + buzzvm_lload(vm, 3); // latitude + 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); + return buzzvm_ret0(vm); } /*----------------------------------------/ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 55ac7c1..2711a73 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -750,9 +750,9 @@ 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); - - // roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], - // goto_pos[2], 0); + } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ + ROS_INFO("TAKING A PICTURE HERE!! --------------") + ; } } /*----------------------------------------------