From ec47973fefc707e8c3fb2373016c78abc86f8a0e Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 30 Oct 2018 04:36:54 -0400 Subject: [PATCH 1/3] added low ids height for sim --- buzz_scripts/include/utils/takeoff_heights.bzz | 2 ++ 1 file changed, 2 insertions(+) diff --git a/buzz_scripts/include/utils/takeoff_heights.bzz b/buzz_scripts/include/utils/takeoff_heights.bzz index 81081aa..693c730 100644 --- a/buzz_scripts/include/utils/takeoff_heights.bzz +++ b/buzz_scripts/include/utils/takeoff_heights.bzz @@ -1,7 +1,9 @@ takeoff_heights ={ + .0 = 30.0, .1 = 21.0, .2 = 18.0, .3 = 12.0, + .4 = 28.0, .5 = 12.0, .6 = 3.0, .9 = 15.0, From b62789a583adb0b12a338ad4e5f7b76eadbc1435 Mon Sep 17 00:00:00 2001 From: dave Date: Sun, 4 Nov 2018 16:27:11 -0500 Subject: [PATCH 2/3] added batt/state of all neighbor in webcontrol, enhance serialization for vstig --- buzz_scripts/include/act/states.bzz | 31 +++-- buzz_scripts/include/plan/mapmatrix.bzz | 15 --- .../include/taskallocate/graphformGPS.bzz | 55 +-------- buzz_scripts/include/utils/conversions.bzz | 16 +++ buzz_scripts/include/utils/table.bzz | 115 ++++++++++++++++++ buzz_scripts/include/vstigenv.bzz | 85 ++++++------- buzz_scripts/main.bzz | 6 +- src/buzz_utility.cpp | 6 + src/buzzuav_closures.cpp | 57 ++++++--- 9 files changed, 236 insertions(+), 150 deletions(-) create mode 100644 buzz_scripts/include/utils/table.bzz diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 81f7a73..df51aa2 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -13,6 +13,7 @@ include "act/neighborcomm.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps +WP_STIG = 8 path_it = 0 pic_time = 0 @@ -100,7 +101,7 @@ firsttimeinwp = 1 wpreached = 1 function indiWP() { if(firsttimeinwp) { - v_wp = stigmergy.create(8) + v_wp = stigmergy.create(WP_STIG) storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude) firsttimeinwp = 0 } @@ -111,21 +112,27 @@ function indiWP() { storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude) return } else { - v_wp.put(rc_goto.id,{.lat=rc_goto.latitude, .lon=rc_goto.longitude, .pro=0}) + var ls = packWP2i(rc_goto.latitude, rc_goto.longitude, 0) + v_wp.put(rc_goto.id,ls) reset_rc() } } - wp = v_wp.get(id) - if(wp!=nil) { - #log(wp.pro,wpreached) - if(wp.pro == 0) { - wpreached = 0 - storegoal(wp.lat, wp.lon, pose.position.altitude) - v_wp.put(id,{.lat=wp.lat, .lon=wp.lon, .pro=1}) - return - } - } + #if(vstig_buzzy == 0) { + wpi = v_wp.get(id) + if(wpi!=nil) { + #log(wp.pro,wpreached) + wp = unpackWP2i(wpi) + table_print(wp) + if(wp.pro == 0) { + wpreached = 0 + storegoal(wp.lat, wp.lon, pose.position.altitude) + var ls = packWP2i(wp.lat, wp.lon, 1) + v_wp.put(id,ls) + return + } + } + #} if(wpreached!=1) goto_gps(indiWP_done) diff --git a/buzz_scripts/include/plan/mapmatrix.bzz b/buzz_scripts/include/plan/mapmatrix.bzz index c3d3393..209aed0 100644 --- a/buzz_scripts/include/plan/mapmatrix.bzz +++ b/buzz_scripts/include/plan/mapmatrix.bzz @@ -102,21 +102,6 @@ function is_in(dictionary, x, y){ return 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) { var ir=1 while(ir <= size(t)) { diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index f020e2e..b796a1f 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -6,6 +6,7 @@ include "taskallocate/graphs/shapes_P.bzz" include "taskallocate/graphs/shapes_O.bzz" include "taskallocate/graphs/shapes_L.bzz" include "taskallocate/graphs/shapes_Y.bzz" +#include "utils/table.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS @@ -82,60 +83,6 @@ m_lockstig = 1 # Lennard-Jones parameters, may need change EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots -# -#return the number of value in table -# -function count(table,value){ - var number=0 - var i=0 - while(i ", value) + }) +} + +function table_copy(t) { + var t2 = {} + foreach(t, function(key, value) { + t2[key] = value + }) + return t2 +} + +# +#return the number of value in table +# +function count(table,value){ + var number=0 + var i=0 + while(i0) { - if(b_updating==0) { - u=0 - while(u0) { - if(b_updating==0) { - u=0 - while(u indoor wheeled vehicle V_TYPE = 0 -goal_list = { - .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} -} # Executed once at init time. function init() { @@ -53,7 +51,7 @@ function step() { rc_cmd_listen() # update the vstig (status/net/batt/...) - # uav_updatestig() + #uav_updatestig() # # State machine diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index d9a80ca..9e3ba5f 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -291,6 +291,12 @@ static int testing_buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "reset_rc", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); return VM->state; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index a18a093..60abf02 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -54,7 +54,7 @@ int buzzros_print(buzzvm_t vm) ----------------------------------------------------------- */ { std::ostringstream buffer(std::ostringstream::ate); - buffer << "[" << buzz_utility::get_robotid() << "] "; + buffer << std::fixed << std::setprecision(6) << "[" << buzz_utility::get_robotid() << "] "; for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index) { buzzvm_lload(vm, index); @@ -288,23 +288,42 @@ int buzzuav_addNeiStatus(buzzvm_t vm) / closure to add neighbors status to the BVM /----------------------------------------*/ { - 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_lnum_assert(vm, 1); + buzzvm_lload(vm, 1); // state table + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); + buzzobj_t t = buzzvm_stack_at(vm, 1); + if(buzzdict_size(t->t.value) != 5) { + ROS_WARN("Wrong neighbor status size."); + return vm->state; + } + 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; + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1)); + buzzvm_tget(vm); + uint8_t id = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "ba", 1)); + buzzvm_tget(vm); + newRS.batt_lvl = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "gp", 1)); + buzzvm_tget(vm); + newRS.gps_strenght = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "xb", 1)); + buzzvm_tget(vm); + newRS.xbee = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "st", 1)); + buzzvm_tget(vm); newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value; + buzzvm_pop(vm); + map::iterator it = neighbors_status_map.find(id); if (it != neighbors_status_map.end()) neighbors_status_map.erase(it); @@ -417,8 +436,10 @@ void set_gpsgoal(double goal[3]) rb_from_gps(goal, rb, cur_pos); if (fabs(rb[0]) < 150.0) { goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2]; - ROS_INFO("Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); - } + ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + } else + ROS_WARN("[%i] GPS GOAL TOO FAR !!-- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + } int buzzuav_arm(buzzvm_t vm) From ec869b3bb77f5b4c344450e54169356107872103 Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 5 Nov 2018 04:11:02 -0500 Subject: [PATCH 3/3] enhanced serialization, done with 1st exp. with added complexity (batt/state info) --- buzz_scripts/include/act/states.bzz | 2 - .../include/taskallocate/waypointlist.csv | 93 +------------------ .../include/taskallocate/waypointlist_old.csv | 89 ++++++++++++++++++ buzz_scripts/include/utils/conversions.bzz | 12 +-- buzz_scripts/include/utils/table.bzz | 52 ++++------- buzz_scripts/include/vstigenv.bzz | 2 +- buzz_scripts/main.bzz | 2 +- include/buzzuav_closures.h | 3 +- include/roscontroller.h | 4 +- launch/launch_config/topics.yaml | 1 + launch/rosbuzz.launch | 2 + launch/rosbuzzd.launch | 2 + src/buzz_utility.cpp | 2 +- src/buzzuav_closures.cpp | 27 +++++- src/roscontroller.cpp | 48 +++++++++- 15 files changed, 198 insertions(+), 143 deletions(-) create mode 100644 buzz_scripts/include/taskallocate/waypointlist_old.csv diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index df51aa2..0fb8cbf 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -121,9 +121,7 @@ function indiWP() { #if(vstig_buzzy == 0) { wpi = v_wp.get(id) if(wpi!=nil) { - #log(wp.pro,wpreached) wp = unpackWP2i(wpi) - table_print(wp) if(wp.pro == 0) { wpreached = 0 storegoal(wp.lat, wp.lon, pose.position.altitude) diff --git a/buzz_scripts/include/taskallocate/waypointlist.csv b/buzz_scripts/include/taskallocate/waypointlist.csv index e0a2c42..a11120b 100644 --- a/buzz_scripts/include/taskallocate/waypointlist.csv +++ b/buzz_scripts/include/taskallocate/waypointlist.csv @@ -1,89 +1,4 @@ -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 +1,-73.609219,45.510336,1 +2,-73.608913,45.510723,0 +3,-73.610035,45.510182,1 +4,-73.609235,45.510114,0 \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/waypointlist_old.csv b/buzz_scripts/include/taskallocate/waypointlist_old.csv new file mode 100644 index 0000000..e0a2c42 --- /dev/null +++ b/buzz_scripts/include/taskallocate/waypointlist_old.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/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index e598972..003e908 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -62,18 +62,16 @@ function gps_from_vec(vec) { return Lgoal } -GPSoffset = {.lat=45.50, .lon=-73.62} +GPSoffset = {.lat=45.50, .lon=-73.61} function packWP2i(in_lat, in_long, processed) { var dlat = math.round((in_lat - GPSoffset.lat)*1000000) var dlon = math.round((in_long - GPSoffset.lon)*1000000) - return dlat*100000+dlon*10+processed + return {.dla=dlat, .dlo=dlon*10+processed} } function unpackWP2i(wp_int){ - dlat = (wp_int-wp_int%100000)/100000 - wp_int=wp_int-dlat*100000 - dlon = (wp_int-wp_int%10)/10 - wp_int=wp_int-dlon*10 - return {.lat=dlat/1000000.0+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=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+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=pro} } diff --git a/buzz_scripts/include/utils/table.bzz b/buzz_scripts/include/utils/table.bzz index d2d9f33..ed19a6d 100644 --- a/buzz_scripts/include/utils/table.bzz +++ b/buzz_scripts/include/utils/table.bzz @@ -31,39 +31,30 @@ function count(table,value){ # function i2s(value){ if(value==1){ - return "GRAPH_FREE" - } - else if(value==2){ return "GRAPH_ASKING" } - else if(value==3){ + else if(value==2){ return "GRAPH_JOINING" } - else if(value==4){ + else if(value==3){ return "GRAPH_JOINED" } - else if(value==5){ - return "GRAPH_LOCK" - } - else if(value==6){ + else if(value==4){ return "TURNEDOFF" } - else if(value==7){ - return "LAND" - } - else if(value==8){ + else if(value==5){ return "BARRIERWAIT" } - else if(value==9){ + else if(value==6){ return "INDIWP" } - else if(value==10){ + else if(value==7){ return "TASK_ALLOCATE" } - else if(value==11){ + else if(value==8){ return "LAUNCH" } - else if(value==12){ + else if(value==9){ return "STOP" } else { @@ -74,41 +65,32 @@ function i2s(value){ #map from state to int # function s2i(value){ - if(value=="GRAPH_FREE"){ + if(value=="GRAPH_ASKING"){ return 1 } - else if(value=="GRAPH_ASKING"){ + else if(value=="GRAPH_JOINING"){ return 2 } - else if(value=="GRAPH_JOINING"){ + else if(value=="GRAPH_JOINED"){ return 3 } - else if(value=="GRAPH_JOINED"){ + else if(value=="TURNEDOFF"){ return 4 } - else if(value=="GRAPH_LOCK"){ + else if(value=="BARRIERWAIT"){ return 5 } - else if(value=="TURNEDOFF"){ + else if(value=="INDIWP"){ return 6 } - else if(value=="LAND"){ + else if(value=="TASK_ALLOCATE"){ return 7 } - else if(value=="BARRIERWAIT"){ + else if(value=="LAUNCH"){ return 8 } - else if(value=="INDIWP"){ - return 9 - } - else if(value=="TASK_ALLOCATE"){ - return 10 - } - else if(value=="LAUNCH"){ - return 11 - } else if(value=="STOP"){ - return 12 + return 9 } else return 0 diff --git a/buzz_scripts/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz index c6fabb0..f4b9c90 100644 --- a/buzz_scripts/include/vstigenv.bzz +++ b/buzz_scripts/include/vstigenv.bzz @@ -94,7 +94,7 @@ function usertab_print(t) { function stattab_print() { if(v_status.size()>0) { - var state_struct = {.id = -1, .gp = -1, .ba = -1, .xb = -1, .st = -1} + var state_struct = {.id = -1, .gp = 0, .ba = 0, .xb = 0, .st = 0} neighbors.foreach(function(rid, data) { var nei_state = v_status.get(rid) if(nei_state!=nil){ diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index be13c3c..cb310c1 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -51,7 +51,7 @@ function step() { rc_cmd_listen() # update the vstig (status/net/batt/...) - #uav_updatestig() + uav_updatestig() # # State machine diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index f8ae4d5..967f521 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -20,7 +20,8 @@ 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); +void setWPlist(std::string file); +void check_targets_sim(double lat, double lon, double *res); /* * closure to move following a vector diff --git a/include/roscontroller.h b/include/roscontroller.h index 74f12ef..599f116 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -115,6 +115,7 @@ private: uint64_t payload; std::map neighbours_pos_map; + std::map targets_found; std::map raw_neighbours_pos_map; std::map neighbours_time_map; int timer_step = 0; @@ -143,7 +144,7 @@ private: bool debug = false; bool setmode = false; bool BClpose = false; - std::string bzzfile_name; + std::string bzzfile_name, WPfile; std::string bcfname, dbgfname; std::string stand_by; std::string capture_srv_name; @@ -155,6 +156,7 @@ private: ros::ServiceClient stream_client; ros::Publisher payload_pub; ros::Publisher MPpayload_pub; + ros::Publisher targetf_pub; ros::Publisher neigh_pos_pub; ros::Publisher bvmstate_pub; ros::Publisher grid_pub; diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml index 82efabe..a3c2851 100644 --- a/launch/launch_config/topics.yaml +++ b/launch/launch_config/topics.yaml @@ -13,6 +13,7 @@ topics: bstate: bvmstate npose: neighbours_pos fstatus: fleet_status + targetf: targets_found services: fcclient: cmd/command armclient: cmd/arming diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index 6ab3f50..010de1a 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -6,6 +6,7 @@ + @@ -13,6 +14,7 @@ + diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch index 0b34233..20912ec 100644 --- a/launch/rosbuzzd.launch +++ b/launch/rosbuzzd.launch @@ -6,6 +6,7 @@ + @@ -13,6 +14,7 @@ + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 9e3ba5f..4ce60b5 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -17,7 +17,7 @@ static buzzvm_t VM = 0; static char* BO_FNAME = 0; static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; -static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets +static uint32_t MAX_MSG_SIZE = 210;//250; // Maximum Msg size for sending update packets static uint8_t Robot_id = 0; static std::vector IN_MSG; std::map users_map; diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 60abf02..8cdfa92 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -98,12 +98,13 @@ int buzzros_print(buzzvm_t vm) return buzzvm_ret0(vm); } -void setWPlist(string path) +void setWPlist(string file) /* / set the absolute path for a csv list of waypoints ----------------------------------------------------------- */ { - WPlistname = path + "include/taskallocate/waypointlist.csv"; + WPlistname = file;//path + "include/taskallocate/waypointlist.csv"; + parse_gpslist(); } float constrainAngle(float x) @@ -181,6 +182,28 @@ void parse_gpslist() fin.close(); } +void check_targets_sim(double lat, double lon, double *res) +/* +/ check if a listed target is close +----------------------------------------------------------- */ +{ + map::iterator it; + for (it = wplist_map.begin(); it != wplist_map.end(); ++it) + { + double rb[3]; + double ref[2]={lat, lon}; + double tar[2]={it->second.latitude, it->second.longitude}; + rb_from_gps(tar, rb, ref); + if(rb[0]<3.0){ + ROS_WARN("FOUND A TARGET!!! [%i]", it->first); + res[0] = it->first; + res[1] = it->second.latitude; + res[2] = it->second.longitude; + res[3] = it->second.altitude; + } + } +} + int buzz_exportmap(buzzvm_t vm) /* / Buzz closure to export a 2D map diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 362d477..d75caf8 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -28,7 +28,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time()) bcfname = fname + ".bo"; dbgfname = fname + ".bdb"; buzz_update::set_bzz_file(bzzfile_name.c_str(),debug); - buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); + buzzuav_closures::setWPlist(WPfile); // Initialize variables if(setmode) SetMode("LOITER", 0); @@ -259,6 +259,13 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node"); } + if (n_c.getParam("WPfile", WPfile)) + ; + else + { + ROS_ERROR("Provide a .csv file to with target WP list"); + system("rosnode kill rosbuzz_node"); + } // Obtain debug mode from launch file parameter if (n_c.getParam("debug", debug)) ; @@ -417,8 +424,15 @@ void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handl ROS_ERROR("Provide a fleet status out topic name in YAML file"); system("rosnode kill rosbuzz_node"); } + if (node_handle.getParam("topics/targetf", topic)) + targetf_pub = n_c.advertise(topic, 5); + else + { + ROS_ERROR("Provide a fleet status out topic name in YAML file"); + system("rosnode kill rosbuzz_node"); + } if (node_handle.getParam("topics/npose", topic)) - neigh_pos_pub = n_c.advertise(topic, MAX_NUMBER_OF_ROBOTS); + neigh_pos_pub = n_c.advertise(topic, 5); else { ROS_ERROR("Provide a Neighbor pose out topic name in YAML file"); @@ -529,11 +543,14 @@ std::string roscontroller::Compile_bzz(std::string bzzfile_name) void roscontroller::neighbours_pos_publisher() /* -/ Publish neighbours pos and id in neighbours pos topic +/ Publish neighbours pos and id in neighbours pos topic AND TARGET ACQUIRED (SIMULATED) /----------------------------------------------------*/ { auto current_time = ros::Time::now(); map::iterator it; + rosbuzz::neigh_pos msg_target_out; + msg_target_out.header.frame_id = "/world"; + msg_target_out.header.stamp = current_time; rosbuzz::neigh_pos neigh_pos_array; neigh_pos_array.header.frame_id = "/world"; neigh_pos_array.header.stamp = current_time; @@ -551,7 +568,32 @@ void roscontroller::neighbours_pos_publisher() // cout<<"iterator it val: "<< it-> first << " After convertion: " // <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<second).x, (it->second).y, tf); + if(tf[0]!=-1){ + buzz_utility::Pos_struct pos_tmp; + pos_tmp.x = tf[1]; + pos_tmp.y = tf[2]; + pos_tmp.z = tf[3]; + map::iterator it = targets_found.find(round(tf[0])); + if (it != targets_found.end()) + targets_found.erase(it); + targets_found.insert(make_pair(round(tf[0]), pos_tmp)); + } } + for (it = targets_found.begin(); it != targets_found.end(); ++it) + { + sensor_msgs::NavSatFix target_tmp; + target_tmp.header.stamp = current_time; + target_tmp.header.frame_id = "/world"; + target_tmp.position_covariance_type = it->first; // custom robot id storage + target_tmp.latitude = (it->second).x; + target_tmp.longitude = (it->second).y; + target_tmp.altitude = (it->second).z; + msg_target_out.pos_neigh.push_back(target_tmp); + } + targetf_pub.publish(msg_target_out); neigh_pos_pub.publish(neigh_pos_array); }