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)
 }
 
 #
@@ -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()
 }
 
 #

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
@@ -55,3 +56,4 @@ m_vecNodes[5] = {
     .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
@@ -55,3 +56,4 @@ m_vecNodes[5] = {
     .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,
@@ -55,3 +55,4 @@ m_vecNodes[5] = {
     .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
@@ -55,3 +55,4 @@ m_vecNodes[5] = {
     .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

@@ -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;
 			 			}
@@ -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,6 +24,11 @@ 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;
@@ -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){

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);
+   */
+}
+
 }