graph test

buzz_scripts/graphform.bzz

@@ -175,7 +175,6 @@ function i2r(value){
 	else if(value==2){
 	return "REQ_GRANTED"
 	}
-	
 }
 #map from response to int
 function r2i(value){
@@ -320,7 +319,6 @@ function motion_vector(){
     return m_vector
 }
 
-
 function start_listen(){
 neighbors.listen("m",
     function(vid,value,rid){
@@ -468,7 +466,7 @@ while(i<m_neighbourCunt){
 }
     m_navigation.x=0.0
     m_navigation.y=0.0
-    goto(m_navigation.x,m_navigation.y)
+    uav_moveto(m_navigation.x,m_navigation.y)
 }
 
 #
@@ -502,7 +500,7 @@ function DoFree() {
 	while(i<size(m_vecNodes) and (unFoundLable==0)){
 		#if the node is unassigned and the predecessor is insight
 		if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLables,m_vecNodes[i].Pred)==1){
-			unFoundLable=m_vecNodes[i].Lable 
+			unFoundLable=m_vecNodes[i].Lable
 			IDofPred=find(m_MessageLable,m_vecNodes[unFoundLable].Pred)
 		}
 		i=i+1
@@ -646,11 +644,11 @@ function DoJoining(){
 		m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing)
 		uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
 
-		
+
 
 		#test if is already in desired position
 		if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
-			#log(S2Target_dis,S2Target_bearing)	
+			#log(S2Target_dis,S2Target_bearing)
 			TransitionToJoined()
 			return
 		}
@@ -705,7 +703,7 @@ function DoJoined(){
 		#if it is the pred
 		if(m_MessageState[i]=="STATE_JOINED" and m_MessageLable[i]==m_vecNodes[m_nLabel].Pred){
 			seenPred=1
-			m_unWaitCount=m_unJoiningLostPeriod 
+			m_unWaitCount=m_unJoiningLostPeriod
 		}
 		i=i+1
 	}
@@ -733,7 +731,7 @@ function DoJoined(){
 	#m_unWaitCount=m_unWaitCount-1
 	#if(m_unWaitCount==0){
 	#TransitionToFree()
-	#return 
+	#return
 	#}
 	#}
 
@@ -746,9 +744,9 @@ function DoJoined(){
 	#check if should to transists to lock
 
 
-	if(v_tag.size()==ROBOTS){
+#	if(v_tag.size()==ROBOTS){
-		TransitionToLock()
+#		TransitionToLock()
-	}
+#	}
 }
 
 #
@@ -769,7 +767,7 @@ if(m_nLabel!=0){
     m_navigation=motion_vector()
 }
 #move
-goto(m_navigation.x,m_navigation.y)
+uav_moveto(m_navigation.x,m_navigation.y)
 }
 
 function action(){
@@ -793,7 +791,8 @@ function init() {
 	# Join Swarm
 	#
 	uav_initswarm()
-	Reset();
+	Reset()
+	Read_Graph()
 }
 
 #
@@ -886,4 +885,4 @@ function destroy() {
 	m_vecNodes={}
 	#stop listening 
 	neighbors.ignore("m")
-}
+}

buzz_scripts/include/Graph_drone.graph

@@ -0,0 +1,5 @@
+0	-1	-1	-1  3000.0
+1	0	1000.0	0.0 5000.0
+2	0	1000.0	1.57    7000.0
+3	0	1000.0	3.14    9000.0
+4	0	1000.0	4.71    11000.0

buzz_scripts/include/graphs/shapes_L.bzz

@@ -1,6 +1,7 @@
 #Table of the nodes in the graph
 m_vecNodes={}
-m_vecNodes_fixed={}
+
+function Read_Graph(){
 m_vecNodes[0] = {      # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
     .Lable = 0,   # Lable of the point
     .Pred = -1,   # Lable of its predecessor
@@ -54,4 +55,5 @@ m_vecNodes[5] = {
     .height = 14000,
     .State="UNASSIGNED",
     .StateAge=0
+}
 }

buzz_scripts/include/graphs/shapes_O.bzz

@@ -1,6 +1,7 @@
 #Table of the nodes in the graph
 m_vecNodes={}
 m_vecNodes_fixed={}
+function Read_Graph(){
 m_vecNodes[0] = {      # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
     .Lable = 0,   # Lable of the point
     .Pred = -1,   # Lable of its predecessor
@@ -54,4 +55,5 @@ m_vecNodes[5] = {
     .height = 14000,
     .State="UNASSIGNED",
     .StateAge=0
+}
 }

buzz_scripts/include/graphs/shapes_P.bzz

@@ -1,6 +1,6 @@
 #Table of the nodes in the graph
 m_vecNodes={}
-m_vecNodes_fixed={}
+function Read_Graph(){
 m_vecNodes[0] = {      # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
     .Lable = 0,   # Lable of the point
     .Pred = -1,   # Lable of its predecessor
@@ -30,7 +30,7 @@ m_vecNodes[2] = {
 }
 m_vecNodes[3] = {
     .Lable = 3, 
-    .Pred = 1,
+    .Pred = 0,
     .distance = 2000,
     .bearing = 4.71,
     .height = 9000,
@@ -54,4 +54,5 @@ m_vecNodes[5] = {
     .height = 14000,
     .State="UNASSIGNED",
     .StateAge=0
+}
 }

buzz_scripts/include/graphs/shapes_Y.bzz

@@ -1,6 +1,6 @@
 #Table of the nodes in the graph
 m_vecNodes={}
-m_vecNodes_fixed={}
+function Read_Graph(){
 m_vecNodes[0] = {      # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
     .Lable = 0,   # Lable of the point
     .Pred = -1,   # Lable of its predecessor
@@ -54,4 +54,5 @@ m_vecNodes[5] = {
     .height = 14000,
     .State="UNASSIGNED",
     .StateAge=0
+}
 }

buzz_scripts/include/graphs/shapes_square.bzz

@@ -1,6 +1,8 @@
 #Table of the nodes in the graph
 m_vecNodes={}
-m_vecNodes_fixed={}
+
+function Read_Graph(){
+
 m_vecNodes[0] = {      # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
     .Lable = 0,   # Lable of the point
     .Pred = -1,   # Lable of its predecessor
@@ -47,65 +49,4 @@ m_vecNodes[4] = {
     .StateAge=0
 }
 
-#
-# Graph parsing
-#
-function parse_graph(fname) {
-	# Graph data
-	var gd = {}
-	# Open the file
-	var fd = io.fopen(fname, "r")
-	if(not fd) {
-		log("Can't open '", fname, "'")
-		return nil
-	}
-	# Parse the file line by line
-	var rrec # Record read from line
-	var grec # Record parsed into graph
-	io.fforeach(fd, function(line) {
-		  # Parse file line
-		  rrec = string.split(line, "\t ")
-		  # Make record
-		  gd[string.toint(rrec[0])] = {      # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
-			  .Lable = string.toint(rrec[0]),   # Lable of the point
-			  .Pred = string.toint(rrec[1]),   # Lable of its predecessor
-			  .distance = string.tofloat(rrec[2]), # distance to the predecessor
-			  .bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
-              .height = string.tofloat(rrec[4]), # height of this dot
-			  .State="UNASSIGNED",
-              .StateAge=0
-		  }})
-	# All done
-	io.fclose(fd)
-	return gd
-}
-
-function parse_graph_fixed(fname) {
-	# Graph data
-	var gd = {}
-	# Open the file
-	var fd = io.fopen(fname, "r")
-	if(not fd) {
-		log("Can't open '", fname, "'")
-		return nil
-	}
-	# Parse the file line by line
-	var rrec # Record read from line
-	var grec # Record parsed into graph
-	io.fforeach(fd, function(line) {
-		  # Parse file line
-		  rrec = string.split(line, "\t ")
-		  # Make record
-		  gd[string.toint(rrec[0])] = {      # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2
-			  .Pred1 = string.toint(rrec[1]),   # Pred 1 lable
-			  .Pred2 = string.toint(rrec[3]),   # Pred 2 lable
-			  .d1 = string.tofloat(rrec[2]), # Pred 1 distance
-			  .d2 = string.tofloat(rrec[4]), # Pred 2 distance
-			  .Lable=string.toint(rrec[0]),
-			  .State="UNASSIGNED",
-              .StateAge=0              
-		  }})
-	# All done
-	io.fclose(fd)
-	return gd
 }

buzz_scripts/include/shapes.bzz

@@ -0,0 +1,111 @@
+#Table of the nodes in the graph
+m_vecNodes={}
+m_vecNodes_fixed={}
+m_vecNodes[0] = {      # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
+    .Lable = 0,   # Lable of the point
+    .Pred = -1,   # Lable of its predecessor
+    .distance = -1, # distance to the predecessor
+    .bearing = -1, # bearing form the predecessor to this dot
+    .height = 3000, # height of this dot
+    .State="UNASSIGNED",
+    .StateAge=0
+}
+m_vecNodes[1] = {     
+    .Lable = 1,
+    .Pred = 0,
+    .distance = 1000,
+    .bearing = 0.0,
+    .height = 5000,
+    .State="UNASSIGNED",
+    .StateAge=0
+}
+m_vecNodes[2] = {
+    .Lable = 2,
+    .Pred = 0,
+    .distance = 1000,
+    .bearing = 1.57,
+    .height = 7000, 
+    .State="UNASSIGNED",
+    .StateAge=0
+}
+m_vecNodes[3] = {
+    .Lable = 3, 
+    .Pred = 0,
+    .distance = 1000,
+    .bearing = 3.14,
+    .height = 9000,
+    .State="UNASSIGNED",
+    .StateAge=0
+}
+m_vecNodes[4] = {
+    .Lable = 4,
+    .Pred = 0,
+    .distance = 1000,
+    .bearing = 4.71,
+    .height = 11000,
+    .State="UNASSIGNED",
+    .StateAge=0
+}
+
+#
+# Graph parsing
+#
+function parse_graph(fname) {
+	# Graph data
+	var gd = {}
+	# Open the file
+	var fd = io.fopen(fname, "r")
+	if(not fd) {
+		log("Can't open '", fname, "'")
+		return nil
+	}
+	# Parse the file line by line
+	var rrec # Record read from line
+	var grec # Record parsed into graph
+	io.fforeach(fd, function(line) {
+		  # Parse file line
+		  rrec = string.split(line, "\t ")
+		  # Make record
+		  gd[string.toint(rrec[0])] = {      # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
+			  .Lable = string.toint(rrec[0]),   # Lable of the point
+			  .Pred = string.toint(rrec[1]),   # Lable of its predecessor
+			  .distance = string.tofloat(rrec[2]), # distance to the predecessor
+			  .bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
+              .height = string.tofloat(rrec[4]), # height of this dot
+			  .State="UNASSIGNED",
+              .StateAge=0
+		  }})
+	# All done
+	io.fclose(fd)
+	return gd
+}
+
+function parse_graph_fixed(fname) {
+	# Graph data
+	var gd = {}
+	# Open the file
+	var fd = io.fopen(fname, "r")
+	if(not fd) {
+		log("Can't open '", fname, "'")
+		return nil
+	}
+	# Parse the file line by line
+	var rrec # Record read from line
+	var grec # Record parsed into graph
+	io.fforeach(fd, function(line) {
+		  # Parse file line
+		  rrec = string.split(line, "\t ")
+		  # Make record
+		  gd[string.toint(rrec[0])] = {      # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2
+			  .Pred1 = string.toint(rrec[1]),   # Pred 1 lable
+			  .Pred2 = string.toint(rrec[3]),   # Pred 2 lable
+			  .d1 = string.tofloat(rrec[2]), # Pred 1 distance
+			  .d2 = string.tofloat(rrec[4]), # Pred 2 distance
+			  .Lable=string.toint(rrec[0]),
+			  .State="UNASSIGNED",
+              .StateAge=0              
+		  }})
+	# All done
+	io.fclose(fd)
+	return gd
+}

include/buzzuav_closures.h

@@ -47,6 +47,11 @@ void rc_set_goto(double pos[]);
 void rc_call(int rc_cmd);
 /* sets the battery state */
 void set_battery(float voltage,float current,float remaining);
+void set_deque_full(bool state);
+void set_rssi(float value);
+void set_raw_packet_loss(float value);
+void set_filtered_packet_loss(float value);
+void set_api_rssi(float value);
 /* sets current position */
 void set_currentpos(double latitude, double longitude, double altitude);
 /*retuns the current go to position */
@@ -83,6 +88,10 @@ int buzzuav_gohome(buzzvm_t vm);
  * Updates battery information in Buzz
  */
 int buzzuav_update_battery(buzzvm_t vm);
+/*
+ * Updates xbee_status information in Buzz
+ */
+int buzzuav_update_xbee_status(buzzvm_t vm);
 /*
  * Updates current position in Buzz
  */

include/roscontroller.h

@@ -244,6 +244,8 @@ private:
   bool GetRawPacketLoss(const uint8_t short_id, float &result);
   bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
 
+  void get_xbee_status();
+
 };
 
 }

launch/rosbuzz-solo.launch

@@ -43,7 +43,7 @@
 	<!-- run rosbuzz -->
 	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<rosparam file="$(find rosbuzz)/launch/launch_config/solo.yaml"/>
-		<param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
+		<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/flock.bzz" />
 		<param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="buzzcmd" />
 		<param name="in_payload" value="inMavlink"/>
@@ -51,7 +51,7 @@
 		<param name="xbee_status_srv" value="xbee_status"/>
            	<param name="xbee_plugged" value="true"/>
                 <param name="name" value="solos1"/>
-		<param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
+		<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
 	</node>
 
 </launch>

src/buzz_utility.cpp

@@ -18,7 +18,7 @@ namespace buzz_utility{
 	static uint8_t*     BO_BUF          = 0;
 	static buzzdebug_t  DBG_INFO        = 0;
 	static uint32_t     MSG_SIZE        = 600;//250;   // Only 100 bytes of Buzz messages every step
-	static uint32_t     MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets 
+	static uint32_t     MAX_MSG_SIZE    = 10000; // 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;
@@ -78,7 +78,7 @@ namespace buzz_utility{
 
 	int buzzusers_add(int id, double latitude, double longitude, double altitude) {
 		if(VM->state != BUZZVM_STATE_READY) return VM->state;
-		// Get users "p" table 
+		// Get users "p" table
 		/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
 		buzzvm_gload(VM);
 		buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
@@ -94,8 +94,8 @@ namespace buzz_utility{
 			buzzvm_tput(VM);
 			buzzvm_push(VM, data);
 		}
-		// When we get here, the "data" table is on top of the stack 
+		// When we get here, the "data" table is on top of the stack
-		// Push user id 
+		// Push user id
 		buzzvm_pushi(VM, id);
 		// Create entry table
 		buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
@@ -182,6 +182,8 @@ namespace buzz_utility{
    			uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]);
 			/*Size is at first 2 bytes*/
    			uint16_t size=data[0]*sizeof(uint64_t);
+            uint16_t neigh_id = data[1];
+			ROS_WARN("NEIG ID %i",neigh_id);
    			delete[] data;
 			/*size and robot id read*/
 	   		size_t tot = sizeof(uint32_t);
@@ -195,6 +197,7 @@ namespace buzz_utility{
 			 			/* Append message to the Buzz input message queue */
 			 			if(unMsgSize > 0 && unMsgSize <= size - tot ) {
 			    			buzzinmsg_queue_append(VM,
+                                neigh_id,
 				                buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize));
 			    			tot += unMsgSize;
 			 			}
@@ -489,7 +492,7 @@ int create_stig_tables() {
       		ROS_ERROR("[%i] Error registering hooks", Robot_id);
       		return 0;
    	}
-	   
+
    	/* Create vstig tables
 	if(create_stig_tables() != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);
@@ -499,7 +502,7 @@ int create_stig_tables() {
 			//cout << "ERROR!!!!   ----------  " << buzzvm_strerror(VM) << endl;
       		return 0;
    	}*/
-	   
+
    	/* Save bytecode file name */
    	BO_FNAME = strdup(bo_filename);
 
@@ -561,7 +564,7 @@ int create_stig_tables() {
 			//cout << "ERROR!!!!   ----------  " << buzzvm_strerror(VM) << endl;
       		return 0;
    	}*/
-	   
+
    	// Execute the global part of the script
    	if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
 		ROS_ERROR("Error executing global part, VM state : %i",VM->state);
@@ -681,6 +684,7 @@ int create_stig_tables() {
    	void update_sensors(){
 		/* Update sensors*/
 		buzzuav_closures::buzzuav_update_battery(VM);
+    buzzuav_closures::buzzuav_update_xbee_status(VM);
 	   	buzzuav_closures::buzzuav_update_prox(VM);
 	   	buzzuav_closures::buzzuav_update_currentpos(VM);
 	   	buzzuav_closures::update_neighbors(VM);

src/buzzuav_closures.cpp

@@ -24,7 +24,12 @@ namespace buzzuav_closures{
 	static int rc_cmd=0;
 	static int buzz_cmd=0;
 	static float height=0;
-	
+  static bool deque_full = false;
+  static float rssi = 0.0;
+  static float raw_packet_loss = 0.0;
+  static float filtered_packet_loss = 0.0;
+  static float api_rssi = 0.0;
+
 	std::map< int,  buzz_utility::RB_struct> targets_map;
 	std::map< int,  buzz_utility::Pos_struct> neighbors_map;
 
@@ -95,7 +100,7 @@ namespace buzzuav_closures{
 			return x;
 		}
 
-	void rb_from_gps(double nei[], double out[], double cur[]){   
+	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;
@@ -148,7 +153,7 @@ namespace buzzuav_closures{
 		//buzzvm_dup(vm);
 		double rb[3], tmp[3];
 		map< int, buzz_utility::RB_struct >::iterator it;
-		for (it=targets_map.begin(); it!=targets_map.end(); ++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]);
@@ -334,7 +339,60 @@ namespace buzzuav_closures{
 	   buzzvm_gstore(vm);
 	   return vm->state;
 	}
-	/****************************************/
+
+  void set_deque_full(bool state)
+  {
+   deque_full = state;
+  }
+
+  void set_rssi(float value)
+  {
+   rssi = value;
+  }
+
+  void set_raw_packet_loss(float value)
+  {
+   raw_packet_loss = value;
+  }
+
+  void set_filtered_packet_loss(float value)
+  {
+   filtered_packet_loss = value;
+  }
+
+  void set_api_rssi(float value)
+  {
+   api_rssi = value;
+  }
+
+  int buzzuav_update_xbee_status(buzzvm_t vm) {
+     buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1));
+     buzzvm_pusht(vm);
+     buzzvm_dup(vm);
+     buzzvm_pushs(vm, buzzvm_string_register(vm, "deque_full", 1));
+     buzzvm_pushi(vm, static_cast<uint8_t>(deque_full));
+     buzzvm_tput(vm);
+	   buzzvm_dup(vm);
+     buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1));
+     buzzvm_pushf(vm, rssi);
+     buzzvm_tput(vm);
+	   buzzvm_dup(vm);
+     buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1));
+     buzzvm_pushf(vm, raw_packet_loss);
+     buzzvm_tput(vm);
+	   buzzvm_dup(vm);
+     buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1));
+     buzzvm_pushf(vm, filtered_packet_loss);
+     buzzvm_tput(vm);
+	   buzzvm_dup(vm);
+     buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1));
+     buzzvm_pushf(vm, api_rssi);
+     buzzvm_tput(vm);
+     buzzvm_gstore(vm);
+     return vm->state;
+  }
+
+	/***************************************/
 	/*current pos update*/
 	/***************************************/
 	void set_currentpos(double latitude, double longitude, double altitude){
@@ -409,7 +467,7 @@ namespace buzzuav_closures{
 	   rc_cmd=0;
 	   buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
 	   buzzvm_pushi(vm, status);
-	   buzzvm_tput(vm); 
+	   buzzvm_tput(vm);
 	   buzzvm_gstore(vm);
 	   //also set rc_controllers goto
 	   buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
@@ -434,7 +492,7 @@ namespace buzzuav_closures{
 
 	/******************************************************/
 	/*Create an obstacle Buzz table from proximity sensors*/
-	/*  Acessing proximity in buzz script 
+	/*  Acessing proximity in buzz script
 	    proximity[0].angle and proximity[0].value - front
 		""          ""          "" 	      - right and back
 	    proximity[3].angle and proximity[3].value - left

src/roscontroller.cpp

@@ -113,7 +113,7 @@ bool roscontroller::TriggerAPIRssi(const uint8_t short_id)
     srv_request.param_id = "trig_rssi_api_" + std::to_string(short_id);
   }
   mavros_msgs::ParamGet::Response srv_response;
-  if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
+  if(!xbeestatus_srv.call(srv_request, srv_response)){return false;}
 
   return srv_response.success;
 }
@@ -130,7 +130,7 @@ bool roscontroller::GetAPIRssi(const uint8_t short_id, float &result)
     srv_request.param_id = "get_rssi_api_" + std::to_string(short_id);
   }
   mavros_msgs::ParamGet::Response srv_response;
-  if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
+  if(!xbeestatus_srv.call(srv_request, srv_response)){return false;}
 
   result = srv_response.value.real;
   return srv_response.success;
@@ -148,7 +148,7 @@ bool roscontroller::GetRawPacketLoss(const uint8_t short_id, float &result)
     srv_request.param_id = "pl_raw_" + std::to_string(short_id);
   }
   mavros_msgs::ParamGet::Response srv_response;
-  if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
+  if(!xbeestatus_srv.call(srv_request, srv_response)){return false;}
 
   result = srv_response.value.real;
   return srv_response.success;
@@ -166,7 +166,7 @@ bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float &result)
     srv_request.param_id = "pl_filtered_" + std::to_string(short_id);
   }
   mavros_msgs::ParamGet::Response srv_response;
-  if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
+  if(!xbeestatus_srv.call(srv_request, srv_response)){return false;}
 
   result = srv_response.value.real;
   return srv_response.success;
@@ -231,6 +231,7 @@ void roscontroller::RosControllerRun()
       updates_set_robots(no_of_robots);
       // ROS_INFO("ROBOTS: %i , acutal :
       // %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
+      get_xbee_status();
       /*run once*/
       ros::spinOnce();
       /*loop rate of ros*/
@@ -1138,14 +1139,7 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
   }
   return true;
 }
-/*-----------------------------------------------------
-/Obtain robot id by subscribing to xbee robot id topic
-/ TODO: check for integrity of this subscriber call back
-/----------------------------------------------------*/
-/*void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
 
-
-}*/
 void roscontroller::get_number_of_robots() {
   int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1;
   if (no_of_robots == 0) {
@@ -1202,4 +1196,39 @@ void roscontroller::get_number_of_robots() {
   }
   */
 }
+
+void roscontroller::get_xbee_status()
+/* Description:
+ * Call all the xbee node services and update the xbee status
+ ------------------------------------------------------------------ */
+{
+  bool result_bool;
+  float result_float;
+  const uint8_t all_ids = 0xFF;
+  if(GetDequeFull(result_bool))
+  {
+    buzzuav_closures::set_deque_full(result_bool);
+  }
+  if(GetRssi(result_float))
+  {
+    buzzuav_closures::set_rssi(result_float);
+  }
+  if(GetRawPacketLoss(all_ids, result_float))
+  {
+    buzzuav_closures::set_raw_packet_loss(result_float);
+  }
+  if(GetFilteredPacketLoss(all_ids, result_float))
+  {
+    buzzuav_closures::set_filtered_packet_loss(result_float);
+  }
+  // This part needs testing since it can overload the xbee module
+  /*
+   * if(GetAPIRssi(all_ids, result_float))
+   * {
+   * buzzuav_closures::set_api_rssi(result_float);
+   * }
+   * TriggerAPIRssi(all_ids);
+   */
+}
+
 }