From 8e6319e3a53e64f7cb289291468b87c690036a38 Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 3 Mar 2018 19:41:54 -0500 Subject: [PATCH] added yaw control and states pursuit, agreggation and formation (still to tweak) --- buzz_scripts/include/act/states.bzz | 75 ++++++++++++++++++- buzz_scripts/include/plan/rrtstar.bzz | 4 +- .../include/taskallocate/graphformGPS.bzz | 2 +- buzz_scripts/main.bzz | 10 ++- src/buzzuav_closures.cpp | 18 +++-- src/roscontroller.cpp | 14 ++-- 6 files changed, 101 insertions(+), 22 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9dfc8fc..02d1fdc 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -66,7 +66,7 @@ function take_picture() { 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" + BVMSTATE="IDLE" pic_time=0 } pic_time=pic_time+1 @@ -79,11 +79,11 @@ function goto_gps(transf) { 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)) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } function follow() { @@ -94,13 +94,80 @@ function follow() { force=(0.05)*(tab.range)^4 attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) }) - goto_abs(attractor.x, attractor.y, 0.0) + goto_abs(attractor.x, attractor.y, 0.0, 0.0) } else { log("No target in local table!") BVMSTATE = "IDLE" } } +# converge to centroid +function aggregate() { + BVMSTATE="AGGREGATE" + centroid = neighbors.reduce(function(rid, data, centroid) { + centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth)) + return centroid + }, {.x=0, .y=0}) + if(neighbors.count() > 0) + math.vec2.scale(centroid, 1.0 / neighbors.count()) + if(math.vec2.length(centroid)>GOTO_MAXVEL) + centroid = math.vec2.scale(centroid, GOTO_MAXVEL/math.vec2.length(centroid)) + goto_abs(centroid.x, centroid.y, 0.0, 0.0) +} + +# follow one another +rotang = 0.0 +function pursuit() { + BVMSTATE="PURSUIT" + insight = 0 + leader = math.vec2.newp(0.0, 0.0) + var cmdbin = math.vec2.newp(0.0, 0.0) + neighbors.foreach(function(rid, data) { + if(data.distance < 11.0 and data.azimuth < 3.2 and data.azimuth > 2.8) { + insight = 1 + leader = math.vec2.newp(data.distance, data.azimuth) + } + }) + if(insight == 1) { + log("Leader in sight !") + #cmdbin = math.vec2.newp(lj_magnitude(math.vec2.length(leader), 3.0, 0.01), math.vec2.angle(leader)) + cmdbin = math.vec2.newp(2.0, math.vec2.angle(leader)) + } else { + rotang = rotang + math.pi/60 + cmdbin = math.vec2.newp(2.0, rotang) + } + goto_abs(cmdbin.x, cmdbin.y, 0.0, rotang) +} + +# Lennard-Jones interaction magnitude +TARGET = 8.0 +EPSILON = 0.000001 +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function formation() { + BVMSTATE="FORMATION" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + if(math.vec2.length(accum)>GOTO_MAXVEL*15) + accum = math.vec2.scale(accum, 15*GOTO_MAXVEL/math.vec2.length(accum)) + goto_abs(accum.x, accum.y, 0.0, 0.0) +} + function rc_cmd_listen() { if(flight.rc_cmd==22) { log("cmd 22") diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index bfdd065..54c69ab 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -53,7 +53,7 @@ function navigate() { 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) + goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude, 0.0) Path = nil if(V_TYPE == 0) cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) @@ -62,7 +62,7 @@ function navigate() { 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) + goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude, 0.0) } } else path_it = path_it + 1 diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index e35fe37..0252971 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -694,7 +694,7 @@ function DoLock(){ m_navigation=motion_vector() } #move - goto_abs(m_navigation.x, m_navigation.y, 0.0) + goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) BroadcastGraph() } # diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 1cfd45c..d898bb3 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "TASK_ALLOCATE" +AUTO_LAUNCH_STATE = "PURSUIT" ##### # Vehicule type: @@ -26,7 +26,7 @@ function init() { init_stig() init_swarm() - TARGET_ALTITUDE = 25.0 # m + TARGET_ALTITUDE = 10 + id*2.0 # m # start the swarm command listener nei_cmd_listen() @@ -54,6 +54,12 @@ function step() { statef=launch else if(BVMSTATE=="IDLE") statef=idle + else if(BVMSTATE=="AGGREGATE") + statef=aggregate + else if(BVMSTATE=="FORMATION") + statef=formation + else if(BVMSTATE=="PURSUIT") + statef=pursuit else if(BVMSTATE=="TASK_ALLOCATE") { # or bidding ? startGraph() } else if(BVMSTATE=="GRAPH_FREE") { diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 20e0456..b55f9eb 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -13,7 +13,7 @@ namespace buzzuav_closures { // TODO: Minimize the required global variables and put them in the header // static const rosbzz_node::roscontroller* roscontroller_ptr; -static double goto_pos[3]; +static double goto_pos[4]; static double rc_goto_pos[3]; static float rc_gimbal[4]; static float batt[3]; @@ -207,22 +207,26 @@ int buzz_exportmap(buzzvm_t vm) int buzzuav_moveto(buzzvm_t vm) /* -/ Buzz closure to move following a 2D vector +/ Buzz closure to move following a 3D vector + Yaw /----------------------------------------*/ { - buzzvm_lnum_assert(vm, 3); + buzzvm_lnum_assert(vm, 4); buzzvm_lload(vm, 1); // dx buzzvm_lload(vm, 2); // dy - buzzvm_lload(vm, 3); //* dheight + buzzvm_lload(vm, 3); // dheight + buzzvm_lload(vm, 4); // dyaw + buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - float dh = buzzvm_stack_at(vm, 1)->f.value; - float dy = buzzvm_stack_at(vm, 2)->f.value; - float dx = buzzvm_stack_at(vm, 3)->f.value; + float dyaw = buzzvm_stack_at(vm, 1)->f.value; + float dh = buzzvm_stack_at(vm, 2)->f.value; + float dy = buzzvm_stack_at(vm, 3)->f.value; + float dx = buzzvm_stack_at(vm, 4)->f.value; goto_pos[0] = dx; goto_pos[1] = dy; goto_pos[2] = height + dh; + goto_pos[3] = dyaw; // DEBUG // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], // goto_pos[1], goto_pos[2]); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cfd47c0..cf54154 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -711,7 +711,7 @@ script case buzzuav_closures::COMMAND_MOVETO: goto_pos = buzzuav_closures::getgoto(); - roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); + roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); break; case buzzuav_closures::COMMAND_GIMBAL: @@ -935,15 +935,17 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) moveMsg.header.frame_id = 1; // DEBUG - // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", cur_pos.x, cur_pos.y, x, y); + // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f, %.3f", cur_pos.x, cur_pos.y, x, y, yaw); moveMsg.pose.position.x = cur_pos.x + y; moveMsg.pose.position.y = cur_pos.y + x; moveMsg.pose.position.z = z; - moveMsg.pose.orientation.x = 0; - moveMsg.pose.orientation.y = 0; - moveMsg.pose.orientation.z = 0; - moveMsg.pose.orientation.w = 1; + tf::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + moveMsg.pose.orientation.x = q[0]; + moveMsg.pose.orientation.y = q[1]; + moveMsg.pose.orientation.z = q[2]; + moveMsg.pose.orientation.w = q[3]; // To prevent drifting from stable position, uncomment // if(fabs(x)>0.005 || fabs(y)>0.005) {