Merge commit '77886ac037f585465533271a8a5cc1642b456b80' into ros_drones_ws

buzz_scripts/include/act/CA.bzz

@@ -1,10 +1,10 @@
-function LCA(vel_vec) {
+# Lightweight collision avoidance
+function LCA( vel_vec ) {
 	var safety_radius = 3.0
-	var default_velocity = 2.0
 	collide = 0
 
-	var k_v = 5 # velocity gain
-	var k_w = 10   # angular velocity gain
+	var k_v = 5   # x axis gain
+	var k_w = 5   # y axis gain
 
 	cart = neighbors.map(
 		function(rid, data) {
@@ -28,18 +28,24 @@ function LCA(vel_vec) {
 				return accum
 			}, {.distance= safety_radius * 2.0, .angle= 0.0})
 
+
 		d_i = result.distance
 		data_alpha_i = result.angle
 
-		penalty = math.exp(d_i - safety_radius)
-		if( math.cos(math.vec2.angle(vel_vec)) < 0.0){
-			penalty = math.exp(-(d_i - safety_radius))
+		delta = math.vec2.new( d_i * math.cos(data_alpha_i), d_i * math.sin(data_alpha_i) )
+
+		p = math.exp(-(d_i - safety_radius))
+		if ( math.cos( math.vec2.angle(vel_vec) - data_alpha_i ) < 0.0 ) {
+			p = math.exp(d_i - safety_radius)
 		}
 
-		V = math.vec2.length(vel_vec) - math.cos(math.vec2.angle(vel_vec)) * penalty * k_v
-		W = -k_w * penalty * math.sin( math.vec2.angle(vel_vec) )
+		V = -1 * (p / d_i) * k_v * delta.x
+		W = -1 * (p / d_i) * k_w * delta.y
+
+		Uavd = math.vec2.new( V, W )
+
+		return math.vec2.add( vel_vec, Uavd )
 
-		return math.vec2.newp(V, W)
 	}	else
 		return vel_vec
 }

buzz_scripts/include/act/naviguation.bzz

@@ -10,7 +10,6 @@ function goto_gps(transf) {
  	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
-		if(transf!=nil)
 			transf()
 	} else {
 		m_navigation = LimitSpeed(m_navigation, 1.0)

buzz_scripts/include/act/neighborcomm.bzz

@@ -110,20 +110,3 @@ function nei_cmd_listen() {
 				#}
     })
 }
-
-# broadcast GPS goals
-function bd_goal() {
-    neighbors.broadcast("goal", {.id=rc_goto.id, .la=rc_goto.latitude, .lo=rc_goto.longitude, .al=rc_goto.altitude})
-}
-
-# listens to neighbors broadcasting gps goals
-function nei_goal_listen() {
-    neighbors.listen("goal",
-			function(vid, value, rid) {
-                print("Got (", vid, ",", value.id, value.la, value.lo, ") #", rid)
-				if(value.id==id) {
-                    print("Got (", vid, ",", value, ") #", rid)
-                    storegoal(value.la, value.lo, pose.position.altitude)
-                }
-    })
-}

buzz_scripts/include/act/states.bzz

@@ -95,25 +95,46 @@ function stop() {
 	}
 }
 
-# State function for individual waypoint control
+# State functions for individual waypoint control
 firsttimeinwp = 1
+wpreached = 1
 function indiWP() {
 	if(firsttimeinwp) {
-		nei_goal_listen()
+		v_wp = stigmergy.create(8)
 		storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
 		firsttimeinwp = 0
 	}
 	BVMSTATE = "INDIWP"
 	if(rc_goto.id != -1) {
-		log(rc_goto.id)
 		if(rc_goto.id == id) {
+			wpreached = 0
 			storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
-		} else 
-			bd_goal()
+			return
+		} else {
+			v_wp.put(rc_goto.id,{.lat=rc_goto.latitude, .lon=rc_goto.longitude, .pro=0})
+			reset_rc()
+		}
 	}
 
-	goto_gps(nil)
+  wp = v_wp.get(id)
+	if(wp!=nil) {
+		#log(wp.pro,wpreached)
+		if(wp.pro == 0) {
+				wpreached = 0
+        storegoal(wp.lat, wp.lon, pose.position.altitude)
+				v_wp.put(id,{.lat=wp.lat, .lon=wp.lon, .pro=1})
+				return
+			}      
+	}
 
+	if(wpreached!=1)
+		goto_gps(indiWP_done)
+
+}
+
+function indiWP_done() {
+	BVMSTATE = "INDIWP"
+	wpreached = 1
 }
 
 # State function to take a picture from the camera server (developed by HS)
@@ -213,14 +234,62 @@ function formation() {
 # Custom state function for the developer to play with
 function cusfun(){
 	BVMSTATE="CUSFUN"
-	log("cusfun: yay!!!")
-	LOCAL_TARGET = math.vec2.new(5 ,0 )
-	local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
-    if(math.vec2.length(local_set_point) > GOTODIST_TOL){
-		local_set_point = LimitSpeed(local_set_point, 1.0)
-		goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
-    }
-    else{
-    	log("TARGET REACHED")
-    }
+	#log("cusfun: yay!!!")
+	#LOCAL_TARGET = math.vec2.new(5 ,0 )
+	#local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
+    #if(math.vec2.length(local_set_point) > GOTODIST_TOL){
+	#	local_set_point = LimitSpeed(local_set_point, 1.0)
+	#	goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
+    #}
+    #else{
+    #	log("TARGET REACHED")
+    #}
+
+	initialPoint = 0
+
+	# Get waypoint list
+	if (id == 1) {
+		waypoints = {
+			.0 = math.vec2.new(-5.0, -5.0),
+			.1 = math.vec2.new(5.0, 5.0)
+		}
+	} else if (id == 2 ) {
+		waypoints = {
+			.0 = math.vec2.new(5.0, 5.0),
+			.1 = math.vec2.new(-5.0, -5.0)
+		}
+	}
+
+	# Current waypoint
+	target_point = waypoints[point]
+
+	# Get drone position and transform to global frame - depends on lauch file
+	offset_x = 0.0
+	offset_y = 0.0
+	if (id == 1) {
+		offset_x = 5.0
+		offset_y = 5.0
+	} else if (id == 2 ) {
+		offset_x = -5.0
+		offset_y = -5.0
+	}
+	pos_x = pose.position.x + offset_x;
+	pos_y = pose.position.y + offset_y;
+
+	# Get a vector pointing to target and target distance
+	vector_to_target = math.vec2.sub(target_point, math.vec2.new(pos_x, pos_y))
+	distance_to_target = math.vec2.length(vector_to_target)
+
+	# Run LCA and increment waypoint
+	if(distance_to_target > 0.2){
+		vector_to_target = LCA(vector_to_target)
+		vector_to_target = LimitSpeed(vector_to_target, 0.5)
+		goto_abs(vector_to_target.x, vector_to_target.y, 0.0, 0.0)
+	} else {
+		if(point < size(waypoints) - 1){
+			point = point + 1
+		} else {
+			point = 0
+		}
+	}
 }

buzz_scripts/main.bzz

@@ -11,13 +11,13 @@ include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
 
-AUTO_LAUNCH_STATE = "BIDDING"
+AUTO_LAUNCH_STATE = "INDIWP"
 TARGET     = 9.0
 EPSILON    = 30.0
 ROOT_ID = 3
 graph_id = 3
 graph_loop = 0
-LAND_AFTER_BARRIER_EXPIRE = 0 # 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.
+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.
 
 #####
 # Vehicule type:

include/buzzuav_closures.h

@@ -46,6 +46,10 @@ int buzz_exportmap(buzzvm_t vm);
  * closure to take a picture
  */
 int buzzuav_takepicture(buzzvm_t vm);
+/*
+ * closure to reset RC input
+ */
+int buzzuav_resetrc(buzzvm_t vm);
 /*
  * Returns the current command from local variable
  */

src/buzz_utility.cpp

@@ -237,6 +237,9 @@ static int buzz_register_hooks()
   buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_exportmap));
   buzzvm_gstore(VM);
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "reset_rc", 1));
+  buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_resetrc));
+  buzzvm_gstore(VM);
 
   return VM->state;
 }

src/buzzuav_closures.cpp

@@ -333,6 +333,15 @@ mavros_msgs::Mavlink get_status()
   return payload_out;
 }
 
+int buzzuav_resetrc(buzzvm_t vm)
+/*
+/ Buzz closure to reset the RC input.
+/----------------------------------------*/
+{
+  rc_id = -1;
+  return buzzvm_ret0(vm);
+}
+
 int buzzuav_takepicture(buzzvm_t vm)
 /*
 / Buzz closure to take a picture here.