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

compute user rb

This commit is contained in:
dave 2017-05-10 07:34:57 -04:00
parent 15b8a911d1
commit 7ae5e39785
5 changed files with 101 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/buzzuav_closures.h
View File

@ -9,6 +9,9 @@
#include "buzz_utility.h"
//#include "roscontroller.h"
#define EARTH_RADIUS (double) 6371000.0
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
namespace buzzuav_closures{
typedef enum {
COMMAND_NIL = 0, // Dummy command

18
script/testflocksim.bzz
View File

@ -141,15 +141,24 @@ function land() {
}
}
function users_print(t) {
function users_save(t) {
if(size(t)>0) {
foreach(t, function(id, tab) {
log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
#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(key, value) {
log(key, " -> ", value)
})
}
}
########################################
#
# MAIN FUNCTIONS
@ -216,15 +225,14 @@ neighbors.listen("cmd",
# Read a value from the structure
log(users)
if(size(users)>0){
#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_print(vt.get("p"))
users_save(vt.get("p"))
table_print(users.dataL)
}
# Executed once when the robot (or the simulator) is reset.

84
src/buzz_utility.cpp
View File

@ -57,61 +57,6 @@ 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() {
@ -416,7 +361,7 @@ namespace buzz_utility{
}
static int create_stig_tables() {
/*
// usersvstig = stigmergy.create(123)
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
// get the stigmergy table from the global scope
@ -461,6 +406,24 @@ static int create_stig_tables() {
buzzvm_call(VM, 0);
buzzvm_gstore(VM);*/
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_push(VM,t);
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
buzzvm_pusht(VM);
buzzobj_t data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1));
buzzvm_pusht(VM);
data = buzzvm_stack_at(VM, 1);
buzzvm_tput(VM);
buzzvm_push(VM, data);
return VM->state;
}
/****************************************/
@ -515,7 +478,7 @@ static int create_stig_tables() {
ROS_ERROR("[%i] Error registering hooks", Robot_id);
return 0;
}
/* Create vstig tables
/* Create vstig tables */
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
@ -523,12 +486,7 @@ static int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
buzzvm_push(VM, t);
buzzvm_gstore(VM);
}
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);

66
src/buzzuav_closures.cpp
View File

@ -77,7 +77,7 @@ namespace buzzuav_closures{
/*----------------------------------------/
/ Compute GPS destination from current position and desired Range and Bearing
/----------------------------------------*/
#define EARTH_RADIUS (double) 6371000.0
void gps_from_rb(double range, double bearing, double out[3]) {
double lat = cur_pos[0]*M_PI/180.0;
double lon = cur_pos[1]*M_PI/180.0;
@ -88,6 +88,16 @@ namespace buzzuav_closures{
out[2] = height; //constant height.
}
void rb_from_gps(double nei[], double out[], double cur[]){
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
out[1] = atan2(ned_y,ned_x);
out[2] = 0.0;
}
// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
@ -119,6 +129,47 @@ namespace buzzuav_closures{
return buzzvm_ret0(vm);
}
int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) {
if(vm->state != BUZZVM_STATE_READY) return vm->state;
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
buzzvm_gload(vm);
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, "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, id);
/* 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, range);
buzzvm_tput(vm);
/* Insert longitude */
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1));
buzzvm_pushf(vm, bearing);
buzzvm_tput(vm);
/* Save entry into data table */
buzzvm_push(vm, entry);
buzzvm_tput(vm);
//printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing);
return vm->state;
}
int buzzuav_adduserRB(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* longitude */
@ -127,13 +178,18 @@ namespace buzzuav_closures{
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;
double tmp[3];
tmp[0] = buzzvm_stack_at(vm, 2)->f.value;
tmp[1] = buzzvm_stack_at(vm, 1)->f.value;
tmp[2] = 0.0;
int uid = buzzvm_stack_at(vm, 3)->i.value;
double rb[3];
printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, lat, lon);
rb_from_gps(tmp, rb, cur_pos);
return buzzvm_ret0(vm);
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
return users_add2localtable(vm, uid, rb[0], rb[1]);
}
/*----------------------------------------/

3
src/roscontroller.cpp
View File

@ -528,9 +528,6 @@ namespace rosbzz_node{
/ Compute Range and Bearing of a neighbor in a local reference frame
/ from GPS coordinates
----------------------------------------------------------- */
#define EARTH_RADIUS (double) 6371000.0
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) {
double hyp_az, hyp_el;
double sin_el, cos_el, sin_az, cos_az;