Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

first working implementation of users vstig

This commit is contained in:
dave 2017-05-09 18:25:37 -04:00
parent 6884504c8e
commit 15b8a911d1
6 changed files with 100 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/buzz_utility.h
View File

@ -39,6 +39,7 @@ void update_users();
int make_table(buzzobj_t* t);
int buzzusers_add(int id, double latitude, double longitude, double altitude);
int buzzusers_reset();
int compute_users_rb();
void in_msg_append(uint64_t* payload);

2
include/buzzuav_closures.h
View File

@ -84,7 +84,7 @@ int buzzuav_update_battery(buzzvm_t vm);
* Updates current position in Buzz
*/
int buzzuav_update_currentpos(buzzvm_t vm);
int buzzuav_adduserRB(buzzvm_t vm);
/*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
* use flight.status for flight status

30
script/testflocksim.bzz
View File

@ -141,10 +141,13 @@ function land() {
}
}
function table_print(t) {
foreach(t, function(key, value) {
log(key, " -> ", value)
})
function users_print(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)
})
}
}
########################################
@ -157,10 +160,9 @@ function table_print(t) {
function init() {
s = swarm.create(1)
s.join()
v = stigmergy.create(5)
vt = stigmergy.create(5)
t = {}
v.put("p",t)
v.put("u",1)
vt.put("p",t)
statef=idle
CURSTATE = "IDLE"
}
@ -211,22 +213,18 @@ neighbors.listen("cmd",
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
#log("User position: ", users.range)
# Read a value from the structure
#t = v.get("p")
log(users)
table_print(users)
if(size(users)>0){
tmp = {2 3}
v.put("p", tmp)
v.put("u",2)
#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 ", v.size(), " elements")
table_print(v.get("p"))
log(v.get("u"))
log("The vstig has ", vt.size(), " elements")
users_print(vt.get("p"))
}
# Executed once when the robot (or the simulator) is reset.

2
script/teststig.bzz
View File

@ -23,7 +23,7 @@ function step() {
log("The vstig has ", v.size(), " elements")
log(v.get("u"))
if (id==1) {
tmp = { }
tmp = { .x=3}
v.put("p",tmp)
v.put("u",2)
}

67
src/buzz_utility.cpp
View File

@ -17,7 +17,7 @@ namespace buzz_utility{
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint8_t MSG_SIZE = 50; // Only 100 bytes of Buzz messages every step
static uint8_t MSG_SIZE = 250; // Only 100 bytes of Buzz messages every step
static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static int Robot_id = 0;
@ -57,6 +57,61 @@ namespace buzz_utility{
}
}else
ROS_INFO("[%i] No new users",Robot_id);
//compute_users_rb();
}
int compute_users_rb() {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
/* Get users "userG" stigmergy table */
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
buzzvm_pushi(VM, 1);
buzzvm_callc(VM);
buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
buzzobj_t nbr = buzzvm_stack_at(VM, 1);
buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1));
buzzvm_tget(VM);
buzzvm_type_assert(VM, 1, BUZZTYPE_INT);
int gid = buzzvm_stack_at(VM, 1)->i.value;
ROS_WARN("GOT ID %i FROM V.STIG", gid);
/* Get "data" field */
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1));
buzzvm_tget(VM);
if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
ROS_INFO("Empty data, create a new table");
buzzvm_pop(VM);
buzzvm_push(VM, nbr);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1));
buzzvm_pusht(VM);
buzzobj_t data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
}
/* When we get here, the "data" table is on top of the stack */
/* Push user id */
buzzvm_pushi(VM, gid);
/* Create entry table */
buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
/* Insert range */
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "r", 1));
buzzvm_pushf(VM, 0);
buzzvm_tput(VM);
/* Insert bearing */
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "b", 1));
buzzvm_pushf(VM, 0);
buzzvm_tput(VM);
/* Save entry into data table */
buzzvm_push(VM, entry);
buzzvm_tput(VM);
return VM->state;
}
int buzzusers_reset() {
@ -92,13 +147,13 @@ namespace buzz_utility{
buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
buzzobj_t nbr = buzzvm_stack_at(VM, 1);
/* Get "data" field */
buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_tget(VM);
if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
ROS_INFO("Empty data, create a new table");
buzzvm_pop(VM);
buzzvm_push(VM, nbr);
buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_pusht(VM);
buzzobj_t data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
@ -314,6 +369,9 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_adduserRB));
buzzvm_gstore(VM);
return VM->state;
}
@ -350,6 +408,9 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
return VM->state;
}

21
src/buzzuav_closures.cpp
View File

@ -108,8 +108,8 @@ namespace buzzuav_closures{
float dx = buzzvm_stack_at(vm, 2)->f.value;
double d = sqrt(dx*dx+dy*dy); //range
goto_pos[0]=dx;
goto_pos[1]=dy;
goto_pos[2]=height;
goto_pos[1]=dy;
goto_pos[2]=height;
/*double b = atan2(dy,dx); //bearing
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos);
@ -119,6 +119,23 @@ namespace buzzuav_closures{
return buzzvm_ret0(vm);
}
int buzzuav_adduserRB(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* longitude */
buzzvm_lload(vm, 2); /* latitude */
buzzvm_lload(vm, 3); /* id */
buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float lon = buzzvm_stack_at(vm, 1)->f.value;
float lat = buzzvm_stack_at(vm, 2)->f.value;
int uid = buzzvm_stack_at(vm, 3)->i.value;
printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, lat, lon);
return buzzvm_ret0(vm);
}
/*----------------------------------------/
/ Buzz closure to go directly to a GPS destination from the Mission Planner
/----------------------------------------*/