more fixes to rrt, added Vivek fix to MSG size and fix crashes due to UAVSTATE variable

buzz_scripts/empty.bzz

@@ -0,0 +1,17 @@
+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/graphformGPS.bzz

@@ -237,7 +237,7 @@ function unpackmessage(recv_value){
 #unpack guide message
 #
 function unpack_guide_msg(recv_value){
-log(id,"I pass value=",recv_value)
+#log(id,"I pass value=",recv_value)
     var qian=(recv_value-recv_value%1000)/1000
     recv_value=recv_value-qian*1000
     var bai=(recv_value-recv_value%100)/100
@@ -497,10 +497,9 @@ function DoAsking(){
 		#the request Label be the same as requesed
 		#get a respond
 		if(m_MessageState[i]=="STATE_JOINED"){
-			log("received label = ",m_MessageReqLabel[i])
+			#log("received label = ",m_MessageReqLabel[i])
 			if(m_MessageReqLabel[i]==m_nLabel)
 				if(m_MessageResponse[i]!="REQ_NONE"){
-				log("get response")
 				psResponse=i
 		}}
 		if(m_MessageState[i]=="STATE_JOINING" and m_MessageLabel[i]==m_nLabel){
@@ -604,13 +603,12 @@ function DoJoined(){
 	while(i<m_neighbourCount){
 		if(m_MessageState[i]=="STATE_ASKING"){
 			ReqLabel=m_MessageReqLabel[i]
-			log("ReqLabel var:",ReqLabel)
+			#log("ReqLabel var:",ReqLabel)
-			log("M_vec var",m_vecNodes[ReqLabel].State)
+			#log("M_vec var",m_vecNodes[ReqLabel].State)
 			if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
 				if(m_nLabel==m_vecNodes[ReqLabel].Pred){
 					#is a request, store the index
 					mapRequests[j]=i
-					log("Into if m_nLabel")
 					j=j+1
 				}
 		}
@@ -678,8 +676,7 @@ function DoJoined(){
 	#if(v_tag.size()==ROBOTS){
 	#	TransitionToLock()
 	#}
-  #barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
+  barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
-  barrier_wait(6, TransitionToLock, DoJoined, -1)
 }
 #
 #Do Lock
@@ -732,7 +729,7 @@ function init() {
   #v_tag = stigmergy.create(m_lockstig)
   #uav_initstig()
 	# go to diff. height since no collision avoidance implemented yet
-	TARGET_ALTITUDE = 20.0 + id * 2.5
+	#TARGET_ALTITUDE = 20.0 + id * 2.5
 	statef=turnedoff
 	UAVSTATE = "TURNEDOFF"
 }

buzz_scripts/include/barrier.bzz

@@ -40,7 +40,7 @@ function barrier_set(threshold, transf, resumef, bdt) {
 # Make yourself ready
 #
 function barrier_ready() {
-  log("BARRIER READY -------")
+  #log("BARRIER READY -------")
   barrier.put(id, 1)
   barrier.put("d", 0)
 }
@@ -52,7 +52,7 @@ function barrier_wait(threshold, transf, resumef, bdt) {
   barrier.put(id, 1)
 
   barrier.get(id)
-  log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
+  #log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
   if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
     barrier.put("d", 1)
     timeW = 0

buzz_scripts/include/rrtstar.bzz

@@ -5,23 +5,25 @@
 #####
 include "mapmatrix.bzz"
 
-RRT_TIMEOUT = 200
+RRT_TIMEOUT = 500
 map = nil
 cur_cell = {}
 nei_cell = {}
 
-function UAVpathPlanner(m_navigation) {
+function UAVinit_map(m_navigation) {
     # create a map big enough for the goal
-    init_map(2*math.round(math.vec2.length(m_navigation))+2)
+    init_map(2*math.round(math.vec2.length(m_navigation))+10)
     # center the robot on the grid
-    cur_cell = math.vec2.new(math.round(len/2.0),math.round(len/2.0))
+    cur_cell = math.vec2.new(math.round(map.nb_col/2.0),math.round(map.nb_row/2.0))
+}
+
+function UAVpathPlanner(m_navigation, m_pos) {
+    # place the robot on the grid
+    update_curcell(m_pos,0)
     # create the goal in the map grid
     mapgoal = math.vec2.add(m_navigation,cur_cell)
-    # add all neighbors as obstacles in the grid
+    mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y))
-    neighbors.foreach(function(rid, data) {
+
-            add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
-            })
-    # TODO: add proximity sensor obstacles to the grid
     # search for a path
     return RRTSTAR(mapgoal,math.vec2.new(5,5))  #map.nb_col/20.0,map.nb_row/20.0))
 }
@@ -32,7 +34,7 @@ function Kh4pathPlanner(m_goal, m_pos) {
     m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y))
 
     # place the robot on the grid
-    update_curcell(m_pos)
+    update_curcell(m_pos,1)
     log("Starting from cell: ", cur_cell.x, " ", cur_cell.y)
     log("Going to cell: ", m_goal.x, " ", m_goal.y)
 
@@ -42,9 +44,14 @@ function Kh4pathPlanner(m_goal, m_pos) {
     return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y))
 }
 
-function update_curcell(m_curpos) {
+function update_curcell(m_curpos, kh4) {
-  cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
+  if(kh4) { # for khepera playground
-  cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y))
+    cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
+    cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y))
+  } else { # for uav map relative to home
+    cur_cell = math.vec2.add(cur_cell, m_curpos)
+    cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y))
+  }
   if(cur_cell.x>map.nb_row)
     cur_cell.x=map.nb_row
   if(cur_cell.y>map.nb_col)
@@ -55,9 +62,17 @@ function update_curcell(m_curpos) {
     cur_cell.y=1
 }
 
+function UAVgetneiobs (m_curpos) {
+    update_curcell(m_curpos,0)
+    # add all neighbors as obstacles in the grid
+    neighbors.foreach(function(rid, data) {
+            add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
+            })
+}
+
 function getneiobs (m_curpos) {
   foundobstacle = 0
-  update_curcell(m_curpos)
+  update_curcell(m_curpos,1)
   old_nei = table_copy(nei_cell)
   #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3])
 
@@ -103,7 +118,7 @@ function getneiobs (m_curpos) {
 
 function getproxobs (m_curpos) {
   foundobstacle = 0
-  update_curcell(m_curpos)
+  update_curcell(m_curpos,1)
 
   reduce(proximity,
     function(key, value, acc) {
@@ -141,17 +156,17 @@ function getproxobs (m_curpos) {
   #}
 
   return foundobstacle
-}
+}			
 
-function check_path(m_path, goal_l, m_curpos) {
+function check_path(m_path, goal_l, m_curpos, kh4) {
   pathisblocked = 0
   nb=goal_l
-  update_curcell(m_curpos)
+  update_curcell(m_curpos,kh4)
-  #m_curpos = math.vec2.sub(m_curpos,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
-  #Cvec = math.vec2.new(math.round(m_curpos.x*CM2KH4.x), math.round(m_curpos.y*CM2KH4.y))
   Cvec = cur_cell
   while(nb < m_path.nb_row and nb <= goal_l+1){
     Nvec = getvec(m_path, nb)
+    if(kh4==0)
+        Nvec=vec_from_gps(Nvec.x,Nvec.y)
     if(doesItIntersect(Cvec, Nvec)){
       log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
       pathisblocked = 1
@@ -293,7 +308,7 @@ function RRTSTAR(GOAL,TOL) {
     }
     if(goalReached){
         log("Goal found(",numberOfPoints,")!")
-        Path = getPath(Q,numberOfPoints)
+        Path = getPathGPS(Q,numberOfPoints)
         print_pos(Path)
     } else {
       log("FAILED TO FIND A PATH!!!")
@@ -395,8 +410,8 @@ function getPathGPS(Q,nb){
     while(nb!=1){
         npt=npt+1
         path.nb_row=npt
-        path.mat[npt*2]=rmat(Q,nb,1)
+        path.mat[(npt-1)*2]=rmat(Q,nb,1)
-        path.mat[npt*2+1]=rmat(Q,nb,2)
+        path.mat[(npt-1)*2+1]=rmat(Q,nb,2)
         nb = rmat(Q,nb,3);
     }
 
@@ -434,6 +449,6 @@ function getPath(Q,nb){
         pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.y
         npt = npt - 1
     }
-    log("Double-check path: ", check_path(pathR, 1, cur_cell))
+    #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1))
     return pathR
 }

buzz_scripts/include/uavstates.bzz

@@ -5,7 +5,7 @@
 ########################################
 include "rrtstar.bzz"
 
-TARGET_ALTITUDE = 5.0 # m.
+TARGET_ALTITUDE = 15.0 # m.
 UAVSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
 GOTO_MAXVEL = 2 # m/steps
@@ -14,6 +14,7 @@ GOTODIST_TOL = 0.5  # m.
 GOTOANG_TOL = 0.1 # rad.
 cur_goal_l = 0
 rc_State = 0
+homegps = {.lat=0, .long=0}
 
 function uav_initswarm() {
 	s = swarm.create(1)
@@ -35,8 +36,7 @@ function takeoff() {
 	statef=takeoff
 
 	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
-		#barrier_set(ROBOTS, action, land, -1)
+		barrier_set(ROBOTS, action, land, -1)
-		barrier_set(6, action, land, -1)
 		barrier_ready()
 	} else {
     log("Altitude: ", position.altitude)
@@ -53,8 +53,7 @@ function land() {
 	uav_land()
 	
 	if(flight.status != 2 and flight.status != 3) {
-    	#barrier_set(ROBOTS,turnedoff,land, 21)
+    	barrier_set(ROBOTS,turnedoff,land, 21)
-		barrier_set(6,turnedoff,land, 21)
 		barrier_ready()
 	}
 }
@@ -62,7 +61,7 @@ function land() {
 function set_goto(transf) {
 	UAVSTATE = "GOTOGPS"
 	statef=function() {
-    gotoWP(transf);
+    gotoWP(transf)
   }
 
 	if(rc_goto.id==id){
@@ -90,21 +89,48 @@ function picture() {
   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)
-	print(" has to move ", math.vec2.length(rc_goal))
+	homegps.lat = position.latitude
+	homegps.long = position.longitude
+	m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-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.")
 	else {
 		if(Path==nil){
-			Path = UAVpathPlanner(rc_goal)
+			# create the map
+			if(map==nil)
+				UAVinit_map(rc_goal)
+			# add proximity sensor and neighbor obstacles to the map
+			while(Path==nil) {
+				#getproxobs(m_pos)
+				UAVgetneiobs (m_pos)
+				Path = UAVpathPlanner(rc_goal, m_pos)
+			}
 			cur_goal_l = 1
 		} else if(cur_goal_l<=Path.nb_row) {
 			cur_gps=getvec(Path,cur_goal_l)	#x=latitude, y=longitude
 			cur_pt=vec_from_gps(cur_gps.x,cur_gps.y)
-			print(" first to ", cur_pt.x,cur_pt.y)
+			print(" heading to ", cur_pt.x,cur_pt.y)
 			if(math.vec2.length(cur_pt)>GOTODIST_TOL) {
+				m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1)
+				UAVgetneiobs(m_pos)
+				if(check_path(Path,cur_goal_l,m_pos,0)) {
+					uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude)
+					Path = nil
+					rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
+					while(Path == nil) {
+						#getproxobs(m_pos)
+						UAVgetneiobs (m_pos)
+						Path = UAVpathPlanner(rc_goal, m_pos)
+					}
+					cur_goal_l = 1
+				}
 				cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt))
 				uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude)
 			}

src/buzz_utility.cpp

@@ -17,7 +17,6 @@ namespace buzz_utility{
 	static char*        BO_FNAME        = 0;
 	static uint8_t*     BO_BUF          = 0;
 	static buzzdebug_t  DBG_INFO        = 0;
-	static uint32_t     MSG_SIZE        = 250;   // Only 250 bytes of Buzz messages every step (limited to Xbee frame size)
 	static uint32_t     MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets
 	static uint8_t 	    Robot_id        = 0;
 	static std::vector<uint8_t*> IN_MSG;
@@ -229,7 +228,7 @@ void in_message_process(){
 	      			buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
 	      			/* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */
 					//ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)));
-	      			if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MSG_SIZE) {
+	      			if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) {
 			 			buzzmsg_payload_destroy(&m);
 			 			break;
 	      			}
@@ -467,11 +466,9 @@ int create_stig_tables() {
 	   	if(VM) buzzvm_destroy(&VM);
 		Robot_id = robot_id;
 	   	VM = buzzvm_new((int)robot_id);
-	   	ROS_INFO(" Robot ID -1: %i" , robot_id);
 	   	/* Get rid of debug info */
 	   	if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
 	   	DBG_INFO = buzzdebug_new();
-	   	ROS_INFO(" Robot ID -2: %i" , robot_id);
 	   	/* Read bytecode and fill in data structure */
 	   	FILE* fd = fopen(bo_filename, "rb");
 	   	if(!fd) {
@@ -490,7 +487,6 @@ int create_stig_tables() {
       		return 0;
 	   	}
 	   	fclose(fd);
-	   	ROS_INFO(" Robot ID -3: %i" , robot_id);
 	   	/* Read debug information */
 	   	if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
 	      		buzzvm_destroy(&VM);
@@ -498,7 +494,6 @@ int create_stig_tables() {
 	      		perror(bdbg_filename);
       		return 0;
    	}
-	   	ROS_INFO(" Robot ID -4: %i" , robot_id);
    	/* Set byte code */
    	if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);
@@ -506,7 +501,6 @@ int create_stig_tables() {
       		ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename);
       		return 0;
    	}
-	   	ROS_INFO(" Robot ID -5: %i" , robot_id);
    	/* Register hook functions */
    	if(buzz_register_hooks() != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);
@@ -514,7 +508,11 @@ int create_stig_tables() {
       		ROS_ERROR("[%i] Error registering hooks", Robot_id);
       		return 0;
    	}
-	   	ROS_INFO(" Robot ID -6: %i" , robot_id);
+
+	// Initialize UAVSTATE variable
+	buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
+	buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
+	buzzvm_gstore(VM);
 
    	/* Create vstig tables
 	if(create_stig_tables() != BUZZVM_STATE_READY) {
@@ -578,6 +576,12 @@ int create_stig_tables() {
       		ROS_ERROR("[%i] Error registering hooks (update)", Robot_id);
         	return 0;
    	}
+
+	// Initialize UAVSTATE variable
+	buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
+	buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
+	buzzvm_gstore(VM);
+
    	/* Create vstig tables
 	if(create_stig_tables() != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);
@@ -634,6 +638,12 @@ int create_stig_tables() {
       		ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id);
         	return 0;
    	}
+
+	// Initialize UAVSTATE variable
+	buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
+	buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
+	buzzvm_gstore(VM);
+
    	/* Create vstig tables
 	if(create_stig_tables() != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);