enhanced serialization, done with 1st exp. with added complexity (batt/state info)

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)

buzz_scripts/include/taskallocate/waypointlist.csv

@@ -1,89 +1,4 @@
-0,-73.1531935978886,45.5084960903092,15,15
+1,-73.609219,45.510336,1
-1,-73.1530989420915,45.5085624255498,15,15
+2,-73.608913,45.510723,0
-2,-73.1530042862771,45.5086287608025,15,15
+3,-73.610035,45.510182,1
-3,-73.1529096304626,45.5086950960552,15,15
+4,-73.609235,45.510114,0
-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

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

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
+  var dlon = (wp_int.dlo-wp_int.dlo%10)/10.0
-  wp_int=wp_int-dlat*100000
+  var pro = wp_int.dlo-dlon*10
-  dlon = (wp_int-wp_int%10)/10
+  return {.lat=wp_int.dla/1000000.0+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=pro}
-  wp_int=wp_int-dlon*10
-  return {.lat=dlat/1000000.0+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=wp_int}
 }

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){
+	else if(value==4){
-	return "GRAPH_LOCK"
-	}
-	else if(value==6){
 	return "TURNEDOFF"
 	}
-	else if(value==7){
+	else if(value==5){
-	return "LAND"
-	}
-	else if(value==8){
 	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

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){

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

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

include/roscontroller.h

@@ -115,6 +115,7 @@ private:
 
   uint64_t payload;
   std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
+  std::map<int, buzz_utility::Pos_struct> targets_found;
   std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
   std::map<int, buzz_utility::neighbor_time> 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;

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

launch/rosbuzz.launch

@@ -6,6 +6,7 @@
   <arg name="name" default="robot0"/>
   <arg name="xbee_plugged" default="true"/>
   <arg name="script" default="testalone"/>
+  <arg name="wpfile" default="waypointlist"/>
   <arg name="launch_config" default="topics"/>
   <arg name="debug" default="false" />
   <arg name="setmode" default="false" />
@@ -13,6 +14,7 @@
   <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
     <rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
     <param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
+    <param name="WPfile" value="$(find rosbuzz)/buzz_scripts/include/taskallocate/$(arg wpfile).csv" />
     <param name="debug" value="$(arg debug)" />
     <param name="xbee_plugged" value="$(arg xbee_plugged)"/>
     <param name="name" value="$(arg name)"/>

launch/rosbuzzd.launch

@@ -6,6 +6,7 @@
 	<arg name="name" default="robot0"/>
   <arg name="xbee_plugged" default="true"/>
   <arg name="script" default="testalone"/>
+  <arg name="wpfile" default="waypointlist"/>
   <arg name="launch_config" default="topics"/>
   <arg name="debug" default="true" />
   <arg name="setmode" default="false" />
@@ -13,6 +14,7 @@
   <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
     <rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
     <param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
+    <param name="WPfile" value="$(find rosbuzz)/buzz_scripts/include/taskallocate/$(arg wpfile).csv" />
     <param name="debug" value="$(arg debug)" />
     <param name="xbee_plugged" value="$(arg xbee_plugged)"/>
     <param name="name" value="$(arg name)"/>

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<uint8_t*> IN_MSG;
 std::map<int, Pos_struct> users_map;

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<int, buzz_utility::RB_struct>::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

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<rosbuzz::neigh_pos>(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<rosbuzz::neigh_pos>(topic, MAX_NUMBER_OF_ROBOTS);
+    neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>(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<int, buzz_utility::Pos_struct>::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)<<endl;
     // std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
+    
+    double tf[4] = {-1, 0, 0, 0};
+    buzzuav_closures::check_targets_sim((it->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<int, buzz_utility::Pos_struct>::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);
 }