added minimal ROSBuzz example script

buzz_scripts/empty.bzz

@@ -1,17 +0,0 @@
-include "update.bzz"
-
-# Executed once at init time.
-function init() {
-}
-
-# Executed at each time step.
-function step() {
-}
- 
-# Executed once when the robot (or the simulator) is reset.
-function reset() {
-}
- 
-# Executed once at the end of experiment.
-function destroy() {
-}

buzz_scripts/include/act/states.bzz

@@ -4,7 +4,6 @@
 #
 ########################################
 include "utils/vec2.bzz"
-include "plan/rrtstar.bzz"
 include "act/barrier.bzz"
 include "utils/conversions.bzz"
 
@@ -33,7 +32,7 @@ function 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_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
 			barrier_ready()
 		} else {
 			log("Altitude: ", pose.position.altitude)
@@ -41,7 +40,7 @@ function launch() {
 			uav_takeoff(TARGET_ALTITUDE)
 		}
 	} else {
-		barrier_set(ROBOTS, "NAVIGATE", "LAUNCH")
+		barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
 		barrier_ready()
 	}
 }
@@ -73,78 +72,20 @@ function take_picture() {
   pic_time=pic_time+1
 }
 
-#
+function goto_gps(transf) {
-# Work in progress....
+	m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
-function navigate() {
+	print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
-  BVMSTATE="NAVIGATE"
+ 	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
-	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
+  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)
-	# create the map
+	} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
-	if(map==nil) {
+		transf()
-		dyn_init_map(cur_goal)
+	else
-		if(V_TYPE == 0) {
+		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
-			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"
@@ -208,9 +149,9 @@ 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") {
+				if(value==22 and BVMSTATE!="BARRIERWAIT") {
 		      		BVMSTATE = "LAUNCH"
-				} else if(value==21 and BVMSTATE!="STOP" and BVMSTATE!="BARRIERWAIT") {
+				} else if(value==21 and BVMSTATE!="BARRIERWAIT") {
 		      		BVMSTATE = "STOP"
 				} else if(value==400 and BVMSTATE=="TURNEDOFF") {
 					uav_arm()

buzz_scripts/include/plan/rrtstar.bzz

@@ -14,6 +14,64 @@ cur_cell = {}
 nei_cell = {}
 mapgoal = {}
 
+
+#
+# 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"
+	}
+}
+
 # create a map with its length to fit the goal (rel. pos.) and the current position as the center
 # TODO: test with khepera/argos
 function dyn_init_map(m_navigation) {

buzz_scripts/main.bzz

@@ -2,8 +2,12 @@ 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 "plan/rrtstar.bzz"
 include "vstigenv.bzz"
 
+#State launched after takeoff
+AUTO_LAUNCH_STATE = "IDLE"
+
 #####
 # Vehicule type:
 # 0 -> outdoor flying vehicle
@@ -16,11 +20,6 @@ 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()
@@ -31,7 +30,7 @@ function init() {
 	# start the swarm command listener
 	# nei_cmd_listen()
 
-	# Starting state
+    # Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup.
 	# BVMSTATE = "TURNEDOFF"
 	BVMSTATE = "LAUNCHED"
 }
@@ -51,15 +50,15 @@ function step() {
 		statef=turnedoff
 	else if(BVMSTATE=="STOP")	# ends on turnedoff
 		statef=stop
-	else if(BVMSTATE=="LAUNCHED") # ends on navigate
+	else if(BVMSTATE=="LAUNCHED") # ends on AUTO_LAUNCH_STATE
 		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
+	else if(BVMSTATE=="PATHPLAN")	# ends on navigate, defined in rrtstar
 		statef=rrtstar
-	else if(BVMSTATE=="NAVIGATE")	# ends on idle
+	else if(BVMSTATE=="NAVIGATE")	# ends on idle, defined in rrtstar
 		statef=navigate
 	else if(BVMSTATE == "FOLLOW")	#TODO: not tested in new structure
 		statef=follow

buzz_scripts/minimal.bzz

@@ -0,0 +1,64 @@
+include "update.bzz"
+include "act/states.bzz"
+include "vstigenv.bzz"
+
+# State launched after takeoff
+AUTO_LAUNCH_STATE = "ACTION"
+
+#####
+# Vehicule type:
+# 0 -> outdoor flying vehicle
+# 1 -> indoor flying vehicle
+# 2 -> outdoor wheeled vehicle
+# 3 -> indoor wheeled vehicle
+V_TYPE = 0
+
+# Executed once at init time.
+function init() {
+	init_swarm()
+
+	TARGET_ALTITUDE = 25.0 # m
+
+	# start the swarm command listener
+	nei_cmd_listen()
+    
+    # Starting state: TURNEDOFF to wait for user input.
+	BVMSTATE = "TURNEDOFF"
+}
+
+function action() {
+    BVMSTATE = "ACTION"
+    # do some actions....
+}
+
+# Executed at each time step.
+function step() {
+    # listen to Remote Controller
+	rc_cmd_listen()
+    
+    #
+	# State machine
+	#
+	if(BVMSTATE=="TURNEDOFF")
+		statef=turnedoff
+	else if(BVMSTATE=="STOP")	# ends on turnedoff
+		statef=stop
+	else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
+		statef=launch
+	else if(BVMSTATE=="IDLE")
+		statef=idle
+	else if(BVMSTATE=="ACTION")
+		statef=action
+
+	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() {
+}