diff --git a/.gitignore b/.gitignore index 64f22eb..f426dc9 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ src/test* *.bo *.bdb *.bdbg +buzz_scripts/log* diff --git a/buzz_scripts/empty.bzz b/buzz_scripts/empty.bzz new file mode 100644 index 0000000..3bb5b12 --- /dev/null +++ b/buzz_scripts/empty.bzz @@ -0,0 +1,17 @@ +include "update.bzz" + +# Executed once at init time. +function init() { +} + +# Executed at each time step. +function step() { +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/buzz_scripts/flock.bzz b/buzz_scripts/flock.bzz index c4246ab..b8de0cf 100644 --- a/buzz_scripts/flock.bzz +++ b/buzz_scripts/flock.bzz @@ -38,7 +38,7 @@ function action() { # Move according to vector print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum)) - uav_moveto(accum.x, accum.y) + uav_moveto(accum.x, accum.y, 0.0) UAVSTATE = "LENNARDJONES" # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS @@ -47,11 +47,11 @@ function action() { # } else if(timeW>=WAIT_TIMEOUT/2) { # UAVSTATE ="GOEAST" # timeW = timeW+1 -# uav_moveto(0.0,5.0) +# uav_moveto(0.0, 5.0, 0.0) # } else { # UAVSTATE ="GONORTH" # timeW = timeW+1 -# uav_moveto(5.0,0.0) +# uav_moveto(5.0, 0.0, 0.0) # } } @@ -65,15 +65,19 @@ function action() { function init() { uav_initswarm() uav_initstig() + # TARGET_ALTITUDE = 2.5 + id * 5 + statef=turnedoff + UAVSTATE = "TURNEDOFF" } # Executed at each time step. function step() { uav_rccmd() uav_neicmd() + uav_updatestig() statef() - uav_updatestig() + log("Current state: ", UAVSTATE) log("Swarm size: ",ROBOTS) if(id==0) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index adb8139..ef03438 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -4,18 +4,20 @@ include "string.bzz" include "vec2.bzz" include "update.bzz" -include "barrier.bzz" # don't use a stigmergy id=11 with this header. + +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 "vstigenv.bzz" include "graphs/shapes_Y.bzz" -ROBOT_RADIUS=50 -ROBOT_DIAMETER=2.0*ROBOT_RADIUS -ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER +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 = 250.0 +ROBOT_MAXVEL = 150.0 # # Global variables @@ -42,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} @@ -87,11 +89,11 @@ m_fTargetDistanceTolerance=0 #step cunt step_cunt=0 -#virtual stigmergy +# virtual stigmergy for the LOCK barrier. m_lockstig = 1 # Lennard-Jones parameters, may need change -EPSILON = 4000 #13.5 the LJ parameter for other robots +EPSILON = 1800 #13.5 the LJ parameter for other robots # Lennard-Jones interaction magnitude @@ -100,24 +102,6 @@ function FlockInteraction(dist,target,epsilon){ return mag } -function LimitAngle(angle){ - if(angle>2*math.pi) - return angle-2*math.pi - else if (angle<0) - return angle+2*math.pi - else - return angle -} - -# -# Calculates the angle of the given vector2. -# PARAM v: The vector2. -# RETURN: The angle of the vector. -# -Angle = function(v) { - return math.atan(v.y, v.x) -} - # #return the number of value in table # @@ -204,26 +188,12 @@ function find(table,value){ return index } -function pow(base,exponent){ - var i=0 - var renturn_val=1 - if(exponent==0) - return 1 - else{ - while(i0) @@ -535,7 +505,7 @@ function DoFree() { # Limit the mvt if(math.vec2.length(m_navigation)>ROBOT_MAXVEL*2) m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL*2/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) }else{ #no joined robots in sight i=0 var tempvec={.x=0.0,.y=0.0} @@ -545,7 +515,7 @@ function DoFree() { i=i+1 } m_navigation=math.vec2.scale(tempvec,1.0/i) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } @@ -554,10 +524,10 @@ function DoFree() { if(step_cunt<=1){ m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + 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) } @@ -576,8 +546,10 @@ function DoAsking(){ #the request Label be the same as requesed #get a respond if(m_MessageState[i]=="STATE_JOINED"){ + log("received label = ",m_MessageReqLabel[i]) if(m_MessageReqLabel[i]==m_nLabel) if(m_MessageResponse[i]!="REQ_NONE"){ + log("get response") psResponse=i }} i=i+1 @@ -586,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){ @@ -595,6 +567,7 @@ function DoAsking(){ } } else{ + log("respond id=",m_MessageReqID[psResponse]) if(m_MessageReqID[psResponse]!=m_unRequestId){ m_vecNodes[m_nLabel].State="ASSIGNING" m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod @@ -603,7 +576,6 @@ function DoAsking(){ if(m_MessageReqID[psResponse]==m_unRequestId){ if(m_MessageResponse[psResponse]=="REQ_GRANTED"){ TransitionToJoining() - #TransitionToJoined() return } else{ @@ -614,7 +586,7 @@ function DoAsking(){ } m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } # @@ -653,7 +625,7 @@ function DoJoining(){ #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=Angle(S2Target) + var S2Target_bearing=math.atan(S2Target.y, S2Target.x) m_bias=m_cMeToPred.Bearing-S2PGlobalBearing S2Target_bearing=S2Target_bearing+m_bias @@ -662,7 +634,7 @@ function DoJoining(){ S2Target = math.vec2.scale(S2Target, ROBOT_MAXVEL/math.vec2.length(S2Target)) m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) @@ -683,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 } @@ -692,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 @@ -705,10 +677,13 @@ function DoJoined(){ while(i2*math.pi) - return angle-2*math.pi - else if (angle<0) - return angle+2*math.pi - else - return angle -} - -# -# Calculates the angle of the given vector2. -# PARAM v: The vector2. -# RETURN: The angle of the vector. -# -Angle = function(v) { - return math.atan(v.y, v.x) -} - -# -#return the number of value in table -# -function count(table,value){ - var number=0 - 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(){ - m_eState="STATE_FREE" - m_unWaitCount=m_unLabelSearchWaitTime - m_selfMessage.State=m_eState -} - -# -#Transistion to state asking -# -function TransitionToAsking(un_label){ - m_eState="STATE_ASKING" - m_nLabel=un_label - m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different - m_selfMessage.State=m_eState - m_selfMessage.ReqLable=m_nLabel - m_selfMessage.ReqID=m_unRequestId - - m_unWaitCount=m_unResponseTimeThreshold -} - -# -#Transistion to state joining -# -function TransitionToJoining(){ - m_eState="STATE_JOINING" - m_selfMessage.State=m_eState - m_selfMessage.Lable=m_nLabel - m_unWaitCount=m_unJoiningLostPeriod - - neighbors.listen("reply", - function(vid,value,rid){ - #store the received message - if(value.Lable==m_nLabel){ - m_cMeToPred.GlobalBearing=value.GlobalBearing - - } - }) - -} - -# -#Transistion to state joined -# -function TransitionToJoined(){ - m_eState="STATE_JOINED" - m_selfMessage.State=m_eState - m_selfMessage.Lable=m_nLabel - m_vecNodes[m_nLabel].State="ASSIGNED" - neighbors.ignore("reply") - - #write statues - v_tag.put(m_nLabel, 1) - - m_navigation.x=0.0 - m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) -} - -# -#Transistion to state Lock, lock the current formation -# -function TransitionToLock(){ - m_eState="STATE_LOCK" - m_selfMessage.State=m_eState - m_selfMessage.Lable=m_nLabel - m_vecNodes[m_nLabel].State="ASSIGNED" - - m_navigation.x=0.0 - m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) -} - -# -# Do free -# -function DoFree() { - m_selfMessage.State=m_eState - #wait for a while before looking for a lable - if(m_unWaitCount>0) - m_unWaitCount=m_unWaitCount-1 - - #find a set of joined robots - var setJoinedLables={} - var setJoinedIndexes={} - var i=0 - var j=0 - while(i0){ - TransitionToAsking(unFoundLable) - return - } - - #navigation - #if there is a joined robot within sight, move around joined robots - #else, gather with other free robots - if(size(setJoinedIndexes)>0){ - var tempvec_P={.x=0.0,.y=0.0} - var tempvec_N={.x=0.0,.y=0.0} - i=0 - 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=Angle(S2Target) - m_bias=m_cMeToPred.Bearing-S2PGlobalBearing - #S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17 - m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) - - - - #test if is already in desired position - if(math.abs(S2Target.x)m_MessageRange[mapRequests[i]]) - ReqIndex=i - i=i+1 - } - #get the best index, whose Reqlable and Reqid are - ReqLable=m_MessageReqLable[mapRequests[ReqIndex]] - var ReqID=m_MessageReqID[mapRequests[ReqIndex]] - m_selfMessage.ReqLable=ReqLable - m_selfMessage.ReqID=ReqID - m_selfMessage.Response="REQ_GRANTED" - } - - #lost pred, wait for some time and transit to free - #if(seenPred==0){ - #m_unWaitCount=m_unWaitCount-1 - #if(m_unWaitCount==0){ - #TransitionToFree() - #return - #} - #} - - - m_navigation.x=0.0 - m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) - - - #check if should to transists to lock - - - if(v_tag.size()==ROBOTS){ - TransitionToLock() - } -} - -# -#Do Lock -# -function DoLock(){ -m_selfMessage.State=m_eState -m_selfMessage.Lable=m_nLabel - -m_navigation.x=0.0 -m_navigation.y=0.0 - -#collect preds information -var i=0 -var mypred1={.range=0,.bearing=0} -var mypred2={.range=0,.bearing=0} - -while(i1){ - var cDir={.x=0.0,.y=0.0} - var cDir1={.x=0.0,.y=0.0} - var cDir2={.x=0.0,.y=0.0} - cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing) - cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing) - #cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing) - #cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing) - cDir=math.vec2.add(cDir1,cDir2) - - cDir=math.vec2.scale(cDir,100) - m_navigation.x=cDir.x - m_navigation.y=cDir.y - #log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2) - log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1) -} -#move -uav_moveto(m_navigation.x,m_navigation.y) -} - -function action(){ - statef=action - UAVSTATE="GRAPH" -} - -# -# Executed at init -# -function init() { - # - #Adjust parameters here - # - m_unResponseTimeThreshold=10 - m_unLabelSearchWaitTime=10 - m_fTargetDistanceTolerance=10 - m_unJoiningLostPeriod=100 - - # - # Join Swarm - # - uav_initswarm() - Reset(); -} - -# -# Executed every step -# -function step(){ - uav_rccmd() - uav_neicmd() - #update the graph - UpdateNodeInfo() - #reset message package to be sent - m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} - # - #act according to current state - # - 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() - } - - statef() - - - debug(m_eState,m_nLabel) - log("Current state: ", UAVSTATE) - log("Swarm size: ", ROBOTS) - #navigation - - #broadcast messag - neighbors.broadcast("msg",m_selfMessage) - - # - #clean message storage - m_MessageState={}#store received neighbour message - m_MessageLable={}#store received neighbour message - m_MessageReqLable={}#store received neighbour message - m_MessageReqID={}#store received neighbour message - m_MessageSide={}#store received neighbour message - m_MessageResponse={}#store received neighbour message - m_MessageRange={}#store received neighbour message - m_MessageBearing={}#store received neighbour message - m_neighbourCunt=0 - - - #step cunt+1 - step_cunt=step_cunt+1 -} - -# -# 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") - 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==0){ - m_nLabel=0 - TransitionToJoined() - } - #[B]Other robots are defined as free. - else{ - TransitionToFree() - } -} - -# -# Executed upon destroy -# -function destroy() { - #clear neighbour message - uav_moveto(0.0,0.0) - m_vecNodes={} - #stop listening - neighbors.ignore("msg") -} diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz new file mode 100644 index 0000000..d4e5aae --- /dev/null +++ b/buzz_scripts/graphformGPS.bzz @@ -0,0 +1,849 @@ +# +# 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=80 (auto-increment up) for this header. +include "uavstates.bzz" # require an 'action' function to be defined here. + +include "graphs/shapes_P.bzz" +include "graphs/shapes_O.bzz" +include "graphs/shapes_L.bzz" +include "graphs/shapes_Y.bzz" + +ROBOT_RADIUS = 50 +ROBOT_DIAMETER = 2.0*ROBOT_RADIUS +ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER +ROOT_ID = 2 +old_state = -1 + +# 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")} + +#navigation vector +m_navigation={.x=0,.y=0} + +#Current label being requested or chosen (-1 when none) +m_nLabel=-1 +m_messageID={} +repeat_assign=0 +assign_label=-1 +assign_id=-1 +m_gotjoinedparent = 0 +#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 + +# virtual stigmergy for the LOCK barrier. +m_lockstig = 1 + +# Lennard-Jones parameters, may need change +EPSILON = 4000 #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 TransitionToJoining(){ + UAVSTATE="STATE_JOINING" + m_selfMessage.State=s2i(UAVSTATE) + m_selfMessage.Label=m_nLabel + m_unWaitCount=m_unJoiningLostPeriod +} +# +#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" + + #write statues + #v_tag.put(m_nLabel, m_lockstig) + barrier_create() + 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 + } + if(repeat_assign==0){ + #get the best index, whose ReqLabel and Reqid are + ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]] + var ReqID=m_MessageReqID[mapRequests[ReqIndex]] + assign_label=ReqLabel + assign_id=ReqID + repeat_assign=1 + } + m_selfMessage.ReqLabel=assign_label + m_selfMessage.ReqID=assign_id + m_selfMessage.Response=r2i("REQ_GRANTED") + #m_vecNodes[ReqLabel].State="ASSIGNING" + log("Label=",assign_label) + log("ID=",assign_id) + 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, -1) +} +# +#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) + +} +# +# Executed after takeoff +# +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 = 20.0 + id * 2.5 + statef=turnedoff + UAVSTATE = "TURNEDOFF" +} +# +# Executed every step (main loop) +# +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_ASKING") + statef=DoAsking + else if(UAVSTATE=="STATE_JOINING") + statef=DoJoining + else if(UAVSTATE=="STATE_JOINED") + statef=DoJoined + else if(UAVSTATE=="STATE_LOCK" and old_state!=rc_State) + statef=action + else if(UAVSTATE=="STATE_LOCK" and old_state==rc_State) + 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 + +} +# +# Executed when reset +# +function Reset(){ + m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} + m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + m_navigation={.x=0,.y=0} + m_nLabel=-1 + m_messageID={} + lock_neighbor_id={} + lock_neighbor_dis={} + m_unRequestId=0 + m_bias=0 + m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + m_unWaitCount=0 + repeat_assign=0 + assign_label=-1 + assign_id=-1 + m_gotjoinedparent = 0 + + if(rc_State==0){ + log("Loading P graph") + Read_GraphP() + } else if(rc_State==1) { + log("Loading O graph") + Read_GraphO() + } else if(rc_State==2) { + log("Loading L graph") + Read_GraphL() + } else if(rc_State==3) { + log("Loading Y graph") + Read_GraphY() + } + + #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 6192b65..e280a66 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -7,49 +7,73 @@ # # Constants # -BARRIER_VSTIG = 11 +BARRIER_TIMEOUT = 1200 # in steps +BARRIER_VSTIG = 80 +timeW = 0 +barrier = nil # # Sets a barrier # -function barrier_set(threshold, transf, resumef) { - statef = function() { - barrier_wait(threshold, transf, resumef); +function barrier_create() { + # reset + timeW = 0 + # create barrier vstig + #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, bdt) { + statef = function() { + barrier_wait(threshold, transf, resumef, bdt); + } + UAVSTATE = "BARRIERWAIT" + barrier_create() +} + # # Make yourself ready # function barrier_ready() { + #log("BARRIER READY -------") barrier.put(id, 1) + barrier.put("d", 0) } # # Executes the barrier # -BARRIER_TIMEOUT = 200 -timeW=0 -function barrier_wait(threshold, transf, resumef) { - barrier.get(id) +function barrier_wait(threshold, transf, resumef, bdt) { barrier.put(id, 1) - UAVSTATE = "BARRIERWAIT" - if(barrier.size() >= threshold) { -# getlowest() + + barrier.get(id) + #log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")") + if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { + barrier.put("d", 1) + timeW = 0 transf() - } else if(timeW>=BARRIER_TIMEOUT) { - barrier = nil + } else if(timeW >= BARRIER_TIMEOUT) { + log("------> Barrier Timeout !!!!") + barrier=nil + timeW = 0 resumef() - timeW=0 } + + if(bdt!=-1) + neighbors.broadcast("cmd", bdt) + timeW = timeW+1 } -# get the lowest id of the fleet, but requires too much bandwidth +# get the lowest id of the fleet, but requires too much bandwidth... function getlowest(){ - Lid = 20; - u=20 + Lid = 15; + u=15 while(u>=0){ tab = barrier.get(u) if(tab!=nil){ @@ -59,4 +83,4 @@ function getlowest(){ u=u-1 } log("--> LOWEST ID:",Lid) -} \ No newline at end of file +} diff --git a/buzz_scripts/include/graphs/shapes_L.bzz b/buzz_scripts/include/graphs/shapes_L.bzz index 16f2272..2988d81 100644 --- a/buzz_scripts/include/graphs/shapes_L.bzz +++ b/buzz_scripts/include/graphs/shapes_L.bzz @@ -1,7 +1,7 @@ #Table of the nodes in the graph m_vecNodes={} -function Read_Graph(){ +function Read_GraphL(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point .Pred = -1, # Label of its predecessor diff --git a/buzz_scripts/include/graphs/shapes_O.bzz b/buzz_scripts/include/graphs/shapes_O.bzz index 679a044..068e5a2 100644 --- a/buzz_scripts/include/graphs/shapes_O.bzz +++ b/buzz_scripts/include/graphs/shapes_O.bzz @@ -1,7 +1,7 @@ #Table of the nodes in the graph m_vecNodes={} -m_vecNodes_fixed={} -function Read_Graph(){ + +function Read_GraphO(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point .Pred = -1, # Label of its predecessor diff --git a/buzz_scripts/include/graphs/shapes_P.bzz b/buzz_scripts/include/graphs/shapes_P.bzz index 34029be..14e183b 100644 --- a/buzz_scripts/include/graphs/shapes_P.bzz +++ b/buzz_scripts/include/graphs/shapes_P.bzz @@ -1,7 +1,7 @@ #Table of the nodes in the graph m_vecNodes={} -m_vecNodes_fixed={} -function Read_Graph(){ + +function Read_GraphP(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point .Pred = -1, # Label of its predecessor @@ -14,7 +14,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of Lab m_vecNodes[1] = { .Label = 1, .Pred = 0, - .distance = 2000, + .distance = 1000, .bearing = 0.0, .height = 5000, .State="UNASSIGNED", @@ -23,7 +23,7 @@ m_vecNodes[1] = { m_vecNodes[2] = { .Label = 2, .Pred = 0, - .distance = 2000, + .distance = 1000, .bearing = 1.57, .height = 7000, .State="UNASSIGNED", @@ -32,7 +32,7 @@ m_vecNodes[2] = { m_vecNodes[3] = { .Label = 3, .Pred = 0, - .distance = 2000, + .distance = 1000, .bearing = 4.71, .height = 9000, .State="UNASSIGNED", @@ -41,7 +41,7 @@ m_vecNodes[3] = { m_vecNodes[4] = { .Label = 4, .Pred = 1, - .distance = 1414, + .distance = 707, .bearing = 0.79, .height = 11000, .State="UNASSIGNED", @@ -50,7 +50,7 @@ m_vecNodes[4] = { m_vecNodes[5] = { .Label = 5, .Pred = 2, - .distance = 2000, + .distance = 1000, .bearing = 0.0, .height = 14000, .State="UNASSIGNED", diff --git a/buzz_scripts/include/graphs/shapes_Y.bzz b/buzz_scripts/include/graphs/shapes_Y.bzz index 29c78ec..171c693 100644 --- a/buzz_scripts/include/graphs/shapes_Y.bzz +++ b/buzz_scripts/include/graphs/shapes_Y.bzz @@ -1,7 +1,7 @@ #Table of the nodes in the graph m_vecNodes={} -m_vecNodes_fixed={} -function Read_Graph(){ + +function Read_GraphY(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point .Pred = -1, # Label of its predecessor diff --git a/buzz_scripts/include/graphs/waypointlist.csv b/buzz_scripts/include/graphs/waypointlist.csv new file mode 100644 index 0000000..e0a2c42 --- /dev/null +++ b/buzz_scripts/include/graphs/waypointlist.csv @@ -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 diff --git a/buzz_scripts/include/mapmatrix.bzz b/buzz_scripts/include/mapmatrix.bzz new file mode 100644 index 0000000..a3ab889 --- /dev/null +++ b/buzz_scripts/include/mapmatrix.bzz @@ -0,0 +1,151 @@ + +# Write to matrix +robot_marker = "X" + +function wmat(mat, row, col, val) { + var index = (row-1)*mat.nb_col + (col - 1) + if( row <= mat.nb_row) { # update val + mat.mat[index] = val + } else if(row == mat.nb_row + 1){ # add entry + mat.nb_row = mat.nb_row + 1 + mat.mat[index] = val + } +} + +# Read from matrix +function rmat(mat, row, col) { + #log("rmat ", mat, row, col) + index = (row-1)*mat.nb_col + (col - 1) + if (mat.mat[index] == nil) { + log("Wrong matrix read index: ", row, " ", col) + return -1 + } else { + return mat.mat[index] + } +} + +# copy a full matrix row +function mat_copyrow(out,ro,in,ri){ + var indexI = (ri-1)*in.nb_col + var indexO = (ro-1)*out.nb_col + icr=0 + while(icr 0 and yi > 0) { + #log("Add obstacle in cell: ", xi, " ", yi) + old=rmat(map,xi,yi) + if(old-inc_trust > 0.0) + wmat(map,xi,yi,old-inc_trust) + else + wmat(map,xi,yi,0.0) + } +} + +function remove_obstacle(pos, off, dec_trust) { + xi = math.round(pos.x) + yi = math.round(pos.y) + + if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0){ + #log("Remove obstacle in cell: ", xi, " ", yi) + old=rmat(map, xi, yi) + if(old + dec_trust < 1.0) #x,y + wmat(map, xi, yi, old+dec_trust) + else + wmat(map, xi, yi, 1.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) { + ir=1 + while(ir<=t.nb_row){ + log(ir, ": ", rmat(t,ir,1), " ", rmat(t,ir,2)) + ir = ir + 1 + } +} +function print_map(t) { + ir=t.nb_row + log("Printing a ", t.nb_row, " by ", t.nb_col, " map") + while(ir>0){ + logst=string.concat("\t", string.tostring(ir), "\t:") + ic=t.nb_col + while(ic>0){ + if(ir==cur_cell.x and ic==cur_cell.y) + logst = string.concat(logst, " XXXXXXXX") + else + logst = string.concat(logst, " ", string.tostring(rmat(t,ir,ic))) + ic = ic - 1 + } + log(logst) + ir = ir - 1 + } +} + +function print_map_argos(t){ + ir=t.nb_row + msg = string.tostring(ir) + while(ir>0){ + ic=t.nb_col + while(ic>0){ + if(ir==cur_cell.x and ic==cur_cell.y){ + msg = string.concat(msg, ":", robot_marker) + } + else { + msg = string.concat(msg, ":", string.tostring(rmat(t,ir,ic))) + } + ic = ic - 1 + } + ir = ir - 1 + } + set_argos_map(msg) +} diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz new file mode 100644 index 0000000..b9e84a4 --- /dev/null +++ b/buzz_scripts/include/rrtstar.bzz @@ -0,0 +1,454 @@ +##### +# RRT* Path Planing +# +# map table-based matrix +##### +include "mapmatrix.bzz" + +RRT_TIMEOUT = 500 +map = nil +cur_cell = {} +nei_cell = {} + +function UAVinit_map(m_navigation) { + # create a map big enough for the goal + init_map(2*math.round(math.vec2.length(m_navigation))+10) + # center the robot on the grid + cur_cell = math.vec2.new(math.round(map.nb_col/2.0),math.round(map.nb_row/2.0)) +} + +function UAVpathPlanner(m_navigation, m_pos) { + # place the robot on the grid + update_curcell(m_pos,0) + # create the goal in the map grid + mapgoal = math.vec2.add(m_navigation,cur_cell) + mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y)) + + # search for a path + return RRTSTAR(mapgoal,math.vec2.new(5,5)) #map.nb_col/20.0,map.nb_row/20.0)) +} + +function Kh4pathPlanner(m_goal, m_pos) { + # move 0,0 to a corner instead of the center + m_goal = math.vec2.sub(m_goal,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y)) + + # place the robot on the grid + update_curcell(m_pos,1) + log("Starting from cell: ", cur_cell.x, " ", cur_cell.y) + log("Going to cell: ", m_goal.x, " ", m_goal.y) + + # search for a path + print_map(map) + # print_map_argos(map) + return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y)) +} + +function update_curcell(m_curpos, kh4) { + if(kh4) { # for khepera playground + cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y)) + } else { # for uav map relative to home + cur_cell = math.vec2.add(cur_cell, m_curpos) + cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y)) + } + if(cur_cell.x>map.nb_row) + cur_cell.x=map.nb_row + if(cur_cell.y>map.nb_col) + cur_cell.y=map.nb_col + if(cur_cell.x<1) + cur_cell.x=1 + if(cur_cell.y<1) + cur_cell.y=1 +} + +function UAVgetneiobs (m_curpos) { + update_curcell(m_curpos,0) + # add all neighbors as obstacles in the grid + neighbors.foreach(function(rid, data) { + add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) + }) +} + +function getneiobs (m_curpos) { + foundobstacle = 0 + update_curcell(m_curpos,1) + old_nei = table_copy(nei_cell) + #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) + + neighbors.foreach(function(rid, data) { + #log("nei rel pos: ", data.distance, "m ", data.azimuth*180.0/math.pi, "deg") + Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos) + #log("nei abs pos: ", Ncell.x, "cm ", Ncell.y, "cm") + Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y)) + nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y} + #log("Neighbor in : ", Ncell.x, " ", Ncell.y) + if(old_nei[rid]!=nil) { + if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) { + #log("Neighbor moved ! ", nei_cell[rid].x, " ", nei_cell[rid].y) + remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1) + remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1) + remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1) + remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1) + remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1) + add_obstacle(Ncell, 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) + foundobstacle = 1 + } + } else { + add_obstacle(Ncell, 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) + foundobstacle = 1 + } + }) + + #if(foundobstacle) { + #print_map(map) + #} + + return foundobstacle +} + +function getproxobs (m_curpos) { + foundobstacle = 0 + update_curcell(m_curpos,1) + + reduce(proximity, + function(key, value, acc) { + obs = math.vec2.add(math.vec2.newp(1.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell) + obs2 = math.vec2.add(math.vec2.newp(2.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell) + per = math.vec2.sub(obs,cur_cell) + obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) + obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) + obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) + if(value.value > IR_SENSOR_THRESHOLD) { + if(rmat(map,math.round(obs.x),math.round(obs.y))!=0) { + add_obstacle(obs, 0, 0.25) + add_obstacle(obs2, 0, 0.25) + add_obstacle(obsr, 0, 0.25) + add_obstacle(obsr2, 0, 0.25) + add_obstacle(obsl, 0, 0.25) + add_obstacle(obsl2, 0, 0.25) + foundobstacle = 1 + } + } else if(rmat(map,math.round(obs.x),math.round(obs.y))!=1) { + remove_obstacle(obs, 0, 0.05) + foundobstacle = 1 + } + return acc + }, math.vec2.new(0, 0)) + + #if(foundobstacle) { + # reduce(proximity, + # function(key, value, acc){ + # log(value.value, ", ", value.angle) + # return acc + # }, math.vec2.new(0, 0)) + # print_map(map) + #} + + return foundobstacle +} + +function check_path(m_path, goal_l, m_curpos, kh4) { + pathisblocked = 0 + nb=goal_l + update_curcell(m_curpos,kh4) + Cvec = cur_cell + while(nb < m_path.nb_row and nb <= goal_l+1){ + Nvec = getvec(m_path, nb) + if(kh4==0) + Nvec=vec_from_gps(Nvec.x,Nvec.y) + if(doesItIntersect(Cvec, Nvec)){ + log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") + pathisblocked = 1 + } + Cvec=Nvec + nb = nb + 1 + } + + return pathisblocked +} + +function RRTSTAR(GOAL,TOL) { + HEIGHT = map.nb_col-1 + WIDTH = map.nb_row-1 + RADIUS = 1.25 #TOL.x #map.nb_col/10.0 # to consider 2 points consecutive + + + goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} + #table_print(goalBoundary) + arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}} + Path = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}} + numberOfPoints = 1 + Q = {.nb_col=5,.nb_row=1,.mat={.0=cur_cell.x,.1=cur_cell.y,.2=0,.3=1,.4=0}} + + goalReached = 0; + timeout = 0 + ## + # main search loop + ## + while(goalReached == 0 and timeout < RRT_TIMEOUT) { + + # Point generation only as grid cell centers + pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1)) + #log("Point generated: ", pt.x, " ", pt.y) + + pointList = findPointsInRadius(pt,Q,RADIUS); + + # Find connection that provides the least cost to come + nbCount = 0; + minCounted = 999; + minCounter = 0; + + if(pointList.nb_row!=0) { + #log("Found ", pointList.nb_row, " close point:", pointList.mat) + ipt=1 + while(ipt<=pointList.nb_row){ + pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}} + mat_copyrow(pointNumber,1,pointList,ipt) + + # Follow the line to see if it intersects anything + intersects = doesItIntersect(pt,getvec(pointNumber,1)); + #log("intersects1: ", intersects) + + # If there is no intersection we need consider its connection + nbCount = nbCount + 1; + if(intersects != 1) { + #log(pointNumber, "do not intersect (",pointNumber.mat[3],")") + distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+rmat(Q,pointNumber.mat[3],5) + + if(distance < minCounted) { + minCounted = distance; + minCounter = nbCount; + } + } + ipt = ipt + 1 + } + if(minCounter > 0) { + numberOfPoints = numberOfPoints + 1; + wmat(arrayOfPoints,numberOfPoints,1,pt.x) + wmat(arrayOfPoints,numberOfPoints,2,pt.y) + + wmat(Q,numberOfPoints,1, pt.x) + wmat(Q,numberOfPoints,2, pt.y) + wmat(Q,numberOfPoints,3, rmat(pointList,minCounter,4)); + wmat(Q,numberOfPoints,4, numberOfPoints) + wmat(Q,numberOfPoints,5, minCounted) + + #log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y) + + # Now check to see if any of the other points can be redirected + nbCount = 0; + ipt = 1 + while(ipt goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax) + goalReached = 1; + } + } else { + # Associate with the closest point + pointNum = findClosestPoint(pt,arrayOfPoints); + + # Follow the line to see if it intersects anything + intersects = doesItIntersect(pt,getvec(arrayOfPoints,pointNum)); + #log("intersects3 (", pointNum, "): ", intersects) + + # If there is no intersection we need to add to the tree + if(intersects != 1) { + numberOfPoints = numberOfPoints + 1; + wmat(arrayOfPoints,numberOfPoints,1,pt.x) + wmat(arrayOfPoints,numberOfPoints,2,pt.y) + + wmat(Q,numberOfPoints,1, pt.x) + wmat(Q,numberOfPoints,2, pt.y) + wmat(Q,numberOfPoints,3, pointNum); + wmat(Q,numberOfPoints,4, numberOfPoints) + wmat(Q,numberOfPoints,5, rmat(Q,pointNum,5)+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt))) + + #log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y) + + # Check to see if this new point is within the goal + if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax) + goalReached = 1; + } + } + if(numberOfPoints % 100 == 0) { + log(numberOfPoints, " points processed. Still looking for goal."); + } + timeout = timeout + 1 + } + if(goalReached){ + log("Goal found(",numberOfPoints,")!") + Path = getPathGPS(Q,numberOfPoints) + print_pos(Path) + } else { + log("FAILED TO FIND A PATH!!!") + Path = nil + } + return Path +} + +function findClosestPoint(point,aPt) { + # Go through each points and find the distances between them and the + # target point + distance = 999 + pointNumber = -1 + ifcp=1 + while(ifcp<=aPt.nb_row) { + range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) + + if(range < distance) { + distance = range; + pointNumber = ifcp; + } + ifcp = ifcp + 1 + } + return pointNumber +} + +# Find the closest point in the tree +function findPointsInRadius(point,q,r) { + counted = 0; + pointList = {.nb_col=q.nb_col, .nb_row=counted, .mat={}} + iir=1 + while(iir <= q.nb_row) { + tmpvv = getvec(q,iir) + #log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y) + distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) + + if(distance < r) { + counted = counted+1; + pointList.nb_row=counted + mat_copyrow(pointList,counted,q,iir) + } + + iir = iir + 1 + } + return pointList +} + +# check if the line between 2 point intersect an obstacle +function doesItIntersect(point,vector) { + #log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y) + dif = math.vec2.sub(point,vector) + distance = math.vec2.length(dif) + if(distance==0.0){ + # Find what block we're in right now + xi = math.round(point.x) #+1 + yi = math.round(point.y) #+1 + if(xi!=cur_cell.x and yi!=cur_cell.y){ + if(rmat(map,xi,yi) > 0.5) + return 1 + else + return 0 + } else + return 0 + } + vec = math.vec2.scale(dif,1.0/distance) + pathstep = map.nb_col - 2 + + idii = 1.0 + while(idii <= pathstep) { + range = distance*idii/pathstep + pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); + + # Find what block we're in right now + xi = math.round(pos_chk.x) #+1 + yi = math.round(pos_chk.y) #+1 + #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range) + + if(xi!=cur_cell.x and yi!=cur_cell.y){ + if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) { + if(rmat(map,xi,yi) < 0.5) { # because obstacle use trust values + #log("Obstacle in the way(", xi, " ", yi, ": ", rmat(map,xi,yi), ")!") + return 1; + } + } else { + #log("Outside the map(", xi, " ", yi, ")! ", range, "/", distance, " : ", pos_chk.x, " ", pos_chk.y, " : ", vec.x, " ", vec.y) + return 1; + } + } + idii = idii + 1.0 + } + #log("No intersect!") + return 0 +} + +function getPathGPS(Q,nb){ + path={.nb_col=2, .nb_row=0, .mat={}} + npt=0 + # get the path from the point list + while(nb!=1){ + npt=npt+1 + path.nb_row=npt + path.mat[(npt-1)*2]=rmat(Q,nb,1) + path.mat[(npt-1)*2+1]=rmat(Q,nb,2) + nb = rmat(Q,nb,3); + } + + # re-order the list and make the path points absolute + pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}} + while(npt>0){ + tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell)) + pathR.mat[(path.nb_row-npt)*2]=tmpgoal.latitude + pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.longitude + npt = npt - 1 + } + return pathR +} + +# create a table with only the path's points in order +function getPath(Q,nb){ + path={.nb_col=2, .nb_row=0, .mat={}} + npt=0 + + # log("get the path from the point list") + while(nb!=1){ + npt=npt+1 + path.nb_row=npt + path.mat[(npt-1)*2]=rmat(Q,nb,1) + path.mat[(npt-1)*2+1]=rmat(Q,nb,2) + nb = rmat(Q,nb,3); + } + + # log("re-order the list") + # table_print(path.mat) + pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}} + while(npt>0){ + tmpgoal = getvec(path,npt) + pathR.mat[(path.nb_row-npt)*2]=tmpgoal.x + pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.y + npt = npt - 1 + } + #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1)) + return pathR +} diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 4ecac31..a789600 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -3,14 +3,22 @@ # FLIGHT-RELATED FUNCTIONS # ######################################## -TARGET_ALTITUDE = 5.0 -UAVSTATE = "TURNEDOFF" +include "rrtstar.bzz" -function uav_initswarm(){ +TARGET_ALTITUDE = 15.0 # m. +UAVSTATE = "TURNEDOFF" +PICTURE_WAIT = 20 # steps +GOTO_MAXVEL = 2 # m/steps +GOTO_MAXDIST = 150 # m. +GOTODIST_TOL = 0.5 # m. +GOTOANG_TOL = 0.1 # rad. +cur_goal_l = 0 +rc_State = 0 +homegps = {.lat=0, .long=0} + +function uav_initswarm() { s = swarm.create(1) s.join() - statef=turnedoff - UAVSTATE = "TURNEDOFF" } function turnedoff() { @@ -26,38 +34,130 @@ function idle() { function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff - #log("TakeOff: ", flight.status) - #log("Relative position: ", position.altitude) - + if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,action,land) + barrier_set(ROBOTS, action, land, -1) barrier_ready() - #statef=hexagon - } - else { - log("Altitude: ", TARGET_ALTITUDE) + } else { + log("Altitude: ", position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) - } + } } function land() { UAVSTATE = "LAND" statef=land - #log("Land: ", flight.status) - if(flight.status == 2 or flight.status == 3){ - neighbors.broadcast("cmd", 21) - uav_land() - } - else { - barrier_set(ROBOTS,turnedoff,land) + + neighbors.broadcast("cmd", 21) + uav_land() + + if(flight.status != 2 and flight.status != 3) { + barrier_set(ROBOTS,turnedoff,land, 21) barrier_ready() - timeW=0 - #barrier = nil - #statef=idle } } +function set_goto(transf) { + UAVSTATE = "GOTOGPS" + statef=function() { + gotoWP(transf) + } + + if(rc_goto.id==id){ + 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" + uav_setgimbal(0.0, 0.0, -90.0, 20.0) + if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize + uav_takepicture() + } else if(ptime>=PICTURE_WAIT) { # wait for the picture + statef=action + ptime=0 + } + ptime=ptime+1 +} + +# +# still requires to be tuned, replaning takes too much time.. +# DS 23/11/2017 +function gotoWPRRT(transf) { + rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + homegps.lat = position.latitude + homegps.long = position.longitude + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1) + print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y) + + if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) + log("Sorry this is too far.") + else { + if(Path==nil){ + # create the map + if(map==nil) + UAVinit_map(rc_goal) + # add proximity sensor and neighbor obstacles to the map + while(Path==nil) { + #getproxobs(m_pos) + UAVgetneiobs (m_pos) + Path = UAVpathPlanner(rc_goal, m_pos) + } + cur_goal_l = 1 + } else if(cur_goal_l<=Path.nb_row) { + cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude + cur_pt=vec_from_gps(cur_gps.x,cur_gps.y) + print(" heading to ", cur_pt.x,cur_pt.y) + if(math.vec2.length(cur_pt)>GOTODIST_TOL) { + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1) + UAVgetneiobs(m_pos) + if(check_path(Path,cur_goal_l,m_pos,0)) { + uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) + Path = nil + rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + while(Path == nil) { + #getproxobs(m_pos) + UAVgetneiobs (m_pos) + Path = UAVpathPlanner(rc_goal, m_pos) + } + cur_goal_l = 1 + } + cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt)) + uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude) + } + else + cur_goal_l = cur_goal_l + 1 + } else { + Path = nil + transf() + } + } +} + +function gotoWP(transf) { + m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + + 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) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination + transf() + else + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) +} + function follow() { if(size(targets)>0) { UAVSTATE = "FOLLOW" @@ -67,7 +167,7 @@ function follow() { force=(0.05)*(tab.range)^4 attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) }) - uav_moveto(attractor.x, attractor.y) + uav_moveto(attractor.x, attractor.y, 0.0) } else { log("No target in local table!") #statef=idle @@ -90,11 +190,8 @@ function uav_rccmd() { neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 - UAVSTATE = "FOLLOW" - log(rc_goto.latitude, " ", rc_goto.longitude) - add_targetrb(0,rc_goto.latitude,rc_goto.longitude) - statef = follow - #uav_goto() + UAVSTATE = "GOTOGPS" + statef = goto } else if(flight.rc_cmd==400) { flight.rc_cmd=0 uav_arm() @@ -106,21 +203,90 @@ function uav_rccmd() { } else if (flight.rc_cmd==666){ flight.rc_cmd=0 stattab_send() + } else if (flight.rc_cmd==900){ + flight.rc_cmd=0 + rc_State = 0 + neighbors.broadcast("cmd", 900) + } else if (flight.rc_cmd==901){ + flight.rc_cmd=0 + rc_State = 1 + neighbors.broadcast("cmd", 901) + } else if (flight.rc_cmd==902){ + flight.rc_cmd=0 + rc_State = 2 + neighbors.broadcast("cmd", 902) + } else if (flight.rc_cmd==903){ + flight.rc_cmd=0 + rc_State = 3 + neighbors.broadcast("cmd", 903) } } function uav_neicmd() { neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and UAVSTATE!="TAKEOFF") { - statef=takeoff - } else if(value==21) { - statef=land - } else if(value==400 and UAVSTATE=="IDLE") { - uav_arm() - } else if(value==401 and UAVSTATE=="IDLE"){ - uav_disarm() - } + function(vid, value, rid) { + print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")") + if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") { + statef=takeoff + UAVSTATE = "TAKEOFF" + } else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") { + statef=land + UAVSTATE = "LAND" + } else if(value==400 and UAVSTATE=="TURNEDOFF") { + uav_arm() + } else if(value==401 and UAVSTATE=="TURNEDOFF"){ + uav_disarm() + } else if(value==900){ + rc_State = 0 + } else if(value==901){ + rc_State = 1 + } else if(value==902){ + rc_State = 2 + } else if(value==903){ + rc_State = 3 + } 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 + # }) + } }) -} \ No newline at end of file +} + +function LimitAngle(angle){ + if(angle>2*math.pi) + return angle-2*math.pi + else if (angle<0) + return angle+2*math.pi + else + return angle +} + +function vec_from_gps(lat, lon) { + 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); + #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); + return math.vec2.new(ned_x,ned_y) +} + +function gps_from_vec(vec) { + Lgoal = {.latitude=0, .longitude=0} + 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(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)); + Lgoal.latitude = target_lat*180.0/math.pi; + Lgoal.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; + + return Lgoal +} diff --git a/buzz_scripts/include/utilities.bzz b/buzz_scripts/include/utilities.bzz new file mode 100644 index 0000000..6c4dab6 --- /dev/null +++ b/buzz_scripts/include/utilities.bzz @@ -0,0 +1,152 @@ +# Utilities + +# Rads to degrees +function rtod(r) { + return (r*(180.0/math.pi)) +} + +# Copy a table +function table_deep_copy(new_t, old_t, depth) { + depth = depth - 1 + if (old_t != nil) { + foreach(old_t, function(key, value) { + new_t[key] = value + if(depth != 0) { + new_t[key] = {} + table_deep_copy(new_t[key], value, depth) + } + }) + } +} + +function table_copy(old_t, depth) { + var t = {}; + table_deep_copy(t, old_t, depth); + return t; +} + +# Print the contents of a table +function table_print(t, depth) { + depth = depth - 1 + if (t != nil) { + foreach(t, function(key, value) { + log(key, " -> ", value) + if(depth != 0) { + table_print(t[key], depth) + } + }) + } +} + +# Write a table as if it was a vector +function write_vector(k, index, val) { + var key = string.tostring(index) + k[key] = val +} + +# Read a table as if it was a vector +function read_vector(k, index) { + var key = string.tostring(index) + if (k[key] == nil) { + return -1 + } else { + return k[key] + } +} + +# Write a table as if it was a matrix +function write_matrix(k, row, col, val) { + var key = string.concat(string.tostring(row),"-",string.tostring(col)) + k[key] = val +} + +# Read a table as if it was a matrix +function read_matrix(k, row, col) { + var key = string.concat(string.tostring(row),"-",string.tostring(col)) + if (k[key] == nil) { + return -1 + } else { + return k[key] + } +} + +# Int to String +function itos(i) { + + log("Use 'string.tostring(OJB)' instead") + + if (i==0) { return "0" } + if (i==1) { return "1" } + if (i==2) { return "2" } + if (i==3) { return "3" } + if (i==4) { return "4" } + if (i==5) { return "5" } + if (i==6) { return "6" } + if (i==7) { return "7" } + if (i==8) { return "8" } + if (i==9) { return "9" } + + log("Function 'itos' out of bounds, returning the answer (42)") + return "42" +} + +# String to Int +function stoi(s) { + if (s=='0') { return 0 } + if (s=='1') { return 1 } + if (s=='2') { return 2 } + if (s=='3') { return 3 } + if (s=='4') { return 4 } + if (s=='5') { return 5 } + if (s=='6') { return 6 } + if (s=='7') { return 7 } + if (s=='8') { return 8 } + if (s=='9') { return 9 } + + log("Function 'stoi' out of bounds, returning the answer (42)") + return 42 + +} + +# Force angles in the (-pi,pi) interval +function radians_interval(a) { + var temp = a + while ((temp>2.0*math.pi) or (temp<0.0)) { + if (temp > 2.0*math.pi) { + temp = temp - 2.0*math.pi + } else if (temp < 0.0){ + temp = temp + 2.0*math.pi + } + } + if (temp > math.pi) { + temp = temp - 2.0*math.pi + } + return temp +} + +############################################ + +#base = {} + +#base.create = function() { +# return { +# .method = function(a,b) { +# return a + b +# } +# } +# } + +#x = base.create() +#x.method(3,4) # returns 7 + +#derived = {} + +#derived.create = function() { +# b = base.create() +# b.method = function(a,b) { +# return a * b +# } +#} + +#x = derived.create() +#x.method(3,4) # returns 12 diff --git a/buzz_scripts/include/vec2.bzz b/buzz_scripts/include/vec2.bzz index e2fb9b0..ebfce96 100644 --- a/buzz_scripts/include/vec2.bzz +++ b/buzz_scripts/include/vec2.bzz @@ -41,7 +41,7 @@ math.vec2.length = function(v) { # RETURN: The angle of the vector. # math.vec2.angle = function(v) { - return math.atan2(v.y, v.x) + return math.atan(v.y, v.x) } # diff --git a/buzz_scripts/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz index c983fcc..7fd8e42 100644 --- a/buzz_scripts/include/vstigenv.bzz +++ b/buzz_scripts/include/vstigenv.bzz @@ -1,16 +1,29 @@ -STATUS_VSTIG = 10 -GROUND_VSTIG = 11 +######################################## +# +# FLEET V.STIGMERGY-RELATED FUNCTIONS +# +######################################## +# +# Constants +# +STATUS_VSTIG = 20 +GROUND_VSTIG = 21 +HIGHEST_ROBOTID = 14 WAIT4STEP = 10 -v_status = {} -v_ground = {} + +# +# Init var +# +var v_status = {} +var v_ground = {} b_updating = 0 +counter=WAIT4STEP function uav_initstig() { v_status = stigmergy.create(STATUS_VSTIG) v_ground = stigmergy.create(GROUND_VSTIG) } -counter=WAIT4STEP function uav_updatestig() { # TODO: Push values on update only. if(counter<=0) { @@ -78,7 +91,7 @@ function stattab_print() { if(v_status.size()>0) { if(b_updating==0) { u=0 - while(u<8){ + while(u0) { if(b_updating==0) { u=0 - while(u<8){ + while(u 0) and (data.distance < (RANGE - 10.0)) and (knowledge.free[rid] == 1))}) + # and (data.distance > 10.0) + + # Selects closest parent candidate as parent + var temp = {} + temp = candidates.reduce(function(rid, data, accum) { + accum.min = math.min(accum.min, data.distance) + return accum + }, {.min = RANGE}) + var min = temp.min + + + var flag = 0 + foreach(knowledge.distance, function(key, value) { + if ((flag == 0) and (candidates.data[key] != nil)) { + if (value == min) { + tree.parent.id = key + tree.parent.distance = value + tree.parent.azimuth = knowledge.azimuth[key] + flag = 1 + } + } + #break (when implemented) + }) + +} + +#################################### + +function count() { + + if (nb_sons == 0) { + eq_level = level + } + + else if (nb_sons >= 1) { + var temp = {} + temp = sons.reduce(function(rid, data, accum) { + accum.sum = accum.sum + tree.sons[rid].eq_level + return accum + }, {.sum = 0}) + eq_level = temp.sum - (nb_sons - 1) * level + } + +} + +#################################### + +function change_frame(p01, p1, theta) { + + var p0 = { + .x = math.cos(theta) * p1.x + math.sin(theta) * p1.y + p01.x, + .y = -math.sin(theta) * p1.x + math.cos(theta) * p1.y + p01.y + } + return p0 + +} + +#################################### + +transform_accum = function(rid, data) { + + # Son contribution in frame F1 + var p1 = { + .x = tree.sons[rid].accum_x, + .y = tree.sons[rid].accum_y + } + + # Rotation angle between F0 and F1 + var theta = tree.sons[rid].angle_parent - data.azimuth - math.pi + + # Translation vector from F0 to F1 + var p01 = { + .x = 0, + .y = 0 + } + + var p0 = {} + + if (tree.sons[rid].angle_parent != nil) { + var rot_angle = radians_interval(theta) + p0 = change_frame(p01, p1, rot_angle) + } + else { + p0.x = p1.x + p0.y = p1.y + } + + return p0 +} + +#################################### + +function centroid() { + + # If the robot has a parent + if ((tree.parent != nil) or (root == 1)) { + var sum_F1 = { .x = 0, .y = 0} + + # If the robot has at least one son + if (nb_sons > 0) { + var temp = {} + # Expresses son contrib (in F1) in its own reference frame (F0) + temp = sons.map(transform_accum) + # Sums son contributions expressed in F0 frame + sum_F1 = temp.reduce(function(rid, data, accum) { + accum.x = accum.x + data.x + accum.y = accum.y + data.y + #log("id ", rid, " sum_x ", accum.x, " sum_y ", accum.y) + return accum + }, {.x = 0, .y = 0}) + } + + # If the robot is not the root + if ((root == 0) and (tree.parent.id != nil)) { + #var nb_descendants = eq_level - level + var p0 = knowledge.distance[tree.parent.id]#tree.parent.distance + var theta = knowledge.azimuth[tree.parent.id]#tree.parent.azimuth + # Adds current robot contribution to centroid sum + accum_x = sum_F1.x - (nb_descendants + 1) * p0 * math.cos(theta) + accum_y = sum_F1.y - (nb_descendants + 1) * p0 * math.sin(theta) + } + # If the robot is the root + else if ((root == 1) and (robot_count != 0)) { + # Centroid coordinates in root ref frame + accum_x = sum_F1.x / robot_count + accum_y = sum_F1.y / robot_count + } + } +} + + +################################################################ +################################################################ + +# Tree reconfiguration functions + +function tree_config() { + statef() +} + +function end_fun() { + if (root == 1) { + log("centroid X: ", accum_x, " Y ", accum_y) + } +} + +#################################### + +function root_select() { + + log(id," root_select") + if (tree.parent.id != nil) { + if(neighbors.data[tree.parent.id] != nil) { + angle_parent = neighbors.data[tree.parent.id].azimuth + } + } + + if (root == 1) { + + # Listens for new root acknowledgment + + foreach(knowledge.ackn, function(key, value) { + if (value == better_root) { + #log(id, " got it") + root = 0 + level = 0 + setleds(0,0,0) + } + }) + + if (ack == 1) { + # Triggers transition to new state + trigger.put("a", 1) + } + else if ((root == 1) and (better_root != id) and (trigger.get("a") != 1)) { + setleds(255,0,0) + better_root = id + + # Centroid position in root reference frame (F0) + var c0 = math.vec2.new(accum_x, accum_y) + + # Distance from current root to centroid + var dist_rc = math.vec2.length(c0) + #log("root ", id, " dist_centr ", dist_rc) + + # Distances from neighbors to centroid + var dist_centroid = {} + dist_centroid = neighbors.map(function(rid, data) { + # Neighbour position in frame F0 + var p0 = math.vec2.newp(data.distance, data.azimuth) + # Difference vector between p0 and c0 + var v = math.vec2.sub(p0, c0) + # Distance between robot and centroid + return math.vec2.length(v) + }) + + # Minimal distance to centroid + var temp = {} + temp = dist_centroid.reduce(function(rid, data, accum) { + accum.min = math.min(accum.min, data) + return accum + }, {.min = dist_rc}) + var min = temp.min + #log("min ", min) + + # If the current root is the closest to the centroid + if(dist_rc == min) { + ack = 1 + } + # Otherwise + else { + var flag = 0 + # Selects closest robot to centroid as better root + foreach(dist_centroid.data, function(key, value) { + if(flag == 0) { + if(value == min) { + better_root = key + flag = 1 + } + # break (when implemented) + } + }) + + + #log(id, " better root : ", better_root) + #log("X : ", accum_x, "Y : ", accum_y) + var angle = neighbors.data[better_root].azimuth + # Broadcasts + var message = { + .better_root = better_root, + .centroid_x = accum_x, + .centroid_y = accum_y, + .angle_old_root = angle + } + neighbors.broadcast("msg1", message) + } + } + } + + else if (better_root == nil) { + # Listen for old root message + foreach(knowledge.better_root, function(rid, value) { + if(value == id) { + + var theta = neighbors.data[rid].azimuth + + var p1 = { + .x = knowledge.centroid_x[rid], + .y = knowledge.centroid_y[rid] + } + + var p01 = { + .x = neighbors.data[rid].distance * math.cos(theta), + .y = neighbors.data[rid].distance * math.sin(theta) + } + + var p0 = {} + + if (knowledge.angle_old_root[rid] != nil) { + var rot_angle = radians_interval(knowledge.angle_old_root[rid] - theta - math.pi) + p0 = change_frame(p01, p1, rot_angle) + } + else { + p0.x = p1.x + p0.y = p1.y + } + + accum_x = p0.x + accum_y = p0.y + + centroid_x = accum_x + centroid_y = accum_y + + root = 1 + neighbors.broadcast("got_it", id) + } + }) + } + + # Transitions to new state when all robots are ready + if (trigger.get("a") == 1) { + barrier_set(ROBOTS, end_fun) + barrier_ready() + } + +} + +#################################### + +function tree_select() { + + log(id, " tree_select") + + neighbors.map(function(rid, data) { + knowledge.distance[rid] = data.distance + knowledge.azimuth[rid] = data.azimuth + return 1 + }) + + if (level == 0) { + # Finds a parent + parent_select() + # Updates robot level + if (tree.parent.id != nil) { + #log(" ") + #log("selected") + #log("son ", id) + #log("parent ", tree.parent.id) + #log(" ") + + level = knowledge.level[tree.parent.id] + 1 + angle_parent = neighbors.data[tree.parent.id].azimuth + } + } + else { + # Updates list of sons + foreach(knowledge.parent, function(key, value) { + if(value == id) { + #log(value) + if(tree.sons[key] == nil) { + # Updates robot nb_sons + nb_sons = nb_sons + 1 + # Updates robot free status + if (nb_sons >= N_SONS) { + free = 0 + } + } + tree.sons[key] = { + .distance = knowledge.distance[key], + .azimuth = knowledge.azimuth[key], + .eq_level = knowledge.eq_level[key], + .accum_x = knowledge.accum_x[key], + .accum_y = knowledge.accum_y[key], + .angle_parent = knowledge.angle_parent[key] + } + } + }) + } + + # Creates a subset of neighbors to get the sons + # and parent + sons = {} + sons = neighbors.filter(function(rid, data) { + return (tree.sons[rid] != nil) + }) + parent = {} + parent = neighbors.filter(function(rid, data) { + return (tree.parent.id == rid) + }) + + # Counts robots (updates eq_level) + count() + + # Updates count of robots in the tree + if (root == 1) { + robot_count = eq_level + } + + nb_descendants = eq_level - level + + + # Computes centroid (updates accum_x, accum_y) + centroid() + + # Broadcast information to other robots + var message = { + .level = level, + .parent = tree.parent.id, + .eq_level = eq_level, + .free = free, + .accum_x = accum_x, + .accum_y = accum_y, + .angle_parent = angle_parent + } + neighbors.broadcast("msg2", message) + + # Triggers transition to new state if root count = ROBOTS + if (root == 1) { + if(robot_count == ROBOTS) { + log("centroid X: ", accum_x, " Y ", accum_y) + trigger.put("b", 1) + } + } + + # Transitions to new state when all robots are ready + if (trigger.get("b") == 1) { + barrier_set(ROBOTS, root_select) + barrier_ready() + } +} + +################################################################ +################################################################ + + +# This function is executed every time you press the 'execute' button +function init() { + + trigger = stigmergy.create(TRIGGER_VSTIG) + barrier = stigmergy.create(BARRIER_VSTIG) + + # Trees + old_tree = {} + tree = old_tree + old_tree.parent = {} + old_tree.sons = {} + + # Boolean variables + root = 0 # Root status + free = 1 # Node status (true if accepts sons) + ack = 0 + + # Tree variables + level = 0 + eq_level = 0 + nb_sons = 0 + nb_descendants = 0 + + # Update root status + if (id == 0) { + root = 1 # True if robot is the ro ot + level = 1 + robot_count = 0 + } + + statef = tree_select + + knowledge = { + .level = {}, + .parent = {}, + .eq_level = {}, + .free = {}, + .accum_x = {}, + .accum_y = {}, + .angle_parent = {}, + .distance = {}, + .azimuth = {}, + .better_root = {}, + .centroid_x = {}, + .centroid_y = {}, + .angle_old_root = {}, + .ackn = {} + } + + # Broadcast information to other robots + var message = { + .level = level, + .parent = old_tree.parent.id, + .eq_level = eq_level, + .free = free, + .accum_x = accum_x, + .accum_y = accum_y, + .angle_parent = 0.0 + } + neighbors.broadcast("msg2", message) + + # Listen to information from other robots for root_select + neighbors.listen("msg1", function(vid, value, rid) { + knowledge.better_root[rid] = value.better_root + knowledge.centroid_x[rid] = value.centroid_x + knowledge.centroid_y[rid] = value.centroid_y + knowledge.angle_old_root[rid] = value.angle_old_root + knowledge.angle_parent[rid] = value.angle_parent + }) + + # Listen to information from other robots for tree_select + neighbors.listen("msg2", function(vid, value, rid) { + knowledge.level[rid] = value.level + knowledge.parent[rid] = value.parent + knowledge.eq_level[rid] = value.eq_level + knowledge.free[rid] = value.free + knowledge.accum_x[rid] = value.accum_x + knowledge.accum_y[rid] = value.accum_y + knowledge.angle_parent[rid] = value.angle_parent + }) + + neighbors.listen("got_it", function(vid, value, rid) { + knowledge.ackn[rid] = value + }) + +} + +############################################ + +# This function is executed at each time step +function step() { + + tree_config() + goal = math.vec2.new(0.0, 0.0) + +} +############################################ + +# This function is executed every time you press the 'reset' +# button in the GUI. +function reset() { + # put your code here +} + +############################################ + +# This function is executed only once, when the robot is removed +# from the simulation +function destroy() { + # put your code here +} + +################ \ No newline at end of file diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 4909ed8..692de52 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -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; @@ -77,4 +77,6 @@ int get_robotid(); buzzvm_t get_vm(); void set_robot_var(int ROBOTS); + +int get_inmsg_size(); } diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index c205c47..6a5d18a 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -9,8 +9,9 @@ #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) (double) ((DEG)*((M_PI)/(180.0))) +#define RAD2DEG(RAD) (double) ((RAD)*((180.0)/(M_PI))) namespace buzzuav_closures{ typedef enum { @@ -22,6 +23,8 @@ namespace buzzuav_closures{ COMMAND_DISARM, COMMAND_GOTO, COMMAND_MOVETO, + COMMAND_PICTURE, + COMMAND_GIMBAL, } Custom_CommandCode; /* @@ -30,19 +33,25 @@ 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 buzz_floor(buzzvm_t vm); int buzzuav_moveto(buzzvm_t vm); -int buzzuav_goto(buzzvm_t vm); +int buzzuav_storegoal(buzzvm_t vm); +int buzzuav_setgimbal(buzzvm_t vm); +void parse_gpslist(); +int buzzuav_takepicture(buzzvm_t vm); /* Returns the current command from local variable*/ int getcmd(); /*Sets goto position */ void set_goto(double pos[]); /*Sets goto position from rc client*/ -void rc_set_goto(double pos[]); +void rc_set_goto(int id, double latitude, double longitude, double altitude); +/*Sets gimbal orientation from rc client*/ +void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t); /*sets rc requested command */ void rc_call(int rc_cmd); /* sets the battery state */ @@ -56,6 +65,8 @@ void set_api_rssi(float value); void set_currentpos(double latitude, double longitude, double altitude); /*retuns the current go to position */ double* getgoto(); +std::string getuavstate(); +float* getgimbal(); /* updates flight status*/ void flight_status_update(uint8_t state); /* Update neighbors table */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 31e7537..a2d72d6 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -39,6 +39,7 @@ #define XBEE_STOP_TRANSMISSION 4355356352 #define TIMEOUT 60 #define BUZZRATE 10 +#define CMD_REQUEST_UPDATE 666 using namespace std; @@ -54,6 +55,8 @@ public: //void RosControllerInit(); void RosControllerRun(); + static const string CAPTURE_SRV_DEFAULT_NAME; + private: struct num_robot_count { @@ -68,8 +71,8 @@ private: double longitude=0.0; double latitude=0.0; float altitude=0.0; - }; typedef struct gps GPS ; // not useful in cpp - + }; typedef struct gps GPS ; + GPS target, home, cur_pos; double cur_rel_altitude; @@ -91,7 +94,19 @@ private: /*tmp to be corrected*/ uint8_t no_cnt=0; uint8_t old_val=0 ; - std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,obstacles_topic,stand_by,xbeesrv_name, setpoint_name; + std::string bzzfile_name; + std::string fcclient_name; + std::string armclient; + std::string modeclient; + std::string rcservice_name; + std::string bcfname,dbgfname; + std::string out_payload; + std::string in_payload; + std::string obstacles_topic; + std::string stand_by; + std::string xbeesrv_name; + std::string capture_srv_name; + std::string setpoint_name; std::string stream_client_name; std::string relative_altitude_sub_name; std::string setpoint_nonraw; @@ -101,6 +116,7 @@ private: Num_robot_count count_robots; ros::ServiceClient mav_client; ros::ServiceClient xbeestatus_srv; + ros::ServiceClient capture_srv; ros::Publisher payload_pub; ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; @@ -110,6 +126,7 @@ private: ros::Subscriber users_sub; ros::Subscriber battery_sub; ros::Subscriber payload_sub; + ros::Subscriber flight_estatus_sub; ros::Subscriber flight_status_sub; ros::Subscriber obstacle_sub; ros::Subscriber Robot_id_sub; diff --git a/launch/husky.launch b/launch/husky.launch deleted file mode 100644 index bb883c5..0000000 --- a/launch/husky.launch +++ /dev/null @@ -1,59 +0,0 @@ - - - - - - - - - - - - - - - - magnetic_declination_radians: 0 - roll_offset: 0 - pitch_offset: 0 - yaw_offset: 0 - zero_altitude: false - broadcast_utm_transform: false - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/launch_config/husky.yaml b/launch/launch_config/husky.yaml deleted file mode 100644 index c878cc4..0000000 --- a/launch/launch_config/husky.yaml +++ /dev/null @@ -1,17 +0,0 @@ -topics: - gps : global_position - localpos : local_position - battery : power_status - status : flight_status - fcclient : dji_mavcmd - setpoint : setpoint_position/local - armclient: dji_mavarm - modeclient: dji_mavmode - altitude: rel_alt - stream: set_stream_rate - -type: - gps : sensor_msgs/NavSatFix - battery : mavros_msgs/BatteryStatus - status : mavros_msgs/ExtendedState - altitude: std_msgs/Float64 diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml deleted file mode 100644 index c878cc4..0000000 --- a/launch/launch_config/m100.yaml +++ /dev/null @@ -1,17 +0,0 @@ -topics: - gps : global_position - localpos : local_position - battery : power_status - status : flight_status - fcclient : dji_mavcmd - setpoint : setpoint_position/local - armclient: dji_mavarm - modeclient: dji_mavmode - altitude: rel_alt - stream: set_stream_rate - -type: - gps : sensor_msgs/NavSatFix - battery : mavros_msgs/BatteryStatus - status : mavros_msgs/ExtendedState - altitude: std_msgs/Float64 diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml deleted file mode 100644 index 6ad2ec8..0000000 --- a/launch/launch_config/solo.yaml +++ /dev/null @@ -1,21 +0,0 @@ -topics: - gps : mavros/global_position/global - battery : mavros/battery - status : mavros/state - fcclient: mavros/cmd/command - setpoint: mavros/setpoint_position/local - armclient: mavros/cmd/arming - modeclient: mavros/set_mode - localpos: /mavros/local_position/pose - stream: mavros/set_stream_rate - altitude: mavros/global_position/rel_alt -type: - gps : sensor_msgs/NavSatFix - # for SITL Solo - battery : mavros_msgs/BatteryState - # for solo - #battery : mavros_msgs/BatteryStatus - status : mavros_msgs/State - altitude: std_msgs/Float64 -environment : - environment : solo-simulator diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml new file mode 100644 index 0000000..e7358cd --- /dev/null +++ b/launch/launch_config/topics.yaml @@ -0,0 +1,12 @@ +topics: + gps : global_position/global + battery : battery + status : state + estatus : extended_state + fcclient: cmd/command + setpoint: setpoint_position/local + armclient: cmd/arming + modeclient: set_mode + localpos: local_position/pose + stream: set_stream_rate + altitude: global_position/rel_alt diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch deleted file mode 100644 index fce3b00..0000000 --- a/launch/rosbuzz-solo.launch +++ /dev/null @@ -1,57 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch deleted file mode 100644 index c09d2c5..0000000 --- a/launch/rosbuzz-testing-sitl.launch +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzz-testing.launch b/launch/rosbuzz-testing.launch deleted file mode 100644 index d955758..0000000 --- a/launch/rosbuzz-testing.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index d79e016..c2531f4 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -1,14 +1,22 @@ - - + + + + - - - - - - - - - - + + + + + + + + + + + + + + + + diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch deleted file mode 100644 index 5bc52f4..0000000 --- a/launch/rosbuzz_2_parallel.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch new file mode 100644 index 0000000..de7857d --- /dev/null +++ b/launch/rosbuzzd.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch deleted file mode 100644 index 69da82f..0000000 --- a/launch/rosbuzzm100.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 82b7a0b..00df45f 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -31,5 +31,10 @@ function stoprobot { } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch - rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch + rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch +# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch +} +function uavstate { + let "a = $1 + 900" + rosservice call robot0/buzzcmd 0 $a 0 0 0 0 0 0 0 0 } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 6039dc7..90d5dfa 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -17,7 +17,6 @@ namespace buzz_utility{ static char* BO_FNAME = 0; static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; - static uint32_t MSG_SIZE = 500; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size) static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets static uint8_t Robot_id = 0; static std::vector IN_MSG; @@ -229,7 +228,7 @@ void in_message_process(){ buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); /* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */ //ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t))); - if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MSG_SIZE) { + if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) { buzzmsg_payload_destroy(&m); break; } @@ -304,8 +303,14 @@ 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_goto", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto)); + 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_setgimbal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal)); + 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)); @@ -349,7 +354,13 @@ 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_goto", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + 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)); @@ -487,7 +498,7 @@ int create_stig_tables() { if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("%s: Error loading Buzz script", bo_filename); + ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename); return 0; } /* Register hook functions */ @@ -498,6 +509,11 @@ int create_stig_tables() { return 0; } + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); @@ -550,16 +566,22 @@ int create_stig_tables() { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id); return 0; } // Register hook functions if(buzz_register_hooks() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); + ROS_ERROR("[%i] Error registering hooks (update)", Robot_id); return 0; } + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); @@ -606,16 +628,22 @@ int create_stig_tables() { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id); return 0; } // Register hook functions if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); + ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id); return 0; } + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); @@ -786,4 +814,8 @@ int create_stig_tables() { buzzvm_pushi(VM, ROBOTS); buzzvm_gstore(VM); } + + int get_inmsg_size(){ + return IN_MSG.size(); + } } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 019fe35..b1aa6c0 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -16,82 +16,93 @@ namespace buzzuav_closures{ //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin); static double goto_pos[3]; static double rc_goto_pos[3]; + static float rc_gimbal[4]; static float batt[3]; static float obst[5]={0,0,0,0,0}; static double cur_pos[3]; static uint8_t status; static int cur_cmd = 0; static int rc_cmd=0; + static int rc_id=-1; static int buzz_cmd=0; static float height=0; - static bool deque_full = false; - static int rssi = 0; - static int message_number = 0; - static float raw_packet_loss = 0.0; - static int filtered_packet_loss = 0; - static float api_rssi = 0.0; + static bool deque_full = false; + static int rssi = 0; + 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::RB_struct> wplist_map; std::map< int, buzz_utility::Pos_struct> neighbors_map; std::map< int, buzz_utility::neighbors_status> neighbors_status_map; /****************************************/ /****************************************/ - int buzzros_print(buzzvm_t vm) { - int i; - char buffer [50] = ""; - sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid()); - for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { - buzzvm_lload(vm, i); - buzzobj_t o = buzzvm_stack_at(vm, 1); - buzzvm_pop(vm); - switch(o->o.type) { - case BUZZTYPE_NIL: - sprintf(buffer,"%s BUZZ - [nil]", buffer); - break; - case BUZZTYPE_INT: - sprintf(buffer,"%s %d", buffer, o->i.value); - //fprintf(stdout, "%d", o->i.value); - break; - case BUZZTYPE_FLOAT: - sprintf(buffer,"%s %f", buffer, o->f.value); - break; - case BUZZTYPE_TABLE: - sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value))); - break; - case BUZZTYPE_CLOSURE: - if(o->c.value.isnative) - sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref); - else - sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref); - break; - case BUZZTYPE_STRING: - sprintf(buffer,"%s %s", buffer, o->s.value.str); - break; - case BUZZTYPE_USERDATA: - sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value); - break; - default: - break; - } - } - ROS_INFO(buffer); - //fprintf(stdout, "\n"); - return buzzvm_ret0(vm); - } +int buzzros_print(buzzvm_t vm) +{ + std::ostringstream buffer (std::ostringstream::ate); + buffer << buzz_utility::get_robotid(); + for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index) + { + buzzvm_lload(vm, index); + buzzobj_t o = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + switch (o->o.type) + { + case BUZZTYPE_NIL: + buffer << " BUZZ - [nil]"; + break; + case BUZZTYPE_INT: + buffer << " " << o->i.value; + break; + case BUZZTYPE_FLOAT: + buffer << " " << o->f.value; + break; + case BUZZTYPE_TABLE: + buffer << " [table with " << buzzdict_size(o->t.value) << " elems]"; + break; + case BUZZTYPE_CLOSURE: + if (o->c.value.isnative) + { + buffer << " [n-closure @" << o->c.value.ref << "]"; + } + else + { + buffer << " [c-closure @" << o->c.value.ref << "]"; + } + break; + case BUZZTYPE_STRING: + buffer << " " << o->s.value.str; + break; + case BUZZTYPE_USERDATA: + buffer << " [userdata @" << o->u.value << "]"; + break; + default: + break; + } + } + ROS_INFO("%s",buffer.str().c_str()); + 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 /----------------------------------------*/ void gps_from_rb(double range, double bearing, double out[3]) { - double lat = cur_pos[0]*M_PI/180.0; - double lon = cur_pos[1]*M_PI/180.0; + double lat = RAD2DEG(cur_pos[0]); + double lon = RAD2DEG(cur_pos[1]); out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing)); out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0])); - out[0] = out[0]*180.0/M_PI; - out[1] = out[1]*180.0/M_PI; + out[0] = RAD2DEG(out[0]); + out[1] = RAD2DEG(out[1]); out[2] = height; //constant height. } @@ -103,10 +114,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; @@ -117,31 +128,78 @@ 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 = wplist_map.find(tid); + if(it!=wplist_map.end()) + wplist_map.erase(it); + wplist_map.insert(make_pair(tid, RB_arr)); + } + + ROS_INFO("----->Saved %i waypoints.", wplist_map.size()); + + // Close the file: + fin.close(); + } + + /*----------------------------------------/ / Buzz closure to move following a 2D vector /----------------------------------------*/ int buzzuav_moveto(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 2); - buzzvm_lload(vm, 1); /* dx */ - buzzvm_lload(vm, 2); /* dy */ - //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 dy = buzzvm_stack_at(vm, 1)->f.value; - float dx = buzzvm_stack_at(vm, 2)->f.value; - double d = sqrt(dx*dx+dy*dy); //range - goto_pos[0]=dx; - goto_pos[1]=dy; - goto_pos[2]=height; - /*double b = atan2(dy,dx); //bearing - printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); - gps_from_rb(d, b, goto_pos); - 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 - return buzzvm_ret0(vm); + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); /* dx */ + buzzvm_lload(vm, 2); /* dy */ + buzzvm_lload(vm, 3); /* dheight */ + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + float dh = buzzvm_stack_at(vm, 1)->f.value; + float dy = buzzvm_stack_at(vm, 2)->f.value; + float dx = buzzvm_stack_at(vm, 3)->f.value; + goto_pos[0]=dx; + goto_pos[1]=dy; + goto_pos[2]=height+dh; + /*double b = atan2(dy,dx); //bearing + printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); + gps_from_rb(d, b, goto_pos); + 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 + return buzzvm_ret0(vm); } int buzzuav_update_targets(buzzvm_t vm) { @@ -156,27 +214,27 @@ 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 */ + // When we get here, the "targets" table is on top of the stack //ROS_INFO("Buzz_utility will save user %i.", it->first); - /* Push user id */ + // Push user id buzzvm_pushi(vm, it->first); - /* Create entry table */ + // Create entry table buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); - /* Insert range */ + // Insert range buzzvm_push(vm, entry); buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1)); buzzvm_pushf(vm, rb[0]); buzzvm_tput(vm); - /* Insert longitude */ + // Insert longitude buzzvm_push(vm, entry); buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1)); buzzvm_pushf(vm, rb[1]); buzzvm_tput(vm); - /* Save entry into data table */ + // Save entry into data table buzzvm_push(vm, entry); buzzvm_tput(vm); } @@ -204,8 +262,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); @@ -215,30 +274,30 @@ 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; } int buzzuav_addNeiStatus(buzzvm_t vm){ buzzvm_lnum_assert(vm, 5); - buzzvm_lload(vm, 1); // fc - buzzvm_lload(vm, 2); // xbee - buzzvm_lload(vm, 3); // batt - buzzvm_lload(vm, 4); // gps - buzzvm_lload(vm, 5); // id - buzzvm_type_assert(vm, 5, BUZZTYPE_INT); - 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); + buzzvm_lload(vm, 1); // fc + buzzvm_lload(vm, 2); // xbee + buzzvm_lload(vm, 3); // batt + buzzvm_lload(vm, 4); // gps + buzzvm_lload(vm, 5); // id + buzzvm_type_assert(vm, 5, BUZZTYPE_INT); + 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; uint8_t id = buzzvm_stack_at(vm, 5)->i.value; - newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value; - newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value; - newRS.xbee= buzzvm_stack_at(vm, 2)->i.value; - newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value; - map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id); + newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value; + newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value; + newRS.xbee= buzzvm_stack_at(vm, 2)->i.value; + newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value; + map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id); if(it!=neighbors_status_map.end()) neighbors_status_map.erase(it); neighbors_status_map.insert(make_pair(id, newRS)); @@ -247,7 +306,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); @@ -263,15 +322,69 @@ namespace buzzuav_closures{ return payload_out; } /*----------------------------------------/ - / Buzz closure to go directly to a GPS destination from the Mission Planner + / Buzz closure to take a picture here. /----------------------------------------*/ - int buzzuav_goto(buzzvm_t vm) { - rc_goto_pos[2]=height; - set_goto(rc_goto_pos); - cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; - printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); - buzz_cmd=COMMAND_GOTO; - return buzzvm_ret0(vm); + 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 change locally the gimbal orientation + /----------------------------------------*/ + int buzzuav_setgimbal(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 4); + buzzvm_lload(vm, 1); // time + buzzvm_lload(vm, 2); // pitch + buzzvm_lload(vm, 3); // roll + buzzvm_lload(vm, 4); // yaw + buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + rc_gimbal[0] = buzzvm_stack_at(vm, 4)->f.value; + rc_gimbal[1] = buzzvm_stack_at(vm, 3)->f.value; + rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value; + rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value; + + ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)",rc_gimbal[0],rc_gimbal[1],rc_gimbal[2],rc_gimbal[3]); + buzz_cmd = COMMAND_GIMBAL; + 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); // 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); + 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(wplist_map.size()<=0) + parse_gpslist(); + goal[0] = wplist_map.begin()->second.latitude; + goal[1] = wplist_map.begin()->second.longitude; + goal[2] = wplist_map.begin()->second.altitude; + wplist_map.erase(wplist_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_WARN("Set RC_GOTO ---- %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); } /*----------------------------------------/ @@ -327,6 +440,20 @@ namespace buzzuav_closures{ return goto_pos; } + float* getgimbal() { + return rc_gimbal; + } + + string getuavstate() { + static buzzvm_t VM = buzz_utility::get_vm(); + std::stringstream state_buff; + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); + buzzvm_gload(VM); + buzzobj_t uav_state = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + return uav_state->s.value.str; + } + int getcmd() { return cur_cmd; } @@ -344,12 +471,24 @@ namespace buzzuav_closures{ return cmd; } - void rc_set_goto(double pos[]) { - rc_goto_pos[0] = pos[0]; - rc_goto_pos[1] = pos[1]; - rc_goto_pos[2] = pos[2]; + void rc_set_goto(int id, double latitude, double longitude, double altitude) { + rc_id = id; + rc_goto_pos[0] = latitude; + rc_goto_pos[1] = longitude; + rc_goto_pos[2] = altitude; } + + void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) { + + rc_id = id; + rc_gimbal[0] = yaw; + rc_gimbal[1] = roll; + rc_gimbal[2] = pitch; + rc_gimbal[3] = t; + + } + void rc_call(int rc_cmd_in) { rc_cmd = rc_cmd_in; } @@ -517,6 +656,10 @@ namespace buzzuav_closures{ buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); buzzvm_pusht(vm); buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1)); + buzzvm_pushi(vm, rc_id); + buzzvm_tput(vm); + buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); buzzvm_pushf(vm, rc_goto_pos[0]); buzzvm_tput(vm); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ce77de7..b03ac07 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -2,6 +2,9 @@ #include namespace rosbzz_node { +const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; +const bool debug = false; + /*--------------- /Constructor ---------------*/ @@ -17,6 +20,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); @@ -46,7 +50,14 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv) } std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - path += "Update.log"; + path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/"; + std::string folder_check="mkdir -p "+path; + system(folder_check.c_str()); + for(int i=5;i>0;i--){ + rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(), + (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str()); + } + path += "logger_"+std::to_string(robot_id)+"_0.log"; log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); } @@ -185,9 +196,13 @@ void roscontroller::RosControllerRun() /* Set the Buzz bytecode */ if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id)) { + int packet_loss_tmp,time_step=0; + double cur_packet_loss=0; ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; - init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + /*loop rate of ros*/ + ros::Rate loop_rate(BUZZRATE); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -195,11 +210,43 @@ void roscontroller::RosControllerRun() while (ros::ok() && !buzz_utility::buzz_script_done()) { /*Update neighbors position inside Buzz*/ // buzz_closure::neighbour_pos_callback(neighbours_pos_map); + + /*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh, + neigh pos, RSSI val, Packet loss, filtered packet loss*/ + log<::iterator it = + neighbours_pos_map.begin(); + log << neighbours_pos_map.size()<< ","; + for (; it != neighbours_pos_map.end(); ++it) + { + log << (double)it->second.x << "," << (double)it->second.y + << "," << (double)it->second.z << ","; + } + const uint8_t shrt_id= 0xFF; + float result; + /*Neighbours of the robot published with id in respective topic*/ neighbours_pos_publisher(); send_MPpayload(); /*Check updater state and step code*/ - update_routine(bcfname.c_str(), dbgfname.c_str()); + // update_routine(bcfname.c_str(), dbgfname.c_str()); + if(time_step==BUZZRATE){ + time_step=0; + cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) ); + packet_loss_tmp=0; + if(cur_packet_loss<0) cur_packet_loss=0; + else if (cur_packet_loss>1) cur_packet_loss=1; + } + else{ + packet_loss_tmp+=(int)buzz_utility::get_inmsg_size(); + time_step++; + } + if(debug) ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss); + /*Log In Msg queue size*/ + log<<(int)buzz_utility::get_inmsg_size()<<","; /*Step buzz script */ buzz_utility::buzz_script_step(); /*Prepare messages and publish them in respective topic*/ @@ -207,13 +254,13 @@ void roscontroller::RosControllerRun() /*call flight controler service to set command long*/ flight_controller_service_call(); /*Set multi message available after update*/ - if (get_update_status()) - { - set_read_update_status(); + //if (get_update_status()) + //{ + /* set_read_update_status(); multi_msg = true; log << cur_pos.latitude << "," << cur_pos.longitude << "," << cur_pos.altitude << ","; - collect_data(log); + collect_data(log); map::iterator it = neighbours_pos_map.begin(); log << "," << neighbours_pos_map.size(); @@ -222,26 +269,29 @@ void roscontroller::RosControllerRun() log << "," << (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z; } - log << std::endl; - } + log << std::endl;*/ + //} /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ // no_of_robots=get_number_of_robots(); get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); + /*Retrive the state of the graph and uav and log*/ + std::stringstream state_buff; + state_buff << buzzuav_closures::getuavstate(); + log<0) no_of_robots // =neighbours_pos_map.size()+1; // buzz_utility::set_robot_var(no_of_robots); /*Set no of robots for updates TODO only when not updating*/ // if(multi_msg) - updates_set_robots(no_of_robots); - // ROS_INFO("ROBOTS: %i , acutal : - // %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1); - get_xbee_status(); + //updates_set_robots(no_of_robots); + //get_xbee_status(); // commented out because it may slow down the node too much, to be tested /*run once*/ ros::spinOnce(); - /*loop rate of ros*/ - ros::Rate loop_rate(BUZZRATE); loop_rate.sleep(); + // make sure to sleep for the remainder of our cycle time + if (loop_rate.cycleTime() > ros::Duration(1.0 / (float)BUZZRATE)) + ROS_WARN("ROSBuzz loop missed its desired rate of %iHz... the loop actually took %.4f seconds", BUZZRATE, loop_rate.cycleTime().toSec()); if (fcu_timeout <= 0) buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); else @@ -266,7 +316,8 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) /*Obtain .bzz file name from parameter server*/ if (n_c.getParam("bzzfile_name", bzzfile_name)) ; - else { + else + { ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node"); } @@ -284,13 +335,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) { ROS_INFO("RC service is disabled"); } - } else { + } else + { ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node"); } - /*Obtain robot_id from parameter server*/ - // n_c.getParam("robot_id", robot_id); - // robot_id=(int)buzz_utility::get_robotid(); /*Obtain out payload name*/ n_c.getParam("out_payload", out_payload); /*Obtain in payload name*/ @@ -300,21 +349,26 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) if (n_c.getParam("xbee_plugged", xbeeplugged)) ; - else { + else + { ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node"); } if (!xbeeplugged) { if (n_c.getParam("name", robot_name)) ; - else { + else + { ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node"); } } else n_c.getParam("xbee_status_srv", xbeesrv_name); - std::cout << "////////////////// " << xbeesrv_name; + if(!n_c.getParam("capture_image_srv", capture_srv_name)) + { + capture_srv_name = CAPTURE_SRV_DEFAULT_NAME; + } GetSubscriptionParameters(n_c); // initialize topics to null? @@ -325,70 +379,71 @@ used /-----------------------------------------------------------------------------------*/ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle) { - // todo: make it as an array in yaml? m_sMySubscriptions.clear(); std::string gps_topic, gps_type; if (node_handle.getParam("topics/gps", gps_topic)) ; - else { + else + { ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node"); } - node_handle.getParam("type/gps", gps_type); - m_smTopic_infos.insert(pair(gps_topic, gps_type)); + m_smTopic_infos.insert(pair(gps_topic, "sensor_msgs/NavSatFix")); - std::string battery_topic, battery_type; + std::string battery_topic; node_handle.getParam("topics/battery", battery_topic); - node_handle.getParam("type/battery", battery_type); - m_smTopic_infos.insert( - pair(battery_topic, battery_type)); + m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryStatus")); - std::string status_topic, status_type; + std::string status_topic; node_handle.getParam("topics/status", status_topic); - node_handle.getParam("type/status", status_type); - m_smTopic_infos.insert( - pair(status_topic, status_type)); + m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/State")); + node_handle.getParam("topics/estatus", status_topic); + m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/ExtendedState")); - std::string altitude_topic, altitude_type; + std::string altitude_topic; node_handle.getParam("topics/altitude", altitude_topic); - node_handle.getParam("type/altitude", altitude_type); - m_smTopic_infos.insert( - pair(altitude_topic, altitude_type)); + m_smTopic_infos.insert(pair(altitude_topic, "std_msgs/Float64")); - /*Obtain fc client name from parameter server*/ + // Obtain required topic and service names from the parameter server if (node_handle.getParam("topics/fcclient", fcclient_name)) ; - else { + else + { ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/setpoint", setpoint_name)) ; - else { + else + { ROS_ERROR("Provide a Set Point name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/armclient", armclient)) ; - else { + else + { ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/modeclient", modeclient)) ; - else { + else + { ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/stream", stream_client_name)) ; - else { + else + { ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/localpos", local_pos_sub_name)) ; - else { + else + { ROS_ERROR("Provide a localpos name in YAML file"); system("rosnode kill rosbuzz_node"); } @@ -405,33 +460,29 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c) Subscribe(n_c); - payload_sub = - n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); + payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); - obstacle_sub = n_c.subscribe(obstacles_topic, 5, - &roscontroller::obstacle_dist, this); + obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this); /*publishers*/ payload_pub = n_c.advertise(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); neigh_pos_pub = n_c.advertise("neighbours_pos", 5); - localsetpoint_nonraw_pub = - n_c.advertise(setpoint_name, 5); + localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); + /* Service Clients*/ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); mav_client = n_c.serviceClient(fcclient_name); if (rcclient == true) - service = - n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); + service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); ROS_INFO("Ready to receive Mav Commands from RC client"); xbeestatus_srv = n_c.serviceClient(xbeesrv_name); - stream_client = - n_c.serviceClient(stream_client_name); + capture_srv = n_c.serviceClient(capture_srv_name); + stream_client = n_c.serviceClient(stream_client_name); users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this); - local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, - &roscontroller::local_pos_callback, this); + local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this); multi_msg = true; } @@ -444,20 +495,15 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c) m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it) { if (it->second == "mavros_msgs/ExtendedState") { - flight_status_sub = n_c.subscribe( - it->first, 100, &roscontroller::flight_extended_status_update, this); + flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this); } else if (it->second == "mavros_msgs/State") { - flight_status_sub = n_c.subscribe( - it->first, 100, &roscontroller::flight_status_update, this); + flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); } else if (it->second == "mavros_msgs/BatteryStatus") { - battery_sub = - n_c.subscribe(it->first, 5, &roscontroller::battery, this); + battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); } else if (it->second == "sensor_msgs/NavSatFix") { - current_position_sub = - n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); + current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); } else if (it->second == "std_msgs/Float64") { - relative_altitude_sub = - n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); + relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); } std::cout << "Subscribed to: " << it->first << endl; @@ -469,31 +515,17 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c) /-------------------------------------------------------*/ std::string roscontroller::Compile_bzz(std::string bzzfile_name) { - /*TODO: change to bzzc instead of bzzparse and also add -I for includes*/ /*Compile the buzz code .bzz to .bo*/ stringstream bzzfile_in_compile; std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - // bzzfile_in_compile << path << "/"; - // path = bzzfile_in_compile.str(); - // bzzfile_in_compile.str(""); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0, name.find_last_of(".")); bzzfile_in_compile << "bzzc -I " << path - << "include/"; //<<" "< Received unregistered command: ", req.command); + res.success = true; + break; } return true; }