added yaw control and states pursuit, agreggation and formation (still to tweak)
This commit is contained in:
dave
parent
fd7414f382
commit
8e6319e3a5
|
|
@ -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")
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
}
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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") {
|
||||
|
|
|
|||
|
|
@ -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]);
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue