diff --git a/buzz_scripts/include/act/CA.bzz b/buzz_scripts/include/act/CA.bzz index ed0d055..bbf5ee0 100644 --- a/buzz_scripts/include/act/CA.bzz +++ b/buzz_scripts/include/act/CA.bzz @@ -1,10 +1,10 @@ # Lightweight collision avoidance function LCA( vel_vec ) { - var safety_radius = 3.0 + var safety_radius = 2.0 collide = 0 - var k_v = 5 # x axis gain - var k_w = 5 # y axis gain + var k_v = 4 # x axis gain + var k_w = 4 # y axis gain cart = neighbors.map( function(rid, data) { @@ -17,7 +17,7 @@ function LCA( vel_vec ) { }) if (collide) { log("") - log("------> AVOIDING NEIGHBOR! <------") + log("------> AVOIDING NEIGHBOR! (LCA) <------") log("") result = cart.reduce(function(rid, data, accum) { if(data.distance < accum.distance and data.distance > 0.0){ @@ -48,4 +48,187 @@ function LCA( vel_vec ) { } else return vel_vec -} \ No newline at end of file +} + +robot_radius = 1.0 +safety_radius = 2.0 +combined_radius = 2 * robot_radius + safety_radius +vel_sample_count = 50 + +velocities = {} + +function in_between(theta_right, theta_dif, theta_left) { + if(math.abs(theta_right - theta_left) < (math.pi - 0.000001)) { + if((theta_right <= theta_dif) and (theta_dif <= theta_left)) { + return 1 + } else { + return 0 + } + } else if(math.abs(theta_right - theta_left) > (math.pi + 0.000001)) { + if((theta_right <= theta_dif) or (theta_dif <= theta_left)) { + return 1 + } else { + return 0 + } + } else { + # Exactly pi rad between right and left + if(theta_right <= 0) { + if((theta_right <= theta_dif) and (theta_dif <= theta_left)) { + return 1 + } else { + return 0 + } + } else if(theta_left <= 0) { + if((theta_right <= theta_dif) or (theta_dif <= theta_left)) { + return 1 + } else { + return 0 + } + } + } +} + +# VO magic happens here +function RVO(preferedVelocity) { + + #data_string = string.concat(data_string, ",", string.tostring(preferedVelocity.x), ",", string.tostring(preferedVelocity.y), ",", string.tostring(preferedVelocity.z)) + + final_V = math.vec2.new(0.0, 0.0) + collision = 0 + suitable_V = {} + + var VO_all = {} + + neighbors.foreach( + function(rid, data) { + + var angle = data.azimuth#-(data.azimuth * 0.017454) + + #data_string = string.concat(data_string, ",", string.tostring(data.distance), ",", string.tostring(angle)) + + var distance = data.distance + if(distance <= combined_radius) distance = combined_radius + + theta_BA_ort = math.asin(combined_radius / distance) + theta_BA_left = angle + theta_BA_ort + if(theta_BA_left > math.pi) { + theta_BA_left = theta_BA_left - 2 * math.pi + } else if(theta_BA_left < -math.pi) { + theta_BA_left = theta_BA_left + 2 * math.pi + } + theta_BA_right = angle - theta_BA_ort + if(theta_BA_right > math.pi) { + theta_BA_right = theta_BA_right - 2 * math.pi + } else if(theta_BA_right < -math.pi) { + theta_BA_right = theta_BA_right + 2 * math.pi + } + + neighbor_velocity = velocities[rid] + if(neighbor_velocity == nil) { + neighbor_velocity = math.vec2.new(0.0, 0.0) + } + + #data_string = string.concat(data_string, ",", string.tostring(neighbor_velocity.x), ",", string.tostring(neighbor_velocity.y), ",", string.tostring(neighbor_velocity.z)) + + VO_all[rid] = { + .velocity = math.vec2.new(neighbor_velocity.x, neighbor_velocity.y), + .theta_left = theta_BA_left, + .theta_right = theta_BA_right + } + + } + ) + + # Detect collision + foreach(VO_all, function(rid, vo) { + vAB = math.vec2.sub(preferedVelocity, vo.velocity) + var vel_angle = math.acos(vAB.x / math.vec2.length(vAB)) + if(vAB.y < 0) vel_angle = vel_angle * -1 + + if(in_between(vo.theta_right, vel_angle, vo.theta_left)) { + collision = 1 + } + }) + + # Calculate suitable velocities + if(collision) { + + log("") + log("------> AVOIDING NEIGHBOR! (RVO) <------") + log("") + + var idx = 0 + var n = 0 + while (n < vel_sample_count) { + + v_cand = math.vec2.new( 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL, 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL) + while(math.vec2.length(v_cand) > GOTO_MAXVEL) { + v_cand = math.vec2.new( 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL, 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL) + } + + suit = 1 + + foreach(VO_all, function(rid, vo) { + #vAB = new_V + vAB = math.vec2.sub(v_cand, vo.velocity) + #vAB = math.vec3.sub(new_V, math.vec3.scale(math.vec3.add(preferedVelocity, vo.velocity), 0.5)) + var vel_angle = math.acos(vAB.x / math.vec2.length(vAB)) + if(vAB.y < 0) vel_angle = vel_angle * -1 + + if(in_between(vo.theta_right, vel_angle, vo.theta_left)) { + suit = 0 + } + }) + + if(suit) { + suitable_V[idx] = v_cand + idx = idx + 1 + } + n = n + 1 + } + + # Chose a velocity closer to the desired one + if(size(suitable_V) > 0) { + min_dist = 99999 + sv = 0 + while(sv < size(suitable_V)) { + var RVOdist = math.vec2.length(math.vec2.sub(suitable_V[sv], preferedVelocity)) + if(RVOdist < min_dist) { + min_dist = RVOdist + final_V = suitable_V[sv] + } + sv = sv + 1 + } + } + + #data_string = string.concat(data_string, ",", string.tostring(final_V.x), ",", string.tostring(final_V.y), ",", string.tostring(final_V.z)) + + broadcast_velocity(final_V) + + return final_V + } else { + + #data_string = string.concat(data_string, ",", string.tostring(preferedVelocity.x), ",", string.tostring(preferedVelocity.y), ",", string.tostring(preferedVelocity.z)) + + broadcast_velocity(preferedVelocity) + return preferedVelocity + } +} + +function broadcast_velocity(velocity) { + neighbors.broadcast(string.concat("v", string.tostring(id)), velocity) +} + +function setup_velocity_callbacks() { + r = 1 + while(r <= ROBOTS) { + if(r != id) { + neighbors.listen(string.concat("v", string.tostring(r)), + function(vid, value, rid) { + velocities[rid] = value + } + ) + } + r = r + 1 + } +} diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index d9e0a48..f48aa58 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -18,21 +18,7 @@ hvs = 0; # Sets a barrier # function barrier_create() { - # reset timeW = 1 - # create barrier vstig - #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) - #if(barrier!=nil) { - # barrier=nil - #if(hvs) { - # BARRIER_VSTIG = BARRIER_VSTIG -1 - # hvs = 0 - #}else{ - # BARRIER_VSTIG = BARRIER_VSTIG +1 - # hvs = 1 - #} - #} - #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) if(barrier==nil) barrier = stigmergy.create(BARRIER_VSTIG) } @@ -95,6 +81,43 @@ function barrier_wait(threshold, transf, resumef, bc) { timeW = timeW+1 } +function barrier_wait_graph(threshold, transf, resumef, bc) { + if(barrier==nil) #failsafe + barrier_create() + + barrier.put(id, bc) + if(barrier.get("n")1) { + neighbors.foreach(function (nei,data){ + barrier.get(nei) + }) + } + + # Not to miss any RC inputs, for waypoints, potential and deploy states + check_rc_wp() + + #log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) + if(barrier.size()-2 >= barrier.get("n")) { + if(barrier_allgood(barrier,bc)) { + barrier.put("d", 1) + timeW = 0 + GRAPHSTATE = transf + } else + barrier.put("d", 0) + } + + if(timeW >= BARRIER_TIMEOUT) { + log("------> Barrier Timeout !!!!") + #barrier = nil + timeW = 0 + GRAPHSTATE = resumef + } else if(timeW % 10 == 0 and bc > 0) + neighbors.broadcast("cmd", bc) + + timeW = timeW+1 +} + # Check all members state function barrier_allgood(barrier, bc) { barriergood = 1 diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 1017195..c422c87 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -1,23 +1,9 @@ -GOTO_MAXVEL = 12.5 # m/steps +GOTO_MAXVEL = 2.5 # m/steps GOTO_MAXDIST = 250 # m. GOTODIST_TOL = 0.4 # m. GOTOANG_TOL = 0.1 # rad. -# Geofence polygon -# CEPSUM field -#GPSlimit = {.1={.lat=45.510400, .lng=-73.610421}, -# .2={.lat=45.510896, .lng=-73.608731}, -# .3={.lat=45.510355, .lng=-73.608404}, -# .4={.lat=45.509840, .lng=-73.610072}} -# Pangeae final site -#GPSlimit = {.1={.lat=29.067746, .lng=-13.663315}, -# .2={.lat=29.068724, .lng=-13.662634}, -# .3={.lat=29.068113, .lng=-13.661427}, -# .4={.lat=29.067014, .lng=-13.661564}} -# Pangeae first site -GPSlimit = {.1={.lat=29.021055, .lng=-13.715155}, - .2={.lat=29.021055, .lng=-13.714132}, - .3={.lat=29.019470, .lng=-13.714132}, - .4={.lat=29.019470, .lng=-13.715240}} + +include "utils/field_config.bzz" # Core naviguation function to travel to a GPS target location. function goto_gps(transf) { @@ -30,8 +16,12 @@ function goto_gps(transf) { } else { m_navigation = LimitSpeed(m_navigation, 1.0) gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit[1].lat, GPSlimit[1].lng, 0), .2=vec_from_gps(GPSlimit[2].lat, GPSlimit[2].lng, 0), .3=vec_from_gps(GPSlimit[3].lat, GPSlimit[3].lng, 0), .4=vec_from_gps(GPSlimit[4].lat, GPSlimit[4].lng, 0)} + #TODO: ENSURE THAT IF WE GO OUT WE CAN GET BACK IN!!! geofence(gf) - #m_navigation = LCA(m_navigation) + if(CA_ON==1) + m_navigation = LCA(m_navigation) + if(CA_ON==2) + m_navigation = RVO(m_navigation) goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) } } diff --git a/buzz_scripts/include/act/neighborcomm.bzz b/buzz_scripts/include/act/neighborcomm.bzz index e6f6075..ec5ce1c 100644 --- a/buzz_scripts/include/act/neighborcomm.bzz +++ b/buzz_scripts/include/act/neighborcomm.bzz @@ -59,7 +59,7 @@ function rc_cmd_listen() { destroyGraph() resetWP() check_rc_wp() - barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903) + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 903) barrier_ready(903) neighbors.broadcast("cmd", 903) } else if (flight.rc_cmd==904){ @@ -100,11 +100,11 @@ function nei_cmd_listen() { if(BVMSTATE!=AUTO_LAUNCH_STATE) barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) #barrier_ready(20) - } else if(value==900){ # Shapes + } else if(value==900 and BVMSTATE!="TASK_ALLOCATE"){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) #barrier_ready(900) #neighbors.broadcast("cmd", 900) - } else if(value==901 and BVMSTATE!="DEPLOY" ){ # Pursuit + } else if(value==901 and BVMSTATE!="DEPLOY" ){ # Voronoi destroyGraph() barrier_set(ROBOTS, "DEPLOY", BVMSTATE, 901) #barrier_ready(901) @@ -114,9 +114,9 @@ function nei_cmd_listen() { barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902) #barrier_ready(902) #neighbors.broadcast("cmd", 902) - } else if(value==903 and BVMSTATE!="POTENTIAL" and BVMSTATE!="TURNEDOFF"){ # Formation + } else if(value==903 and BVMSTATE!="PURSUIT" and BVMSTATE!="TURNEDOFF"){ # Pursuit destroyGraph() - barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903) + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 903) #barrier_ready(903) #neighbors.broadcast("cmd", 903) } else if(value==904 and BVMSTATE!="IDLE" and BVMSTATE!="TURNEDOFF"){ # idle diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 0457de3..0f3681e 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -276,7 +276,7 @@ function aggregate() { # State fucntion to circle all together around centroid function pursuit() { BVMSTATE="PURSUIT" - rd = 20.0 + rd = 30.0 pc = 3.2 vd = 15.0 r_vec = neighbors.reduce(function(rid, data, r_vec) { diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index d3036f0..3982bc2 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -11,8 +11,9 @@ include "taskallocate/graphs/shapes_Y.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER -ROOT_ID = 2 -graph_id = 3 +GRAPHSTATE="NONE" +ROOT_ID = 0 +graph_id = 0 graph_loop = 1 # @@ -32,12 +33,12 @@ 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("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} +m_receivedMessage={.State=s2i_graph("GRAPH_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("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} +m_selfMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} #navigation vector m_navigation={.x=0,.y=0} @@ -240,7 +241,7 @@ neighbors.listen("m", Get_DisAndAzi(temp_id) #add the received message # - m_MessageState[m_neighbourCount]=i2s(recv_val.State) + m_MessageState[m_neighbourCount]=i2s_graph(recv_val.State) m_MessageLabel[m_neighbourCount]=recv_val.Label m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel m_MessageReqID[m_neighbourCount]=recv_val.ReqID @@ -249,7 +250,7 @@ neighbors.listen("m", m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing m_messageID[m_neighbourCount]=rid -#log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount]) +log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount]) m_neighbourCount=m_neighbourCount+1 }) @@ -270,7 +271,7 @@ function Get_DisAndAzi(t_id){ #Update node info according to neighbour robots # function UpdateNodeInfo(){ - #Collect informaiton + #Collect information #Update information i=0 @@ -301,13 +302,13 @@ function UpdateNodeInfo(){ # Do free # function DoFree() { - if(BVMSTATE!="GRAPH_FREE"){ - BVMSTATE="GRAPH_FREE" + if(GRAPHSTATE!="GRAPH_FREE"){ + GRAPHSTATE="GRAPH_FREE" m_unWaitCount=m_unLabelSearchWaitTime - m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.State=s2i_graph(GRAPHSTATE) }else{ UpdateGraph() - m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.State=s2i_graph(GRAPHSTATE) #wait for a while before looking for a Label if(m_unWaitCount>0) @@ -316,28 +317,28 @@ function DoFree() { #find a set of joined robots var setJoinedLabels={} var setJoinedIndexes={} - i=0 - j=0 - while(i0){ @@ -347,7 +348,7 @@ function DoFree() { } #set message - m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.State=s2i_graph(GRAPHSTATE) BroadcastGraph() } @@ -356,11 +357,11 @@ function DoFree() { #Do asking # function DoAsking(){ - if(BVMSTATE!="GRAPH_ASKING"){ - BVMSTATE="GRAPH_ASKING" + if(GRAPHSTATE!="GRAPH_ASKING"){ + GRAPHSTATE="GRAPH_ASKING" #math.rng.setseed(id) m_unRequestId=id#math.rng.uniform(0,10) #don't know why the random numbers are the same, add id to make the ReqID different - m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.State=s2i_graph(GRAPHSTATE) m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqID=m_unRequestId m_unWaitCount=m_unResponseTimeThreshold @@ -390,7 +391,7 @@ function DoAsking(){ #no response, wait m_unWaitCount=m_unWaitCount-1 - m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.State=s2i_graph(GRAPHSTATE) m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqID=m_unRequestId #if(m_unWaitCount==0){ @@ -428,9 +429,9 @@ function DoAsking(){ #Do joining # function DoJoining(){ - if(BVMSTATE!="GRAPH_JOINING") { - BVMSTATE="GRAPH_JOINING" - m_selfMessage.State=s2i(BVMSTATE) + if(GRAPHSTATE!="GRAPH_JOINING") { + GRAPHSTATE="GRAPH_JOINING" + m_selfMessage.State=s2i_graph(GRAPHSTATE) m_selfMessage.Label=m_nLabel m_unWaitCount=m_unJoiningLostPeriod } else { @@ -440,7 +441,7 @@ function DoJoining(){ else goto_gps(DoJoined) #pack the communication package - m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.State=s2i_graph(GRAPHSTATE) m_selfMessage.Label=m_nLabel BroadcastGraph() } @@ -477,9 +478,9 @@ function set_rc_goto() { #Do joined # function DoJoined(){ - if(BVMSTATE!="GRAPH_JOINED"){ - BVMSTATE="GRAPH_JOINED" - m_selfMessage.State=s2i(BVMSTATE) + if(GRAPHSTATE!="GRAPH_JOINED"){ + GRAPHSTATE="GRAPH_JOINED" + m_selfMessage.State=s2i_graph(GRAPHSTATE) m_selfMessage.Label=m_nLabel m_vecNodes[m_nLabel].State="ASSIGNED" @@ -492,7 +493,7 @@ function DoJoined(){ goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) } else { UpdateGraph() - m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.State=s2i_graph(GRAPHSTATE) m_selfMessage.Label=m_nLabel #collect all requests @@ -571,7 +572,7 @@ function DoJoined(){ } } # using JOINED as the resume state kinds of just reset the barrier timeout, should be IDLE/ALLOCATE... - barrier_wait(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1) + barrier_wait_graph(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1) BroadcastGraph() } } @@ -581,8 +582,8 @@ function DoJoined(){ timeout_graph = 40 function DoLock() { #[transition to lock... - BVMSTATE="GRAPH_LOCK" - m_selfMessage.State=s2i(BVMSTATE) + GRAPHSTATE="GRAPH_LOCK" + m_selfMessage.State=s2i_graph(GRAPHSTATE) m_selfMessage.Label=m_nLabel m_vecNodes[m_nLabel].State="ASSIGNED" #record neighbor distance @@ -602,7 +603,7 @@ function DoLock() { neighbors.ignore("m") #..] UpdateGraph() - m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.State=s2i_graph(GRAPHSTATE) m_selfMessage.Label=m_nLabel m_navigation.x=0.0 m_navigation.y=0.0 @@ -621,12 +622,12 @@ function DoLock() { if(graph_loop) { if(timeout_graph==0) { - if(graph_id < 3) + if(graph_id < 1) graph_id = graph_id + 1 else graph_id = 0 timeout_graph = 40 - BVMSTATE="TASK_ALLOCATE" + GRAPHSTATE="NONE" } timeout_graph = timeout_graph - 1 } @@ -652,7 +653,7 @@ function UpdateGraph() { #update the graph UpdateNodeInfo() #reset message package to be sent - m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + m_selfMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} } function BroadcastGraph() { @@ -676,8 +677,8 @@ function BroadcastGraph() { # Executed when reset # function resetGraph(){ - m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} - m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + m_receivedMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} + m_selfMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} m_navigation={.x=0,.y=0} m_nLabel=-1 m_messageID={} @@ -695,15 +696,15 @@ function resetGraph(){ if(graph_id==0){ log("Loading P graph") Read_GraphP() +# } else if(graph_id==1) { +# log("Loading O graph") +# Read_GraphO() } else if(graph_id==1) { - log("Loading O graph") - Read_GraphO() - } else if(graph_id==2) { log("Loading L graph") Read_GraphL() - } else if(graph_id==3) { - log("Loading Y graph") - Read_GraphY() +# } else if(graph_id==3) { +# log("Loading Y graph") +# Read_GraphY() } #start listening @@ -719,6 +720,25 @@ function resetGraph(){ DoFree() } } + +function graph_state(){ + if(GRAPHSTATE=="GRAPH_FREE") + stateG=DoFree + else if(GRAPHSTATE=="GRAPH_ASKING") + stateG=DoAsking + else if(GRAPHSTATE=="GRAPH_JOINING") + stateG=DoJoining + else if(GRAPHSTATE=="GRAPH_JOINED") + stateG=DoJoined + else if(GRAPHSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list + stateG=DoLock + else + stateG=resetGraph + + stateG() + + log("Current graph state: ", GRAPHSTATE) +} # # Executed upon destroy # diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index 0efb2ad..d0e14c4 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -62,17 +62,14 @@ function gps_from_vec(vec) { return Lgoal } -GPSoffset = {.lat=45.50, .lng=-73.61} -GPSoffsetL = {.lat=29.01, .lng=-13.70} - function packWP2i(in_lat, in_long, processed) { - var dlat = math.round((in_lat - GPSoffsetL.lat)*1000000) - var dlon = math.round((in_long - GPSoffsetL.lng)*1000000) + var dlat = math.round((in_lat - GPSoffset.lat)*1000000) + var dlon = math.round((in_long - GPSoffset.lng)*1000000) return {.dla=dlat, .dlo=dlon*10+processed} } function unpackWP2i(wp_int){ var dlon = (wp_int.dlo-wp_int.dlo%10)/10.0 var pro = wp_int.dlo-dlon*10 - return {.lat=wp_int.dla/1000000.0+GPSoffsetL.lat, .lng=dlon/1000000.0+GPSoffsetL.lng, .pro=pro} + return {.lat=wp_int.dla/1000000.0+GPSoffset.lat, .lng=dlon/1000000.0+GPSoffset.lng, .pro=pro} } \ No newline at end of file diff --git a/buzz_scripts/include/utils/field_config.bzz b/buzz_scripts/include/utils/field_config.bzz new file mode 100644 index 0000000..3b321b9 --- /dev/null +++ b/buzz_scripts/include/utils/field_config.bzz @@ -0,0 +1,20 @@ +# Geofence polygon +# CEPSUM field +#GPSlimit = {.1={.lat=45.510400, .lng=-73.610421}, +# .2={.lat=45.510896, .lng=-73.608731}, +# .3={.lat=45.510355, .lng=-73.608404}, +# .4={.lat=45.509840, .lng=-73.610072}} +# Pangeae final site +#GPSlimit = {.1={.lat=29.067746, .lng=-13.663315}, +# .2={.lat=29.068724, .lng=-13.662634}, +# .3={.lat=29.068113, .lng=-13.661427}, +# .4={.lat=29.067014, .lng=-13.661564}} +# Pangeae first site +GPSlimit = {.1={.lat=29.021055, .lng=-13.715155}, + .2={.lat=29.021055, .lng=-13.714132}, + .3={.lat=29.019470, .lng=-13.714132}, + .4={.lat=29.019470, .lng=-13.715240}} + + +#GPSoffset = {.lat=45.50, .lng=-73.61} +GPSoffset = {.lat=29.01, .lng=-13.70} \ No newline at end of file diff --git a/buzz_scripts/include/utils/table.bzz b/buzz_scripts/include/utils/table.bzz index cfcc7f7..0294b17 100644 --- a/buzz_scripts/include/utils/table.bzz +++ b/buzz_scripts/include/utils/table.bzz @@ -34,7 +34,9 @@ function count(table,value){ # map from int to state - vstig serialization limits to 9.... # function i2s(value){ - if(value==1){ + if(value==0){ + return "PURSUIT" + }else if(value==1){ return "IDLE" } else if(value==2){ @@ -50,7 +52,7 @@ function i2s(value){ return "BARRIERWAIT" } else if(value==6){ - return "INDIWP" + return "WAYPOINT" } else if(value==7){ return "GOHOME" @@ -59,7 +61,27 @@ function i2s(value){ return "LAUNCH" } else if(value==9){ - return "FORMATION" + return "TASK_ALLOCATE" + } + else { + return "UNDETERMINED" + } +} +function i2s_graph(value){ + if(value==1){ + return "GRAPH_FREE" + } + else if(value==2){ + return "GRAPH_ASKING" + } + else if(value==3){ + return "GRAPH_JOINING" + } + else if(value==4){ + return "GRAPH_JOINED" + } + else if(value==5){ + return "GRAPH_LOCK" } else { return "UNDETERMINED" @@ -69,7 +91,9 @@ function i2s(value){ # map from state to int - vstig serialization limits to 9.... # function s2i(value){ - if(value=="IDLE"){ + if(value=="PURSUIT"){ + return 0 + }else if(value=="IDLE"){ return 1 } else if(value=="DEPLOY"){ @@ -93,9 +117,29 @@ function s2i(value){ else if(value=="LAUNCH"){ return 8 } - else if(value=="POTENTIAL"){ + else if(value=="TASK_ALLOCATE"){ return 9 } else - return 0 + return -1 +} +function s2i_graph(value){ + if(value=="GRAPH_FREE"){ + return 1 + } + else if(value=="GRAPH_ASKING"){ + return 2 + } + else if(value=="GRAPH_JOINING"){ + return 3 + } + else if(value=="GRAPH_JOINED"){ + return 4 + } + else if(value=="GRAPH_LOCK"){ + return 5 + } + else { + return 0 + } } \ No newline at end of file diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index e2c9c82..97ad880 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -80,21 +80,11 @@ function step() { else if(BVMSTATE=="PURSUIT") statef=pursuit else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ? - statef=resetGraph + statef=graph_state else if(BVMSTATE=="BIDDING") # check the absolute path of the waypointlist csv file in bidding.bzz statef=bidding else if(BVMSTATE=="DEPLOY") # check the absolute path of the waypointlist csv file in bidding.bzz statef=voronoicentroid - else if(BVMSTATE=="GRAPH_FREE") - statef=DoFree - else if(BVMSTATE=="GRAPH_ASKING") - statef=DoAsking - else if(BVMSTATE=="GRAPH_JOINING") - statef=DoJoining - else if(BVMSTATE=="GRAPH_JOINED") - statef=DoJoined - else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list - statef=DoLock else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar statef=rrtstar else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar diff --git a/include/VoronoiDiagramGenerator.h b/include/rosbuzz/VoronoiDiagramGenerator.h similarity index 100% rename from include/VoronoiDiagramGenerator.h rename to include/rosbuzz/VoronoiDiagramGenerator.h diff --git a/include/buzz_update.h b/include/rosbuzz/buzz_update.h similarity index 99% rename from include/buzz_update.h rename to include/rosbuzz/buzz_update.h index af9d41f..f203a1b 100644 --- a/include/buzz_update.h +++ b/include/rosbuzz/buzz_update.h @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #define delete_p(p) \ diff --git a/include/buzz_utility.h b/include/rosbuzz/buzz_utility.h similarity index 93% rename from include/buzz_utility.h rename to include/rosbuzz/buzz_utility.h index c5aa69e..3387eed 100644 --- a/include/buzz_utility.h +++ b/include/rosbuzz/buzz_utility.h @@ -1,8 +1,8 @@ #pragma once #include -#include "buzz_utility.h" -#include "buzzuav_closures.h" -#include "buzz_update.h" +#include +#include +#include #include #include #include @@ -100,6 +100,8 @@ buzzvm_t get_vm(); void set_robot_var(int ROBOTS); +void set_ca_on_var(int CA_ON); + int get_inmsg_size(); std::vector get_inmsg_vector(); diff --git a/include/buzzuav_closures.h b/include/rosbuzz/buzzuav_closures.h similarity index 98% rename from include/buzzuav_closures.h rename to include/rosbuzz/buzzuav_closures.h index 6321da3..f9b5569 100644 --- a/include/buzzuav_closures.h +++ b/include/rosbuzz/buzzuav_closures.h @@ -5,8 +5,8 @@ #include "mavros_msgs/CommandCode.h" #include "mavros_msgs/Mavlink.h" #include "ros/ros.h" -#include "buzz_utility.h" -#include "rosbuzz/mavrosCC.h" +#include +#include #define EARTH_RADIUS (double)6371000.0 #define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0))) diff --git a/include/roscontroller.h b/include/rosbuzz/roscontroller.h similarity index 99% rename from include/roscontroller.h rename to include/rosbuzz/roscontroller.h index 7c2684a..091f0a7 100644 --- a/include/roscontroller.h +++ b/include/rosbuzz/roscontroller.h @@ -35,8 +35,8 @@ #include #include #include -#include "buzzuav_closures.h" -#include "rosbuzz/mavrosCC.h" +#include +#include /* * ROSBuzz message types @@ -131,6 +131,7 @@ private: int rc_cmd; float fcu_timeout; int armstate; + int ca_on; int barrier; int update; int message_number = 0; diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index 244df4e..5bc3a5a 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -12,6 +12,7 @@ + @@ -24,5 +25,6 @@ + diff --git a/src/VoronoiDiagramGenerator.cpp b/src/VoronoiDiagramGenerator.cpp index cafafc9..7262fd5 100644 --- a/src/VoronoiDiagramGenerator.cpp +++ b/src/VoronoiDiagramGenerator.cpp @@ -27,7 +27,7 @@ * OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE. */ -#include "VoronoiDiagramGenerator.h" +#include VoronoiDiagramGenerator::VoronoiDiagramGenerator() { diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 72d5787..3f2f982 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -6,7 +6,7 @@ * @copyright 2016 MistLab. All rights reserved. */ -#include "buzz_update.h" +#include namespace buzz_update { diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index ce911f0..15ab3cf 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -6,7 +6,7 @@ * @copyright 2016 MistLab. All rights reserved. */ -#include "buzz_utility.h" +#include namespace buzz_utility { @@ -565,6 +565,16 @@ void set_robot_var(int ROBOTS) buzzvm_gstore(VM); } +void set_ca_on_var(int CA_ON) +/* +/ set swarm size in the BVM +-----------------------------*/ +{ + buzzvm_pushs(VM, buzzvm_string_register(VM, "CA_ON", 1)); + buzzvm_pushi(VM, CA_ON); + buzzvm_gstore(VM); +} + int get_inmsg_size() /* / get the incoming msgs size diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index d21f06c..d2afea8 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -6,9 +6,9 @@ * @copyright 2016 MistLab. All rights reserved. */ -#include "buzzuav_closures.h" +#include #include "math.h" -#include "VoronoiDiagramGenerator.h" +#include namespace buzzuav_closures { diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index 3816f55..9433c0a 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -6,7 +6,7 @@ * @copyright 2016 MistLab. All rights reserved. */ -#include "roscontroller.h" +#include int main(int argc, char** argv) /* diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 43940cf..ba5c569 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -6,7 +6,7 @@ * @copyright 2016 MistLab. All rights reserved. */ -#include "roscontroller.h" +#include #include namespace rosbuzz_node { @@ -130,6 +130,7 @@ void roscontroller::RosControllerRun() ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE."); // DEBUG // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); + buzz_utility::set_ca_on_var(ca_on); while (ros::ok() && !buzz_utility::buzz_script_done()) { // Publish topics @@ -300,6 +301,9 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) // Obtain standby script to run during update n_c.getParam("stand_by", stand_by); + // Obtain collision avoidance mode + n_c.getParam("ca_on", ca_on); + if (n_c.getParam("xbee_plugged", xbeeplugged)) ; else