From ec869b3bb77f5b4c344450e54169356107872103 Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 5 Nov 2018 04:11:02 -0500 Subject: [PATCH] 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); }