tweak graph control, fix obstacles and and rc_client fleet_status

buzz_scripts/graphform.bzz

@@ -8,14 +8,14 @@ include "barrier.bzz"	# don't use a stigmergy id=11 with this header.
 include "uavstates.bzz"	# require an 'action' function to be defined here.
 include "vstigenv.bzz"
 
-include "graphs/shapes_L.bzz"
+include "graphs/shapes_Y.bzz"
 
 ROBOT_RADIUS=50
 ROBOT_DIAMETER=2.0*ROBOT_RADIUS
 ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
 
 # max velocity in cm/step
-ROBOT_MAXVEL = 200.0
+ROBOT_MAXVEL = 250.0
 
 #
 # Global variables
@@ -91,8 +91,7 @@ step_cunt=0
 m_lockstig = 1
 
 # Lennard-Jones parameters, may need change
-EPSILON    = 13.5 #the LJ parameter for other robots
+EPSILON	= 4000 #13.5 the LJ parameter for other robots
-EPSILON_FOR1 = 10.0 #the LJ parameter for the robot labeled 1
 
 # Lennard-Jones interaction magnitude
 
@@ -307,6 +306,7 @@ function LJ_vec(i){
     	cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),bearing)   
     }
        #log(id,"dis=",dis,"target=",target,"label=",nei_id)
+		#log("x=",cDir.x,"y=",cDir.y)  
     return cDir
 }
 #calculate the motion vector
@@ -320,6 +320,7 @@ function motion_vector(){
 		i=i+1
     }
     m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
+	#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)    
     return m_vector
 }
 
@@ -532,8 +533,8 @@ function DoFree() {
 		tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
 		m_navigation=math.vec2.add(tempvec_P,tempvec_N)
 		# Limit the mvt 
-		if(math.vec2.length(m_navigation)>ROBOT_MAXVEL)
+		if(math.vec2.length(m_navigation)>ROBOT_MAXVEL*2)
-			m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL/math.vec2.length(m_navigation))
+			m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL*2/math.vec2.length(m_navigation))
 		uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
 	}else{ #no joined robots in sight
 		i=0
@@ -594,8 +595,11 @@ function DoAsking(){
 		}
 	}
 	else{
-	if(m_MessageReqID[psResponse]!=m_unRequestId)
+		if(m_MessageReqID[psResponse]!=m_unRequestId){
+			m_vecNodes[m_nLabel].State="ASSIGNING"
+			m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
 			TransitionToFree()
+    	}
 	if(m_MessageReqID[psResponse]==m_unRequestId){
 		if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
 			TransitionToJoining()
@@ -663,8 +667,8 @@ function DoJoining(){
 
 
 		#test if is already in desired position
-		if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
+		if(math.vec2.length(S2Target)<m_fTargetDistanceTolerance and S2Target_bearing<m_fTargetAngleTolerance){
-			log("LOCKING! ", math.vec2.length(S2Target), ", ", S2Target_bearing)
+			log("JOINED! ", math.vec2.length(S2Target), ", ", S2Target_bearing)
 			TransitionToJoined()
 			return
 		}
@@ -717,7 +721,7 @@ function DoJoined(){
 			}
 		}
 		#if it is the pred
-		if(m_MessageState[i]=="STATE_JOINED" and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
+		if((m_MessageState[i]=="STATE_JOINED" or m_MessageState[i]=="STATE_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
 			seenPred=1
 			m_unWaitCount=m_unJoiningLostPeriod
 		}
@@ -740,6 +744,8 @@ function DoJoined(){
 		m_selfMessage.ReqLabel=ReqLabel
 		m_selfMessage.ReqID=ReqID
 		m_selfMessage.Response=r2i("REQ_GRANTED")
+		m_vecNodes[ReqLabel].State="ASSIGNING"
+    	m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
 	}
 
 	#lost pred, wait for some time and transit to free
@@ -759,7 +765,7 @@ function DoJoined(){
 
 	#check if should to transists to lock
 	#write statues
-	#v_tag.put(m_nLabel, 1)
+	v_tag.get(m_nLabel)
 	log(v_tag.size(), " of ", ROBOTS, " ready to lock")
 	if(v_tag.size()==ROBOTS){
 		TransitionToLock()
@@ -776,9 +782,8 @@ m_navigation.x=0.0
 m_navigation.y=0.0
 #calculate motion vection
 if(m_nLabel==0){
-    m_navigation.x=0.0#change value so that robot 0 will move
+    m_navigation.x=0.25	#change value so that robot 0 will move
     m_navigation.y=0.0
-    log(";",m_nLabel,";",0)
 }
 if(m_nLabel!=0){
     m_navigation=motion_vector()
@@ -801,7 +806,8 @@ function init() {
 	#
 	m_unResponseTimeThreshold=10
 	m_unLabelSearchWaitTime=10
-	m_fTargetDistanceTolerance=1
+	m_fTargetDistanceTolerance=50
+	m_fTargetAngleTolerance=0.1
 	m_unJoiningLostPeriod=100
 
 	#
@@ -849,8 +855,8 @@ function step(){
 	debug(m_eState,m_nLabel)
 	log("Current state: ", UAVSTATE)
 	log("Swarm size: ", ROBOTS)
-	if(id==0)
+#	if(id==0)
-    	stattab_print()
+#    	stattab_print()
 	
 	#navigation
 	#broadcast messag

buzz_scripts/include/barrier.bzz

@@ -36,7 +36,7 @@ function barrier_wait(threshold, transf, resumef) {
   barrier.put(id, 1)
   UAVSTATE = "BARRIERWAIT"
   if(barrier.size() >= threshold) {
-    #barrier = nil
+#    getlowest()
     transf()
   } else if(timeW>=BARRIER_TIMEOUT) {
     barrier = nil
@@ -45,3 +45,18 @@ function barrier_wait(threshold, transf, resumef) {
   }
   timeW = timeW+1
 }
+
+# get the lowest id of the fleet, but requires too much bandwidth
+function getlowest(){
+  Lid = 20;
+  u=20
+  while(u>=0){
+				tab = barrier.get(u)
+				if(tab!=nil){
+          if(tab<Lid)
+            Lid=tab
+        }
+        u=u-1
+  }
+  log("--> LOWEST ID:",Lid)
+}

buzz_scripts/include/graphs/shapes_L.bzz

@@ -14,7 +14,7 @@ m_vecNodes[0] = {      # The .graph file is stored according the sequence of Lab
 m_vecNodes[1] = {     
     .Label = 1,
     .Pred = 0,
-    .distance = 2000,
+    .distance = 800,
     .bearing = 0.0,
     .height = 5000,
     .State="UNASSIGNED",
@@ -23,7 +23,7 @@ m_vecNodes[1] = {
 m_vecNodes[2] = {
     .Label = 2,
     .Pred = 0,
-    .distance = 2000,
+    .distance = 800,
     .bearing = 1.57,
     .height = 7000, 
     .State="UNASSIGNED",
@@ -32,7 +32,7 @@ m_vecNodes[2] = {
 m_vecNodes[3] = {
     .Label = 3, 
     .Pred = 1,
-    .distance = 2000,
+    .distance = 800,
     .bearing = 0.0,
     .height = 9000,
     .State="UNASSIGNED",
@@ -41,7 +41,7 @@ m_vecNodes[3] = {
 m_vecNodes[4] = {
     .Label = 4,
     .Pred = 2,
-    .distance = 2000,
+    .distance = 800,
     .bearing = 1.57,
     .height = 11000,
     .State="UNASSIGNED",
@@ -50,7 +50,7 @@ m_vecNodes[4] = {
 m_vecNodes[5] = {
     .Label = 5,
     .Pred = 4,
-    .distance = 2000,
+    .distance = 800,
     .bearing = 1.57,
     .height = 14000,
     .State="UNASSIGNED",

buzz_scripts/include/graphs/shapes_Y.bzz

@@ -14,7 +14,7 @@ m_vecNodes[0] = {      # The .graph file is stored according the sequence of Lab
 m_vecNodes[1] = {     
     .Label = 1,
     .Pred = 0,
-    .distance = 2000,
+    .distance = 1000,
     .bearing = 4.71,
     .height = 5000,
     .State="UNASSIGNED",
@@ -23,7 +23,7 @@ m_vecNodes[1] = {
 m_vecNodes[2] = {
     .Label = 2,
     .Pred = 0,
-    .distance = 1414,
+    .distance = 707,
     .bearing = 2.36,
     .height = 7000, 
     .State="UNASSIGNED",
@@ -32,7 +32,7 @@ m_vecNodes[2] = {
 m_vecNodes[3] = {
     .Label = 3, 
     .Pred = 0,
-    .distance = 1414,
+    .distance = 707,
     .bearing = 0.79,
     .height = 9000,
     .State="UNASSIGNED",
@@ -41,7 +41,7 @@ m_vecNodes[3] = {
 m_vecNodes[4] = {
     .Label = 4,
     .Pred = 2,
-    .distance = 1414,
+    .distance = 707,
     .bearing = 2.36,
     .height = 11000,
     .State="UNASSIGNED",
@@ -50,7 +50,7 @@ m_vecNodes[4] = {
 m_vecNodes[5] = {
     .Label = 5,
     .Pred = 3,
-    .distance = 1414,
+    .distance = 707,
     .bearing = 0.79,
     .height = 14000,
     .State="UNASSIGNED",

buzz_scripts/include/uavstates.bzz

@@ -103,6 +103,9 @@ function uav_rccmd() {
 		flight.rc_cmd=0
 		uav_disarm()
 		neighbors.broadcast("cmd", 401)
+	} else if (flight.rc_cmd==666){
+		flight.rc_cmd=0
+		stattab_send()
 	}
 }
 

buzz_scripts/include/vstigenv.bzz

@@ -87,3 +87,26 @@ function stattab_print() {
 		}
 	}
 }
+
+function stattab_send() {
+	if(v_status.size()>0) {
+		if(b_updating==0) {
+			u=0
+			while(u<8){
+				tab = v_status.get(u)
+				if(tab!=nil){
+					recv_value=tab
+					gps=(recv_value-recv_value%1000000)/1000000
+					recv_value=recv_value-gps*1000000
+					batt=(recv_value-recv_value%1000)/1000
+					recv_value=recv_value-batt*1000
+					xbee=(recv_value-recv_value%10)/10
+					recv_value=recv_value-xbee*10
+					fc=recv_value
+					add_neighborStatus(u,gps,batt,xbee,fc)
+				}
+				u=u+1
+			}
+		}
+	}
+}

include/buzz_utility.h

@@ -29,6 +29,14 @@ struct rb_struct
 };
 typedef struct rb_struct RB_struct;
 
+struct neiStatus
+{
+  uint gps_strenght = 0;
+  uint batt_lvl = 0;
+  uint xbee = 0;
+  uint flight_status = 0;
+}; typedef struct neiStatus neighbors_status ;
+
 
 uint16_t* u64_cvt_u16(uint64_t u64);
 

include/buzzuav_closures.h

@@ -5,9 +5,9 @@
 #include <stdio.h>
 #include "uav_utility.h"
 #include "mavros_msgs/CommandCode.h"
+#include "mavros_msgs/Mavlink.h"
 #include "ros/ros.h"
 #include "buzz_utility.h"
-//#include "roscontroller.h"
 
 	#define EARTH_RADIUS (double) 6371000.0
 	#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
@@ -61,6 +61,9 @@ void flight_status_update(uint8_t state);
 /* Update neighbors table */
 void neighbour_pos_callback(int id, float range, float bearing, float elevation);
 void update_neighbors(buzzvm_t vm);
+int buzzuav_addNeiStatus(buzzvm_t vm);
+mavros_msgs::Mavlink get_status();
+
 /*Flight status*/
 void set_obstacle_dist(float dist[]);
 

include/roscontroller.h

@@ -91,7 +91,7 @@ private:
   /*tmp to be corrected*/
   uint8_t no_cnt=0;
   uint8_t old_val=0	;
-  std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
+  std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,obstacles_topic,stand_by,xbeesrv_name, setpoint_name;
   std::string stream_client_name;
   std::string relative_altitude_sub_name;
   std::string setpoint_nonraw;
@@ -102,6 +102,7 @@ private:
   ros::ServiceClient mav_client;
   ros::ServiceClient xbeestatus_srv;
   ros::Publisher payload_pub;
+  ros::Publisher MPpayload_pub;
   ros::Publisher neigh_pos_pub;
   ros::Publisher localsetpoint_nonraw_pub;
   ros::ServiceServer service;
@@ -155,6 +156,8 @@ private:
   /*Neighbours pos publisher*/
   void neighbours_pos_publisher();
 
+  void send_MPpayload();
+
   /*Prepare messages and publish*/
   void prepare_msg_and_publish();
 

src/buzz_utility.cpp

@@ -325,6 +325,9 @@ void in_message_process(){
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_targetrb", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB));
    		buzzvm_gstore(VM);
+		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_neighborStatus", 1));
+   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus));
+   		buzzvm_gstore(VM);
 
    	return VM->state;
 	}
@@ -367,6 +370,9 @@ void in_message_process(){
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_targetrb", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
+		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_neighborStatus", 1));
+   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+   		buzzvm_gstore(VM);
 
    	return VM->state;
 	}

src/buzzuav_closures.cpp

@@ -26,12 +26,14 @@ namespace buzzuav_closures{
 	static float height=0;
   static bool deque_full = false;
   static int rssi = 0;
+  static int message_number = 0;
   static float raw_packet_loss = 0.0;
   static int filtered_packet_loss = 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;
+	std::map< int, buzz_utility::neighbors_status> neighbors_status_map;
 
 	/****************************************/
 	/****************************************/
@@ -218,6 +220,48 @@ namespace buzzuav_closures{
 		return 0;
 	}
 
+	int buzzuav_addNeiStatus(buzzvm_t vm){
+		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);
+		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;
+    	newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value;
+    	map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id);
+		if(it!=neighbors_status_map.end())
+			neighbors_status_map.erase(it);
+		neighbors_status_map.insert(make_pair(id, newRS));
+		return vm->state;
+	}
+
+	mavros_msgs::Mavlink get_status(){
+		mavros_msgs::Mavlink payload_out;
+    	map< int, buzz_utility::neighbors_status >::iterator it;
+		for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++it){
+			payload_out.payload64.push_back(it->first);
+			payload_out.payload64.push_back(it->second.gps_strenght);
+			payload_out.payload64.push_back(it->second.batt_lvl);
+			payload_out.payload64.push_back(it->second.xbee);
+			payload_out.payload64.push_back(it->second.flight_status);
+		}
+    	/*Add Robot id and message number to the published message*/
+		payload_out.sysid = (uint8_t)neighbors_status_map.size();
+		/*payload_out.msgid = (uint32_t)message_number;
+
+		message_number++;*/
+		return payload_out;
+	}
 	/*----------------------------------------/
 	/ Buzz closure to go directly to a GPS destination from the Mission Planner
 	/----------------------------------------*/

src/roscontroller.cpp

@@ -172,6 +172,10 @@ bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float &result)
   return srv_response.success;
 }
 
+void roscontroller::send_MPpayload(){
+		MPpayload_pub.publish(buzzuav_closures::get_status());
+}
+
 /*-------------------------------------------------
 /rosbuzz_node loop method executed once every step
 /--------------------------------------------------*/
@@ -193,6 +197,7 @@ void roscontroller::RosControllerRun()
       // buzz_closure::neighbour_pos_callback(neighbours_pos_map);
       /*Neighbours of the robot published with id in respective topic*/
       neighbours_pos_publisher();
+      send_MPpayload();
       /*Check updater state and step code*/
       update_routine(bcfname.c_str(), dbgfname.c_str());
       /*Step buzz script */
@@ -387,6 +392,8 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle)
     ROS_ERROR("Provide a localpos name in YAML file");
     system("rosnode kill rosbuzz_node");
   }
+
+  node_handle.getParam("obstacles", obstacles_topic);
 }
 
 /*--------------------------------------------------------
@@ -398,23 +405,18 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
 
   Subscribe(n_c);
 
-  // current_position_sub = n_c.subscribe("/global_position", 1000,
-  // &roscontroller::current_pos,this);
-  // battery_sub = n_c.subscribe("/power_status", 1000,
-  // &roscontroller::battery,this);
   payload_sub =
-      n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt, this);
+      n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
-  // flight_status_sub =n_c.subscribe("/flight_status",100,
+
-  // &roscontroller::flight_extended_status_update,this);
+  obstacle_sub = n_c.subscribe(obstacles_topic, 5,
-  // Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000,
-  // &roscontroller::set_robot_id,this);
-  obstacle_sub = n_c.subscribe("guidance/obstacle_distance", 100,
                                &roscontroller::obstacle_dist, this);
+
   /*publishers*/
-  payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
+  payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
-  neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 1000);
+  MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
+  neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5);
   localsetpoint_nonraw_pub =
-      n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 1000);
+      n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
   /* Service Clients*/
   arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
   mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
@@ -427,8 +429,8 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
   stream_client =
       n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
 
-  users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos, this);
+  users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this);
-  local_pos_sub = n_c.subscribe(local_pos_sub_name, 1000,
+  local_pos_sub = n_c.subscribe(local_pos_sub_name, 5,
                                 &roscontroller::local_pos_callback, this);
 
   multi_msg = true;
@@ -449,13 +451,13 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
           it->first, 100, &roscontroller::flight_status_update, this);
     } else if (it->second == "mavros_msgs/BatteryStatus") {
       battery_sub =
-          n_c.subscribe(it->first, 1000, &roscontroller::battery, this);
+          n_c.subscribe(it->first, 5, &roscontroller::battery, this);
     } else if (it->second == "sensor_msgs/NavSatFix") {
       current_position_sub =
-          n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
+          n_c.subscribe(it->first, 5, &roscontroller::current_pos, this);
     } else if (it->second == "std_msgs/Float64") {
       relative_altitude_sub =
-          n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this);
+          n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this);
     }
 
     std::cout << "Subscribed to: " << it->first << endl;
@@ -1125,14 +1127,17 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
     rc_goto[0] = req.param5;
     rc_goto[1] = req.param6;
     rc_goto[2] = req.param7;
-    // for test
-    // SetLocalPosition(rc_goto[0], rc_goto[1], rc_goto[2], 0);
-
     buzzuav_closures::rc_set_goto(rc_goto);
     rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
     buzzuav_closures::rc_call(rc_cmd);
     res.success = true;
     break;
+  case 666:
+    ROS_INFO("RC_Call: Update Fleet Status!!!!");
+    rc_cmd = 666;
+    buzzuav_closures::rc_call(rc_cmd);
+    res.success = true;
+    break;
   default:
     res.success = false;
     break;