diff --git a/.gitignore b/.gitignore index 7d62213..ae47e75 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,7 @@ src/test* .cproject .project +.basm +.bo +.bdb +.bdbg diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 3667363..fb744e5 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -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); @@ -28,13 +34,17 @@ uint16_t* u64_cvt_u16(uint64_t u64); int buzz_listen(const char* type, int msg_size); -void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map); +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); +void in_msg_append(uint64_t* payload); -void in_msg_process(uint64_t* payload); +uint64_t* obt_out_msg(); -uint64_t* out_msg_process(); +void update_sensors(); int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index eff6835..2e62d32 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -46,12 +46,13 @@ void rc_call(int rc_cmd); void set_battery(float voltage,float current,float remaining); /* sets current position */ void set_currentpos(double latitude, double longitude, double altitude); -void set_userspos(double latitude, double longitude, double altitude); /*retuns the current go to position */ double* getgoto(); /* updates flight status*/ void flight_status_update(uint8_t state); - +/* Update neighbors table */ +void neighbour_pos_callback(int id, float range, float bearing, float elevation); +void update_neighbors(buzzvm_t vm); /*Flight status*/ void set_obstacle_dist(float dist[]); @@ -83,7 +84,6 @@ int buzzuav_update_battery(buzzvm_t vm); * Updates current position in Buzz */ int buzzuav_update_currentpos(buzzvm_t vm); -buzzobj_t buzzuav_update_userspos(buzzvm_t vm); /* * Updates flight status and rc command in Buzz, put it in a tabel to acess it diff --git a/include/roscontroller.h b/include/roscontroller.h index e1e00c8..43cb117 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -137,7 +137,7 @@ private: void Rosparameters_get(ros::NodeHandle& n_c_priv); /*compiles buzz script from the specified .bzz file*/ - void Compile_bzz(); + std::string Compile_bzz(std::string bzzfile_name); /*Flight controller service call*/ void flight_controller_service_call(); diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch index c44b99e..01d9402 100644 --- a/launch/rosbuzzm100.launch +++ b/launch/rosbuzzm100.launch @@ -2,16 +2,18 @@ - + + - + - - - + + + + diff --git a/script/include/string.bzz b/script/include/string.bzz new file mode 100644 index 0000000..4140a1b --- /dev/null +++ b/script/include/string.bzz @@ -0,0 +1,92 @@ +# +# Returns the string character at the given position. +# PARAM s: The string +# PARAM n: The position of the wanted character +# RETURN The character at the wanted position, or nil +# +string.charat = function(s, n) { + return string.sub(s, n, n+1) +} + +# +# Returns the index of the first occurrence of the given string m +# within another string s. If none is found, this function returns +# nil. +# PARAM s: The string +# PARAM m: The string to match +# RETURN: The position of the first match, or nil +# +string.indexoffirst = function(s, m) { + var ls = string.length(s) + var lm = string.length(m) + var i = 0 + while(i < ls-lm+1) { + if(string.sub(s, i, i+lm) == m) return i + i = i + 1 + } + return nil +} + +# +# Returns the index of the last occurrence of the given string m +# within another string s. If none is found, this function returns +# nil. +# PARAM s: The string +# PARAM m: The string to match +# RETURN: The position of the last match, or nil +# +string.indexoflast = function(s, m) { + var ls = string.length(s) + var lm = string.length(m) + var i = ls - lm + 1 + while(i >= 0) { + if(string.sub(s, i, i+lm) == m) return i + i = i - 1 + } + return nil +} + +# Splits a string s using the delimiters in d. The string list is +# returned in a table indexed by value (starting at 0). +# PARAM s: The string +# PARAM d: A string containing the delimiters +# RETURN: A table containing the tokens +string.split = function(s, d) { + var i1 = 0 # index to move along s (token start) + var i2 = 0 # index to move along s (token end) + var c = 0 # token count + var t = {} # token list + var ls = string.length(s) + var ld = string.length(d) + # Go through string s + while(i2 < ls) { + # Try every delimiter + var j = 0 # index to move along d + var f = nil # whether the delimiter was found or not + while(j < ld and (not f)) { + if(string.charat(s, i2) == string.charat(d, j)) { + # Delimiter found + f = 1 + # Is it worth adding a new token? + if(i2 > i1) { + t[c] = string.sub(s, i1, i2) + c = c + 1 + } + # Start new token + i1 = i2 + 1 + } + else { + # Next delimiter + j = j + 1 + } + } + # Next string character + i2 = i2 + 1 + } + # Is it worth adding a new token? + if(i2 > i1) { + t[c] = string.sub(s, i1, i2) + } + # Return token list + return t; +} diff --git a/script/include/vec2.bzz b/script/include/vec2.bzz new file mode 100644 index 0000000..e2fb9b0 --- /dev/null +++ b/script/include/vec2.bzz @@ -0,0 +1,107 @@ +# +# Create a new namespace for vector2 functions +# +math.vec2 = {} + +# +# Creates a new vector2. +# PARAM x: The x coordinate. +# PARAM y: The y coordinate. +# RETURN: A new vector2. +# +math.vec2.new = function(x, y) { + return { .x = x, .y = y } +} + +# +# Creates a new vector2 from polar coordinates. +# PARAM l: The length of the vector2. +# PARAM a: The angle of the vector2. +# RETURN: A new vector2. +# +math.vec2.newp = function(l, a) { + return { + .x = l * math.cos(a), + .y = l * math.sin(a) + } +} + +# +# Calculates the length of the given vector2. +# PARAM v: The vector2. +# RETURN: The length of the vector. +# +math.vec2.length = function(v) { + return math.sqrt(v.x * v.x + v.y * v.y) +} + +# +# Calculates the angle of the given vector2. +# PARAM v: The vector2. +# RETURN: The angle of the vector. +# +math.vec2.angle = function(v) { + return math.atan2(v.y, v.x) +} + +# +# Returns the normalized form of a vector2. +# PARAM v: The vector2. +# RETURN: The normalized form. +# +math.vec2.norm = function(v) { + var l = math.length(v) + return { + .x = v.x / l, + .y = v.y / l + } +} + +# +# Calculates v1 + v2. +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 + v2 +# +math.vec2.add = function(v1, v2) { + return { + .x = v1.x + v2.x, + .y = v1.y + v2.y + } +} + +# +# Calculates v1 - v2. +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 + v2 +# +math.vec2.sub = function(v1, v2) { + return { + .x = v1.x - v2.x, + .y = v1.y - v2.y + } +} + +# +# Scales a vector by a numeric constant. +# PARAM v: A vector2. +# PARAM s: A number (float or int). +# RETURN: s * v +# +math.vec2.scale = function(v, s) { + return { + .x = v.x * s, + .y = v.y * s + } +} + +# +# Calculates v1 . v2 (the dot product) +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 . v2 +# +math.vec2.dot = function(v1, v2) { + return v1.x * v2.x + v1.y * v2.y +} diff --git a/script/stand_by.bdb b/script/stand_by.bdb new file mode 100644 index 0000000..de2da98 Binary files /dev/null and b/script/stand_by.bdb differ diff --git a/script/stand_by.bo b/script/stand_by.bo new file mode 100644 index 0000000..643951a Binary files /dev/null and b/script/stand_by.bo differ diff --git a/src/stand_by.bzz b/script/stand_by.bzz similarity index 100% rename from src/stand_by.bzz rename to script/stand_by.bzz diff --git a/src/test.bzz b/script/test.bzz similarity index 100% rename from src/test.bzz rename to script/test.bzz diff --git a/src/test1.bzz b/script/test1.bzz similarity index 100% rename from src/test1.bzz rename to script/test1.bzz diff --git a/src/testalone.bzz b/script/testalone.bzz similarity index 100% rename from src/testalone.bzz rename to script/testalone.bzz diff --git a/src/testflockfev.bzz b/script/testflockfev.bzz similarity index 94% rename from src/testflockfev.bzz rename to script/testflockfev.bzz index c3573cd..1845eb9 100644 --- a/src/testflockfev.bzz +++ b/script/testflockfev.bzz @@ -43,19 +43,15 @@ function hexagon() { math.vec2.scale(accum, 1.0 / neighbors.count()) # Move according to vector #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) -# uav_moveto(accum.x,accum.y) + uav_moveto(accum.x,accum.y) - if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS - timeW =0 - statef=land - } else { - if(timeW >= (WAIT_TIMEOUT / 2)){ - uav_moveto(5.0,0.0) - } else { - uav_moveto(0.0,5.0) - } - timeW = timeW+1 - } +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } } ######################################## @@ -90,7 +86,7 @@ function barrier_ready() { # # Executes the barrier # -WAIT_TIMEOUT = 300 +WAIT_TIMEOUT = 200 timeW=0 function barrier_wait(threshold, transf) { barrier.get(id) diff --git a/src/testflockfev_barrier.bzz b/script/testflockfev_barrier.bzz similarity index 100% rename from src/testflockfev_barrier.bzz rename to script/testflockfev_barrier.bzz diff --git a/src/testflocksim.bzz b/script/testflocksim.bzz similarity index 88% rename from src/testflocksim.bzz rename to script/testflocksim.bzz index 107edbf..2dcca47 100644 --- a/src/testflocksim.bzz +++ b/script/testflocksim.bzz @@ -8,7 +8,7 @@ include "/home/ubuntu/buzz/src/include/vec2.bzz" updated="update_ack" update_no=0 function updated_neigh(){ -neighbors.broadcast(updated, update_no) + neighbors.broadcast(updated, update_no) } TARGET_ALTITUDE = 10.0 @@ -141,11 +141,26 @@ function land() { } } +function table_print(t) { + foreach(t, function(key, value) { + log(key, " -> ", value) + }) +} + +######################################## +# +# MAIN FUNCTIONS +# +######################################## + # Executed once at init time. function init() { s = swarm.create(1) -# s.select(1) s.join() + v = stigmergy.create(5) + t = {} + v.put("p",t) + v.put("u",1) statef=idle CURSTATE = "IDLE" } @@ -196,12 +211,22 @@ neighbors.listen("cmd", statef() log("Current state: ", CURSTATE) log("Swarm size: ",ROBOTS) - log("User position: ", users.range) + #log("User position: ", users.range) # Read a value from the structure - #x = usersvstig.get(46) + #t = v.get("p") + log(users) + table_print(users) + if(size(users)>0){ + tmp = {2 3} + v.put("p", tmp) + v.put("u",2) + } + # Get the number of keys in the structure - #log("The vstig has ", usersvstig.size(), " elements") + log("The vstig has ", v.size(), " elements") + table_print(v.get("p")) + log(v.get("u")) } # Executed once when the robot (or the simulator) is reset. diff --git a/script/teststig.bdb b/script/teststig.bdb new file mode 100644 index 0000000..cc42d70 Binary files /dev/null and b/script/teststig.bdb differ diff --git a/script/teststig.bo b/script/teststig.bo new file mode 100644 index 0000000..63e7db3 Binary files /dev/null and b/script/teststig.bo differ diff --git a/script/teststig.bzz b/script/teststig.bzz new file mode 100644 index 0000000..0ef792e --- /dev/null +++ b/script/teststig.bzz @@ -0,0 +1,38 @@ +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +function init(){ + s = swarm.create(1) + s.join() + v = stigmergy.create(5) + #t= {} + #v.put("p",t) + v.put("u",1) +} + +function step() { + log("Swarm size: ",ROBOTS) + log("The vstig has ", v.size(), " elements") + log(v.get("u")) + if (id==1) { + #tmp = { .x=3 } + #v.put("p",tmp) + v.put("u",2) + } + #log(v.get("p")) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} \ No newline at end of file diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index d622228..3a77dba 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -27,80 +27,72 @@ static int neigh=-1; static int updater_msg_ready ; static int updated=0; +/*Initialize updater*/ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ fprintf(stdout,"intiialized file monitor.\n"); fd=inotify_init1(IN_NONBLOCK); if ( fd < 0 ) { - perror( "inotify_init error" ); - } - /* watch /.bzz file for any activity and report it back to me */ + perror( "inotify_init error" ); + } + /* watch /.bzz file for any activity and report it back to update */ wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS ); - + /*load the .bo under execution into the updater*/ uint8_t* BO_BUF = 0; FILE* fp = fopen(bo_filename, "rb"); - - if(!fp) { - perror(bo_filename); - - } - fseek(fp, 0, SEEK_END); - - size_t bcode_size = ftell(fp); - rewind(fp); - - BO_BUF = (uint8_t*)malloc(bcode_size); - if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { - perror(bo_filename); - - fclose(fp); - //return 0; - } - fclose(fp); + if(!fp) { + perror(bo_filename); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { + perror(bo_filename); + fclose(fp); + //return 0; + } + fclose(fp); + /*Load stand_by .bo file into the updater*/ uint8_t* STD_BO_BUF = 0; fp = fopen(stand_by_script, "rb"); + if(!fp) { + perror(stand_by_script); - if(!fp) { - perror(stand_by_script); - - } - fseek(fp, 0, SEEK_END); - - size_t stdby_bcode_size = ftell(fp); - rewind(fp); - - STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); - if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { - perror(stand_by_script); - - fclose(fp); - //return 0; - } - fclose(fp); - - updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); - /* Create a new table for updater*/ - updater->bcode = BO_BUF; - updater->outmsg_queue = NULL; - updater->inmsg_queue = NULL; - updater->bcode_size = (size_t*) malloc(sizeof(size_t)); - updater->update_no = (uint8_t*) malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->update_no) =0; - *(size_t*)(updater->bcode_size)=bcode_size; - updater->standby_bcode = STD_BO_BUF; - updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); - *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size; - updater->mode=(int*)malloc(sizeof(int)); - *(int*)(updater->mode)=CODE_RUNNING; - //no_of_robot=barrier; - updater_msg_ready=0; - //neigh = 0; - //updater->outmsg_queue= - // update_table->barrier=nvs; + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { + perror(stand_by_script); + fclose(fp); + //return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->bcode_size = (size_t*) malloc(sizeof(size_t)); + updater->update_no = (uint8_t*) malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) =0; + *(size_t*)(updater->bcode_size)=bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size; + updater->mode=(int*)malloc(sizeof(int)); + *(int*)(updater->mode)=CODE_RUNNING; + //no_of_robot=barrier; + updater_msg_ready=0; + //neigh = 0; + //updater->outmsg_queue= + // update_table->barrier=nvs; } - +/*Check for .bzz file chages*/ int check_update(){ - struct inotify_event *event; char buf[1024]; int check =0; @@ -108,7 +100,6 @@ int check_update(){ int len=read(fd,buf,1024); while(imask & (IN_MODIFY| IN_DELETE_SELF)){ @@ -123,20 +114,12 @@ int check_update(){ check=1; old_update =1; } - } - /* update index to start of next event */ i+=sizeof(struct inotify_event)+event->len; - } - if (!check) old_update=0; - /*if(update){ - buzz_script_set(update_bo, update_bdbg); - update = 0; - }*/ - return check; +return check; } @@ -163,57 +146,56 @@ void code_message_outqueue_append(){ } void code_message_inqueue_append(uint8_t* msg,uint16_t size){ -updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); -fprintf(stdout,"in ms append code size %d\n", (int) size); -updater->inmsg_queue->queue = (uint8_t*)malloc(size); -updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); -memcpy(updater->inmsg_queue->queue, msg, size); -*(uint16_t*)(updater->inmsg_queue->size) = size; - + updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + fprintf(stdout,"in ms append code size %d\n", (int) size); + updater->inmsg_queue->queue = (uint8_t*)malloc(size); + updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memcpy(updater->inmsg_queue->queue, msg, size); + *(uint16_t*)(updater->inmsg_queue->size) = size; } void code_message_inqueue_process(){ -int size=0; -fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) ); -fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); -fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); + int size=0; + fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) ); + fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); + fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); -if( *(int*) (updater->mode) == CODE_RUNNING){ - //fprintf(stdout,"[debug]Inside inmsg code running"); - if( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){ - //fprintf(stdout,"[debug]Inside update number comparision"); - uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue); - size +=sizeof(uint16_t); - uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size); - size +=sizeof(uint16_t); - //fprintf(stdout,"in queue process Update no %d\n", (int) update_no); - //fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); - //FILE *fp; - //fp=fopen("update.bo", "wb"); - //fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); - //fclose(fp); - if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size), - (char*) dbgf_name,(size_t)update_bcode_size) ) { - *(uint16_t*)(updater->update_no)=update_no; - neigh=1; + if( *(int*) (updater->mode) == CODE_RUNNING){ + //fprintf(stdout,"[debug]Inside inmsg code running"); + if( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){ + //fprintf(stdout,"[debug]Inside update number comparision"); + uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue); + size +=sizeof(uint16_t); + uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size); + size +=sizeof(uint16_t); + //fprintf(stdout,"in queue process Update no %d\n", (int) update_no); + //fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); + //FILE *fp; + //fp=fopen("update.bo", "wb"); + //fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); + //fclose(fp); + if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size), + (char*) dbgf_name,(size_t)update_bcode_size) ) { + *(uint16_t*)(updater->update_no)=update_no; + neigh=1; + } } } -} -//fprintf(stdout,"in queue freed\n"); -delete_p(updater->inmsg_queue->queue); -delete_p(updater->inmsg_queue->size); -delete_p(updater->inmsg_queue); + //fprintf(stdout,"in queue freed\n"); + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); } void update_routine(const char* bcfname, const char* dbgfname){ -dbgf_name=(char*)dbgfname; -buzzvm_t VM = buzz_utility::get_vm(); + dbgf_name=(char*)dbgfname; + buzzvm_t VM = buzz_utility::get_vm(); -buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); -//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + //fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); if(*(int*)updater->mode==CODE_RUNNING){ buzzvm_function_call(VM, "updated_neigh", 0); if(check_update()){ @@ -226,65 +208,55 @@ buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0,name.find_last_of(".")); bzzfile_in_compile << "bzzparse "<update_no); - *(uint16_t*)(updater->update_no) =update_no +1; - code_message_outqueue_append(); - VM = buzz_utility::get_vm(); - fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); - neigh=-1; - fprintf(stdout,"Sending code... \n"); - code_message_outqueue_append(); + uint16_t update_no =*(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) =update_no +1; + code_message_outqueue_append(); + VM = buzz_utility::get_vm(); + fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + neigh=-1; + fprintf(stdout,"Sending code... \n"); + code_message_outqueue_append(); } delete_p(BO_BUF); } @@ -330,7 +302,6 @@ return (uint8_t*)updater->outmsg_queue->queue; uint8_t* getupdate_out_msg_size(){ //fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); - return (uint8_t*)updater->outmsg_queue->size; } @@ -397,17 +368,17 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ } void destroy_out_msg_queue(){ -//fprintf(stdout,"out queue freed\n"); -delete_p(updater->outmsg_queue->queue); -delete_p(updater->outmsg_queue->size); -delete_p(updater->outmsg_queue); -updater_msg_ready=0; + //fprintf(stdout,"out queue freed\n"); + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + updater_msg_ready=0; } int get_update_status(){ return updated; } void set_read_update_status(){ -updated=0; + updated=0; } int get_update_mode(){ return (int)*(int*)(updater->mode); @@ -420,44 +391,44 @@ buzz_updater_elem_t get_updater(){ return updater; } void destroy_updater(){ -delete_p(updater->bcode); -delete_p(updater->bcode_size); -delete_p(updater->standby_bcode); -delete_p(updater->standby_bcode_size); -delete_p(updater->mode); -delete_p(updater->update_no); -if(updater->outmsg_queue){ -delete_p(updater->outmsg_queue->queue); -delete_p(updater->outmsg_queue->size); -delete_p(updater->outmsg_queue); -} -if(updater->inmsg_queue){ -delete_p(updater->inmsg_queue->queue); -delete_p(updater->inmsg_queue->size); -delete_p(updater->inmsg_queue); -} -// -inotify_rm_watch(fd,wd); -close(fd); -} + delete_p(updater->bcode); + delete_p(updater->bcode_size); + delete_p(updater->standby_bcode); + delete_p(updater->standby_bcode_size); + delete_p(updater->mode); + delete_p(updater->update_no); + if(updater->outmsg_queue){ + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + } + if(updater->inmsg_queue){ + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + // + inotify_rm_watch(fd,wd); + close(fd); + } void set_bzz_file(const char* in_bzz_file){ -bzz_file=in_bzz_file; + bzz_file=in_bzz_file; } void updates_set_robots(int robots){ -no_of_robot=robots; + no_of_robot=robots; } void collect_data(){ -//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end); -double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; -time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; -//int bytecodesize=(int); -fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no); -//FILE *Fileptr; -//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a"); -//fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no); -//fclose(Fileptr); + //fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end); + double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; + time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; + //int bytecodesize=(int); + fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no); + //FILE *Fileptr; + //Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a"); + //fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no); + //fclose(Fileptr); } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index c9cbd69..dfe19d6 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -6,7 +6,7 @@ * @copyright 2016 MistLab. All rights reserved. */ -#include +#include "buzz_utility.h" namespace buzz_utility{ @@ -19,38 +19,141 @@ namespace buzz_utility{ static buzzdebug_t DBG_INFO = 0; 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; + static int Robot_id = 0; + + std::map< int, Pos_struct> users_map; /****************************************/ - /*adds neighbours position*/ - void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){ - /* 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){ - buzzneighbors_add(VM, - it->first, - (it->second).x, - (it->second).y, - (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); + users_map.insert(make_pair(id, pos_arr)); + //ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); + } + + void update_users(){ + if(users_map.size()>0) { + /* Reset users information */ + buzzusers_reset(); + /* Get user id and update user information */ + map< int, Pos_struct >::iterator it; + for (it=users_map.begin(); it!=users_map.end(); ++it){ + //ROS_INFO("Buzz_utility will save user %i.", it->first); + buzzusers_add(it->first, + (it->second).x, + (it->second).y, + (it->second).z); + buzzusers_add(it->first+1, + (it->second).x, + (it->second).y, + (it->second).z); + } + }else + ROS_INFO("[%i] No new users",Robot_id); + } + + 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, "users", 1)); + buzzvm_push(VM, t); + buzzvm_gstore(VM); + //buzzvm_pushi(VM, 2); + //buzzvm_callc(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, "users", 1)); + buzzvm_gload(VM); + //buzzvm_pushi(VM, 1); + //buzzvm_callc(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, "data", 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_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); + ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); + // forcing the new table into the stigmergy.... + /*buzzobj_t newt = buzzvm_stack_at(VM, 0); + 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_push(VM, nbr); + buzzvm_pushi(VM, 2); + buzzvm_callc(VM);*/ + //buzzvm_gstore(VM); + return VM->state; } /**************************************************************************/ /*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */ /**************************************************************************/ uint16_t* u64_cvt_u16(uint64_t u64){ - uint16_t* out = new uint16_t[4]; - uint32_t int32_1 = u64 & 0xFFFFFFFF; - uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32; - out[0] = int32_1 & 0xFFFF; - out[1] = (int32_1 & (0xFFFF0000) ) >> 16; - out[2] = int32_2 & 0xFFFF; - out[3] = (int32_2 & (0xFFFF0000) ) >> 16; - //cout << " values " <> 32; + out[0] = int32_1 & 0xFFFF; + out[1] = (int32_1 & (0xFFFF0000) ) >> 16; + out[2] = int32_2 & 0xFFFF; + out[3] = (int32_2 & (0xFFFF0000) ) >> 16; + //cout << " values " < sizeof(uint16_t) && unMsgSize > 0); free(pl); - /* Process messages */ - buzzvm_process_inmsgs(VM); + } /***************************************************/ /*Obtains messages from buzz out message Queue*/ /***************************************************/ - uint64_t* out_msg_process(){ - buzzvm_process_outmsgs(VM); + uint64_t* obt_out_msg(){ + uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE); memset(buff_send, 0, MAX_MSG_SIZE); /*Taking into consideration the sizes included at the end*/ @@ -151,8 +253,8 @@ namespace buzz_utility{ cout<<" payload from out msg "<<*(payload_64+i)<pc, VM->errormsg); } - return msg; + return msg; } /****************************************/ @@ -189,15 +291,15 @@ namespace buzz_utility{ buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); @@ -212,7 +314,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; } /**************************************************/ @@ -224,15 +327,15 @@ namespace buzz_utility{ buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); @@ -247,86 +350,124 @@ 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)); + // 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_dump(VM); +// buzzvm_closure_call(VM, 1); + buzzvm_pushi(VM, 1); + buzzvm_callc(VM); + buzzvm_gstore(VM); + buzzvm_dump(VM); + + //buzzusers_reset(); + buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); + + 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_push(VM, t); + buzzvm_dump(VM); +// buzzvm_closure_call(VM, 2); + buzzvm_pushi(VM, 2); + buzzvm_callc(VM); + //buzzvm_gstore(VM); + //buzzvm_dump(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, "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: " <errormsg << endl; + //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; + 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); + 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); @@ -336,29 +477,16 @@ 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); } - int buzz_update_users_stigmergy(buzzobj_t tbl){ - // 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); - // 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); - return 1; - } /****************************************/ /*Sets a new update */ /****************************************/ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ -/* // Reset the Buzz VM + // Reset the Buzz VM if(VM) buzzvm_destroy(&VM); VM = buzzvm_new(Robot_id); // Get rid of debug info @@ -386,20 +514,28 @@ buzzvm_dup(VM); fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); return 0; } - + /* Create vstig tables */ + 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; + } // Execute the global part of the script buzzvm_execute_script(VM); // Call the Init() function buzzvm_function_call(VM, "init", 0); // All OK -*/ return 1; + return 1; } /****************************************/ /*Performs a initialization test */ /****************************************/ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ -/* // Reset the Buzz VM + // Reset the Buzz VM if(VM) buzzvm_destroy(&VM); VM = buzzvm_new(Robot_id); // Get rid of debug info @@ -427,13 +563,21 @@ buzzvm_dup(VM); fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); return 0; } - + /* Create vstig tables */ + 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; + } // Execute the global part of the script buzzvm_execute_script(VM); // Call the Init() function buzzvm_function_call(VM, "init", 0); // All OK -*/ return 1; + return 1; } /****************************************/ @@ -447,73 +591,75 @@ buzzvm_dup(VM); typedef struct buzzswarm_elem_s* buzzswarm_elem_t; void check_swarm_members(const void* key, void* data, void* params) { - buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; - int* status = (int*)params; - if(*status == 3) return; - fprintf(stderr, "CHECKING SWARM MEMBERS:%i\n",buzzdarray_get(e->swarms, 0, uint16_t)); - if(buzzdarray_size(e->swarms) != 1) { - fprintf(stderr, "Swarm list size is not 1\n"); - *status = 3; - } - else { - int sid = 1; - if(!buzzdict_isempty(VM->swarms)) { - if(*buzzdict_get(VM->swarms, &sid, uint8_t) && - buzzdarray_get(e->swarms, 0, uint16_t) != sid) { - fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", - sid, - buzzdarray_get(e->swarms, 0, uint16_t)); - *status = 3; - return; - } - } - if(buzzdict_size(VM->swarms)>1) { - sid = 2; - if(*buzzdict_get(VM->swarms, &sid, uint8_t) && - buzzdarray_get(e->swarms, 0, uint16_t) != sid) { - fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", - sid, - buzzdarray_get(e->swarms, 0, uint16_t)); - *status = 3; - return; - } - } - } + buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; + int* status = (int*)params; + if(*status == 3) return; + fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n", + buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age); + if(buzzdarray_size(e->swarms) != 1) { + fprintf(stderr, "Swarm list size is not 1\n"); + *status = 3; + } + else { + int sid = 1; + if(!buzzdict_isempty(VM->swarms)) { + if(*buzzdict_get(VM->swarms, &sid, uint8_t) && + buzzdarray_get(e->swarms, 0, uint16_t) != sid) { + fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", + sid, + buzzdarray_get(e->swarms, 0, uint16_t)); + *status = 3; + return; + } + } + if(buzzdict_size(VM->swarms)>1) { + sid = 2; + if(*buzzdict_get(VM->swarms, &sid, uint8_t) && + buzzdarray_get(e->swarms, 0, uint16_t) != sid) { + fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", + sid, + buzzdarray_get(e->swarms, 0, uint16_t)); + *status = 3; + return; + } + } + } } + /*Step through the buzz script*/ + void update_sensors(){ + /* Update sensors*/ + buzzuav_closures::buzzuav_update_battery(VM); + buzzuav_closures::buzzuav_update_prox(VM); + buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::update_neighbors(VM); + update_users(); + buzzuav_closures::buzzuav_update_flight_status(VM); + } void buzz_script_step() { - /* - * Update sensors - */ - 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); - buzzuav_closures::buzzuav_update_flight_status(VM); - - //buzz_update_users_stigmergy(tbl); - - - /* - * Call Buzz step() function - */ - if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { - fprintf(stderr, "%s: execution terminated abnormally: %s\n\n", + /* Process messages */ + buzzvm_process_inmsgs(VM); + /*Update sensors*/ + update_sensors(); + /* Call Buzz step() function */ + if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { + fprintf(stderr, "%s: execution terminated abnormally: %s\n\n", BO_FNAME, buzz_error_info()); - buzzvm_dump(VM); - } - //buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step - //usleep(10000); - /*Print swarm*/ - //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); - //int SwarmSize = buzzdict_size(VM->swarmmembers)+1; - //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); + buzzvm_dump(VM); + } + /* Process out messages */ + buzzvm_process_outmsgs(VM); + /*Print swarm*/ + //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); + //int SwarmSize = buzzdict_size(VM->swarmmembers)+1; + //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); + set_robot_var(buzzdict_size(VM->swarmmembers)+1); - /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ -// int status = 1; -// buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); + /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ + //int status = 1; + //buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); } /****************************************/ @@ -553,6 +699,8 @@ buzzvm_dup(VM); buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::update_neighbors(VM); + update_users(); buzzuav_closures::buzzuav_update_flight_status(VM); int a = buzzvm_function_call(VM, "step", 0); @@ -563,26 +711,15 @@ buzzvm_dup(VM); return a == BUZZVM_STATE_READY; } -/* uint16_t get_robotid() { - // Get hostname - char hstnm[30]; - gethostname(hstnm, 30); - // Make numeric id from hostname - // NOTE: here we assume that the hostname is in the format Knn - int id = strtol(hstnm + 4, NULL, 10); - //fprintf(stdout, "Robot id from get rid buzz util: %i\n",id); - return (uint16_t)id; - }*/ - buzzvm_t get_vm() { return VM; } -void set_robot_var(int ROBOTS){ - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, ROBOTS); - buzzvm_gstore(VM); -} + void set_robot_var(int ROBOTS){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, ROBOTS); + buzzvm_gstore(VM); + } } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 7451a73..a54aeca 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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 @@ -19,12 +19,15 @@ namespace buzzuav_closures{ static float batt[3]; static float obst[5]={0,0,0,0,0}; static double cur_pos[3]; - static double users_pos[3]; static uint8_t status; static int cur_cmd = 0; static int rc_cmd=0; static int buzz_cmd=0; static float height=0; + + + std::map< int, buzz_utility::Pos_struct> neighbors_map; + /****************************************/ /****************************************/ @@ -246,11 +249,33 @@ namespace buzzuav_closures{ cur_pos[1]=longitude; cur_pos[2]=altitude; } - void set_userspos(double latitude, double longitude, double altitude){ - users_pos[0]=latitude; - users_pos[1]=longitude; - users_pos[2]=altitude; + /*adds neighbours position*/ + void neighbour_pos_callback(int id, float range, float bearing, float elevation){ + buzz_utility::Pos_struct pos_arr; + pos_arr.x=range; + pos_arr.y=bearing; + pos_arr.z=elevation; + map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id); + if(it!=neighbors_map.end()) + neighbors_map.erase(it); + neighbors_map.insert(make_pair(id, pos_arr)); } + + /* update at each step the VM table */ + void update_neighbors(buzzvm_t vm){ + /* Reset neighbor information */ + buzzneighbors_reset(vm); + /* Get robot id and update neighbor information */ + map< int, buzz_utility::Pos_struct >::iterator it; + for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){ + buzzneighbors_add(vm, + it->first, + (it->second).x, + (it->second).y, + (it->second).z); + } + } + /****************************************/ int buzzuav_update_currentpos(buzzvm_t vm) { buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); @@ -270,26 +295,6 @@ namespace buzzuav_closures{ buzzvm_gstore(vm); return vm->state; } - buzzobj_t buzzuav_update_userspos(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1)); - buzzvm_pushf(vm, users_pos[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1)); - buzzvm_pushf(vm, users_pos[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1)); - buzzvm_pushf(vm, users_pos[2]); - buzzvm_tput(vm); - buzzvm_gstore(vm); - - return buzzvm_stack_at(vm, 0); - //return vm->state; - } void flight_status_update(uint8_t state){ status=state; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index a948de0..1970b41 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -13,7 +13,9 @@ namespace rosbzz_node{ /*Initialize publishers, subscribers and client*/ Initialize_pub_sub(n_c); /*Compile the .bzz file to .basm, .bo and .bdbg*/ - Compile_bzz(); + std::string fname = Compile_bzz(bzzfile_name); + bcfname = fname + ".bo"; + dbgfname = fname + ".bdb"; set_bzz_file(bzzfile_name.c_str()); /*Initialize variables*/ // Solo things @@ -22,14 +24,15 @@ namespace rosbzz_node{ multi_msg = true; // set stream rate - wait for the FC to be started SetStreamRate(0, 10, 1); - /// Get Robot Id - wait for Xbee to be started + // Get Robot Id - wait for Xbee to be started if(xbeeplugged) GetRobotId(); else - robot_id=strtol(robot_name.c_str() + 5, NULL, 10);; + 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; + + home[0]=0.0;home[1]=0.0;home[2]=0.0; } /*--------------------- @@ -49,7 +52,7 @@ namespace rosbzz_node{ /* mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; - mavros_msgs::ParamGet::Response robot_id_srv_response; + mavros_msgs::ParamGet::Response robot_id_srv_response; while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ ros::Duration(0.1).sleep(); ROS_ERROR("Waiting for Xbee to respond to get device ID"); @@ -68,39 +71,39 @@ namespace rosbzz_node{ /* Set the Buzz bytecode */ if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { fprintf(stdout, "Bytecode file found and set\n"); - //init_update_monitor(bcfname.c_str(),stand_by.c_str()); + std::string standby_bo = Compile_bzz(stand_by) + ".bo"; + init_update_monitor(bcfname.c_str(),standby_bo.c_str()); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// while (ros::ok() && !buzz_utility::buzz_script_done()) { - std::cout<0) no_of_robots =neighbours_pos_map.size()+1; - buzz_utility::set_robot_var(no_of_robots); + //buzz_utility::set_robot_var(no_of_robots); /*Set no of robots for updates*/ updates_set_robots(no_of_robots); - /*run once*/ - ros::spinOnce(); + /*run once*/ + ros::spinOnce(); /*loop rate of ros*/ ros::Rate loop_rate(10); loop_rate.sleep(); @@ -109,10 +112,9 @@ namespace rosbzz_node{ else fcu_timeout -= 1/10; /*sleep for the mentioned loop rate*/ - timer_step+=1; + timer_step+=1; maintain_pos(timer_step); - - + } /* Destroy updater and Cleanup */ //update_routine(bcfname.c_str(), dbgfname.c_str(),1); @@ -152,7 +154,8 @@ namespace rosbzz_node{ if(!xbeeplugged){ if(n_c.getParam("name", robot_name)); else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");} - } + }else + n_c.getParam("xbee_status_srv", xbeesrv_name); GetSubscriptionParameters(n_c); // initialize topics to null? @@ -220,8 +223,8 @@ namespace rosbzz_node{ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); mav_client = n_c.serviceClient(fcclient_name); - if(rcclient==true) - service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this); + if(rcclient==true) + service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this); ROS_INFO("Ready to receive Mav Commands from RC client"); xbeestatus_srv = n_c.serviceClient(xbeesrv_name); stream_client = n_c.serviceClient(stream_client_name); @@ -259,28 +262,35 @@ namespace rosbzz_node{ /*-------------------------------------------------------- / Create Buzz bytecode from the bzz script inputed /-------------------------------------------------------*/ - void roscontroller::Compile_bzz(){ + std::string roscontroller::Compile_bzz(std::string bzzfile_name){ /*TODO: change to bzzc instead of bzzparse and also add -I for includes*/ /*Compile the buzz code .bzz to .bo*/ stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")); - bzzfile_in_compile<::iterator it = neighbours_pos_map.find(id); if(it!=neighbours_pos_map.end()) - neighbours_pos_map.erase(it); + neighbours_pos_map.erase(it); neighbours_pos_map.insert(make_pair(id, pos_arr)); } /*----------------------------------------------------------------------------------- @@ -508,7 +517,7 @@ namespace rosbzz_node{ void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){ map< int, buzz_utility::Pos_struct >::iterator it = raw_neighbours_pos_map.find(id); if(it!=raw_neighbours_pos_map.end()) - raw_neighbours_pos_map.erase(it); + raw_neighbours_pos_map.erase(it); raw_neighbours_pos_map.insert(make_pair(id, pos_arr)); } @@ -566,95 +575,92 @@ namespace rosbzz_node{ void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){ - // calculate earth radii - double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); - double prime_vertical_radius = equatorial_radius * sqrt(temp); - double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; - double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)); + // calculate earth radii + /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); + double prime_vertical_radius = equatorial_radius * sqrt(temp); + double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; + double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));*/ /*double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - double ned[3]; - ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; - ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS - double ecef[3]; - double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2]; - double d = WGS84_E * sin(llh[0]); - double N = WGS84_A / sqrt(1. - d*d); - ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double ref_ecef[3]; - llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2]; - d = WGS84_E * sin(llh[0]); - N = WGS84_A / sqrt(1. - d*d); - ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double M[3][3]; - ecef2ned_matrix(ref_ecef, M); - double ned[3]; - matrix_multiply(3, 3, 1, (double *)M, ecef, ned); - - out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]); - out[0] = std::floor(out[0] * 1000000) / 1000000; + double d_lat = nei[0] - cur[0]; + double ned[3]; + ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; + ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS + double ecef[3]; + double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2]; + double d = WGS84_E * sin(llh[0]); + double N = WGS84_A / sqrt(1. - d*d); + ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); + ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); + ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); + double ref_ecef[3]; + llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2]; + d = WGS84_E * sin(llh[0]); + N = WGS84_A / sqrt(1. - d*d); + ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); + ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); + ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); + double M[3][3]; + ecef2ned_matrix(ref_ecef, M); + double ned[3]; + matrix_multiply(3, 3, 1, (double *)M, ecef, ned); + + out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]); + out[0] = std::floor(out[0] * 1000000) / 1000000; out[1] = atan2(ned[1],ned[0]); out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = 0.0;*/ - 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])); + 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[0] = std::floor(out[0] * 1000000) / 1000000; + //out[0] = std::floor(out[0] * 1000000) / 1000000; out[1] = atan2(ned_y,ned_x); - //out[1] = std::floor(out[1] * 1000000) / 1000000; + //out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = 0.0; - - - } void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){ - // calculate earth radii - /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); - double prime_vertical_radius = equatorial_radius * sqrt(temp); - double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; - double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)); - - double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; - out[0] = std::floor(out[0] * 1000000) / 1000000; - out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS - out[1] = std::floor(out[1] * 1000000) / 1000000; + // calculate earth radii + /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); + double prime_vertical_radius = equatorial_radius * sqrt(temp); + double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; + double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)); + + double d_lon = nei[1] - cur[1]; + double d_lat = nei[0] - cur[0]; + out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; + out[0] = std::floor(out[0] * 1000000) / 1000000; + out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS + out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = cur[2]; - // Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav) - double ecef[3]; - double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2]; - double d = WGS84_E * sin(llh[0]); - double N = WGS84_A / sqrt(1. - d*d); - ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double ref_ecef[3]; - llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2]; - d = WGS84_E * sin(llh[0]); - N = WGS84_A / sqrt(1. - d*d); - ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double M[3][3]; - ecef2ned_matrix(ref_ecef, M); - matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/ - - double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - out[0] = DEG2RAD(d_lat) * EARTH_RADIUS; - //out[0] = std::floor(out[0] * 1000000) / 1000000; - out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); - //out[1] = std::floor(out[1] * 1000000) / 1000000; + // Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav) + double ecef[3]; + double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2]; + double d = WGS84_E * sin(llh[0]); + double N = WGS84_A / sqrt(1. - d*d); + ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); + ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); + ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); + double ref_ecef[3]; + llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2]; + d = WGS84_E * sin(llh[0]); + N = WGS84_A / sqrt(1. - d*d); + ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); + ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); + ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); + double M[3][3]; + ecef2ned_matrix(ref_ecef, M); + matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/ + + double d_lon = nei[1] - cur[1]; + double d_lat = nei[0] - cur[0]; + out[0] = DEG2RAD(d_lat) * EARTH_RADIUS; + //out[0] = std::floor(out[0] * 1000000) / 1000000; + out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + //out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = 0.0; } @@ -718,7 +724,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); } } @@ -766,7 +773,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!"); } @@ -861,10 +868,12 @@ namespace rosbzz_node{ /*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]); /*Put RID and pos*/ - raw_neighbours_pos_put((int)out[1],raw_neigh_pos); + raw_neighbours_pos_put((int)out[1],raw_neigh_pos); + /* TODO: remove roscontroller local map array for neighbors */ neighbours_pos_put((int)out[1],n_pos); + buzzuav_closures::neighbour_pos_callback((int)out[1],n_pos.x,n_pos.y,n_pos.z); delete[] out; - buzz_utility::in_msg_process((message_obt+3)); + buzz_utility::in_msg_append((message_obt+3)); } } diff --git a/src/stand_by.basm b/src/stand_by.basm deleted file mode 100644 index 4033176..0000000 --- a/src/stand_by.basm +++ /dev/null @@ -1,144 +0,0 @@ -!19 -'updated -'update_ack -'init -'barrier -'stigmergy -'create -'put -'id -'barrier_val -'onconflict -'data -'stand_by -'get -'size -'neighbors -'listen -'update_no -'ROBOTS -'step - - pushs 2 - pushcn @__label_1 - gstore - pushs 11 - pushcn @__label_5 - gstore - pushs 18 - pushcn @__label_9 - gstore - nop - -@__label_0 - pushs 0 |1,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 1 |1,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gstore |1,20,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - -@__exitpoint - done - -@__label_1 - pushs 3 |4,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 4 |4,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |4,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 5 |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |4,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 101 |4,27,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 1 |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gstore |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 3 |5,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |5,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 6 |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |5,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 7 |5,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |5,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 1 |5,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 2 |5,17,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |5,17,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 8 |6,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 0 |6,12,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gstore |6,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 3 |7,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |7,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 9 |7,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |7,18,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushl @__label_2 |7,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 1 |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret0 |12,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - -@__label_2 - lload 3 |8,4,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 10 |8,6,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |8,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lload 2 |8,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 10 |8,16,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |8,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lt |8,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lload 3 |8,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 10 |8,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |8,33,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lload 2 |8,37,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 10 |8,39,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |8,44,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - eq |8,44,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - or |8,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - jumpz @__label_3 |8,47,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lload 2 |8,55,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret1 |8,55,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - jump @__label_4 |9,5,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz -@__label_3 |9,5,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lload 3 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret1 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz -@__label_4 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret0 |10,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - -@__label_5 - pushs 3 |16,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |16,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 12 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |16,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 7 |16,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |16,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 1 |16,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |16,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 8 |17,12,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 3 |17,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |17,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 13 |17,22,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |17,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 0 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gstore |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 14 |19,9,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |19,9,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 15 |19,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |19,16,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 0 |19,24,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |19,24,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushl @__label_6 |20,3,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 2 |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret0 |25,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - -@__label_6 - lload 2 |21,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 16 |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - eq |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - jumpz @__label_7 |21,27,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 8 |21,38,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 17 |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gstore |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz -@__label_7 |22,3,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret0 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - -@__label_9 - pushs 11 |30,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |30,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 0 |30,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |30,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret0 |32,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz diff --git a/src/stand_by.bdb b/src/stand_by.bdb deleted file mode 100644 index 72f975b..0000000 Binary files a/src/stand_by.bdb and /dev/null differ diff --git a/src/stand_by.bdbg b/src/stand_by.bdbg deleted file mode 100644 index cda97bc..0000000 Binary files a/src/stand_by.bdbg and /dev/null differ diff --git a/src/stand_by.bo b/src/stand_by.bo deleted file mode 100644 index 62e6638..0000000 Binary files a/src/stand_by.bo and /dev/null differ