restructured bzz for path planning

buzz_scripts/include/act/barrier.bzz

@@ -28,9 +28,9 @@ function barrier_create() {
   barrier = stigmergy.create(BARRIER_VSTIG)
 }
 
-function barrier_set(threshold, transf, resumef, bdt) {
+function barrier_set(threshold, transf, resumef) {
   statef = function() {
-    barrier_wait(threshold, transf, resumef, bdt);
+    barrier_wait(threshold, transf, resumef);
   }
   BVMSTATE = "BARRIERWAIT"
   barrier_create()
@@ -48,7 +48,7 @@ function barrier_ready() {
 #
 # Executes the barrier
 #
-function barrier_wait(threshold, transf, resumef, bdt) {
+function barrier_wait(threshold, transf, resumef) {
   barrier.put(id, 1)
 
   barrier.get(id)
@@ -56,31 +56,13 @@ function barrier_wait(threshold, transf, resumef, bdt) {
   if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
     barrier.put("d", 1)
     timeW = 0
-    transf()
+    BVMSTATE = transf
   } else if(timeW >= BARRIER_TIMEOUT) {
     log("------> Barrier Timeout !!!!")
     barrier=nil
     timeW = 0
-    resumef()
+    BVMSTATE = resumef
   }
 
-  if(bdt!=-1)
-    neighbors.broadcast("cmd", bdt)
-
   timeW = timeW+1
 }
-
-# get the lowest id of the fleet, but requires too much bandwidth...
-function getlowest(){
-  Lid = 15;
-  u=15
-  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/act/states.bzz

@@ -0,0 +1,234 @@
+########################################
+#
+# FLIGHT-RELATED FUNCTIONS
+#
+########################################
+include "utils/vec2.bzz"
+include "plan/rrtstar.bzz"
+include "act/barrier.bzz"
+include "utils/conversions.bzz"
+
+TARGET_ALTITUDE = 15.0 # m.
+BVMSTATE = "TURNEDOFF"
+PICTURE_WAIT = 20 # steps
+GOTO_MAXVEL = 1.5 # m/steps
+GOTO_MAXDIST = 150 # m.
+GOTODIST_TOL = 0.5  # m.
+GOTOANG_TOL = 0.1 # rad.
+path_it = 0
+rc_State = 0
+pic_time = 0
+g_it = 0
+
+function turnedoff() {
+    BVMSTATE = "TURNEDOFF"
+}
+
+function idle() {
+    BVMSTATE = "IDLE"
+}
+
+function launch() {
+	BVMSTATE = "LAUNCH"
+	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
+		homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
+		if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+			barrier_set(ROBOTS, "NAVIGATE", "LAUNCH")
+			barrier_ready()
+		} else {
+			log("Altitude: ", pose.position.altitude)
+			neighbors.broadcast("cmd", 22)
+			uav_takeoff(TARGET_ALTITUDE)
+		}
+	} else {
+		barrier_set(ROBOTS, "NAVIGATE", "LAUNCH")
+		barrier_ready()
+	}
+}
+
+function stop() {
+    BVMSTATE = "STOP"
+	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+		if(flight.status != 2 and flight.status != 3) {
+			barrier_set(ROBOTS,"TURNEDOFF","STOP")
+			barrier_ready()
+		}
+	} else {
+			barrier_set(ROBOTS,"TURNEDOFF","STOP")
+			barrier_ready()
+	}	
+}
+
+function take_picture() {
+  BVMSTATE="PICTURE"
+  uav_setgimbal(0.0, 0.0, -90.0, 20.0)
+	if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
+	  uav_takepicture()
+  } else if(pic_time>=PICTURE_WAIT) { # wait for the picture
+  BVMSTATE="IDLE"
+    pic_time=0 
+  }
+  pic_time=pic_time+1
+}
+
+#
+# Work in progress....
+function navigate() {
+  BVMSTATE="NAVIGATE"
+	if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
+		uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
+		cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
+		m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
+	}
+
+	print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y)
+	if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) {
+		log("Sorry this is too far.")
+		return
+	}
+
+	# create the map
+	if(map==nil) {
+		dyn_init_map(cur_goal)
+		if(V_TYPE == 0) {
+			homegps.lat = pose.position.latitude
+			homegps.long = pose.position.longitude
+		}
+	}
+	
+	if(Path==nil) {			
+		# add proximity sensor and neighbor obstacles to the map
+		populateGrid(m_pos)
+		# start the planner
+		path_it = 1
+		pathPlanner(cur_goal, m_pos)
+	} else if(path_it <= size(Path)) {
+		var cur_path_pt = convert_pt(getvec(Path, path_it))
+		print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y)
+		if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) {
+			populateGrid(m_pos)
+			if(check_path(Path, path_it, m_pos, 0)) {
+        # stop
+				goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude)
+				Path = nil
+				if(V_TYPE == 0)
+					cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
+				# re-start the planner
+				path_it = 1
+				pathPlanner(cur_goal, m_pos)
+			} else {
+				cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt))
+				goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude)
+			}
+		} else
+			path_it = path_it + 1
+	} else {
+		Path = nil
+  		BVMSTATE="IDLE"
+	}
+}
+
+# function goto_direct(transf) {
+# 	m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
+# 	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.")
+#   else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
+# 		m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
+# 		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
+# 	} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
+# 		transf()
+# 	else
+# 		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
+# }
+
+function follow() {
+	if(size(targets)>0) {
+		BVMSTATE = "FOLLOW"
+		attractor=math.vec2.newp(0,0)
+		foreach(targets, function(id, tab) {
+			force=(0.05)*(tab.range)^4
+			attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
+		})
+		uav_moveto(attractor.x, attractor.y, 0.0)
+	} else {
+		log("No target in local table!")
+		BVMSTATE = "IDLE"
+	}
+}
+
+function rc_cmd_listen() {
+    if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		BVMSTATE = "LAUNCH"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		flight.rc_cmd=0
+		BVMSTATE = "STOP"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		BVMSTATE = "PATHPLAN"
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	} else if (flight.rc_cmd==666){
+		flight.rc_cmd=0
+		stattab_send()
+	} else if (flight.rc_cmd==900){
+		flight.rc_cmd=0
+		rc_State = 0
+		neighbors.broadcast("cmd", 900)
+	} else if (flight.rc_cmd==901){
+		flight.rc_cmd=0
+		rc_State = 1
+		neighbors.broadcast("cmd", 901)
+	} else if (flight.rc_cmd==902){
+		flight.rc_cmd=0
+		rc_State = 2
+		neighbors.broadcast("cmd", 902)
+	} else if (flight.rc_cmd==903){
+		flight.rc_cmd=0
+		rc_State = 3
+		neighbors.broadcast("cmd", 903)
+	}
+}
+
+function nei_cmd_listen() {
+    neighbors.listen("cmd",
+			function(vid, value, rid) {
+				print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
+				if(value==22 and BVMSTATE!="LAUNCH" and BVMSTATE!="BARRIERWAIT") {
+		      		BVMSTATE = "LAUNCH"
+				} else if(value==21 and BVMSTATE!="STOP" and BVMSTATE!="BARRIERWAIT") {
+		      		BVMSTATE = "STOP"
+				} else if(value==400 and BVMSTATE=="TURNEDOFF") {
+					uav_arm()
+				} else if(value==401 and BVMSTATE=="TURNEDOFF"){
+					uav_disarm()
+				} else if(value==900){
+					rc_State = 0
+				} else if(value==901){
+					rc_State = 1
+				} else if(value==902){
+					rc_State = 2
+				} else if(value==903){
+					rc_State = 3
+				} else if(value==16 and BVMSTATE=="IDLE"){
+				#  neighbors.listen("gt",function(vid, value, rid) {
+				#    print("Got (", vid, ",", value, ") from robot #", rid)
+				#    # if(gt.id == id) statef=goto
+				#  }) 
+				}
+    })
+}

buzz_scripts/include/plan/rrtstar.bzz

@@ -3,7 +3,7 @@
 #
 # map table-based matrix
 #####
-include "mapmatrix.bzz"
+include "plan/mapmatrix.bzz"
 
 RRT_TIMEOUT = 500
 RRT_RUNSTEP = 3
@@ -78,7 +78,7 @@ function getcell(m_curpos) {
 }
 
 function populateGrid(m_pos) {
-    getproxobs(m_pos)
+    # getproxobs(m_pos)
     getneiobs (m_pos)
     export_map(map)
 }
@@ -211,8 +211,7 @@ function check_path(m_path, goal_l, m_curpos, kh4) {
 ##
 function rrtstar() {
     # update state machine variables
-    statef = rrtstar
+    BVMSTATE = "PATHPLAN"
-    BVMSTATE = "RRTSTAR"
 
 
     step_timeout = 0
@@ -336,16 +335,14 @@ function rrtstar() {
 
     if(goalReached){
       log("Goal found(",numberOfPoints,")!")
-      Path = getPath(Q,numberOfPoints, 1)
+      Path = convert_path(getPath(Q,numberOfPoints))
       # done. resume the state machine
-      BVMSTATE = "GOTOGPS"
+      BVMSTATE = "NAVIGATE"
-      statef = old_statef
     } else if(timeout >= RRT_TIMEOUT) {
       log("FAILED TO FIND A PATH!!!")
       Path = nil
       # done. resume the state machine
-      BVMSTATE = "GOTOGPS"
+      BVMSTATE = "NAVIGATE"
-      statef = old_statef
     }
 }
 
@@ -437,7 +434,7 @@ function doesItIntersect(point,vector) {
 }
 
 # create a table with only the path's points in order
-function getPath(Q,nb,gps){
+function getPath(Q,nb){
     var pathL={}
     var npt=0
     # get the path from the point list
@@ -460,15 +457,9 @@ function getPath(Q,nb,gps){
     var totpt = npt + 1
     while(npt > 0){
         pathR[totpt-npt] = {}
-        if(gps) {
+        var tmpgoal = getvec(pathL,npt)
-            tmpgoal = gps_from_vec(math.vec2.sub(getvec(pathL,npt),cur_cell))
-            pathR[totpt-npt][1]=tmpgoal.latitude
-            pathR[totpt-npt][2]=tmpgoal.longitude
-        } else {
-            tmpgoal = getvec(path,npt)
         pathR[totpt-npt][1]=tmpgoal.x
         pathR[totpt-npt][2]=tmpgoal.y
-        }
         npt = npt - 1
     }
     #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1))

buzz_scripts/include/shapes.bzz

@@ -1,111 +0,0 @@
-#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
-}

buzz_scripts/include/uavstates.bzz

@@ -1,293 +0,0 @@
-########################################
-#
-# FLIGHT-RELATED FUNCTIONS
-#
-########################################
-include "vec2.bzz"
-include "rrtstar.bzz"
-
-TARGET_ALTITUDE = 15.0 # m.
-BVMSTATE = "TURNEDOFF"
-PICTURE_WAIT = 20 # steps
-GOTO_MAXVEL = 1.5 # m/steps
-GOTO_MAXDIST = 150 # m.
-GOTODIST_TOL = 0.5  # m.
-GOTOANG_TOL = 0.1 # rad.
-cur_goal_l = 0
-rc_State = 0
-
-function uav_initswarm() {
-	s = swarm.create(1)
-	s.join()
-}
-
-function turnedoff() {
-    statef=turnedoff
-    BVMSTATE = "TURNEDOFF"
-}
-
-function idle() {
-    statef=idle
-    BVMSTATE = "IDLE"
-}
-
-function takeoff() {
-	BVMSTATE = "TAKEOFF"
-	statef=takeoff
-	homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
-
-	if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
-		barrier_set(ROBOTS, action, land, -1)
-		barrier_ready()
-	} else {
-    log("Altitude: ", pose.position.altitude)
-		neighbors.broadcast("cmd", 22)
-		uav_takeoff(TARGET_ALTITUDE)
-  }
-}
-
-function land() {
-	BVMSTATE = "LAND"
-	statef=land
-
-	neighbors.broadcast("cmd", 21)
-	uav_land()
-	
-	if(flight.status != 2 and flight.status != 3) {
-    	barrier_set(ROBOTS,turnedoff,land, 21)
-		barrier_ready()
-	}
-}
-
-function set_goto(transf) {
-	BVMSTATE = "GOTOGPS"
-	statef=function() {
-    gotoWPRRT(transf)
-  }
-
-	if(rc_goto.id==id){
-		if(s!=nil){
-      if(s.in())
-        s.leave()
-    }
-	} else {
-		neighbors.broadcast("cmd", 16)
-		neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
-	}
-}
-
-ptime=0
-function picture() {
-  statef=picture
-  BVMSTATE="PICTURE"
-  uav_setgimbal(0.0, 0.0, -90.0, 20.0)
-	if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize
-	  uav_takepicture()
-  } else if(ptime>=PICTURE_WAIT) { # wait for the picture
-    statef=action
-    ptime=0 
-  }
-  ptime=ptime+1
-}
-
-#
-# still requires to be tuned, replaning takes too much time..
-# DS 23/11/2017
-function gotoWPRRT(transf) {
-	rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
-	m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
-	print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y)
-
-	if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) {
-		log("Sorry this is too far.")
-		return
-	}
-	# create the map
-	if(map==nil) {
-		dyn_init_map(rc_goal)
-		homegps.lat = pose.position.latitude
-		homegps.long = pose.position.longitude
-	}
-	
-	if(Path==nil) {			
-		# add proximity sensor and neighbor obstacles to the map
-		populateGrid(m_pos)
-		# start the planner
-		pathPlanner(rc_goal, m_pos)
-		cur_goal_l = 1
-	} else if(cur_goal_l <= size(Path)) {
-		var cur_goal_gps = getvec(Path, cur_goal_l)	#x=latitude, y=longitude
-		var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0)
-		print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y)
-		if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) {
-			populateGrid(m_pos)
-			if(check_path(Path, cur_goal_l, m_pos, 0)) {
-				uav_moveto(0.0, 0.0, rc_goto.altitude - pose.position.altitude)
-				Path = nil
-				rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
-				# re-start the planner
-				pathPlanner(rc_goal, m_pos)
-				cur_goal_l = 1
-			} else {
-				cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt))
-				uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - pose.position.altitude)
-			}
-		} else
-			cur_goal_l = cur_goal_l + 1
-	} else {
-		Path = nil
-		transf()
-	}
-}
-
-function gotoWP(transf) {
-	m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
-	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.")
-  else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
-		m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
-		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
-	} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
-		transf()
-	else
-		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
-}
-
-function follow() {
-	if(size(targets)>0) {
-		BVMSTATE = "FOLLOW"
-		statef=follow
-		attractor=math.vec2.newp(0,0)
-		foreach(targets, function(id, tab) {
-			force=(0.05)*(tab.range)^4
-			attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
-		})
-		uav_moveto(attractor.x, attractor.y, 0.0)
-	} else {
-		log("No target in local table!")
-		#statef=idle
-	}
-}
-
-function uav_rccmd() {
-    if(flight.rc_cmd==22) {
-		log("cmd 22")
-		flight.rc_cmd=0
-		statef = takeoff
-		BVMSTATE = "TAKEOFF"		
-		neighbors.broadcast("cmd", 22)
-	} else if(flight.rc_cmd==21) {
-		log("cmd 21")
-		log("To land")
-		flight.rc_cmd=0
-		statef = land
-		BVMSTATE = "LAND"
-		neighbors.broadcast("cmd", 21)
-	} else if(flight.rc_cmd==16) {
-		flight.rc_cmd=0
-		BVMSTATE = "GOTOGPS"
-		statef = goto
-	} else if(flight.rc_cmd==400) {
-		flight.rc_cmd=0
-		uav_arm()
-		neighbors.broadcast("cmd", 400)
-	} else if (flight.rc_cmd==401){
-		flight.rc_cmd=0
-		uav_disarm()
-		neighbors.broadcast("cmd", 401)
-	} else if (flight.rc_cmd==666){
-		flight.rc_cmd=0
-		stattab_send()
-	} else if (flight.rc_cmd==900){
-		flight.rc_cmd=0
-		rc_State = 0
-		neighbors.broadcast("cmd", 900)
-	} else if (flight.rc_cmd==901){
-		flight.rc_cmd=0
-		rc_State = 1
-		neighbors.broadcast("cmd", 901)
-	} else if (flight.rc_cmd==902){
-		flight.rc_cmd=0
-		rc_State = 2
-		neighbors.broadcast("cmd", 902)
-	} else if (flight.rc_cmd==903){
-		flight.rc_cmd=0
-		rc_State = 3
-		neighbors.broadcast("cmd", 903)
-	}
-}
-
-function uav_neicmd() {
-    neighbors.listen("cmd",
-			function(vid, value, rid) {
-				print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
-				if(value==22 and BVMSTATE!="TAKEOFF" and BVMSTATE!="BARRIERWAIT") {
-					statef=takeoff
-		      		BVMSTATE = "TAKEOFF"
-				} else if(value==21 and BVMSTATE!="LAND" and BVMSTATE!="BARRIERWAIT") {
-					statef=land
-		      		BVMSTATE = "LAND"
-				} else if(value==400 and BVMSTATE=="TURNEDOFF") {
-					uav_arm()
-				} else if(value==401 and BVMSTATE=="TURNEDOFF"){
-					uav_disarm()
-				} else if(value==900){
-					rc_State = 0
-				} else if(value==901){
-					rc_State = 1
-				} else if(value==902){
-					rc_State = 2
-				} else if(value==903){
-					rc_State = 3
-				} else if(value==16 and BVMSTATE=="IDLE"){
-				#  neighbors.listen("gt",function(vid, value, rid) {
-				#    print("Got (", vid, ",", value, ") from robot #", rid)
-				#    # if(gt.id == id) statef=goto
-				#  }) 
-				}
-    })
-}
-
-function LimitAngle(angle){
-	if(angle>2*math.pi)
-		return angle-2*math.pi
-	else if (angle<0)
-		return angle+2*math.pi
-	else
-		return angle
-}
-
-function vec_from_gps(lat, lon, home_ref) {
-  d_lon = lon - pose.position.longitude
-  d_lat = lat - pose.position.latitude
-  if(home_ref == 1) {
-	  d_lon = lon - homegps.long
-	  d_lat = lat - homegps.lat
-  }
-  ned_x = d_lat/180*math.pi * 6371000.0;
-  ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
-	#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
-	#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
-  return math.vec2.new(ned_x,ned_y)
-}
-
-function gps_from_vec(vec) {
-  Lgoal = {.latitude=0, .longitude=0}
-  Vrange = math.vec2.length(vec)
-  Vbearing = LimitAngle(math.atan(vec.y, vec.x))
-#  print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude)
-  latR = pose.position.latitude*math.pi/180.0;
-  lonR = pose.position.longitude*math.pi/180.0;
-  target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
-  target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
-  Lgoal.latitude = target_lat*180.0/math.pi;
-  Lgoal.longitude = target_lon*180.0/math.pi;
-  #d_lat = (vec.x / 6371000.0)*180.0/math.pi;
-  #goal.latitude = d_lat + pose.position.latitude;
-  #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
-  #goal.longitude = d_lon + pose.position.longitude;
-
-  return Lgoal
-}

buzz_scripts/include/utilities.bzz

@@ -1,152 +0,0 @@
-# Utilities 
-
-# Rads to degrees
-function rtod(r) {
-   return (r*(180.0/math.pi))
-}
-
-# Copy a table
-function table_deep_copy(new_t, old_t, depth) { 
-  	depth = depth - 1
-		if (old_t != nil) {
-  		foreach(old_t, function(key, value) {
-    		new_t[key] = value
-    		if(depth != 0) { 
-      		new_t[key] = {}
-      		table_deep_copy(new_t[key], value, depth)
-    		}
-  		})  
-		}
-}
-
-function table_copy(old_t, depth) {
-  	var t = {};
-  	table_deep_copy(t, old_t, depth);
-  	return t;
-}
-
-# Print the contents of a table
-function table_print(t, depth) {
-  	depth = depth - 1 	
-  	if (t != nil) {
-	  	foreach(t, function(key, value) {
-        	log(key, " -> ", value)
-			  	if(depth != 0) {
-      	  	table_print(t[key], depth)
-    	  	}
-      	})
-  	}
-}
-
-# Write a table as if it was a vector
-function write_vector(k, index, val) {
-		var key = string.tostring(index)
-		k[key] = val
-}
-
-# Read a table as if it was a vector
-function read_vector(k, index) {
-    var key = string.tostring(index)
-    if (k[key] == nil) {
-        return -1
-    } else {
-        return k[key]
-    }
-}
-
-# Write a table as if it was a matrix
-function write_matrix(k, row, col, val) {
-    var key = string.concat(string.tostring(row),"-",string.tostring(col))
-    k[key] = val
-}
-
-# Read a table as if it was a matrix
-function read_matrix(k, row, col) {
-    var key = string.concat(string.tostring(row),"-",string.tostring(col))
-    if (k[key] == nil) {
-        return -1
-    } else {
-        return k[key]
-    }
-}
-
-# Int to String 
-function itos(i) {
-    
-    log("Use 'string.tostring(OJB)' instead")
-    
-    if (i==0) { return "0" }
-    if (i==1) { return "1" }
-    if (i==2) { return "2" }
-    if (i==3) { return "3" }
-    if (i==4) { return "4" }
-    if (i==5) { return "5" }
-    if (i==6) { return "6" }
-    if (i==7) { return "7" }
-    if (i==8) { return "8" }
-    if (i==9) { return "9" }
-    
-    log("Function 'itos' out of bounds, returning the answer (42)")
-    return "42"
-}
-
-# String to Int
-function stoi(s) {
-   if (s=='0') { return 0 }
-   if (s=='1') { return 1 }
-   if (s=='2') { return 2 }
-   if (s=='3') { return 3 }
-   if (s=='4') { return 4 }
-   if (s=='5') { return 5 }
-   if (s=='6') { return 6 }
-   if (s=='7') { return 7 }
-   if (s=='8') { return 8 }
-   if (s=='9') { return 9 }
-   
-   log("Function 'stoi' out of bounds, returning the answer (42)")
-   return 42
-   
-}
-
-# Force angles in the (-pi,pi) interval
-function radians_interval(a) {
-    var temp = a
-    while ((temp>2.0*math.pi) or (temp<0.0)) {
-        if (temp > 2.0*math.pi) {
-            temp = temp - 2.0*math.pi
-        } else if (temp < 0.0){
-            temp = temp + 2.0*math.pi
-        }
-    }
-    if (temp > math.pi) {
-        temp = temp - 2.0*math.pi
-    }
-    return temp
-}
-
-############################################
-
-#base = {}
-
-#base.create = function() {
-#    return {
-#       .method = function(a,b) {
-#          return a + b
-#       }
-#    }
-#  }
-
-#x = base.create()
-#x.method(3,4) # returns 7
-
-#derived = {}
-
-#derived.create = function() {
-#   b = base.create()
-#   b.method = function(a,b) {
-#      return a * b
-#   }
-#}
-
-#x = derived.create()
-#x.method(3,4) # returns 12

buzz_scripts/include/utils/conversions.bzz

@@ -0,0 +1,63 @@
+function LimitAngle(angle){
+	if(angle>2*math.pi)
+		return angle-2*math.pi
+	else if (angle<0)
+		return angle+2*math.pi
+	else
+		return angle
+}
+
+# TODO: add other conversions....
+function convert_path(P) {
+    var pathR={}
+    if(V_TYPE == 0) {
+        var n = 1
+        while(n <= size(P)){
+          pathR[n] = {}
+          var tmpgoal = gps_from_vec(math.vec2.sub(getvec(P,n),cur_cell))
+          pathR[n][1]=tmpgoal.latitude
+          pathR[n][2]=tmpgoal.longitude
+          n = n + 1
+        }
+    }
+    return pathR
+}
+
+# TODO: add other conversions....
+function convert_pt(in) {
+    if(V_TYPE == 0)
+        return vec_from_gps(in.x, in.y, 0)
+}
+
+function vec_from_gps(lat, lon, home_ref) {
+  d_lon = lon - pose.position.longitude
+  d_lat = lat - pose.position.latitude
+  if(home_ref == 1) {
+	  d_lon = lon - homegps.long
+	  d_lat = lat - homegps.lat
+  }
+  ned_x = d_lat/180*math.pi * 6371000.0;
+  ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
+	#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
+	#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
+  return math.vec2.new(ned_x,ned_y)
+}
+
+function gps_from_vec(vec) {
+  Lgoal = {.latitude=0, .longitude=0}
+  Vrange = math.vec2.length(vec)
+  Vbearing = LimitAngle(math.atan(vec.y, vec.x))
+#  print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude)
+  latR = pose.position.latitude*math.pi/180.0;
+  lonR = pose.position.longitude*math.pi/180.0;
+  target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
+  target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
+  Lgoal.latitude = target_lat*180.0/math.pi;
+  Lgoal.longitude = target_lon*180.0/math.pi;
+  #d_lat = (vec.x / 6371000.0)*180.0/math.pi;
+  #goal.latitude = d_lat + pose.position.latitude;
+  #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
+  #goal.longitude = d_lon + pose.position.longitude;
+
+  return Lgoal
+}

buzz_scripts/include/vstigenv.bzz

@@ -19,7 +19,13 @@ var v_ground = {}
 b_updating = 0
 vstig_counter = WAIT4STEP
 
-function uav_initstig() {
+
+function init_swarm() {
+	s = swarm.create(1)
+	s.join()
+}
+
+function init_stig() {
 	v_status = stigmergy.create(STATUS_VSTIG)
 	v_ground = stigmergy.create(GROUND_VSTIG)
 }

buzz_scripts/main.bzz

@@ -0,0 +1,81 @@
+include "update.bzz"
+# don't use a stigmergy id=11 with this header, for barrier 
+# it requires an 'action' function to be defined here.
+include "act/states.bzz"
+include "vstigenv.bzz"
+
+#####
+# Vehicule type:
+# 0 -> outdoor flying vehicle
+# 1 -> indoor flying vehicle
+# 2 -> outdoor wheeled vehicle
+# 3 -> indoor wheeled vehicle
+V_TYPE = 0
+
+goal_list = {
+	.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
+}
+
+function action() {
+  statef=action
+  set_goto(idle)
+}
+
+# Executed once at init time.
+function init() {
+  	init_stig()
+	init_swarm()
+
+	TARGET_ALTITUDE = 25.0 # m
+
+	# start the swarm command listener
+	# nei_cmd_listen()
+
+	# Starting state
+	# BVMSTATE = "TURNEDOFF"
+	BVMSTATE = "LAUNCHED"
+}
+
+# Executed at each time step.
+function step() {
+	# listen to Remote Controller
+	rc_cmd_listen()
+
+	# update the vstig (status/net/batt/...)
+  	# uav_updatestig()
+
+	#
+	# graph state machine
+	#
+	if(BVMSTATE=="TURNEDOFF")
+		statef=turnedoff
+	else if(BVMSTATE=="STOP")	# ends on turnedoff
+		statef=stop
+	else if(BVMSTATE=="LAUNCHED") # ends on navigate
+		statef=launch
+	else if(BVMSTATE=="IDLE")
+		statef=idle
+	else if(BVMSTATE=="TASK_ALLOCATE")	#TODO: not tested in new structure
+		statef=makegraph	# or bidding
+	else if(BVMSTATE=="PATHPLAN")	# ends on navigate
+		statef=rrtstar
+	else if(BVMSTATE=="NAVIGATE")	# ends on idle
+		statef=navigate
+	else if(BVMSTATE == "FOLLOW")	#TODO: not tested in new structure
+		statef=follow
+	else if(BVMSTATE == "PICTURE")	#TODO: not tested in new structure
+		statef=take_picture
+
+	statef()
+
+	log("Current state: ", BVMSTATE)
+
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}

buzz_scripts/testRRT.bzz

@@ -1,45 +0,0 @@
-include "update.bzz"
-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"
-
-function action() {
-  statef=action
-  uav_storegoal(45.5088103899,-73.1540826153,2.5)
-  set_goto(idle)
-}
-
-# Executed once at init time.
-function init() {
-  	uav_initstig()
-	uav_initswarm()
-
-	TARGET_ALTITUDE = 2.5 # m
-	# statef=turnedoff
-	# BVMSTATE = "TURNEDOFF"
-	statef = takeoff
-	BVMSTATE = "TAKEOFF"
-}
-
-# Executed at each time step.
-function step() {
-	uav_rccmd()
-
-	statef()
-
-	log("Current state: ", BVMSTATE)
-	# log("Obstacles: ")
-	# reduce(proximity,
-    # function(key, value, acc) {
-	# 	log(key, " - ", value.angle, value.value)
-	# 	return acc
-	# }, math.vec2.new(0, 0))
-}
- 
-# Executed once when the robot (or the simulator) is reset.
-function reset() {
-}
- 
-# Executed once at the end of experiment.
-function destroy() {
-}

src/buzz_utility.cpp

@@ -201,7 +201,7 @@ static int buzz_register_hooks()
   buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
   buzzvm_gstore(VM);
   buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
@@ -255,7 +255,7 @@ static int testing_buzz_register_hooks()
   buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
   buzzvm_gstore(VM);
   buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));