tuned user tracking

This commit is contained in:
dave 2017-05-23 23:32:25 -04:00
parent 833c2b30c3
commit 0810963359
5 changed files with 484 additions and 21 deletions

View File

@ -38,7 +38,7 @@
#define XBEE_MESSAGE_CONSTANT 586782343 #define XBEE_MESSAGE_CONSTANT 586782343
#define XBEE_STOP_TRANSMISSION 4355356352 #define XBEE_STOP_TRANSMISSION 4355356352
#define TIMEOUT 60 #define TIMEOUT 60
#define BUZZRATE 50 #define BUZZRATE 10
using namespace std; using namespace std;

430
script/tentacle.bzz Normal file
View File

@ -0,0 +1,430 @@
include "vec2.bzz"
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 5.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 10.0
#################################################
### UTILITY FUNCTIONS ###########################
#################################################
# Write a table as if it was a matrix
function write_knowledge(k, row, col, val) {
var key = string.concat(string.tostring(row),"-",string.tostring(col))
k[key] = val
log("Writing knowledge:", val, " to ", row, " ", col)
}
# Read a table as if it was a matrix
function read_knowledge(k, row, col) {
var key = string.concat(string.tostring(row),"-",string.tostring(col))
if (k[key] == nil) {
log("Warning: reading 'nil' value from the knowledge table at", row, " ", col, ", returning -1")
return -1
} else {
return k[key]
}
}
# Int to String
function itos(i) {
log("Use 'string.tostring(OJB)' instead")
if (i==0) { return "0" }
if (i==1) { return "1" }
if (i==2) { return "2" }
if (i==3) { return "3" }
if (i==4) { return "4" }
if (i==5) { return "5" }
if (i==6) { return "6" }
if (i==7) { return "7" }
if (i==8) { return "8" }
if (i==9) { return "9" }
log("Function 'itos' out of bounds, returning the answer (42)")
return "42"
}
# String to Int
function stoi(s) {
if (s=='0') { return 0 }
if (s=='1') { return 1 }
if (s=='2') { return 2 }
if (s=='3') { return 3 }
if (s=='4') { return 4 }
if (s=='5') { return 5 }
if (s=='6') { return 6 }
if (s=='7') { return 7 }
if (s=='8') { return 8 }
if (s=='9') { return 9 }
log("Function 'stoi' out of bounds, returning the answer (42)")
return 42
}
# Rads to degrees
function rtod(r) {
return (r*(180.0/math.pi))
}
# Degrees to rads
function dtor(d) {
return (math.pi*(d/180.0))
}
# Force angles in the (-180,180) interval
function degrees_interval(a) {
var temp = a
while ((temp>360.0) or (temp<0.0)) {
if (temp > 360.0) {
temp = temp - 360.0
} else if (temp < 0.0){
temp = temp + 360.0
}
}
if (temp > 180.0) {
temp = temp - 360.0
}
return temp
}
# Force angles in the (-pi,pi) interval
function radians_interval(a) {
var temp = a
while ((temp>2.0*math.pi) or (temp<0.0)) {
if (temp > 2.0*math.pi) {
temp = temp - 2.0*math.pi
} else if (temp < 0.0){
temp = temp + 2.0*math.pi
}
}
if (temp > math.pi) {
temp = temp - 2.0*math.pi
}
return temp
}
#################################################
### MOVEMENT/COMMUNICATION PRIMITIVES ###########
#################################################
# Lennard-Jones interaction magnitude
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 hexagon() {
# 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())
# Move according to vector
moveto(accum.x, accum.y)
}
function inform_your_neighborhood() {
# Reset to 0 the visibility of all neighbors
foreach(knowledge, function(key, value) {
column = string.sub(key, string.length(key)-1,string.length(key))
if (column=='3') {
knowledge[key] = 0
}
})
neighbors.foreach( function(rid, data) {
# For each neighbor, send a message with its azimuth, as seen by the broadcasting robot
message_id = string.tostring(rid)
neighbors.broadcast(message_id, rtod(data.azimuth))
# Record the neighbor azimuth in my own knowledge table
write_knowledge(knowledge, rid, 0, rtod(data.azimuth))
# Record the neighbor distance in my own knowledge table
write_knowledge(knowledge, rid, 2, data.distance)
# Set neighbor as visible
write_knowledge(knowledge, rid, 3, 1)
})
# Send a message with the desired direction, as seen by the broadcasting robot
neighbors.broadcast("direction", local_dir)
}
function listen_to_your_neighborhood() {
# For all "senders" in my neighborhood, record my azimuth, as seen by them
message_id = string.tostring(id)
neighbors.listen(message_id, function(vid, value, rid) {
write_knowledge(knowledge, rid, 1, value)
})
}
#################################################
### ACTUAL CONTROLLERS ##########################
#################################################
function zero() {
# Do not move
moveto(0.0,0.0)
# Tell the neighbors of the center where to go
inform_your_neighborhood()
}
function onetwo() {
if (id == 1) {
angle = 45
} else {
angle = -135
}
# Broadcast information
inform_your_neighborhood()
# If the center 0 is in sight
if (read_knowledge(knowledge, 0, 3) == 1) {
arm_offset = degrees_interval(read_knowledge(knowledge, 0, 1) - angle)
if (arm_offset<3 and arm_offset>(-3)) {
hexagon() # Underlying Lennard-Jones potential behavior
} else {
local_rotation = degrees_interval( read_knowledge(knowledge, 0, 1) + (180.0 - read_knowledge(knowledge, 0, 0)) )
local_arm = degrees_interval(angle - local_rotation)
if (read_knowledge(knowledge, 0, 2) > 250.0) {
x_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
} else if (read_knowledge(knowledge, 0, 2) < 30.0) {
x_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
} else {
spiraling = 2.0+(id/10.0) # Fun stuff but be careful with this, it affects how a robots turns around a central node, use random number generation, eventually
if (arm_offset > 0) { # Clockwise
x_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
} else { # Counterclockwise
x_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
y_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
}
}
speed = 100
moveto(speed * x_mov,speed * y_mov)
}
} else {
hexagon()
}
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
barrier.put(id, 1)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
#barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
#log("TakeOff: ", flight.status)
#log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
if(id==0)
barrier_set(ROBOTS, zero)
else
barrier_set(ROBOTS, onetwo)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
#log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
barrier_set(ROBOTS,idle)
barrier_ready()
timeW=0
#barrier = nil
#statef=idle
}
}
function users_save(t) {
if(size(t)>0) {
foreach(t, function(id, tab) {
#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
add_user_rb(id,tab.la,tab.lo)
})
}
}
# printing the contents of a table: a custom function
function table_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}
#################################################
### BUZZ FUNCTIONS ##############################
#################################################
# Executed at init time
function init() {
s = swarm.create(0)
s.join()
# Local knowledge table
knowledge = {}
# Update local knowledge with information from the neighbors
listen_to_your_neighborhood()
# Variables initialization
iteration = 0
vt = stigmergy.create(5)
t = {}
vt.put("p",t)
statef=idle
CURSTATE = "IDLE"
}
# Executed every time step
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
#uav_goto()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
# Read a value from the structure
# log(users)
#users_print(users.dataG)
if(size(users.dataG)>0)
vt.put("p", users.dataG)
# Get the number of keys in the structure
#log("The vstig has ", vt.size(), " elements")
users_save(vt.get("p"))
#table_print(users.dataL)
# Count the number of steps
iteration = iteration + 1
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Execute at exit
function destroy() {
}

View File

@ -16,11 +16,12 @@ CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters # Lennard-Jones parameters
TARGET = 12.0 TARGET = 12.0
EPSILON = 0.5 EPSILON = 14.0
# Lennard-Jones interaction magnitude # Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) { function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
#return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
} }
# Neighbor data to LJ interaction vector # Neighbor data to LJ interaction vector
@ -32,25 +33,51 @@ function lj_vector(rid, data) {
function lj_sum(rid, data, accum) { function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum) return math.vec2.add(data, accum)
} }
function user_attract(t) {
fus = math.vec2.new(0.0, 0.0)
if(size(t)>0) {
foreach(t, function(u, tab) {
#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, TARGET / 2.0, EPSILON * 2.0), tab.b))
})
math.vec2.scale(fus, 1.0 / size(t))
}
#print("User attract:", fus.x," ", fus.y, " [", size(t), "]")
return fus
}
# Calculates and actuates the flocking interaction # Calculates and actuates the flocking interaction
function hexagon() { function hexagon() {
statef=hexagon statef=hexagon
CURSTATE = "HEXAGON"
# Calculate accumulator # Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0) if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count()) accum = math.vec2.scale(accum, 1.0 / neighbors.count())
accum = math.vec2.add(accum, user_attract(users.dataL))
accum = math.vec2.scale(accum, 1.0 / 2.0)
if(math.vec2.length(accum) > 0.75) {
accum = math.vec2.scale(accum, 0.75 / math.vec2.length(accum))
}
# Move according to vector # Move according to vector
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle) print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x,accum.y) uav_moveto(accum.x, accum.y)
CURSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0 # timeW =0
# statef=land # statef=land
# } else { # } else if(timeW>=WAIT_TIMEOUT/2) {
# CURSTATE ="GOEAST"
# timeW = timeW+1 # timeW = timeW+1
# uav_moveto(0.0,0.0) # uav_moveto(0.0,5.0)
# } else {
# CURSTATE ="GONORTH"
# timeW = timeW+1
# uav_moveto(5.0,0.0)
# } # }
} }
@ -89,9 +116,10 @@ WAIT_TIMEOUT = 200
timeW=0 timeW=0
function barrier_wait(threshold, transf) { function barrier_wait(threshold, transf) {
barrier.get(id) barrier.get(id)
barrier.put(id, 1)
CURSTATE = "BARRIERWAIT" CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) { if(barrier.size() >= threshold) {
barrier = nil #barrier = nil
transf() transf()
} else if(timeW>=WAIT_TIMEOUT) { } else if(timeW>=WAIT_TIMEOUT) {
barrier = nil barrier = nil
@ -112,8 +140,8 @@ CURSTATE = "IDLE"
function takeoff() { function takeoff() {
CURSTATE = "TAKEOFF" CURSTATE = "TAKEOFF"
statef=takeoff statef=takeoff
log("TakeOff: ", flight.status) #log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude) #log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon) barrier_set(ROBOTS,hexagon)
@ -129,15 +157,17 @@ function takeoff() {
function land() { function land() {
CURSTATE = "LAND" CURSTATE = "LAND"
statef=land statef=land
log("Land: ", flight.status) #log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){ if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
uav_land() uav_land()
} }
else { else {
barrier_set(ROBOTS,idle)
barrier_ready()
timeW=0 timeW=0
barrier = nil #barrier = nil
statef=idle #statef=idle
} }
} }
@ -194,7 +224,8 @@ function step() {
} else if(flight.rc_cmd==16) { } else if(flight.rc_cmd==16) {
flight.rc_cmd=0 flight.rc_cmd=0
statef = idle statef = idle
uav_goto() #uav_goto()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} else if(flight.rc_cmd==400) { } else if(flight.rc_cmd==400) {
flight.rc_cmd=0 flight.rc_cmd=0
uav_arm() uav_arm()

View File

@ -41,8 +41,8 @@ namespace buzz_utility{
void update_users(){ void update_users(){
if(users_map.size()>0) { if(users_map.size()>0) {
/* Reset users information */ /* Reset users information */
// buzzusers_reset(); buzzusers_reset();
create_stig_tables(); // create_stig_tables();
/* Get user id and update user information */ /* Get user id and update user information */
map< int, Pos_struct >::iterator it; map< int, Pos_struct >::iterator it;
for (it=users_map.begin(); it!=users_map.end(); ++it){ for (it=users_map.begin(); it!=users_map.end(); ++it){
@ -68,6 +68,8 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);*/ buzzvm_tget(VM);*/
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_push(VM, t); buzzvm_push(VM, t);
buzzvm_gstore(VM); buzzvm_gstore(VM);
//buzzvm_pushi(VM, 2); //buzzvm_pushi(VM, 2);

View File

@ -683,7 +683,7 @@ namespace rosbzz_node{
{ {
for(int it=0; it<n; ++it) for(int it=0; it<n; ++it)
{ {
buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude); buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude);
} }
} }
@ -715,8 +715,8 @@ namespace rosbzz_node{
moveMsg.header.stamp = ros::Time::now(); moveMsg.header.stamp = ros::Time::now();
moveMsg.header.seq = setpoint_counter++; moveMsg.header.seq = setpoint_counter++;
moveMsg.header.frame_id = 1; moveMsg.header.frame_id = 1;
float ned_x, ned_y; // float ned_x, ned_y;
gps_ned_home(ned_x, ned_y); // gps_ned_home(ned_x, ned_y);
// ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]); // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]);
// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
@ -827,7 +827,7 @@ namespace rosbzz_node{
gps_rb(nei_pos, cvt_neighbours_pos_payload); gps_rb(nei_pos, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/ /*Extract robot id of the neighbour*/
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3)); uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
//cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl; ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
/*pass neighbour position to local maintaner*/ /*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]); buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
/*Put RID and pos*/ /*Put RID and pos*/