diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index 78caa0d..3ba5262 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -6,7 +6,7 @@ include "vec2.bzz" include "update.bzz" include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. -include "barrier.bzz" # reserve stigmergy id=11 for this header. +include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. include "uavstates.bzz" # require an 'action' function to be defined here. include "graphs/shapes_Y.bzz" @@ -268,14 +268,14 @@ function target4label(nei_id){ #calculate LJ vector for neibhor stored in i function LJ_vec(i){ var dis=m_MessageRange[i] - var bearing=m_MessageBearing[i] + var ljbearing=m_MessageBearing[i] var nei_id=m_messageID[i] var target=target4label(nei_id) var cDir={.x=0.0,.y=0.0} if(target!="miss"){ - cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),bearing) + cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),ljbearing) } - #log(id,"dis=",dis,"target=",target,"label=",nei_id) + #log(id,"dis=",dis,"target=",target,"label=",nei_id) #log("x=",cDir.x,"y=",cDir.y) return cDir } @@ -383,65 +383,11 @@ function TransitionToAsking(un_label){ # #Transistion to state joining # -function set_rc_goto() { - #get information of pred - var i=0 - var IDofPred=-1 - while(imath.pi) - S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi - else - S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi - - var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing) - - #the vector from self to target in global coordinate - var S2Target=math.vec2.add(S2Pred,P2Target) - #change the vector to local coordinate of self - var S2Target_bearing=math.atan(S2Target.y, S2Target.x) - m_bias=m_cMeToPred.Bearing-S2PGlobalBearing - S2Target_bearing=S2Target_bearing+m_bias - - gps_from_rb(math.vec2.length(S2Target)/100.0, S2Target_bearing) - uav_storegoal(goal.latitude, goal.longitude, position.altitude) - } -} - function TransitionToJoining(){ UAVSTATE="STATE_JOINING" m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel m_unWaitCount=m_unJoiningLostPeriod - - set_rc_goto() - - neighbors.listen("r", - function(vid,value,rid){ - var recv_table={.Label=0,.Bearing=0.0} - recv_table=unpack_guide_msg(value) - #store the received message - if(recv_table.Label==m_nLabel){ - m_cMeToPred.GlobalBearing=recv_table.Bearing - } - }) } # @@ -452,11 +398,10 @@ function TransitionToJoined(){ m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel m_vecNodes[m_nLabel].State="ASSIGNED" - neighbors.ignore("r") #write statues #v_tag.put(m_nLabel, m_lockstig) - barrier_create(m_lockstig) + barrier_create() barrier_ready() m_navigation.x=0.0 @@ -591,13 +536,47 @@ function DoAsking(){ # #Do joining # +m_gotjoinedparent = 0 function DoJoining(){ - gotoWP(TransitionToJoined) + + if(m_gotjoinedparent!=1) + set_rc_goto() + else + gotoWP(TransitionToJoined) + #pack the communication package m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel } +function set_rc_goto() { + #get information of pred + var i=0 + var IDofPred=-1 + while(i Prev. br. ", barrier, " ", BARRIER_VSTIG) + if(barrier!=nil) { + barrier=nil + BARRIER_VSTIG = BARRIER_VSTIG +1 + } + #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) + barrier = stigmergy.create(BARRIER_VSTIG) } -function barrier_set(threshold, transf, resumef, vstig_nb) { +function barrier_set(threshold, transf, resumef, bdt) { statef = function() { - barrier_wait(threshold, transf, resumef); + barrier_wait(threshold, transf, resumef, bdt); } UAVSTATE = "BARRIERWAIT" - barrier_create(vstig_nb) + barrier_create() } # # Make yourself ready # function barrier_ready() { - log("BARRIER READY -------") barrier.put(id, 1) barrier.put("d", 0) } @@ -40,20 +47,23 @@ function barrier_ready() { # # Executes the barrier # -function barrier_wait(threshold, transf, resumef) { +function barrier_wait(threshold, transf, resumef, bdt) { barrier.put(id, 1) barrier.get(id) - if(barrier.size() >= threshold or barrier.get("d")==1) { + #log("----->BS: ", barrier.size()) + if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { barrier.put("d", 1) -# barrier = nil timeW = 0 transf() } else if(timeW >= BARRIER_TIMEOUT) { + log("------> Barrier Timeout !!!!") timeW = 0 -# barrier = nil resumef() - } + } + + if(bdt!=-1) + neighbors.broadcast("cmd", bdt) timeW = timeW+1 } 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/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 5d90034..cdf9e3b 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -3,10 +3,13 @@ # FLIGHT-RELATED FUNCTIONS # ######################################## -TARGET_ALTITUDE = 5.0 +TARGET_ALTITUDE = 5.0 # m. UAVSTATE = "TURNEDOFF" -GOTO_MAXVEL = 2 -TAKEOFF_VSTIG = 11 +PICTURE_WAIT = 40 # steps +GOTO_MAXVEL = 2 # m/steps +GOTO_MAXDIST = 150 # m. +GOTODIST_TOL = 0.5 # m. +GOTOANG_TOL = 0.1 # rad. goal = {.range=0, .bearing=0, .latitude=0, .longitude=0} function uav_initswarm() { @@ -24,18 +27,13 @@ function idle() { UAVSTATE = "IDLE" } -firstpassT = 1 function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff - - if(firstpassT) { - barrier_create(TAKEOFF_VSTIG) - firstpassT = 0 - } if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_wait(ROBOTS, action, land) + barrier_set(ROBOTS, action, land, -1) + barrier_ready() } else { log("Altitude: ", TARGET_ALTITUDE) neighbors.broadcast("cmd", 22) @@ -43,55 +41,65 @@ function takeoff() { } } -firstpassL = 1 function land() { UAVSTATE = "LAND" statef=land - if(firstpassL) { - barrier_create(TAKEOFF_VSTIG+1) - firstpassL = 0 - } - neighbors.broadcast("cmd", 21) uav_land() if(flight.status != 2 and flight.status != 3){ - barrier_wait(ROBOTS,turnedoff,land) + barrier_set(ROBOTS,turnedoff,land, 21) + barrier_ready() } } -function goto() { +function set_goto(transf) { UAVSTATE = "GOTO" - statef=goto + statef=function() { + gotoWP(transf); + } if(rc_goto.id==id){ - s.leave() - gotoWP(picture) + if(s!=nil){ + if(s.in()) + s.leave() + } } else { neighbors.broadcast("cmd", 16) neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) } } +ptime=0 function picture() { + statef=picture + UAVSTATE="PICTURE" #TODO: change gimbal orientation - uav_takepicture() - statef=idle + if(ptime==PICTURE_WAIT/2) + uav_takepicture() + else if(ptime>=PICTURE_WAIT) { + statef=action + ptime=0 + } + ptime=ptime+1 } function gotoWP(transf) { # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) rb_from_gps(rc_goto.latitude, rc_goto.longitude) - print(id, " has to move ", goal.range) + print(" has to move ", goal.range) m_navigation = math.vec2.newp(goal.range, goal.bearing) - if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { + + if(math.vec2.length(m_navigation)>GOTO_MAXDIST) + log("Sorry this is too far.") + else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) - } else if(math.vec2.length(m_navigation)>0.25) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) - else + } else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination transf() + else + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) } function follow() { @@ -145,16 +153,18 @@ function uav_rccmd() { function uav_neicmd() { neighbors.listen("cmd", function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and UAVSTATE!="TAKEOFF") { + print("Got (", vid, ",", value, ") from robot #", rid, "(", UAVSTATE, ")") + if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") { statef=takeoff - } else if(value==21) { + UAVSTATE = "TAKEOFF" + } else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") { statef=land - } else if(value==400 and UAVSTATE=="IDLE") { + UAVSTATE = "LAND" + } else if(value==400 and UAVSTATE=="TURNEDOFF") { uav_arm() - } else if(value==401 and UAVSTATE=="IDLE"){ + } else if(value==401 and UAVSTATE=="TURNEDOFF"){ uav_disarm() - } else if(value==16){ + } else if(value==16 and UAVSTATE=="IDLE"){ # neighbors.listen("gt",function(vid, value, rid) { # print("Got (", vid, ",", value, ") from robot #", rid) # # if(gt.id == id) statef=goto @@ -173,7 +183,7 @@ function LimitAngle(angle){ } function rb_from_gps(lat, lon) { - print("gps2rb: ",lat,lon,position.latitude,position.longitude) +# print("gps2rb: ",lat,lon,position.latitude,position.longitude) d_lon = lon - position.longitude d_lat = lat - position.latitude ned_x = d_lat/180*math.pi * 6371000.0; @@ -182,12 +192,18 @@ function rb_from_gps(lat, lon) { goal.bearing = LimitAngle(math.atan(ned_y,ned_x)); } -function gps_from_rb(range, bearing) { - print("rb2gps: ",range,bearing,position.latitude,position.longitude) +function gps_from_vec(vec) { + Vrange = math.vec2.length(vec) + Vbearing = LimitAngle(math.atan(vec.y, vec.x)) +# print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude) latR = position.latitude*math.pi/180.0; lonR = position.longitude*math.pi/180.0; - target_lat = math.asin(math.sin(latR) * math.cos(range/6371000.0) + math.cos(latR) * math.sin(range/6371000.0) * math.cos(bearing)); - target_lon = lonR + math.atan(math.sin(bearing) * math.sin(range/6371000.0) * math.cos(latR), math.cos(range/6371000.0) - math.sin(latR) * math.sin(target_lat)); + target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); + target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); goal.latitude = target_lat*180.0/math.pi; goal.longitude = target_lon*180.0/math.pi; + #d_lat = (vec.x / 6371000.0)*180.0/math.pi; + #goal.latitude = d_lat + position.latitude; + #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; + #goal.longitude = d_lon + position.longitude; } \ No newline at end of file diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz new file mode 100644 index 0000000..97b92bf --- /dev/null +++ b/buzz_scripts/testaloneWP.bzz @@ -0,0 +1,35 @@ +include "vec2.bzz" +include "update.bzz" +include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header. +include "uavstates.bzz" # require an 'action' function to be defined here. +include "vstigenv.bzz" + +function action() { + uav_storegoal(-1.0,-1.0,-1.0) + set_goto(picture) +} + +# Executed once at init time. +function init() { + statef=turnedoff + UAVSTATE = "TURNEDOFF" + uav_initstig() + TARGET_ALTITUDE = 15.0 +} + +# Executed at each time step. +function step() { + uav_rccmd() + + statef() + + log("Current state: ", UAVSTATE) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/include/buzz_utility.h b/include/buzz_utility.h index f3f9550..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; diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 61c7000..47c5388 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -9,8 +9,8 @@ #include "ros/ros.h" #include "buzz_utility.h" - #define EARTH_RADIUS (double) 6371000.0 - #define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0))) +#define EARTH_RADIUS (double) 6371000.0 +#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0))) namespace buzzuav_closures{ typedef enum { @@ -31,13 +31,14 @@ namespace buzzuav_closures{ * The command to use in Buzz is buzzros_print takes any available datatype in Buzz */ int buzzros_print(buzzvm_t vm); - +void setWPlist(std::string path); /* * buzzuav_goto(latitude,longitude,altitude) function in Buzz * commands the UAV to go to a position supplied */ int buzzuav_moveto(buzzvm_t vm); int buzzuav_storegoal(buzzvm_t vm); +void parse_gpslist(); int buzzuav_takepicture(buzzvm_t vm); /* Returns the current command from local variable*/ int getcmd(); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 418b297..d1f6804 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -32,6 +32,7 @@ namespace buzzuav_closures{ static float raw_packet_loss = 0.0; static int filtered_packet_loss = 0; static float api_rssi = 0.0; + string WPlistname = ""; std::map< int, buzz_utility::RB_struct> targets_map; std::map< int, buzz_utility::Pos_struct> neighbors_map; @@ -83,6 +84,10 @@ namespace buzzuav_closures{ return buzzvm_ret0(vm); } + void setWPlist(string path){ + WPlistname = path + "include/graphs/waypointlist.csv"; + } + /*----------------------------------------/ / Compute GPS destination from current position and desired Range and Bearing /----------------------------------------*/ @@ -105,10 +110,10 @@ namespace buzzuav_closures{ } void rb_from_gps(double nei[], double out[], double cur[]){ - double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; - double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + double d_lon = nei[1] - cur[1]; + double d_lat = nei[0] - cur[0]; + double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); out[1] = constrainAngle(atan2(ned_y,ned_x)); out[2] = 0.0; @@ -119,6 +124,52 @@ namespace buzzuav_closures{ double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958}; double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677}; + void parse_gpslist() + { + // Open file: + ROS_INFO("WP list file: %s", WPlistname.c_str()); + std::ifstream fin(WPlistname.c_str()); // Open in text-mode. + + // Opening may fail, always check. + if (!fin) { + ROS_ERROR("GPS list parser, could not open file."); + return; + } + + // Prepare a C-string buffer to be used when reading lines. + const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need. + char buffer[MAX_LINE_LENGTH] = {}; + const char * DELIMS = "\t ,"; // Tab, space or comma. + + // Read the file and load the data: + double lat, lon; + int alt, tilt, tid; + buzz_utility::RB_struct RB_arr; + // Read one line at a time. + while ( fin.getline(buffer, MAX_LINE_LENGTH) ) { + // Extract the tokens: + tid = atoi(strtok( buffer, DELIMS )); + lon = atof(strtok( NULL, DELIMS )); + lat = atof(strtok( NULL, DELIMS )); + alt = atoi(strtok( NULL, DELIMS )); + tilt = atoi(strtok( NULL, DELIMS )); + //ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid); + RB_arr.latitude=lat; + RB_arr.longitude=lon; + RB_arr.altitude=alt; + // Insert elements. + map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(tid); + if(it!=targets_map.end()) + targets_map.erase(it); + targets_map.insert(make_pair(tid, RB_arr)); + } + + ROS_INFO("----->Saved %i waypoints.", targets_map.size()); + + // Close the file: + fin.close(); + } + /*----------------------------------------/ / Buzz closure to move following a 2D vector /----------------------------------------*/ @@ -158,9 +209,9 @@ namespace buzzuav_closures{ double rb[3], tmp[3]; map< int, buzz_utility::RB_struct >::iterator it; for (it=targets_map.begin(); it!=targets_map.end(); ++it){ - tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height; - rb_from_gps(tmp, rb, cur_pos); - ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]); + tmp[0]=(it->second).latitude; tmp[1]=(it->second).longitude; tmp[2]=height; + rb_from_gps(tmp, rb, cur_pos); + //ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]); buzzvm_push(vm, targettbl); /* When we get here, the "targets" table is on top of the stack */ //ROS_INFO("Buzz_utility will save user %i.", it->first); @@ -206,8 +257,9 @@ namespace buzzuav_closures{ if(fabs(rb[0])<100.0) { //printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]); buzz_utility::RB_struct RB_arr; - RB_arr.la=tmp[0]; - RB_arr.lo=tmp[1]; + RB_arr.latitude=tmp[0]; + RB_arr.longitude=tmp[1]; + RB_arr.altitude=tmp[2]; RB_arr.r=rb[0]; RB_arr.b=rb[1]; map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(uid); @@ -217,7 +269,7 @@ namespace buzzuav_closures{ //ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); return vm->state; } else - printf(" ---------- Target too far %f\n",rb[0]); + ROS_WARN(" ---------- Target too far %f",rb[0]); return 0; } @@ -249,7 +301,7 @@ namespace buzzuav_closures{ mavros_msgs::Mavlink get_status(){ mavros_msgs::Mavlink payload_out; - map< int, buzz_utility::neighbors_status >::iterator it; + map< int, buzz_utility::neighbors_status >::iterator it; for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++it){ payload_out.payload64.push_back(it->first); payload_out.payload64.push_back(it->second.gps_strenght); @@ -284,11 +336,26 @@ namespace buzzuav_closures{ buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - float lat = buzzvm_stack_at(vm, 3)->f.value; - float lon = buzzvm_stack_at(vm, 2)->f.value; - float alt = buzzvm_stack_at(vm, 1)->f.value; - ROS_ERROR("DEBUG ---- %f %f %f",lat,lon,alt); - rc_set_goto((int)buzz_utility::get_robotid(), lat, lon, alt); + double goal[3]; + goal[0] = buzzvm_stack_at(vm, 3)->f.value; + goal[1] = buzzvm_stack_at(vm, 2)->f.value; + goal[2] = buzzvm_stack_at(vm, 1)->f.value; + if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){ + if(targets_map.size()<=0) + parse_gpslist(); + goal[0] = targets_map.begin()->second.latitude; + goal[1] = targets_map.begin()->second.longitude; + goal[2] = targets_map.begin()->second.altitude; + targets_map.erase(targets_map.begin()->first); + } + + double rb[3]; + + rb_from_gps(goal, rb, cur_pos); + if(fabs(rb[0])<150.0) + rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); + + ROS_ERROR("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]); return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2711a73..6354584 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -17,6 +17,7 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv) bcfname = fname + ".bo"; dbgfname = fname + ".bdb"; set_bzz_file(bzzfile_name.c_str()); + buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); /*Initialize variables*/ // Solo things SetMode("LOITER", 0); @@ -751,8 +752,8 @@ void roscontroller::flight_controller_service_call() } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ - ROS_INFO("TAKING A PICTURE HERE!! --------------") - ; + ROS_INFO("TAKING A PICTURE HERE!! --------------"); + ros::Duration(0.2).sleep(); } } /*---------------------------------------------- @@ -1092,7 +1093,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { gps_rb(nei_pos, cvt_neighbours_pos_payload); /*Extract robot id of the neighbour*/ uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); - ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); +// ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); /*pass neighbour position to local maintaner*/ buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1],