Yolo bounding box integration into the BVM.

buzz_scripts/include/act/naviguation.bzz

@@ -1,4 +1,4 @@
-GOTO_MAXVEL = 2.5 # m/steps
+GOTO_MAXVEL = 12.5 # m/steps
 GOTO_MAXDIST = 250 # m.
 GOTODIST_TOL = 0.4  # m.
 GOTOANG_TOL = 0.1 # rad.
@@ -41,3 +41,20 @@ function LimitSpeed(vel_vec, factor){
   	vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec))
 	return vel_vec
 }
+
+# Core naviguation function to travel to provided GPS target location.
+function goto_gps_in(transf, gps_to_go) {
+	m_navigation = vec_from_gps(gps_to_go.latitude, gps_to_go.longitude, 0)
+	print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
+ 	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
+		log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
+	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
+			;
+			# transf()
+	} else {
+		#log("move to x", m_navigation.x," y ", m_navigation.y)
+		m_navigation = LimitSpeed(m_navigation, 1.0)
+		#log("after limit speed move to x", m_navigation.x," y ", m_navigation.y)
+		goto_abs(m_navigation.x, m_navigation.y, gps_to_go.altitude - pose.position.altitude, 0.0)
+	}
+}

buzz_scripts/include/act/neighborcomm.bzz

@@ -126,6 +126,11 @@ function nei_cmd_listen() {
 							#barrier_ready(904)
 							#neighbors.broadcast("cmd", 904)
 						}
+						else if(value==555 and BVMSTATE=="YOLO_DEMO"){
+							YOLO_NEI_TARGET = 1
+						} else if(value==556 and BVMSTATE=="YOLO_DEMO"){
+							YOLO_NEI_TARGET = 2
+						}
 					}
 				}
     })

buzz_scripts/include/act/states.bzz

@@ -22,6 +22,16 @@ pic_time = 0
 g_it = 0
 homegps = {}
 
+YOLO_TARGET = 1
+YOLO_NEI_TARGET = 0
+yolo_init = 0
+yolo_left_buff = 0
+yolo_right_buff = 0
+YOLO_L_GOAL_1 = math.vec2.new(20,0,0)
+YOLO_L_GOAL_2 = math.vec2.new(-20,0,0)
+YOLO_GPS_1 = {.lat =0, .long=0}
+YOLO_GPS_2 = {.lat =0, .long=0}
+
 # Core state function when on the ground
 function turnedoff() {
     BVMSTATE = "TURNEDOFF"
@@ -32,6 +42,89 @@ function idle() {
     BVMSTATE = "IDLE"
 }
 
+# yolo Demostration
+function yolo_demo(){
+    var left_count = 0
+    var right_count = 0
+    var img_bound_x={.l=0,.r=320} # assumed left image pixel x boundary
+    var img_bound_y={.t=0,.b=480} # assumed left image pixel y boundary
+    if(yolo_init == 0){
+        yolo_init = 1
+        yolo_gps_in =  {.latitude=pose.position.latitude, .longitude=pose.position.longitude, .altitude= pose.position.altitude}
+        yolo_left_buff = 0
+        yolo_right_buff = 0
+         YOLO_GPS_1 = gps_from_vec(YOLO_L_GOAL_1)
+         YOLO_GPS_2 = gps_from_vec(YOLO_L_GOAL_2)
+    }
+    #log("POS lat : ",pose.position.latitude, " long: ", pose.position.longitude )
+    if(yolo_boxes != nil){
+      # log("yolo tab size",size(yolo_boxes))
+      var yol_i = 0
+      while(yol_i < yolo_boxes.size){
+        var table_id = string.tostring(yol_i)
+        if(yolo_boxes[table_id].class == "person"){
+            var diff_x = (yolo_boxes[table_id].xmax - yolo_boxes[table_id].xmin) / 2
+            var mid_x = yolo_boxes[table_id].xmax - diff_x
+            if( (mid_x >=img_bound_x.l and mid_x <=img_bound_x.r)){
+                # person is in the left side of the image
+                left_count = left_count + 1
+            }
+            else{
+                # person is in the right side of the image
+                right_count = right_count + 1
+            }
+        }
+        yol_i = yol_i + 1
+      }
+        # log("left_count: ", left_count, " right_count: ", right_count) 
+        if(left_count > right_count){
+            if(1){
+                YOLO_TARGET = 1
+                neighbors.broadcast("cmd", 555)
+                #storegoal(YOLO_GPS_1.latitude, YOLO_GPS_1.longitude, pose.position.altitude)
+                yolo_gps_in = {.latitude=YOLO_GPS_1.latitude, .longitude=YOLO_GPS_1.longitude, .altitude= pose.position.altitude}
+                # log("left dominating")
+                yolo_left_buff = 0
+                yolo_right_buff = 0
+            }
+            else{
+                yolo_left_buff = yolo_left_buff + 1
+                yolo_right_buff = 0
+            }
+        }
+        else if(left_count < right_count){
+            if(1){
+                YOLO_TARGET = 2
+                neighbors.broadcast("cmd", 556)
+                yolo_gps_in = {.latitude=YOLO_GPS_2.latitude, .longitude=YOLO_GPS_2.longitude, .altitude= pose.position.altitude}
+                # log("right dominating")
+                yolo_right_buff = 0
+                yolo_left_buff = 0
+            }
+            else{
+                yolo_right_buff = yolo_right_buff + 1
+                yolo_left_buff = 0
+            }
+        }
+        goto_gps_in(yolo_demo, yolo_gps_in)
+    }
+    if( YOLO_NEI_TARGET == 1){
+        # YOLO_NEI_TARGET =0
+        YOLO_TARGET = 1
+        yolo_gps_in = {.latitude=YOLO_GPS_1.latitude, .longitude=YOLO_GPS_1.longitude, .altitude= pose.position.altitude}
+        # log("left dominating")
+        goto_gps_in(yolo_demo, yolo_gps_in)
+    }
+    else if(YOLO_NEI_TARGET == 2){
+    	YOLO_TARGET = 2
+        # YOLO_NEI_TARGET = 0
+        yolo_gps_in = {.latitude=YOLO_GPS_2.latitude, .longitude=YOLO_GPS_2.longitude, .altitude= pose.position.altitude}
+        # log("right dominating")
+        goto_gps_in(yolo_demo, yolo_gps_in)
+    }
+}
+
+
 # Core state function to launch the robot: takeoff and wait for others, or stop (land)
 function launch() {
 	BVMSTATE = "LAUNCH"

buzz_scripts/main.bzz

@@ -12,7 +12,7 @@ include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
 
-AUTO_LAUNCH_STATE = "IDLE"
+AUTO_LAUNCH_STATE = "YOLO_DEMO"
 LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.
 
 #####
@@ -39,7 +39,7 @@ function init() {
 	nei_cmd_listen()
 
     # Starting state: TURNEDOFF to wait for user input.
-	BVMSTATE = "TURNEDOFF"
+	BVMSTATE = "LAUNCH"
 }
 
 # Executed at each time step.
@@ -58,6 +58,8 @@ function step() {
 		statef=turnedoff
 	else if(BVMSTATE=="CUSFUN")
 		statef=cusfun
+	else if(BVMSTATE == "YOLO_DEMO")
+    	statef=yolo_demo
 	else if(BVMSTATE=="STOP")	# ends on turnedoff
 		statef=stop
 	else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
@@ -107,6 +109,7 @@ function step() {
 
 	log("Current state: ", BVMSTATE)
 
+
 }
  
 # Executed once when the robot (or the simulator) is reset.

include/buzzuav_closures.h

@@ -14,6 +14,22 @@
 
 namespace buzzuav_closures
 {
+struct bounding_box_struct
+{
+  std::string obj_class;
+  double probability;
+  int64_t xmin;
+  int64_t ymin;
+  int64_t xmax;
+  int64_t ymax;
+  bounding_box_struct(std::string c, double prob, int64_t xmi, int64_t ymi, int64_t xma, int64_t yma)
+    : obj_class(c), probability(prob), xmin(xmi), ymin(ymi), xmax(xma), ymax(yma){};
+  bounding_box_struct()
+  {
+  }
+};
+typedef struct bounding_box_struct bounding_box;
+
 /*
  * prextern int() function in Buzz
  * This function is used to print data from buzz
@@ -76,6 +92,14 @@ void rc_call(int rc_cmd);
  * sets the battery state
  */
 void set_battery(float voltage, float current, float remaining);
+/*
+ * Update yolo boxes into buzz
+ */
+int buzzuav_update_yolo_boxes(buzzvm_t vm);
+/*
+ *Stores the received bounding boxes
+ */
+void store_bounding_boxes(std::vector<bounding_box> bbox);
 /*
  * sets the xbee network status
  */

include/roscontroller.h

@@ -147,6 +147,7 @@ private:
   std::string bcfname, dbgfname;
   std::string stand_by;
   std::string capture_srv_name;
+  std::string yolobox_sub_name;
 
   // ROS service, publishers and subscribers
   ros::ServiceClient mav_client;
@@ -171,6 +172,7 @@ private:
   ros::Subscriber Robot_id_sub;
   ros::Subscriber relative_altitude_sub;
   ros::Subscriber local_pos_sub;
+  ros::Subscriber yolo_sub;
 
   std::map<std::string, std::string> m_smTopic_infos;
 
@@ -257,6 +259,9 @@ private:
   /*payload callback callback*/
   void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
 
+  /*yolo bounding box callback function*/
+  void yolo_box_process(const mavros_msgs::Mavlink::ConstPtr& msg);
+
   /* RC commands service */
   bool rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res);
 

launch/launch_config/topics.yaml

@@ -14,6 +14,7 @@ topics:
   npose: neighbours_pos
   fstatus: fleet_status
   targetf: targets_found
+  yolobox: intermediator_ros/bounding_boxes
 services:
   fcclient:   cmd/command
   armclient:  cmd/arming

src/buzz_utility.cpp

@@ -483,6 +483,7 @@ void update_sensors()
   buzzuav_closures::buzzuav_update_currentpos(VM);
   buzzuav_closures::update_neighbors(VM);
   buzzuav_closures::buzzuav_update_flight_status(VM);
+  buzzuav_closures::buzzuav_update_yolo_boxes(VM);
 }
 
 void buzz_script_step()

src/buzzuav_closures.cpp

@@ -38,6 +38,7 @@ static float raw_packet_loss = 0.0;
 static int filtered_packet_loss = 0;
 static float api_rssi = 0.0;
 static bool logVoronoi = false;
+static std::vector<bounding_box> yolo_boxes;
 
 std::ofstream voronoicsv;
 
@@ -1170,6 +1171,59 @@ int buzzuav_update_battery(buzzvm_t vm)
   return vm->state;
 }
 
+void store_bounding_boxes(std::vector<bounding_box> bbox){
+  yolo_boxes.clear();
+  for(int i = 0; i< bbox.size(); i++){
+    yolo_boxes.push_back(bbox[i]);
+  }
+}
+
+int buzzuav_update_yolo_boxes(buzzvm_t vm){
+  if(yolo_boxes.size()>0){  
+    buzzvm_pushs(vm, buzzvm_string_register(vm, "yolo_boxes", 1));
+    buzzvm_pusht(vm);
+    buzzvm_dup(vm);
+    buzzvm_pushs(vm, buzzvm_string_register(vm, "size", 1));
+    buzzvm_pushf(vm, yolo_boxes.size());
+    buzzvm_tput(vm);
+    
+    for(int i=0; i< yolo_boxes.size(); i++){
+      buzzvm_dup(vm);
+      std::string index = std::to_string(i);
+      buzzvm_pushs(vm, buzzvm_string_register(vm, index.c_str(), 1));
+      buzzvm_pusht(vm);
+      buzzvm_dup(vm);
+      buzzvm_pushs(vm, buzzvm_string_register(vm, "class", 1));
+      buzzvm_pushs(vm, buzzvm_string_register(vm, yolo_boxes[i].obj_class.c_str(), 1));
+      buzzvm_tput(vm);
+      buzzvm_dup(vm);
+      buzzvm_pushs(vm, buzzvm_string_register(vm, "probability", 1));
+      buzzvm_pushf(vm, yolo_boxes[i].probability);
+      buzzvm_tput(vm);
+      buzzvm_dup(vm);
+      buzzvm_pushs(vm, buzzvm_string_register(vm, "xmin", 1));
+      buzzvm_pushf(vm, yolo_boxes[i].xmin);
+      buzzvm_tput(vm);
+      buzzvm_dup(vm);
+      buzzvm_pushs(vm, buzzvm_string_register(vm, "xmax", 1));
+      buzzvm_pushf(vm, yolo_boxes[i].xmax);
+      buzzvm_tput(vm);
+      buzzvm_dup(vm);
+      buzzvm_pushs(vm, buzzvm_string_register(vm, "ymin", 1));
+      buzzvm_pushf(vm, yolo_boxes[i].ymin);
+      buzzvm_tput(vm);
+      buzzvm_dup(vm);
+      buzzvm_pushs(vm, buzzvm_string_register(vm, "ymax", 1));
+      buzzvm_pushf(vm, yolo_boxes[i].ymax);
+      buzzvm_tput(vm);
+      buzzvm_tput(vm);
+    }
+    buzzvm_gstore(vm);
+    yolo_boxes.clear();
+  }
+  return vm->state;
+}
+
 /*
 / Set of function to update interface variable of xbee network status
 ----------------------------------------------------------------------*/

src/roscontroller.cpp

@@ -365,6 +365,8 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
 
   node_handle.getParam("topics/inpayload", topic);
   m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs::Mavlink"));
+
+  node_handle.getParam("topics/yolobox", yolobox_sub_name);
 }
 
 void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handle)
@@ -433,7 +435,7 @@ void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handl
     system("rosnode kill rosbuzz_node");
   }
   if (node_handle.getParam("topics/fstatus", topic))
-    MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>(topic, 5);
+    MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>(topic, 1);
   else
   {
     ROS_ERROR("Provide a fleet status out topic name in YAML file");
@@ -447,7 +449,7 @@ void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handl
     system("rosnode kill rosbuzz_node");
   }
   if (node_handle.getParam("topics/npose", topic))
-    neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>(topic, 5);
+    neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>(topic, 1);
   else
   {
     ROS_ERROR("Provide a Neighbor pose out topic name in YAML file");
@@ -477,7 +479,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c, ros::NodeHandle& n_
   // Subscribers
 
   Subscribe(n_c);
-
+  yolo_sub = n_c.subscribe(yolobox_sub_name, 1, &roscontroller::yolo_box_process, this);
   // Publishers and service Clients
 
   PubandServ(n_c, n_c_priv);
@@ -1217,12 +1219,9 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
 /----------------------------------------------------------------------------------------
 / Message format of payload (Each slot is uint64_t)
 / _________________________________________________________________________________________________
-/|	|     |	    |						     |
+/|	   |     |	   |						    |        |             |        |                              |
- * |
+/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size.........  |
-/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs
+/|_____|_____|_____|________________|________|_____________|________|______________________________|
- * with size.........  |
-/|_____|_____|_____|________________________________________________|______________________________|
-
 -----------------------------------------------------------------------------------------------------*/
 {
   // decode msg header
@@ -1302,6 +1301,54 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
   }
 }
 
+void roscontroller::yolo_box_process(const mavros_msgs::Mavlink::ConstPtr& msg){
+  uint64_t size = (msg->payload64.size()*sizeof(uint64_t));
+  uint8_t message_obt[size];
+  size=0;
+  //  Go throught the obtained payload
+  for (int i = 0; i < (int)msg->payload64.size(); i++)
+  {
+    uint64_t msg_tmp = msg->payload64[i];
+    memcpy((void*)(message_obt+size), (void*)(&msg_tmp), sizeof(uint64_t));
+    size +=sizeof(uint64_t);
+  }
+
+  std::vector<buzzuav_closures::bounding_box> box;
+  int tot = 0;
+  size = 0; 
+  memcpy((void*)&size, (void*)(message_obt+tot), sizeof(uint64_t));
+  tot += sizeof(uint64_t);
+  for(int i=0; i< size; i++){
+    double probability=0;
+    int64_t xmin=0;
+    int64_t ymin=0;
+    int64_t xmax=0;
+    int64_t ymax=0;
+    memcpy((void*)&probability, (void*)(message_obt+tot), sizeof(uint64_t));
+    tot += sizeof(uint64_t);
+    memcpy((void*)&xmin, (void*)(message_obt+tot), sizeof(uint64_t));
+    tot += sizeof(uint64_t);
+    memcpy((void*)&ymin, (void*)(message_obt+tot), sizeof(uint64_t));
+    tot += sizeof(uint64_t);
+    memcpy((void*)&xmax, (void*)(message_obt+tot), sizeof(uint64_t));
+    tot += sizeof(uint64_t);
+    memcpy((void*)&ymax, (void*)(message_obt+tot), sizeof(uint64_t));
+    tot += sizeof(uint64_t);
+    uint16_t str_size=0;
+    memcpy((void*)&str_size, (void*)(message_obt+tot), sizeof(uint16_t));
+    tot += sizeof(uint16_t);
+    char char_class[str_size * sizeof(char) + 1]; 
+    memcpy((void*)char_class, (void*)(message_obt+tot), str_size);
+    /* Set the termination character */
+    char_class[str_size] = '\0';
+    tot += str_size;
+    string obj_class(char_class);
+    buzzuav_closures::bounding_box cur_box(obj_class,probability,xmin,ymin,xmax,ymax);
+    box.push_back(cur_box);
+  }
+  buzzuav_closures::store_bounding_boxes(box);
+}
+
 bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res)
 /*
 / Catch the ROS service call from a custom remote controller (Mission Planner)