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/buzzuav_closures.h b/include/buzzuav_closures.h index 4a586ae..2e62d32 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -46,7 +46,6 @@ 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*/ @@ -85,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/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 100% rename from src/testflockfev.bzz rename to script/testflockfev.bzz 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 90% rename from src/testflocksim.bzz rename to script/testflocksim.bzz index fa5f3f5..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 @@ -147,11 +147,20 @@ function table_print(t) { }) } +######################################## +# +# 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" } @@ -202,15 +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 - t = vt.get("p") - log(t) - table_print(t) + #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 ", vt.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_utility.cpp b/src/buzz_utility.cpp index 18201c7..dfe19d6 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -66,29 +66,29 @@ namespace buzz_utility{ //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_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_tget(VM);*/ + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); buzzvm_push(VM, t); - buzzvm_pushi(VM, 2); - buzzvm_callc(VM); + 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_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_callc(VM); + 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 */ @@ -129,7 +129,7 @@ namespace buzz_utility{ 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); + /*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)); @@ -137,7 +137,7 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); buzzvm_push(VM, nbr); buzzvm_pushi(VM, 2); - buzzvm_callc(VM); + buzzvm_callc(VM);*/ //buzzvm_gstore(VM); return VM->state; } @@ -358,7 +358,6 @@ 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); @@ -368,19 +367,23 @@ static int create_stig_tables() { // 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_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); @@ -451,7 +454,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); @@ -459,7 +462,12 @@ 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); @@ -626,7 +634,6 @@ static int create_stig_tables() { buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::update_neighbors(VM); update_users(); - buzzuav_closures::buzzuav_update_userspos(VM); buzzuav_closures::buzzuav_update_flight_status(VM); } @@ -645,13 +652,14 @@ static int create_stig_tables() { /* Process out messages */ buzzvm_process_outmsgs(VM); /*Print swarm*/ - buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); + //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); + //int status = 1; + //buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); } /****************************************/ diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 2638f80..afaf1a9 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -19,7 +19,6 @@ 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; @@ -275,19 +274,7 @@ namespace buzzuav_closures{ (it->second).z); } } - 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; - } + /****************************************/ int buzzuav_update_currentpos(buzzvm_t vm) { buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); @@ -307,30 +294,6 @@ namespace buzzuav_closures{ buzzvm_gstore(vm); 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); - 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); - buzzobj_t tbl = buzzvm_stack_at(vm, 0); - buzzvm_gstore(vm); - - return tbl; - //return vm->state; - } void flight_status_update(uint8_t state){ status=state; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5691d58..e2e11f9 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 @@ -68,7 +70,8 @@ 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 ////////////////////////////////////////////////////// @@ -95,7 +98,7 @@ namespace rosbzz_node{ //no_of_robots=get_number_of_robots(); get_number_of_robots(); //if(neighbours_pos_map.size() >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*/ @@ -258,28 +261,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<