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

vstig tables work

This commit is contained in:
dave 2017-05-06 23:40:50 -04:00
parent 52bed0536f
commit 5070268491
5 changed files with 284 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
include/buzz_utility.h
View File

@ -21,6 +21,12 @@ struct pos_struct
pos_struct(){}
};
#define function_register(TABLE, FNAME, FPOINTER) \
buzzvm_push(VM, (TABLE)); \
buzzvm_pushs(VM, buzzvm_string_register(VM, (FNAME), 1)); \
buzzvm_pushcc(VM, buzzvm_function_register(VM, (FPOINTER))); \
buzzvm_tput(VM);
typedef struct pos_struct Pos_struct ;
uint16_t* u64_cvt_u16(uint64_t u64);
@ -29,6 +35,12 @@ int buzz_listen(const char* type,
int msg_size);
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
void update_neighbors();
void add_user(int id, double latitude, double longitude, float altitude);
void update_users();
int make_table(buzzobj_t* t);
int buzzusers_add(int id, double latitude, double longitude, double altitude);
int buzzusers_reset();
int buzz_update_users_stigmergy(buzzobj_t tbl);

232
src/buzz_utility.cpp
View File

@ -6,7 +6,7 @@
* @copyright 2016 MistLab. All rights reserved.
*/
#include <buzz_utility.h>
#include "buzz_utility.h"
namespace buzz_utility{
@ -20,17 +20,27 @@ namespace buzz_utility{
static uint8_t MSG_SIZE = 50; // 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;
buzzobj_t usersvstig;
buzzobj_t key;
std::map< int, Pos_struct> users_map;
std::map< int, Pos_struct> neighbors_map;
/****************************************/
/*adds neighbours position*/
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){
neighbors_map.clear();
map< int, Pos_struct >::iterator it;
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
neighbors_map.insert(make_pair(it->first,it->second));
}
}
/* update at each step the VM table */
void update_neighbors(){
/* Reset neighbor information */
buzzneighbors_reset(VM);
/* Get robot id and update neighbor information */
map< int, Pos_struct >::iterator it;
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
buzzneighbors_add(VM,
it->first,
(it->second).x,
@ -38,6 +48,104 @@ namespace buzz_utility{
(it->second).z);
}
}
void add_user(int id, double latitude, double longitude, float altitude)
{
Pos_struct pos_arr;
pos_arr.x=latitude;
pos_arr.y=longitude;
pos_arr.z=altitude;
map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id);
if(it!=users_map.end())
users_map.erase(it);
ROS_INFO("Buzz_utility got new user: %i", id);
users_map.insert(make_pair(id, pos_arr));
}
void update_users(){
/* Reset neighbor information */
buzzusers_reset();
/* Get robot id and update neighbor information */
map< int, Pos_struct >::iterator it;
for (it=users_map.begin(); it!=users_map.end(); ++it){
buzzusers_add(it->first,
(it->second).x,
(it->second).y,
(it->second).z);
}
}
int buzzusers_reset() {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
/* Make new table */
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
//make_table(&t);
if(VM->state != BUZZVM_STATE_READY) return VM->state;
/* Register table as global symbol */
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
//buzzvm_gload(VM);
buzzvm_push(VM, t);
buzzvm_pushi(VM, 2);
buzzvm_call(VM, 0);
//buzzvm_gstore(VM);
return VM->state;
}
int buzzusers_add(int id, double latitude, double longitude, double altitude) {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
/* Get users "p" 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_gload(VM);
buzzvm_pushi(VM, 1);
buzzvm_call(VM, 0);
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_tget(VM);
if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
/* Empty data, create a new table */
buzzvm_pop(VM);
buzzvm_push(VM, nbr);
buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 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, id);
/* Create entry table */
buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
/* Insert latitude */
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1));
buzzvm_pushf(VM, latitude);
buzzvm_tput(VM);
/* Insert longitude */
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1));
buzzvm_pushf(VM, longitude);
buzzvm_tput(VM);
/* Insert altitude */
buzzvm_push(VM, entry);
buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1));
buzzvm_pushf(VM, altitude);
buzzvm_tput(VM);
/* Save entry into data table */
buzzvm_push(VM, entry);
buzzvm_tput(VM);
return VM->state;
}
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
@ -212,7 +320,8 @@ 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);
return BUZZVM_STATE_READY;
return VM->state;
}
/**************************************************/
@ -247,17 +356,59 @@ 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);
return BUZZVM_STATE_READY;
return VM->state;
}
static int create_stig_tables() {
// usersvstig = stigmergy.create(123)
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
//buzzvm_gstore(VM);
// get the stigmergy table from the global scope
buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
buzzvm_gload(VM);
// get the create method from the stigmergy table
buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
buzzvm_tget(VM);
// value of the stigmergy id
buzzvm_pushi(VM, 5);
// call the stigmergy.create() method
// buzzvm_closure_call(VM, 1);
buzzvm_pushi(VM, 1);
buzzvm_call(VM, 0);
buzzvm_gstore(VM);
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
buzzvm_pusht(VM);
buzzvm_pushi(VM, 2);
buzzvm_call(VM, 0);
//buzzvm_gstore(VM);*/
buzzusers_reset();
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "u", 1));
buzzvm_pusht(VM);
buzzvm_pushi(VM, 2);
buzzvm_call(VM, 0);
buzzvm_gstore(VM);*/
return VM->state;
}
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) {
cout << " Robot ID: " <<robot_id<< endl;
ROS_INFO(" Robot ID: %i", robot_id);
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM);
Robot_id = robot_id;
@ -294,39 +445,25 @@ namespace buzz_utility{
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error loading Buzz script\n\n", bo_filename);
ROS_ERROR("%s: Error loading Buzz script", bo_filename);
return 0;
}
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename);
ROS_ERROR("[%i] Error registering hooks", Robot_id);
return 0;
}
buzzvm_dup(VM);
// usersvstig = stigmergy.create(123)
buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
// value of the stigmergy id
buzzvm_pushi(VM, 5);
// get the stigmergy table from the global scope
// buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
// buzzvm_gload(VM);
// get the create method from the stigmergy table
// buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
// buzzvm_tget(VM);
// call the stigmergy.create() method
// buzzvm_closure_call(VM, 1);
// now the stigmergy is at the top of the stack - save it for future reference
// usersvstig = buzzvm_stack_at(VM, 0);
//buzzvm_pop(VM);
// assign the virtual stigmergy to the global symbol v
// create also a global variable for it, so the garbage collector does not remove it
//buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
//buzzvm_push(VM, usersvstig);
buzzvm_gstore(VM);
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);
@ -336,24 +473,47 @@ buzzvm_dup(VM);
buzzvm_function_call(VM, "init", 0);
/* All OK */
ROS_INFO("[%i] INIT DONE!!!", Robot_id);
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
/****************************************/
/*Update users positions */
/****************************************/
int buzz_update_users_stigmergy(buzzobj_t tbl){
// push the key (here it's an int with value 46)
/* // push the key (here it's an int with value 46)
buzzvm_pushi(VM, 46);
// push the table
buzzvm_push(VM, tbl);
// the stigmergy is stored in the vstig variable
// let's push it on the stack
buzzvm_push(VM, usersvstig);
buzzvm_push(VM, peoplevstig);
// get the put method from myvstig
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
// call the vstig.put() method
buzzvm_closure_call(VM, 2);
buzzvm_closure_call(VM, 2);*/
// put something in it....
//buzzvm_push(VM, peoplevstig);
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
//buzzvm_pushs(VM, buzzvm_string_register(VM, "id", 1));
buzzvm_pushi(VM, Robot_id);
//buzzvm_gload(VM);
buzzvm_push(VM, tbl);
buzzvm_pushi(VM, 2);
buzzvm_call(VM, 0);
//buzzvm_closure_call(VM, 2);
//buzzvm_gstore(VM);
return 1;
}
/****************************************/
/*Sets a new update */
/****************************************/
@ -489,7 +649,9 @@ buzzvm_dup(VM);
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM);
update_neighbors();
update_users();
buzzuav_closures::buzzuav_update_userspos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
//buzz_update_users_stigmergy(tbl);

16
src/buzzuav_closures.cpp
View File

@ -7,7 +7,7 @@
*/
//#define _GNU_SOURCE
#include "buzzuav_closures.h"
//#include "roscontroller.h"
namespace buzzuav_closures{
// TODO: Minimize the required global variables and put them in the header
@ -246,6 +246,14 @@ namespace buzzuav_closures{
cur_pos[2]=altitude;
}
void set_userspos(double latitude, double longitude, double altitude){
/*buzz_utility::Pos_struct pos_arr;
pos_arr.x=latitude;
pos_arr.y=longitude;
pos_arr.z=altitude;
map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id);
if(it!=users_map.end())
users_map.erase(it);
users_map.insert(make_pair(id, pos_arr));*/
users_pos[0]=latitude;
users_pos[1]=longitude;
users_pos[2]=altitude;
@ -270,6 +278,9 @@ namespace buzzuav_closures{
return vm->state;
}
buzzobj_t buzzuav_update_userspos(buzzvm_t vm) {
/*map< int, buzz_utility::Pos_struct >::iterator it;
for (it=users_map.begin(); it!=users_map.end(); ++it){
}*/
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -284,9 +295,10 @@ namespace buzzuav_closures{
buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1));
buzzvm_pushf(vm, users_pos[2]);
buzzvm_tput(vm);
buzzobj_t tbl = buzzvm_stack_at(vm, 0);
buzzvm_gstore(vm);
return buzzvm_stack_at(vm, 0);
return tbl;
//return vm->state;
}

7
src/roscontroller.cpp
View File

@ -29,6 +29,7 @@ namespace rosbzz_node{
robot_id=strtol(robot_name.c_str() + 5, NULL, 10);;
setpoint_counter = 0;
fcu_timeout = TIMEOUT;
home[0]=0.0;home[1]=0.0;home[2]=0.0;
}
@ -110,7 +111,6 @@ namespace rosbzz_node{
timer_step+=1;
maintain_pos(timer_step);
}
/* Destroy updater and Cleanup */
//update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@ -709,7 +709,8 @@ namespace rosbzz_node{
us[2] = data.pos_neigh[it].altitude;
double out[3];
cvt_rangebearing_coordinates(us, out, cur_pos);
buzzuav_closures::set_userspos(out[0], out[1], out[2]);
//buzzuav_closures::set_userspos(out[0], out[1], out[2]);
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);
}
}
@ -757,7 +758,7 @@ namespace rosbzz_node{
moveMsg.pose.orientation.w = 1;
// To prevent drifting from stable position.
if(fabs(x)>0.1 || fabs(y)>0.1) {
if(fabs(x)>0.01 || fabs(y)>0.01) {
localsetpoint_nonraw_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!");
}

14
src/testflocksim.bzz
View File

@ -141,6 +141,12 @@ function land() {
}
}
function table_print(t) {
foreach(t, function(key, value) {
log(key, " -> ", value)
})
}
# Executed once at init time.
function init() {
s = swarm.create(1)
@ -199,9 +205,13 @@ neighbors.listen("cmd",
log("User position: ", users.range)
# Read a value from the structure
#x = usersvstig.get(46)
t = vt.get("p")
log(t)
#table_print(t)
#t = vt.get("u")
#table_print(t)
# Get the number of keys in the structure
#log("The vstig has ", usersvstig.size(), " elements")
log("The vstig has ", vt.size(), " elements")
}
# Executed once when the robot (or the simulator) is reset.