From 4c5d2e5ef2440f9dd737524dfeef70926c083426 Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 11 Sep 2018 00:02:20 -0400 Subject: [PATCH] add height table and fix large ID bug in ACF --- buzz_scripts/include/act/barrier.bzz | 11 ++++- buzz_scripts/include/act/states.bzz | 5 +-- .../include/taskallocate/graphformGPS.bzz | 41 ++++++++++--------- .../include/utils/takeoff_heights.bzz | 10 +++++ buzz_scripts/main.bzz | 5 ++- src/roscontroller.cpp | 2 +- 6 files changed, 46 insertions(+), 28 deletions(-) create mode 100644 buzz_scripts/include/utils/takeoff_heights.bzz diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 1c1c68d..eb6398d 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -11,7 +11,8 @@ BARRIER_TIMEOUT = 200 # in steps BARRIER_VSTIG = 80 timeW = 0 barrier = nil -bc = 0; +hvs = 0; + # # Sets a barrier @@ -23,7 +24,13 @@ function barrier_create() { #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) if(barrier!=nil) { barrier=nil - # BARRIER_VSTIG = BARRIER_VSTIG +1 + if(hvs) { + BARRIER_VSTIG = BARRIER_VSTIG -1 + hvs = 0 + }else{ + BARRIER_VSTIG = BARRIER_VSTIG +1 + hvs = 1 + } } #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) barrier = stigmergy.create(BARRIER_VSTIG) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 2287fc3..0869852 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -12,9 +12,9 @@ TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 1.5 # m/steps +GOTO_MAXVEL = 0.85 # m/steps GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 0.8 # m. +GOTODIST_TOL = 0.4 # m. GOTOANG_TOL = 0.1 # rad. path_it = 0 pic_time = 0 @@ -270,7 +270,6 @@ function nei_cmd_listen() { AUTO_LAUNCH_STATE = "IDLE" BVMSTATE = "GOHOME" } else if(value==21 and BVMSTATE!="TURNEDOFF") { - AUTO_LAUNCH_STATE = "TURNEDOFF" BVMSTATE = "STOP" } else if(value==400 and BVMSTATE=="TURNEDOFF") { arm() diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 15ddb8a..9110566 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -176,7 +176,7 @@ function find(table,value){ # function packmessage(send_table){ var send_value - send_value=100000*send_table.State+10000*send_table.Label+1000*send_table.ReqLabel+10*send_table.ReqID+send_table.Response + send_value=1000000*send_table.State+100000*send_table.Label+10000*send_table.ReqLabel+100*send_table.ReqID+send_table.Response return send_value } # @@ -200,14 +200,14 @@ function pack_guide_msg(send_table){ #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 wan=(recv_value-recv_value%1000000)/1000000 + recv_value=recv_value-wan*1000000 + var qian=(recv_value-recv_value%100000)/100000 + recv_value=recv_value-qian*100000 + var bai=(recv_value-recv_value%10000)/10000 + recv_value=recv_value-bai*10000 + var shi=(recv_value-recv_value%100)/100 + recv_value=recv_value-shi*100 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 @@ -310,10 +310,10 @@ neighbors.listen("m", # #Function used to get the station info of the sender of the message # -function Get_DisAndAzi(id){ +function Get_DisAndAzi(t_id){ neighbors.foreach( function(rid, data) { - if(rid==id){ + if(rid==t_id){ m_receivedMessage.Range=data.distance*100.0 m_receivedMessage.Bearing=data.azimuth } @@ -411,7 +411,8 @@ function DoFree() { function DoAsking(){ if(BVMSTATE!="GRAPH_ASKING"){ BVMSTATE="GRAPH_ASKING" - m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different + #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.ReqLabel=m_nLabel m_selfMessage.ReqID=m_unRequestId @@ -513,17 +514,17 @@ function set_rc_goto() { m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing) - m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate + m_cMeToPred.Range=m_MessageRange[IDofPred]#the position of self to pred in local coordinate m_cMeToPred.Bearing=m_MessageBearing[IDofPred] - var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing) - var S2Target=math.vec2.add(S2Pred,P2Target) + var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing) + var S2Target=math.vec2.add(S2Pred,P2Target) - goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x))) - print("Saving GPS goal: ",goal.latitude, goal.longitude) - storegoal(goal.latitude, goal.longitude, pose.position.altitude) - m_gotjoinedparent = 1 - } + goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x))) + print("Saving GPS goal (", m_nLabel, " from ", IDofPred, "): ",goal.latitude, goal.longitude, "(", S2Target.x/100, S2Target.y/100, ")") + storegoal(goal.latitude, goal.longitude, pose.position.altitude) + m_gotjoinedparent = 1 + } } # #Do joined diff --git a/buzz_scripts/include/utils/takeoff_heights.bzz b/buzz_scripts/include/utils/takeoff_heights.bzz new file mode 100644 index 0000000..4838861 --- /dev/null +++ b/buzz_scripts/include/utils/takeoff_heights.bzz @@ -0,0 +1,10 @@ +takeoff_heights ={ + .1 = 21.0, + .2 = 18.0, + .3 = 12.0, + .5 = 24.0, + .6 = 3.0, + .9 = 15.0, + .11 = 6.0, + .16 = 9.0 +} \ No newline at end of file diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 59dfba5..6d63d39 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -6,6 +6,7 @@ include "plan/rrtstar.bzz" include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" include "timesync.bzz" +include "utils/takeoff_heights.bzz" #State launched after takeoff AUTO_LAUNCH_STATE = "TASK_ALLOCATE" @@ -14,7 +15,7 @@ AUTO_LAUNCH_STATE = "TASK_ALLOCATE" LOWEST_ROBOT_ID = 0 TARGET = 9.0 EPSILON = 30.0 -ROOT_ID = 2 +ROOT_ID = 3 graph_id = 3 graph_loop = 0 @@ -37,7 +38,7 @@ function init() { # initGraph() - TARGET_ALTITUDE = 4 + (id-LOWEST_ROBOT_ID)*3.0 # m + TARGET_ALTITUDE = takeoff_heights[id] #4 + (id-LOWEST_ROBOT_ID)*3.0 # m # start the swarm command listener nei_cmd_listen() diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 7bf5674..0d40cc5 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1024,7 +1024,7 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) // To prevent drifting from stable position, uncomment //if(fabs(x)>0.005 || fabs(y)>0.005) { - // localsetpoint_nonraw_pub.publish(moveMsg); + localsetpoint_nonraw_pub.publish(moveMsg); //} }