compute user rb
This commit is contained in:
parent
15b8a911d1
commit
7ae5e39785
@ -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
|
||||
|
@ -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.
|
||||
|
@ -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);
|
||||
|
@ -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]);
|
||||
}
|
||||
|
||||
/*----------------------------------------/
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user